File content as of revision 1:d8c9f6b16279:

#include "Controller_Loops.h"
#include "AHRS.h"


//using namespace std;

// Constructor for the controller loop. In this loop all(!!) controllers are called 
// in sequence. 3 different cyle times are possible. The fast inner loop runs with 
// sample time Ts (Rate controller), the successing angle estimation (EKF) is 
// downsampled by ds1 and the outer loops with ds2 (Ts=1/200, typically ds1 = 2, ds2=4)
Controller_Loops::Controller_Loops(float Ts,uint8_t ds1,uint8_t ds2) : cntrl_ahrs(1,Ts * (float)ds1,true), thread(osPriorityHigh, 4096), dout1(PB_5)
{
    // -----------------------------------------------------------------------------
    rate_cntrl[0].setCoefficients(0.201,0.15,0.005,0.020, Ts, 0.0f, 0.0f);      // angular rate controller Roll
    rate_cntrl[1].setCoefficients(0.2,0.15,0.005,0.020, Ts, 0.0f, 0.0f);      // angular rate controller Pitch
    rate_cntrl[2].setCoefficients(0.234, 0.0, 0.00320, 0.05, Ts, 0.0f, 0.0f); // angular rate controller Yaw    
    Kv[0] = 4.0f;
    Kv[1] = 4.0f;
    // -----------------------------------------------------------------------------
    this->Ts = Ts;              // fast sample time
    if(ds1==0) ds1 = 1;         // downsampling of AHRS and angle control loop
    if(ds2==0) ds2 = 1;         //      "       of Position and velocity
    downsamp_1 = ds1;
    downsamp_2 = ds2;
    downsamp_counter_1 = 0;
    downsamp_counter_2 = 0;

    vel_cntrl[0].setCoefficients( 0.2, 0.02, 0.04, 0.05,Ts*(float)downsamp_2,-.1,.1);   // PID controller for velocity (OF), x
    vel_cntrl[1].setCoefficients( 0.2, 0.02, 0.04, 0.05,Ts*(float)downsamp_2,-.1,.1);   // PID controller for velocity (OF), y
//    vel_cntrl[2].setCoefficients( 0.2, 0.02, 0.04, 0.05,Ts*(float)downsamp_2,-.1,.1);   // PID controller for velocity (OF), y
    pos_cntrl[2].setCoefficients(2.47,2.0, 2.38,0.05,Ts*(float)downsamp_2,6.0,15.0);
 
    scale_PM1_to_vel = 1.5f;        // e.g. Stick to the right corresp. to 1.5m/s desired speed
    scale_PM1_to_rate = 3.0f;       
    scale_PM1_to_angle = 0.5f;      // e.g. Stick to the right corr. to 0.5 rad desired angle
    max_delta = 0.1f;               // maximum allowed tracking error for z-Lift
    max_climb_rate = 0.7f;          // climbrate in z
    this->with_ahrs = false;

}       // end of constructor 
Controller_Loops::Controller_Loops(float Ts,uint8_t ds1,uint8_t ds2, bool wa) :  cntrl_ahrs(1,Ts * (float)ds1,true), thread(osPriorityHigh, 4096), dout1(PB_5)
{
    // -----------------------------------------------------------------------------
    rate_cntrl[0].setCoefficients(0.201,0.15,0.005,0.020, Ts, -copter.max_Mxy, copter.max_Mxy);      // angular rate controller Roll
    rate_cntrl[1].setCoefficients(0.2,0.15,0.005,0.020, Ts,  -copter.max_Mxy, copter.max_Mxy);      // angular rate controller Pitch
    rate_cntrl[2].setCoefficients(0.234, 0.0, 0.00320, 0.05, Ts,  -copter.max_Mxy, copter.max_Mxy); // angular rate controller Yaw    
    Kv[0] = 4.0f;
    Kv[1] = 4.0f;

    // -----------------------------------------------------------------------------
    this->Ts = Ts;              // fast sample time
    if(ds1==0) ds1 = 1;         // downsampling of AHRS and angle control loop
    if(ds2==0) ds2 = 1;         //      "       of Position and velocity
    downsamp_1 = ds1;
    downsamp_2 = ds2;
    downsamp_counter_1 = 0;
    downsamp_counter_2 = 0;

    vel_cntrl[0].setCoefficients( 0.2, 0.02, 0.04, 0.05,Ts*(float)downsamp_2,-.1,.1);   // PID controller for velocity (OF), x
    vel_cntrl[1].setCoefficients( 0.2, 0.02, 0.04, 0.05,Ts*(float)downsamp_2,-.1,.1);   // PID controller for velocity (OF), y
//    vel_cntrl[2].setCoefficients( 0.2, 0.02, 0.04, 0.05,Ts*(float)downsamp_2,-.1,.1);   // PID controller for velocity (OF), y
    pos_cntrl[2].setCoefficients(2.47,2.0, 2.38,0.05,Ts*(float)downsamp_2,6.0,15.0);
 
    scale_PM1_to_vel = 1.5f;        // e.g. Stick to the right corresp. to 1.5m/s desired speed
    scale_PM1_to_rate = 3.0f;       
    scale_PM1_to_angle = 0.5f;      // e.g. Stick to the right corr. to 0.5 rad desired angle
    max_delta = 0.1f;               // maximum allowed tracking error for z-Lift
    max_climb_rate = 0.7f;          // climbrate in z
    this->with_ahrs = wa;           // loop ahrs internally or externally
}       // end of constructor 

