altb_pmic
branch for tests with T265
Dependencies: Lib_Cntrl AHRS Lib_Misc
Diff: Threads_and_main/Controller_Loops.cpp
- Revision:
- 2:e7874762cc25
- Parent:
- 1:d8c9f6b16279
- Child:
- 3:bc24fee36ba3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Threads_and_main/Controller_Loops.cpp Mon Oct 21 17:16:11 2019 +0000
@@ -0,0 +1,268 @@
+#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();
+ 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
+ {
+ 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]);
+ 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]);
+ 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){
+ 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){
+ des_z = 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,16.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;
+
+ 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
+
+ }
+
+// *****************************************************************************
+// Dead zone, from lo ... hi
+float Controller_Loops::deadzone(float x,float lo,float hi){
+return (x>hi)*(x-hi)+(x<lo)*(x-lo);
+}