ArduCopter: prioritise rate controllers, rate controller targets converted to body frame

ArduCopter/ArduCopter.pde

@@ -567,6 +567,8 @@ static float cos_roll_x         = 1;
 static float cos_pitch_x        = 1;
 static float cos_yaw_x          = 1;
 static float sin_yaw_y;
+static float sin_roll;
+static float sin_pitch;
 
 ////////////////////////////////////////////////////////////////////////////////
 // SIMPLE Mode
@@ -575,6 +577,19 @@ static float sin_yaw_y;
 // or in SuperSimple mode when the copter leaves a 20m radius from home.
 static int32_t initial_simple_bearing;
 
+////////////////////////////////////////////////////////////////////////////////
+// Rate contoller targets
+////////////////////////////////////////////////////////////////////////////////
+static int32_t roll_rate_target_ef = 0;
+static int32_t roll_rate_trim_ef = 0;      // normally i term from stabilize controller
+static int32_t pitch_rate_target_ef = 0;
+static int32_t pitch_rate_trim_ef = 0;      // normally i term from stabilize controller
+static int32_t yaw_rate_target_ef = 0;
+static int32_t yaw_rate_trim_ef = 0;      // normally i term from stabilize controller
+static int32_t roll_rate_target_bf = 0;     // body frame roll rate target
+static int32_t pitch_rate_target_bf = 0;    // body frame pitch rate target
+static int32_t yaw_rate_target_bf = 0;      // body frame yaw rate target
+
 ////////////////////////////////////////////////////////////////////////////////
 // ACRO Mode
 ////////////////////////////////////////////////////////////////////////////////
@@ -1073,6 +1088,13 @@ static void fast_loop()
     // some space available
     gcs_send_message(MSG_RETRY_DEFERRED);
 
+    // run low level rate controllers that only require IMU data
+    run_rate_controllers();
+
+    // write out the servo PWM values
+    // ------------------------------
+    set_servos_4();
+
     // Read radio
     // ----------
     read_radio();
@@ -1100,9 +1122,8 @@ static void fast_loop()
     update_yaw_mode();
     update_roll_pitch_mode();
 
-    // write out the servo PWM values
-    // ------------------------------
-    set_servos_4();
+    // update targets to rate controllers
+    update_rate_contoller_targets();
 
     // agmatthews - USERHOOKS
 #ifdef USERHOOK_FASTLOOP
@@ -1564,7 +1585,7 @@ void update_yaw_mode(void)
 {
     switch(yaw_mode) {
     case YAW_ACRO:
-        g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in);
+        get_acro_yaw(g.rc_4.control_in);
         return;
         break;
 
@@ -1575,12 +1596,12 @@ void update_yaw_mode(void)
 
     case YAW_HOLD:
         if(g.rc_4.control_in != 0) {
-            g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in);
+            get_acro_yaw(g.rc_4.control_in);
             yaw_stopped = false;
             yaw_timer = 150;
 
         }else if (!yaw_stopped) {
-            g.rc_4.servo_out = get_acro_yaw(0);
+            get_acro_yaw(0);
             yaw_timer--;
 
             if((yaw_timer == 0) || (fabs(omega.z) < .17)) {
@@ -1593,21 +1614,21 @@ void update_yaw_mode(void)
             if(motors.armed() == false || ((g.rc_3.control_in == 0) && (control_mode <= ACRO) && !failsafe))
                 nav_yaw = ahrs.yaw_sensor;
 
-            g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
+            get_stabilize_yaw(nav_yaw);
         }
         return;
         break;
 
     case YAW_LOOK_AT_HOME:
         //nav_yaw updated in update_navigation()
-        g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
+        get_stabilize_yaw(nav_yaw);
         break;
 
     case YAW_AUTO:
         nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -60, 60);                 // 40 deg a second
         //Serial.printf("nav_yaw %d ", nav_yaw);
         nav_yaw  = wrap_360(nav_yaw);
