Diff: Sources/Controller_Loops.cpp

Revision:

0:a479dc61e931

Child:

1:d8c9f6b16279

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Controller_Loops.cpp	Wed Oct 02 15:31:12 2019 +0000
@@ -0,0 +1,146 @@
+#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;
+}
\ No newline at end of file