branch for tests with T265
Dependencies: Lib_Cntrl AHRS Lib_Misc
Threads_and_main/Controller_Loops.cpp
- Committer:
- altb2
- Date:
- 2019-10-28
- Revision:
- 3:bc24fee36ba3
- Parent:
- 2:e7874762cc25
File content as of revision 3:bc24fee36ba3:
#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); }