altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Threads_and_main/Controller_Loops.cpp@3:bc24fee36ba3, 2019-10-28 (annotated)

Committer:
altb2
Date:
Mon Oct 28 07:54:10 2019 +0000
Revision:
3:bc24fee36ba3
Parent:
2:e7874762cc25 
- with complementFilter for Yaw (Magneteometer), Lidar, AltHold running, Flow sensor reading, control sth wrong, Yaw not tested in detail

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:a479dc61e931 1 #include "Controller_Loops.h"
altb2 1:d8c9f6b16279 2 #include "AHRS.h"
altb2 1:d8c9f6b16279 3
altb2 1:d8c9f6b16279 4
altb2 1:d8c9f6b16279 5 //using namespace std;
altb2 0:a479dc61e931 6
altb2 1:d8c9f6b16279 7 // Constructor for the controller loop. In this loop all(!!) controllers are called
altb2 1:d8c9f6b16279 8 // in sequence. 3 different cyle times are possible. The fast inner loop runs with
altb2 1:d8c9f6b16279 9 // sample time Ts (Rate controller), the successing angle estimation (EKF) is
altb2 1:d8c9f6b16279 10 // downsampled by ds1 and the outer loops with ds2 (Ts=1/200, typically ds1 = 2, ds2=4)
altb2 1:d8c9f6b16279 11 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)
altb2 0:a479dc61e931 12 {
altb2 0:a479dc61e931 13 // -----------------------------------------------------------------------------
altb2 2:e7874762cc25 14 this->Ts = Ts; // fast sample time
altb2 2:e7874762cc25 15 if(ds1==0) ds1 = 1; // downsampling of AHRS and angle control loop
altb2 2:e7874762cc25 16 if(ds2==0) ds2 = 1; // " of Position and velocity
altb2 2:e7874762cc25 17 downsamp_1 = ds1;
altb2 2:e7874762cc25 18 downsamp_2 = ds2;
altb2 2:e7874762cc25 19 downsamp_counter_1 = 0;
altb2 2:e7874762cc25 20 downsamp_counter_2 = 0;
altb2 2:e7874762cc25 21 this->with_ahrs = false;
altb2 2:e7874762cc25 22 // -----------------------------------------------------------------------------
altb2 2:e7874762cc25 23 parametrize_controllers();
altb2 2:e7874762cc25 24
altb2 2:e7874762cc25 25 } // end of constructor
altb2 2:e7874762cc25 26 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)
altb2 2:e7874762cc25 27 {
altb2 0:a479dc61e931 28 // -----------------------------------------------------------------------------
altb2 1:d8c9f6b16279 29 this->Ts = Ts; // fast sample time
altb2 1:d8c9f6b16279 30 if(ds1==0) ds1 = 1; // downsampling of AHRS and angle control loop
altb2 1:d8c9f6b16279 31 if(ds2==0) ds2 = 1; // " of Position and velocity
altb2 0:a479dc61e931 32 downsamp_1 = ds1;
altb2 0:a479dc61e931 33 downsamp_2 = ds2;
altb2 0:a479dc61e931 34 downsamp_counter_1 = 0;
altb2 1:d8c9f6b16279 35 downsamp_counter_2 = 0;
altb2 2:e7874762cc25 36 this->with_ahrs = wa; // loop ahrs internally or externally
altb2 1:d8c9f6b16279 37 // -----------------------------------------------------------------------------
altb2 2:e7874762cc25 38 parametrize_controllers();
altb2 2:e7874762cc25 39
altb2 1:d8c9f6b16279 40 } // end of constructor
altb2 1:d8c9f6b16279 41
altb2 1:d8c9f6b16279 42 // ---------------- Controller_Loops::init_loop -------------------
altb2 1:d8c9f6b16279 43 void Controller_Loops::init_loop(void){
altb2 1:d8c9f6b16279 44 Rate_Controller_Running = false; // Roll- Pitch and Yaw Rate Controllers
altb2 1:d8c9f6b16279 45 Attitude_Controller_Running = false; // Roll and Pitch angular controllers
altb2 0:a479dc61e931 46 Yaw_Controller_Running = false; // Yaw angular controllers (if Magnetometer on)
altb2 0:a479dc61e931 47 Althold_Controller_Running = false;
altb2 0:a479dc61e931 48 Vel_Controller_Running = false;
altb2 0:a479dc61e931 49 }
altb2 0:a479dc61e931 50
altb2 1:d8c9f6b16279 51 // ---------------- Controller_Loops::loop ------------------- the cyclic loop (THREAD)!
altb2 0:a479dc61e931 52 void Controller_Loops::loop(void){
altb2 0:a479dc61e931 53 uint8_t k;
altb2 1:d8c9f6b16279 54
altb2 0:a479dc61e931 55 while(1){
altb2 0:a479dc61e931 56 thread.signal_wait(signal);
altb2 1:d8c9f6b16279 57 dout1.write(1);
altb2 1:d8c9f6b16279 58 if(with_ahrs)
altb2 1:d8c9f6b16279 59 cntrl_ahrs.read_imu_sensors();
altb2 1:d8c9f6b16279 60 else{}; // otherwise ahrs has its own thread
altb2 1:d8c9f6b16279 61 dout1.write(0);
altb2 1:d8c9f6b16279 62 mutex.lock();
altb2 1:d8c9f6b16279 63 if(Rate_Controller_Running) // calc the inner loop first! RollPitchYaw
altb2 1:d8c9f6b16279 64 {
altb2 0:a479dc61e931 65 for(k=0;k<3;k++)
altb2 1:d8c9f6b16279 66 data.cntrl_Mxyz[k]= rate_cntrl[k](data.cntrl_rate_rpy_des[k] - data.sens_gyr[k], data.sens_gyr[k]);
altb2 0:a479dc61e931 67 }
altb2 0:a479dc61e931 68 else
altb2 0:a479dc61e931 69 {
altb2 0:a479dc61e931 70 for(k=0;k<3;k++)
altb2 1:d8c9f6b16279 71 data.cntrl_Mxyz[k]=0.0f;
altb2 0:a479dc61e931 72 data.F_Thrust = 0.0; // be shure that thrust is down
altb2 0:a479dc61e931 73 }
altb2 1:d8c9f6b16279 74 copter.motor_mix(data.cntrl_Mxyz,data.F_Thrust,data.wMot); // mix torques and Thrust to motor speeds
altb2 0:a479dc61e931 75 for(k = 0;k < copter.nb_motors;k++)
altb2 1:d8c9f6b16279 76 motor_pwms[k].pulsewidth_us(copter.motor.n2pwm(data.wMot[k])); // map motor speeds to pwm out
altb2 1:d8c9f6b16279 77 // -------------------------------------------------------------------------
altb2 0:a479dc61e931 78 // now calc all the other controllers!!!!
altb2 1:d8c9f6b16279 79 downsamp_counter_1++;
altb2 1:d8c9f6b16279 80 downsamp_counter_2++;
altb2 1:d8c9f6b16279 81 if(downsamp_counter_1 == downsamp_1)
altb2 0:a479dc61e931 82 {
altb2 1:d8c9f6b16279 83 downsamp_counter_1 = 0;
altb2 0:a479dc61e931 84 // first do the angle estimation
altb2 1:d8c9f6b16279 85 if(with_ahrs)
altb2 3:bc24fee36ba3 86 {
altb2 3:bc24fee36ba3 87 cntrl_ahrs.update();
altb2 3:bc24fee36ba3 88 // HACK!! Run 2 other EKFs and log data
altb2 2:e7874762cc25 89 cntrl_ahrs.ekf_rp.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_acc[0],data.sens_acc[1]);
altb2 2:e7874762cc25 90 data.est_RP_RPY[0] = cntrl_ahrs.getRoll(3);
altb2 2:e7874762cc25 91 data.est_RP_RPY[1] = cntrl_ahrs.getPitch(3);
altb2 2:e7874762cc25 92 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]);
altb2 2:e7874762cc25 93 data.est_RP_RPY[2] = cntrl_ahrs.getRoll(4);
altb2 2:e7874762cc25 94 data.est_RP_RPY[3] = cntrl_ahrs.getPitch(4);
altb2 3:bc24fee36ba3 95 data.est_RP_RPY[4] = cntrl_ahrs.getYaw(4);
altb2 2:e7874762cc25 96 }
altb2 1:d8c9f6b16279 97 else{}; // otherwise ahrs has its own thread
altb2 0:a479dc61e931 98 // ------ start different controller loops -------------------------------------------------
altb2 1:d8c9f6b16279 99 // handle xy Direction and z differently!!!
altb2 1:d8c9f6b16279 100 if(downsamp_counter_2 == downsamp_2)
altb2 0:a479dc61e931 101 {
altb2 1:d8c9f6b16279 102 downsamp_counter_2 = 0;
altb2 1:d8c9f6b16279 103 if(Pos_Controller_Running) // only xy
altb2 1:d8c9f6b16279 104 {
altb2 0:a479dc61e931 105 for(k=0;k<2;k++)
altb2 1:d8c9f6b16279 106 data.cntrl_vel_xyz_des[k] = pos_cntrl[k](data.cntrl_pos_xyz_des[k] - data.est_xyz[k]);
altb2 1:d8c9f6b16279 107 }
altb2 1:d8c9f6b16279 108 else
altb2 1:d8c9f6b16279 109 {
altb2 3:bc24fee36ba3 110 data.cntrl_vel_xyz_des[0] = scale_PM1_to_vel * myRC.PM1[0]; // x-direction
altb2 3:bc24fee36ba3 111 data.cntrl_vel_xyz_des[1] = -scale_PM1_to_vel * myRC.PM1[1]; // y-direction, Stick reversed (Stick to the right is +1 (-y))
altb2 1:d8c9f6b16279 112 }
altb2 1:d8c9f6b16279 113 if(Vel_Controller_Running)
altb2 3:bc24fee36ba3 114 { // here directions cross: vel contrl in +x -> do a positive pitch (axis[1])
altb2 3:bc24fee36ba3 115 // vel contrl in +y -> do a negative roll (axis[0])
altb2 3:bc24fee36ba3 116 data.cntrl_att_rpy_des[1] = vel_cntrl[0](data.cntrl_vel_xyz_des[0] - data.est_Vxyz[0]);
altb2 3:bc24fee36ba3 117 data.cntrl_att_rpy_des[0] = -vel_cntrl[1](data.cntrl_vel_xyz_des[1] - data.est_Vxyz[1]);
altb2 0:a479dc61e931 118 }
altb2 0:a479dc61e931 119 else{
altb2 0:a479dc61e931 120 for(k=0;k<2;k++)
altb2 1:d8c9f6b16279 121 data.cntrl_att_rpy_des[k] = scale_PM1_to_angle * myRC.PM1[k];
altb2 0:a479dc61e931 122 }
altb2 0:a479dc61e931 123 } // if downsamp_2 end of slowest loop
altb2 0:a479dc61e931 124 if(Althold_Controller_Running){
altb2 2:e7874762cc25 125 //data.cntrl_vel_xyz_des[2] = pos_cntrl[2](calc_des_pos(myRC.PM1[3]) - data.est_xyz[2]); // see function below
altb2 2:e7874762cc25 126 //data.F_Thrust = vel_cntrl[2](data.cntrl_vel_xyz_des[2] - data.est_Vxyz[2]);
altb2 2:e7874762cc25 127 data.F_Thrust = pos_cntrl[2](calc_des_pos(myRC.PM1[3]) - data.est_xyz[2]); // see function below
altb2 0:a479dc61e931 128 }
altb2 0:a479dc61e931 129 else{
altb2 1:d8c9f6b16279 130 data.F_Thrust = PM1_2_F_Thrust(myRC.PM1[3]);
altb2 0:a479dc61e931 131 }
altb2 1:d8c9f6b16279 132 if(Attitude_Controller_Running) // control angles
altb2 3:bc24fee36ba3 133 {
altb2 0:a479dc61e931 134 for(k=0;k<2;k++)
altb2 0:a479dc61e931 135 data.cntrl_rate_rpy_des[k] = Kv[k] * (data.cntrl_att_rpy_des[k] - data.est_RPY[k]);
altb2 3:bc24fee36ba3 136 data.cntrl_rate_rpy_des[2] = Kv[2] * (calc_des_yaw(-myRC.PM1[2]) - data.est_RPY[2]);
altb2 3:bc24fee36ba3 137 }
altb2 0:a479dc61e931 138 else
altb2 3:bc24fee36ba3 139 {
altb2 0:a479dc61e931 140 for(k=0;k<2;k++) // contoll angular rates
altb2 1:d8c9f6b16279 141 data.cntrl_rate_rpy_des[k] = scale_PM1_to_angle * myRC.PM1[k]; // Acro mode,
altb2 3:bc24fee36ba3 142 data.cntrl_rate_rpy_des[2] = -scale_PM1_to_angle * myRC.PM1[2]; // Yaw-axis reversed
altb2 3:bc24fee36ba3 143 }
altb2 1:d8c9f6b16279 144 } // if(downsamp_1 ...
altb2 1:d8c9f6b16279 145 mutex.unlock();
altb2 0:a479dc61e931 146 } // the thread
altb2 0:a479dc61e931 147 }
altb2 0:a479dc61e931 148 // ------------------- start controllers ----------------
altb2 1:d8c9f6b16279 149 void Controller_Loops::start_loop(void){
altb2 1:d8c9f6b16279 150 thread.start(callback(this, &Controller_Loops::loop));
altb2 0:a479dc61e931 151 ticker.attach(callback(this, &Controller_Loops::sendSignal), Ts);
altb2 0:a479dc61e931 152 }
altb2 1:d8c9f6b16279 153 // ------------------- start controllers ----------------
altb2 1:d8c9f6b16279 154 void Controller_Loops::reset_all(void){
altb2 1:d8c9f6b16279 155 for(uint8_t k = 0;k<3;k++)
altb2 1:d8c9f6b16279 156 rate_cntrl[k].reset(0.0);
altb2 1:d8c9f6b16279 157 for(uint8_t k = 0;k<3;k++)
altb2 1:d8c9f6b16279 158 vel_cntrl[k].reset(0.0);
altb2 1:d8c9f6b16279 159 for(uint8_t k = 0;k<3;k++)
altb2 1:d8c9f6b16279 160 pos_cntrl[k].reset(0.0);
altb2 3:bc24fee36ba3 161 des_z = 0.0;
altb2 3:bc24fee36ba3 162 des_yaw = data.est_RPY[2];
altb2 1:d8c9f6b16279 163 }
altb2 0:a479dc61e931 164
altb2 0:a479dc61e931 165 // this is for realtime OS
altb2 0:a479dc61e931 166 void Controller_Loops::sendSignal() {
altb2 0:a479dc61e931 167 thread.signal_set(signal);
altb2 0:a479dc61e931 168 }
altb2 1:d8c9f6b16279 169 // *****************************************************************************
altb2 1:d8c9f6b16279 170 // nonlinear characteristics for thrust
altb2 1:d8c9f6b16279 171 float Controller_Loops::PM1_2_F_Thrust(float x)
altb2 1:d8c9f6b16279 172 {
altb2 1:d8c9f6b16279 173 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);
altb2 1:d8c9f6b16279 174 }
altb2 0:a479dc61e931 175
altb2 0:a479dc61e931 176 // --------------- calc_des_pos ---------------------------------------
altb2 0:a479dc61e931 177 // integrate desired velocity to position. Test, if tracking error not too large
altb2 1:d8c9f6b16279 178 float Controller_Loops::calc_des_pos(float des_vel){
altb2 2:e7874762cc25 179 des_vel = deadzone(des_vel,-.1,.1);
altb2 0:a479dc61e931 180 if(((des_z - data.est_xyz[2]) < max_delta) & des_vel > 0)
altb2 0:a479dc61e931 181 des_z += Ts * downsamp_1 * des_vel * max_climb_rate;
altb2 0:a479dc61e931 182 else if(((des_z - data.est_xyz[2]) > -max_delta) & des_vel < 0)
altb2 0:a479dc61e931 183 des_z += Ts * downsamp_1 * des_vel * max_climb_rate;
altb2 2:e7874762cc25 184 data.cntrl_pos_xyz_des[2] = des_z;
altb2 0:a479dc61e931 185 return des_z;
altb2 0:a479dc61e931 186 }
altb2 3:bc24fee36ba3 187 // ---------------------------------------------------------------
altb2 1:d8c9f6b16279 188 void Controller_Loops::reset_des_z(void){
altb2 0:a479dc61e931 189 des_z = 0.0;
altb2 0:a479dc61e931 190 }
altb2 3:bc24fee36ba3 191 // ---------------------------------------------------------------
altb2 1:d8c9f6b16279 192 void Controller_Loops::reset_des_z(float init){
altb2 3:bc24fee36ba3 193 this->des_z = init;
altb2 3:bc24fee36ba3 194 }
altb2 3:bc24fee36ba3 195 // ---------------------------------------------------------------
altb2 3:bc24fee36ba3 196 float Controller_Loops::calc_des_yaw(float des_yaw_rate){
altb2 3:bc24fee36ba3 197 des_yaw_rate = deadzone(des_yaw_rate,-0.04f,0.04f);
altb2 3:bc24fee36ba3 198 if(((des_yaw - data.est_RPY[2]) < max_delta_yaw) & des_yaw_rate > 0)
altb2 3:bc24fee36ba3 199 des_yaw += Ts * downsamp_1 * des_yaw_rate * max_yaw_rate;
altb2 3:bc24fee36ba3 200 else if(((des_yaw - data.est_RPY[2]) > -max_delta_yaw) & des_yaw_rate < 0)
altb2 3:bc24fee36ba3 201 des_yaw += Ts * downsamp_1 * des_yaw_rate * max_yaw_rate;
altb2 3:bc24fee36ba3 202 data.cntrl_att_rpy_des[2] = des_yaw;
altb2 3:bc24fee36ba3 203 return des_yaw;
altb2 3:bc24fee36ba3 204 }
altb2 3:bc24fee36ba3 205 // ---------------------------------------------------------------
altb2 3:bc24fee36ba3 206 void Controller_Loops::reset_des_yaw(float init){
altb2 3:bc24fee36ba3 207 this->des_yaw = init;
altb2 1:d8c9f6b16279 208 }
altb2 1:d8c9f6b16279 209 // ------------------- destructor -----------------
altb2 1:d8c9f6b16279 210 Controller_Loops::~Controller_Loops() {
altb2 1:d8c9f6b16279 211 ticker.detach();
altb2 1:d8c9f6b16279 212 }
altb2 1:d8c9f6b16279 213
altb2 1:d8c9f6b16279 214
altb2 1:d8c9f6b16279 215 // ------------------------------------------------------------
altb2 1:d8c9f6b16279 216 void Controller_Loops::enable_acro(void)
altb2 1:d8c9f6b16279 217 {
altb2 1:d8c9f6b16279 218 Rate_Controller_Running = true;
altb2 2:e7874762cc25 219 Attitude_Controller_Running = false;
altb2 2:e7874762cc25 220 Althold_Controller_Running = false;
altb2 2:e7874762cc25 221 Vel_Controller_Running = false;
altb2 2:e7874762cc25 222
altb2 1:d8c9f6b16279 223 }
altb2 1:d8c9f6b16279 224 // ------------------------------------------------------------
altb2 1:d8c9f6b16279 225 void Controller_Loops::enable_stabilized(void)
altb2 1:d8c9f6b16279 226 {
altb2 1:d8c9f6b16279 227 Rate_Controller_Running = true;
altb2 1:d8c9f6b16279 228 Attitude_Controller_Running = true;
altb2 2:e7874762cc25 229 Althold_Controller_Running = false;
altb2 2:e7874762cc25 230 Vel_Controller_Running = false;
altb2 1:d8c9f6b16279 231 }
altb2 1:d8c9f6b16279 232 // ------------------------------------------------------------
altb2 1:d8c9f6b16279 233 void Controller_Loops::enable_alt_hold(void)
altb2 1:d8c9f6b16279 234 {
altb2 1:d8c9f6b16279 235 Rate_Controller_Running = true;
altb2 1:d8c9f6b16279 236 Attitude_Controller_Running = true;
altb2 1:d8c9f6b16279 237 Althold_Controller_Running = true;
altb2 2:e7874762cc25 238 Vel_Controller_Running = false;
altb2 2:e7874762cc25 239
altb2 1:d8c9f6b16279 240 }
altb2 1:d8c9f6b16279 241 // ------------------------------------------------------------
altb2 1:d8c9f6b16279 242 void Controller_Loops::enable_vel_of_z_pos(void)
altb2 1:d8c9f6b16279 243 {
altb2 1:d8c9f6b16279 244 Rate_Controller_Running = true;
altb2 1:d8c9f6b16279 245 Attitude_Controller_Running = true;
altb2 1:d8c9f6b16279 246 Althold_Controller_Running = true;
altb2 1:d8c9f6b16279 247 Vel_Controller_Running = true;
altb2 1:d8c9f6b16279 248 }
altb2 1:d8c9f6b16279 249 // ------------------------------------------------------------
altb2 1:d8c9f6b16279 250 void Controller_Loops::disable_all(void)
altb2 1:d8c9f6b16279 251 {
altb2 1:d8c9f6b16279 252 Rate_Controller_Running = false;
altb2 1:d8c9f6b16279 253 Attitude_Controller_Running = false;
altb2 1:d8c9f6b16279 254 Althold_Controller_Running = false;
altb2 1:d8c9f6b16279 255 Vel_Controller_Running = false;
altb2 1:d8c9f6b16279 256 }
altb2 1:d8c9f6b16279 257 // ------------------------------------------------------------
altb2 1:d8c9f6b16279 258 void Controller_Loops::set_controller_limits(UAV copter)
altb2 1:d8c9f6b16279 259 {
altb2 1:d8c9f6b16279 260 rate_cntrl[0].set_limits(-copter.max_Mxy, copter.max_Mxy); // angular rate controller Roll
altb2 1:d8c9f6b16279 261 rate_cntrl[1].set_limits(-copter.max_Mxy, copter.max_Mxy); // angular rate controller Pitch
altb2 1:d8c9f6b16279 262 rate_cntrl[2].set_limits(-copter.max_Mz, copter.max_Mz); // angular rate controller YAW
altb2 2:e7874762cc25 263 }
altb2 2:e7874762cc25 264 // ------------------------------------------------------------
altb2 2:e7874762cc25 265 void Controller_Loops::parametrize_controllers(){
altb2 2:e7874762cc25 266 vel_cntrl[0].setCoefficients( 0.2, 0.02, 0.04, 0.05,Ts*(float)downsamp_2,-.1,.1); // PID controller for velocity (OF), x
altb2 2:e7874762cc25 267 vel_cntrl[1].setCoefficients( 0.2, 0.02, 0.04, 0.05,Ts*(float)downsamp_2,-.1,.1); // PID controller for velocity (OF), y
altb2 2:e7874762cc25 268 //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
altb2 3:bc24fee36ba3 269 pos_cntrl[2].setCoefficients(8.08, 9.66, 3.71, 0.0262,Ts*(float)downsamp_1,5.0f,22.0f); // Output is Thrust/N
altb2 2:e7874762cc25 270
altb2 2:e7874762cc25 271 rate_cntrl[0].setCoefficients(0.2,0.15,0.005,0.020, Ts, -copter.max_Mxy, copter.max_Mxy); // angular rate controller Roll
altb2 2:e7874762cc25 272 rate_cntrl[1].setCoefficients(0.2,0.15,0.005,0.020, Ts, -copter.max_Mxy, copter.max_Mxy); // angular rate controller Pitch
altb2 2:e7874762cc25 273 rate_cntrl[2].setCoefficients(0.234, 0.0, 0.00320, 0.05, Ts, -copter.max_Mxy, copter.max_Mxy); // angular rate controller Yaw
altb2 2:e7874762cc25 274 Kv[0] = 4.0f;
altb2 2:e7874762cc25 275 Kv[1] = 4.0f;
altb2 3:bc24fee36ba3 276 Kv[2] = 2.0f;
altb2 2:e7874762cc25 277
altb2 2:e7874762cc25 278 scale_PM1_to_vel = 1.5f; // e.g. Stick to the right corresp. to 1.5m/s desired speed
altb2 2:e7874762cc25 279 scale_PM1_to_rate = 3.0f;
altb2 2:e7874762cc25 280 scale_PM1_to_angle = 0.5f; // e.g. Stick to the right corr. to 0.5 rad desired angle
altb2 2:e7874762cc25 281 max_delta = 0.25f; // maximum allowed tracking error for z-Lift
altb2 2:e7874762cc25 282 max_climb_rate = 0.7f; // climbrate in z
altb2 3:bc24fee36ba3 283 max_delta_yaw = 0.4f; // maximum allowed tracking error for yaw
altb2 3:bc24fee36ba3 284 max_yaw_rate = 1.5f; // max yawrate
altb2 3:bc24fee36ba3 285
altb2 2:e7874762cc25 286
altb2 2:e7874762cc25 287 }
altb2 2:e7874762cc25 288
altb2 2:e7874762cc25 289 // *****************************************************************************
altb2 2:e7874762cc25 290 // Dead zone, from lo ... hi
altb2 2:e7874762cc25 291 float Controller_Loops::deadzone(float x,float lo,float hi){
altb2 2:e7874762cc25 292 return (x>hi)*(x-hi)+(x<lo)*(x-lo);
altb2 2:e7874762cc25 293 }