-        g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
+        get_stabilize_yaw(nav_yaw);
         break;
     }
 }
@@ -1634,8 +1655,8 @@ void update_roll_pitch_mode(void)
             pitch_axis = wrap_360(pitch_axis);
 
             // in this mode, nav_roll and nav_pitch = the iterm
-            g.rc_1.servo_out = get_stabilize_roll(roll_axis);
-            g.rc_2.servo_out = get_stabilize_pitch(pitch_axis);
+            get_stabilize_roll(roll_axis);
+            get_stabilize_pitch(pitch_axis);
 
             if (g.rc_3.control_in == 0) {
                 roll_axis = 0;
@@ -1649,12 +1670,12 @@ void update_roll_pitch_mode(void)
                 g.rc_1.servo_out = g.rc_1.control_in;
                 g.rc_2.servo_out = g.rc_2.control_in;
             } else {
-                g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in);
-                g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in);
+                get_acro_roll(g.rc_1.control_in);
+                get_acro_pitch(g.rc_2.control_in);
             }
 #else
-            g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in);
-            g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in);
+            get_acro_roll(g.rc_1.control_in);
+            get_acro_pitch(g.rc_2.control_in);
 #endif
         }
         break;
@@ -1669,8 +1690,9 @@ void update_roll_pitch_mode(void)
         control_pitch           = g.rc_2.control_in;
 
         // in this mode, nav_roll and nav_pitch = the iterm
-        g.rc_1.servo_out = get_stabilize_roll(control_roll);
-        g.rc_2.servo_out = get_stabilize_pitch(control_pitch);
+        get_stabilize_roll(control_roll);
+        get_stabilize_pitch(control_pitch);
+
         break;
 
     case ROLL_PITCH_AUTO:
@@ -1685,8 +1707,8 @@ void update_roll_pitch_mode(void)
         control_roll            = g.rc_1.control_mix(nav_roll);
         control_pitch           = g.rc_2.control_mix(nav_pitch);
 
-        g.rc_1.servo_out        = get_stabilize_roll(control_roll);
-        g.rc_2.servo_out        = get_stabilize_pitch(control_pitch);
+        get_stabilize_roll(control_roll);
+        get_stabilize_pitch(control_pitch);
         break;
 
     case ROLL_PITCH_STABLE_OF:
@@ -1699,8 +1721,8 @@ void update_roll_pitch_mode(void)
         control_pitch           = g.rc_2.control_in;
 
         // mix in user control with optical flow
-        g.rc_1.servo_out 		= get_stabilize_roll(get_of_roll(control_roll));
-        g.rc_2.servo_out 		= get_stabilize_pitch(get_of_pitch(control_pitch));
+        get_stabilize_roll(get_of_roll(control_roll));
+        get_stabilize_pitch(get_of_pitch(control_pitch));
         break;
 
     // THOR
@@ -2127,6 +2149,10 @@ static void update_trig(void){
     sin_yaw_y               = yawvector.x;                                              // 1y = north
     cos_yaw_x               = yawvector.y;                                              // 0x = north
 
+    // added to convert earth frame to body frame for rate controllers
+    sin_roll = sin(ahrs.roll);
+    sin_pitch = sin(ahrs.pitch);
+
     //flat:
     // 0 ° = cos_yaw:  0.00, sin_yaw:  1.00,
     // 90° = cos_yaw:  1.00, sin_yaw:  0.00,

ArduCopter/Attitude.pde

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-static int16_t
+static void
 get_stabilize_roll(int32_t target_angle)
 {
     // angle error
@@ -29,11 +29,13 @@ get_stabilize_roll(int32_t target_angle)
         i_stab                          = g.pi_stabilize_roll.get_integrator();
     }
 
-    return get_rate_roll(target_rate) + i_stab;
+    // set targets for rate controller
+    roll_rate_target_ef = target_rate;
+    roll_rate_trim_ef = i_stab;
 #endif
 }
 