// ---------------- Controller_Loops::init_loop -------------------
void Controller_Loops::init_loop(void){
    Rate_Controller_Running = false;         // Roll- Pitch and Yaw Rate Controllers
    Attitude_Controller_Running = false;     // Roll and Pitch angular controllers
    Yaw_Controller_Running = false;          // Yaw angular controllers (if Magnetometer on)
    Althold_Controller_Running = false;
    Vel_Controller_Running = false;
}

// ---------------- Controller_Loops::loop ------------------- the cyclic loop (THREAD)!
void Controller_Loops::loop(void){
uint8_t k;

while(1){
    thread.signal_wait(signal);
    dout1.write(1);
    if(with_ahrs)
        cntrl_ahrs.read_imu_sensors();
    else{};         // otherwise ahrs has its own thread
    dout1.write(0);
    mutex.lock();
    if(Rate_Controller_Running)     // calc the inner loop first! RollPitchYaw
        {
        for(k=0;k<3;k++)
            data.cntrl_Mxyz[k]= rate_cntrl[k](data.cntrl_rate_rpy_des[k] - data.sens_gyr[k], data.sens_gyr[k]);
        }
    else
        {
        for(k=0;k<3;k++)
            data.cntrl_Mxyz[k]=0.0f;    
        data.F_Thrust = 0.0;        // be shure that thrust is down
        }
    copter.motor_mix(data.cntrl_Mxyz,data.F_Thrust,data.wMot);        // mix torques and Thrust to motor speeds
    for(k = 0;k < copter.nb_motors;k++)
        motor_pwms[k].pulsewidth_us(copter.motor.n2pwm(data.wMot[k]));    // map motor speeds to pwm out
    // -------------------------------------------------------------------------
    // now calc all the other controllers!!!!
    downsamp_counter_1++;
    downsamp_counter_2++;
    if(downsamp_counter_1 == downsamp_1)
        {
        downsamp_counter_1 = 0;
    // first do the angle estimation
        if(with_ahrs)
            cntrl_ahrs.update();
        else{};         // otherwise ahrs has its own thread
        // ------ start different controller loops -------------------------------------------------
        // handle xy Direction and z differently!!!
        if(downsamp_counter_2 == downsamp_2)
            {
            downsamp_counter_2 = 0;
            if(Pos_Controller_Running)      // only xy
                {
                for(k=0;k<2;k++)
                    data.cntrl_vel_xyz_des[k] = pos_cntrl[k](data.cntrl_pos_xyz_des[k] -  data.est_xyz[k]);
                }
            else
                {
                for(k=0;k<2;k++)
                    data.cntrl_vel_xyz_des[k] = scale_PM1_to_vel * myRC.PM1[k];
                }
            if(Vel_Controller_Running)
                {
                for(k=0;k<2;k++)
                    data.cntrl_att_rpy_des[k] = vel_cntrl[k](data.cntrl_vel_xyz_des[k] - data.est_Vxyz[k]);
                }
            else{
                for(k=0;k<2;k++)
                    data.cntrl_att_rpy_des[k] = scale_PM1_to_angle * myRC.PM1[k];    
                }
            }   // if downsamp_2 end of slowest loop
        if(Althold_Controller_Running){
            data.cntrl_vel_xyz_des[2] = pos_cntrl[2](calc_des_pos(myRC.PM1[3]) -  data.est_xyz[2]);    // see function below
            data.F_Thrust = vel_cntrl[2](data.cntrl_vel_xyz_des[2] -  data.est_Vxyz[2]);
            }
        else{
            data.F_Thrust = PM1_2_F_Thrust(myRC.PM1[3]);
            }
        if(Attitude_Controller_Running) // control angles
            for(k=0;k<2;k++)
                data.cntrl_rate_rpy_des[k] = Kv[k] * (data.cntrl_att_rpy_des[k] - data.est_RPY[k]);
        else
            for(k=0;k<2;k++)            // contoll angular rates
                data.cntrl_rate_rpy_des[k] = scale_PM1_to_angle * myRC.PM1[k];      // Acro mode, 
        if(Yaw_Controller_Running) // control angles
            ;//to be done!!!! data.cntrl_rate_rpy_des[2] = Kv[2] * (data.cntrl_att_rpy_des[2] - data.est_RPY[2]); // handle Yaw in extra manner
        else
            data.cntrl_rate_rpy_des[2] = -scale_PM1_to_rate * myRC.PM1[2];      // Yaw: just control rate!
        }       // if(downsamp_1  ...
    mutex.unlock();
    }   // the thread
}
// ------------------- start controllers ----------------
void Controller_Loops::start_loop(void){
    thread.start(callback(this, &Controller_Loops::loop));
    ticker.attach(callback(this, &Controller_Loops::sendSignal), Ts);
}
// ------------------- start controllers ----------------
void Controller_Loops::reset_all(void){
for(uint8_t k = 0;k<3;k++)
    rate_cntrl[k].reset(0.0);
for(uint8_t k = 0;k<3;k++)
    vel_cntrl[k].reset(0.0);
for(uint8_t k = 0;k<3;k++)
    pos_cntrl[k].reset(0.0);

}

