Diff: Threads_and_main/Controller_Loops.cpp

Revision:

3:bc24fee36ba3

Parent:

2:e7874762cc25

diff -r e7874762cc25 -r bc24fee36ba3 Threads_and_main/Controller_Loops.cpp
--- a/Threads_and_main/Controller_Loops.cpp	Mon Oct 21 17:16:11 2019 +0000
+++ b/Threads_and_main/Controller_Loops.cpp	Mon Oct 28 07:54:10 2019 +0000
@@ -83,15 +83,16 @@
         downsamp_counter_1 = 0;
     // first do the angle estimation
         if(with_ahrs)
-            {
-            cntrl_ahrs.update();
+            {       
+            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);            
+            data.est_RP_RPY[4] = cntrl_ahrs.getYaw(4);              
             }
         else{};         // otherwise ahrs has its own thread
         // ------ start different controller loops -------------------------------------------------
@@ -106,13 +107,14 @@
                 }
             else
                 {
-                for(k=0;k<2;k++)
-                    data.cntrl_vel_xyz_des[k] = scale_PM1_to_vel * myRC.PM1[k];
+                    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)
-                {
-                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]);
+                {   // 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++)
@@ -128,15 +130,17 @@
             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, 
-        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!
+            data.cntrl_rate_rpy_des[2] = -scale_PM1_to_angle * myRC.PM1[2];      // Yaw-axis reversed 
+            }            
         }       // if(downsamp_1  ...
     mutex.unlock();
     }   // the thread
@@ -154,7 +158,8 @@
     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
@@ -179,11 +184,27 @@
     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;
+    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() {
@@ -245,19 +266,23 @@
     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
+    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
+    
   
     }