-static int16_t
+static void
 get_stabilize_pitch(int32_t target_angle)
 {
     // angle error
@@ -60,17 +62,20 @@ get_stabilize_pitch(int32_t target_angle)
     }else{
         i_stab                          = g.pi_stabilize_pitch.get_integrator();
     }
-    return get_rate_pitch(target_rate) + i_stab;
+
+    // set targets for rate controller
+    pitch_rate_target_ef = target_rate;
+    pitch_rate_trim_ef = i_stab;
 
 #endif
 }
 
-static int16_t
+static void
 get_stabilize_yaw(int32_t target_angle)
 {
     int32_t target_rate,i_term;
     int32_t angle_error;
-    int32_t output;
+    int32_t output = 0;
 
     // angle error
     angle_error             = wrap_180(target_angle - ahrs.yaw_sensor);
@@ -89,12 +94,12 @@ get_stabilize_yaw(int32_t target_angle)
     // do not use rate controllers for helicotpers with external gyros
 #if FRAME_CONFIG == HELI_FRAME
     if(!motors.ext_gyro_enabled) {
-        output = get_rate_yaw(target_rate) + i_term;
+        yaw_rate_target_ef = target_rate;
+        yaw_rate_trim_ef = i_term;
     }else{
+        // TO-DO: fix this for helicopters which don't use rate controller
         output = constrain((target_rate + i_term), -4500, 4500);
     }
-#else
-    output = get_rate_yaw(target_rate) + i_term;
 #endif
 
 #if LOGGING_ENABLED == ENABLED
@@ -109,29 +114,39 @@ get_stabilize_yaw(int32_t target_angle)
     }
 #endif
 
-    // ensure output does not go beyond barries of what an int16_t can hold
-    return constrain(output,-32000,32000);
+    // set targets for rate controller
+    yaw_rate_target_ef = target_rate;
+    yaw_rate_trim_ef = i_term;
 }
 
-static int16_t
+static void
 get_acro_roll(int32_t target_rate)
 {
     target_rate = target_rate * g.acro_p;
-    return get_rate_roll(target_rate);
+
+    // set targets for rate controller
+    roll_rate_target_ef = target_rate;
+    roll_rate_trim_ef = 0;
 }
 
-static int16_t
+static void
 get_acro_pitch(int32_t target_rate)
 {
     target_rate = target_rate * g.acro_p;
-    return get_rate_pitch(target_rate);
+
+    // set targets for rate controller
+    pitch_rate_target_ef = target_rate;
+    pitch_rate_trim_ef = 0;
 }
 
-static int16_t
+static void
 get_acro_yaw(int32_t target_rate)
 {
     target_rate = g.pi_stabilize_yaw.get_p(target_rate);
-    return get_rate_yaw(target_rate);
+
+    // set targets for rate controller
+    yaw_rate_target_ef = target_rate;
+    yaw_rate_trim_ef = 0;
 }
 
 /*
@@ -208,6 +223,35 @@ get_acro_yaw(int32_t target_rate)
  *  }
  */
 
