File content as of revision 0:a479dc61e931:

#include "Controller_Loops.h"
using namespace std;

Controller_Loops::Controller_Loops(float Ts,uint8_t ds1,uint8_t ds2)
{
    // -----------------------------------------------------------------------------
    rate_cntrl[0].set_Coefficients(0.2,0.15,0.005,0.020, TsCntrl, 0.0f, 0.0f);            // Pxh, similar to pixhawk default
    rate_cntrl[1].set_Coefficients(0.2,0.15,0.005,0.020, TsCntrl, 0.0f, 0.0f);
    rate_cntrl[2].set_Coefficients(0.234485, 0.0, 0.00320, 0.05, TsCntrl, 0.0f, 0.0f);            //
    // -----------------------------------------------------------------------------
    this->Ts = Ts;
    if(ds1==0) ds1 = 1;
    if(ds1==0) ds2 = 1;
    downsamp_1 = ds1;
    downsamp_2 = ds2;
    downsamp_counter_1 = 0;

    vel_cntrl[0].set_Coefficients( 0.2, 0.02, 0.04, 0.05,TsCntrl/(float)downsamp_2,-.1,.1);   // PID controller version
    vel_cntrl[1].set_Coefficients( 0.2, 0.02, 0.04, 0.05,TsCntrl/(float)downsamp_2,-.1,.1);   // PID controller version
    pos_cntrl[2].set_Coefficients((2.47,2.0, 2.38,0.05,TsCntrl/(float)downsamp_2,6.0,15.0);
 
    scale_PM1_to_vel = 1.5f;
    scale_PM1_to_rate = 3.0f;
    scale_PM1_to_angle = 0.5f;
    max_delta = 0.1f;
    max_climb_rate = 0.7f;

}
// ---------------- Controller_Loops::init_loops -------------------
void Controller_Loops::init_loops(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;
    thread.start(callback(this, &Controller_Loops::loop));
    ticker.attach(callback(this, &Controller_Loops::sendSignal), Ts);
}
// ---------------- Controller_Loops::run_loops -------------------
void Controller_Loops::run_loops(void){
}


void Controller_Loops::loop(void){
float motor_speeds[4];
uint8_t k;
while(1){
    thread.signal_wait(signal);
    imu.readGyro();
    imu.readAccel();
    imu.readMag();
    sens_gyr[0] = raw_gx2gx(imu.gyroX));
    sens_gyr[1] = raw_gy2gy(imu.gyroY));
    sens_gyr[2] = raw_gz2gz(imu.gyroZ));
    sens_acc[0] = raw_ax2ax(imu.accX));
    sens_acc[1] = raw_ay2ay(imu.accY));
    sens_acc[2] = raw_az2az(imu.accZ));
    if(Rate_controller_running){
        for(k=0;k<3;k++)
            Mxyz[k]= rate_cntrl(data.cntrl_rate_rpy_des[k] - data.sens_gyr[k], data.sens_gyr[k]);
        }
    else
        {
        for(k=0;k<3;k++)
            Mxyz[k]=0.0;    
        data.F_Thrust = 0.0;        // be shure that thrust is down
        }
    copter.motor_mix(Mxyz,motor_speeds);        // mix torques and Thrust to motor speeds
    for(k = 0;k < copter.nb_motors;k++)
        motor_pwms[k].pulsewidth_us(copter.motors.n2pwm(motorspeed[k]));    // map motor speeds to pwm out
    // now calc all the other controllers!!!!
    downsampcounter_1++;
    downsampcounter_2++;
    if(downsampcounter_1 == downsamp_1)
        {
        downsampcounter_1 = 0;
    // first do the angle estimation
        ahrs.update();
        // ------ start different controller loops -------------------------------------------------
        // handle xy Direction differently!!!
        if(downsampcounter_2 == downsamp_2)
            {
            downsampcounter_2 = 0;
            if(Pos_Controller_Running){
                for(k=0;k<2;k++)
                    data.cntrl_vel_des[k] = pos_cntrl[k](data.cntrl_pos_des[k] -  data.est_xyz[k]);
                }
            else{
                for(k=0;k<2;k++)
                    data.cntrl_vel_des[k] = scale_PM1_to_vel * data.RC_PM1[k];
                }
            if(Vel_Controller_Running){
                for(k=0;k<2;k++)
                    data.cntrl_att_rpy_des[k] = vel_cntrl[k](data.cntrl_vel_des[k] - est_vel[k]);
                }
            else{
                for(k=0;k<2;k++)
                    data.cntrl_att_rpy_des[k] = data.RC_PM1[k];    
                }
            }   // if downsamp_2 end of slowest loop
        if(Althold_Controller_Running){
            data.cntrl_vel_des[2] = pos_cntrl[2](calc_des_pos(data.RC_PM1[3]) -  data.est_xyz[2]);    // see function below
            data.F_Thrust = vel_cntrl[2](data.cntrl_vel_des[2] -  data.est_vxyz[2]);
            }
        else{
            data.F_Thrust = PM1_2_F_Thrust(data.RC_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_2_angle(data.RC_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 * data.RC_PM1[2];      // Acro mode, 
        }       // if(downsamp ...
    }   // the thread
}
// ------------------- start controllers ----------------
void Controller_Loops::start_controller(void){
    thread.start(callback(this, &Controller_Loops::controller_main_loop));
    ticker.attach(callback(this, &Controller_Loops::sendSignal), Ts);
}

// this is for realtime OS
void Controller_Loops::sendSignal() {
    thread.signal_set(signal);
}

// --------------- calc_des_pos ---------------------------------------
// integrate desired velocity to position. Test, if tracking error not too large
float 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 reset_des_z(void){
    des_z = 0.0;
}
void reset_des_z(float init){
    des_z = init;
}