File content as of revision 4:dc844fde64d7:

#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)
{
    // -----------------------------------------------------------------------------
    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;
    this->with_ahrs = false;
// -----------------------------------------------------------------------------
    parametrize_controllers();
    
}       // 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)
{
    // -----------------------------------------------------------------------------
    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;
    this->with_ahrs = wa;           // loop ahrs internally or externally
    // -----------------------------------------------------------------------------
    parametrize_controllers();
    
}       // 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(); 
            // HACK!! Run 2 other EKFs and log data
            cntrl_ahrs.ekf_rp.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_acc[0],data.sens_acc[1]);
            data.est_RP_RPY[0] = cntrl_ahrs.getRoll(3);
            data.est_RP_RPY[1] = cntrl_ahrs.getPitch(3);
            cntrl_ahrs.ekf_rpy.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_gyr[2],data.sens_acc[0],data.sens_acc[1],data.sens_mag[0],data.sens_mag[1]);
            data.est_RP_RPY[2] = cntrl_ahrs.getRoll(4);
            data.est_RP_RPY[3] = cntrl_ahrs.getPitch(4);
            data.est_RP_RPY[4] = cntrl_ahrs.getYaw(4);              
            }
        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
                {
                    data.cntrl_vel_xyz_des[0] = scale_PM1_to_vel * myRC.PM1[0];     // x-direction
                    data.cntrl_vel_xyz_des[1] = -scale_PM1_to_vel * myRC.PM1[1];    // y-direction, Stick reversed (Stick to the right is +1 (-y))
                }
            if(Vel_Controller_Running)
                {   // here directions cross: vel contrl in +x -> do a positive pitch (axis[1])
                    //                        vel contrl in +y -> do a negative roll  (axis[0])
                    data.cntrl_att_rpy_des[1] = vel_cntrl[0](data.cntrl_vel_xyz_des[0] - data.est_Vxyz[0]);
                    data.cntrl_att_rpy_des[0] = -vel_cntrl[1](data.cntrl_vel_xyz_des[1] - data.est_Vxyz[1]);
                }
            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]);
            data.F_Thrust = pos_cntrl[2](calc_des_pos(myRC.PM1[3]) -  data.est_xyz[2]);    // see function below
            }
        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]);
            data.cntrl_rate_rpy_des[2] = Kv[2] * (calc_des_yaw(-myRC.PM1[2]) - data.est_RPY[2]);
            }
        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, 
            data.cntrl_rate_rpy_des[2] = -scale_PM1_to_angle * myRC.PM1[2];      // Yaw-axis reversed 
            }            
        }       // 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);
des_z = 0.0;
des_yaw = data.est_RPY[2];
}

// 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){
    des_vel = deadzone(des_vel,-.1,.1);
    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;
    data.cntrl_pos_xyz_des[2] = des_z;
    return des_z;
    }
// ---------------------------------------------------------------
void Controller_Loops::reset_des_z(void){
    des_z = 0.0;
}
// ---------------------------------------------------------------
void Controller_Loops::reset_des_z(float init){
    this->des_z = init;
}
// ---------------------------------------------------------------
float Controller_Loops::calc_des_yaw(float des_yaw_rate){
    des_yaw_rate = deadzone(des_yaw_rate,-0.04f,0.04f);
    if(((des_yaw - data.est_RPY[2]) < max_delta_yaw) & des_yaw_rate > 0)
        des_yaw += Ts * downsamp_1 * des_yaw_rate * max_yaw_rate;
    else if(((des_yaw - data.est_RPY[2]) > -max_delta_yaw) & des_yaw_rate < 0)
        des_yaw += Ts * downsamp_1 * des_yaw_rate * max_yaw_rate;
    data.cntrl_att_rpy_des[2] = des_yaw;
    return des_yaw;
    }
// ---------------------------------------------------------------
void Controller_Loops::reset_des_yaw(float init){
    this->des_yaw = init;
}
// ------------------- destructor -----------------
Controller_Loops::~Controller_Loops() {
    ticker.detach();
    }
    
    
// ------------------------------------------------------------    
void Controller_Loops::enable_acro(void)
{
    Rate_Controller_Running = true;
    Attitude_Controller_Running = false;    
    Althold_Controller_Running = false;
    Vel_Controller_Running = false;
    
    }
// ------------------------------------------------------------    
void Controller_Loops::enable_stabilized(void)
{
    Rate_Controller_Running = true;   
    Attitude_Controller_Running = true;    
    Althold_Controller_Running = false;
    Vel_Controller_Running = false;
    }
// ------------------------------------------------------------    
void Controller_Loops::enable_alt_hold(void)
{
    Rate_Controller_Running = true;   
    Attitude_Controller_Running = true;    
    Althold_Controller_Running = true;  
    Vel_Controller_Running = false;
    
    }
// ------------------------------------------------------------    
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
}
// ------------------------------------------------------------    
void Controller_Loops::parametrize_controllers(){
    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( 4.21, 7.64, -0.3, 0.0713,Ts*(float)downsamp_2,5.0,15.0);   // PID vz: Kp = 4.21, Ki = 7.64, Kd = -0.3, Tf = 0.0713 not used
    pos_cntrl[2].setCoefficients(8.08, 9.66, 3.71, 0.0262,Ts*(float)downsamp_1,5.0f,22.0f);     // Output is Thrust/N
    
    rate_cntrl[0].setCoefficients(0.2,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;
    Kv[2] = 2.0f;

    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.25f;               // maximum allowed tracking error for z-Lift
    max_climb_rate = 0.7f;          // climbrate in z
    max_delta_yaw = 0.4f;           // maximum allowed tracking error for yaw
    max_yaw_rate = 1.5f;          // max yawrate
    
  
    }
    
// *****************************************************************************
// Dead zone, from lo ... hi
float Controller_Loops::deadzone(float x,float lo,float hi){
return (x>hi)*(x-hi)+(x<lo)*(x-lo);
}