+ // update_rate_contoller_targets - converts earth frame rates to body frame rates for rate controllers
+ void
+ update_rate_contoller_targets()
+ {
+    static int16_t counter = 0;
+    // convert earth frame rates to body frame rates
+    roll_rate_target_bf = roll_rate_target_ef + sin_pitch * yaw_rate_target_ef;
+    pitch_rate_target_bf = cos_roll_x * pitch_rate_target_ef + sin_roll * yaw_rate_target_ef;
+    yaw_rate_target_bf = cos_pitch_x * cos_roll_x * yaw_rate_target_ef + sin_roll * pitch_rate_target_ef;
+
+    //counter++;
+    if( counter >= 100 ) {
+        counter = 0;
+        Serial.printf_P(PSTR("\nr:%ld\tp:%ld\ty:%ld\t"),ahrs.roll_sensor, ahrs.pitch_sensor, ahrs.yaw_sensor);
+        Serial.printf_P(PSTR("Rrate:%ld/%ld\tPrate:%ld/%ld\tYrate:%ld/%ld\n"),roll_rate_target_ef, roll_rate_target_bf, pitch_rate_target_ef, pitch_rate_target_bf, yaw_rate_target_ef, yaw_rate_target_bf);
+    }
+ }
+ 
+// run roll, pitch and yaw rate controllers and send output to motors
+// targets for these controllers comes from stabilize controllers
+void
+run_rate_controllers()
+{
+    // call rate controllers
+    g.rc_1.servo_out = get_rate_roll(roll_rate_target_bf) + roll_rate_trim_ef;
+    g.rc_2.servo_out = get_rate_pitch(pitch_rate_target_bf) + pitch_rate_trim_ef;
+    g.rc_4.servo_out = get_rate_yaw(yaw_rate_target_bf) + yaw_rate_trim_ef;
+}
+
 static int16_t
 get_rate_roll(int32_t target_rate)
 {
@@ -346,6 +390,7 @@ get_rate_yaw(int32_t target_rate)
 #else
     // output control:
     int16_t yaw_limit = 2200 + abs(g.rc_4.control_in);
+
     // smoother Yaw control:
     return constrain(output, -yaw_limit, yaw_limit);
 #endif

ArduCopter/flip.pde

@@ -27,7 +27,8 @@ void init_flip()
 void roll_flip()
 {
     // Pitch
-    g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
+    //g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
+    get_stabilize_pitch(g.rc_2.control_in);
 
     int32_t roll = ahrs.roll_sensor * flip_dir;
 
@@ -55,7 +56,8 @@ void roll_flip()
 
     case 2:
         if (flip_timer < 100) {
-            g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
+            //g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
+            get_stabilize_roll(g.rc_1.control_in);
             g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC;
             flip_timer++;
         }else{

ArduCopter/toy.pde

@@ -109,12 +109,12 @@ void roll_pitch_toy()
     int16_t yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
 
     if(g.rc_1.control_in != 0) {    // roll
-        g.rc_4.servo_out = get_acro_yaw(yaw_rate/2);
+        get_acro_yaw(yaw_rate/2);
         yaw_stopped = false;
         yaw_timer = 150;
 
     }else if (!yaw_stopped) {
-        g.rc_4.servo_out = get_acro_yaw(0);
+        get_acro_yaw(0);
         yaw_timer--;
 
         if((yaw_timer == 0) || (fabs(omega.z) < .17)) {
@@ -125,7 +125,7 @@ void roll_pitch_toy()
         if(motors.armed() == false || g.rc_3.control_in == 0)
             nav_yaw = ahrs.yaw_sensor;
 
-        g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
+        get_stabilize_yaw(nav_yaw);
     }
 #endif
 
@@ -167,13 +167,17 @@ void roll_pitch_toy()
 
 #if TOY_EDF == ENABLED
     // Output the attitude
-    g.rc_1.servo_out = get_stabilize_roll(roll_rate);
-    g.rc_2.servo_out = get_stabilize_pitch(g.rc_6.control_in);             // use CH6 to trim pitch
+    //g.rc_1.servo_out = get_stabilize_roll(roll_rate);
+    //g.rc_2.servo_out = get_stabilize_pitch(g.rc_6.control_in);             // use CH6 to trim pitch
+    get_stabilize_roll(roll_rate);
+    get_stabilize_pitch(g.rc_6.control_in);             // use CH6 to trim pitch
 
 #else
     // Output the attitude
-    g.rc_1.servo_out = get_stabilize_roll(roll_rate);
-    g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
+    //g.rc_1.servo_out = get_stabilize_roll(roll_rate);
+    //g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
+    get_stabilize_roll(roll_rate);
+    get_stabilize_pitch(g.rc_2.control_in);
 #endif
 
 }