// this is for realtime OS
void Controller_Loops::sendSignal() {
    thread.signal_set(signal);
}
// *****************************************************************************
// nonlinear characteristics for thrust
float Controller_Loops::PM1_2_F_Thrust(float x)
{
    return 9.81f*copter.weight*(0.5f*x+1.1f - 0.06f/(x+1.1f));         // y=0.5*x+1.1-0.06./(x+1.1);
}

// --------------- calc_des_pos ---------------------------------------
// integrate desired velocity to position. Test, if tracking error not too large
float Controller_Loops::calc_des_pos(float des_vel){
    if(((des_z - data.est_xyz[2]) < max_delta) & des_vel > 0)
        des_z += Ts * downsamp_1 * des_vel * max_climb_rate;
    else if(((des_z - data.est_xyz[2]) > -max_delta) & des_vel < 0)
        des_z += Ts * downsamp_1 * des_vel * max_climb_rate;
    return des_z;
    }
void Controller_Loops::reset_des_z(void){
    des_z = 0.0;
}
void Controller_Loops::reset_des_z(float init){
    des_z = init;
}
// ------------------- destructor -----------------
Controller_Loops::~Controller_Loops() {
    ticker.detach();
    }
    
    
// ------------------------------------------------------------    
void Controller_Loops::enable_acro(void)
{
    Rate_Controller_Running = true;
    pc.printf("Cntrls Enabled ACRO\r\n");

    }
// ------------------------------------------------------------    
void Controller_Loops::enable_stabilized(void)
{
    Rate_Controller_Running = true;   
    Attitude_Controller_Running = true;    
    //pc.printf("Cntrls Enabled STABILIZED\r\n");
    }
// ------------------------------------------------------------    
void Controller_Loops::enable_alt_hold(void)
{
    Rate_Controller_Running = true;   
    Attitude_Controller_Running = true;    
    Althold_Controller_Running = true;  
    }
// ------------------------------------------------------------    
void Controller_Loops::enable_vel_of_z_pos(void)
{
    Rate_Controller_Running = true;   
    Attitude_Controller_Running = true;    
    Althold_Controller_Running = true;
    Vel_Controller_Running = true;
    }
// ------------------------------------------------------------    
void Controller_Loops::disable_all(void)
{
    Rate_Controller_Running = false;   
    Attitude_Controller_Running = false;    
    Althold_Controller_Running = false;
    Vel_Controller_Running = false;
    }
// ------------------------------------------------------------    
void Controller_Loops::set_controller_limits(UAV copter)
{
rate_cntrl[0].set_limits(-copter.max_Mxy, copter.max_Mxy);      // angular rate controller Roll
rate_cntrl[1].set_limits(-copter.max_Mxy, copter.max_Mxy);      // angular rate controller Pitch      
rate_cntrl[2].set_limits(-copter.max_Mz, copter.max_Mz);      // angular rate controller YAW
}