ArduCopter: fixed acro mode

Changes included:
Removing earth frame roll_rate_trim, pitch_rate_trim and yaw_rate_trim.
Switch ACRO mode to use YAW_ACRO instead of YAW_HOLD.
Changed YAW_ACRO to use stabilize yaw when axis_enabled.
Reset ACRO roll, pitch and yaw targets to current attitude when first entering ACRO.

ArduCopter/ArduCopter.pde

@@ -590,12 +590,10 @@ static int32_t initial_simple_bearing;
 ////////////////////////////////////////////////////////////////////////////////
 // Rate contoller targets
 ////////////////////////////////////////////////////////////////////////////////
+static uint8_t rate_targets_frame = EARTH_FRAME;    // indicates whether rate targets provided in earth or body frame
 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
@@ -1594,7 +1592,16 @@ void update_yaw_mode(void)
 {
     switch(yaw_mode) {
     case YAW_ACRO:
-        get_acro_yaw(g.rc_4.control_in);
+        if(g.axis_enabled) {
+            nav_yaw    += (float)g.rc_4.control_in * g.axis_lock_p;
+            nav_yaw = wrap_360(nav_yaw);
+            if (g.rc_3.control_in == 0) {
+                nav_yaw = ahrs.yaw_sensor;
+            }
+            get_stabilize_yaw(nav_yaw);
+        }else{
+            get_acro_yaw(g.rc_4.control_in);
+        }
         return;
         break;
 
@@ -1605,12 +1612,12 @@ void update_yaw_mode(void)
 
     case YAW_HOLD:
         if(g.rc_4.control_in != 0) {
-            get_acro_yaw(g.rc_4.control_in);
+            get_stabilize_rate_yaw(g.rc_4.control_in);
             yaw_stopped = false;
             yaw_timer = 150;
 
         }else if (!yaw_stopped) {
-            get_acro_yaw(0);
+            get_stabilize_rate_yaw(0);
             yaw_timer--;
 
             if((yaw_timer == 0) || (fabs(omega.z) < .17)) {
@@ -1660,18 +1667,17 @@ void update_roll_pitch_mode(void)
             roll_axis       += (float)g.rc_1.control_in * g.axis_lock_p;
             pitch_axis      += (float)g.rc_2.control_in * g.axis_lock_p;
 
-            roll_axis = wrap_360(roll_axis);
-            pitch_axis = wrap_360(pitch_axis);
-
-            // in this mode, nav_roll and nav_pitch = the iterm
-            get_stabilize_roll(roll_axis);
-            get_stabilize_pitch(pitch_axis);
+            roll_axis = wrap_180(roll_axis);
+            pitch_axis = wrap_180(pitch_axis);
 
             if (g.rc_3.control_in == 0) {
                 roll_axis = 0;
                 pitch_axis = 0;
             }
 
+            // in this mode, nav_roll and nav_pitch = the iterm
+            get_stabilize_roll(roll_axis);
+            get_stabilize_pitch(pitch_axis);
         }else{
             // ACRO does not get SIMPLE mode ability
 #if FRAME_CONFIG == HELI_FRAME

ArduCopter/Attitude.pde

@@ -30,8 +30,7 @@ get_stabilize_roll(int32_t target_angle)
     }
 
     // set targets for rate controller
-    roll_rate_target_ef = target_rate;
-    roll_rate_trim_ef = i_stab;
+    set_roll_rate_target(target_rate+i_stab, EARTH_FRAME);
 #endif
 }
 
@@ -64,9 +63,7 @@ get_stabilize_pitch(int32_t target_angle)
     }
 
     // set targets for rate controller
-    pitch_rate_target_ef = target_rate;
-    pitch_rate_trim_ef = i_stab;
-
+    set_pitch_rate_target(target_rate + i_stab, EARTH_FRAME);
 #endif
 }
 
@@ -111,8 +108,16 @@ get_stabilize_yaw(int32_t target_angle)
 #endif
 
     // set targets for rate controller
-    yaw_rate_target_ef = target_rate;
-    yaw_rate_trim_ef = i_term;
+    set_yaw_rate_target(target_rate+i_term, EARTH_FRAME);
+}
+
+static void
+get_stabilize_rate_yaw(int32_t target_rate)
+{
+    target_rate = g.pi_stabilize_yaw.get_p(target_rate);
+
+    // set targets for rate controller
+    set_yaw_rate_target(target_rate, EARTH_FRAME);
 }
 
 static void
@@ -121,8 +126,7 @@ get_acro_roll(int32_t target_rate)
     target_rate = target_rate * g.acro_p;
 
     // set targets for rate controller
-    roll_rate_target_ef = target_rate;
-    roll_rate_trim_ef = 0;
+    set_roll_rate_target(target_rate, BODY_FRAME);
 }
 
 static void
@@ -131,18 +135,16 @@ get_acro_pitch(int32_t target_rate)
     target_rate = target_rate * g.acro_p;
 
     // set targets for rate controller
-    pitch_rate_target_ef = target_rate;
-    pitch_rate_trim_ef = 0;
+    set_pitch_rate_target(target_rate, BODY_FRAME);
 }
 
 static void
 get_acro_yaw(int32_t target_rate)
 {
-    target_rate = g.pi_stabilize_yaw.get_p(target_rate);
+    target_rate = target_rate * g.acro_p;
 
     // set targets for rate controller
-    yaw_rate_target_ef = target_rate;
-    yaw_rate_trim_ef = 0;
+    set_yaw_rate_target(target_rate, BODY_FRAME);
 }
 
 /*
@@ -219,15 +221,47 @@ 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()
- {
-    // 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;
- }
+// set_roll_rate_target - to be called by upper controllers to set roll rate targets in the earth frame
+void set_roll_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) {
+    rate_targets_frame = earth_or_body_frame;
+    if( earth_or_body_frame == BODY_FRAME ) {
+        roll_rate_target_bf = desired_rate;
+    }else{
+        roll_rate_target_ef = desired_rate;
+    }
+}
+
+// set_pitch_rate_target - to be called by upper controllers to set pitch rate targets in the earth frame
+void set_pitch_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) {
+    rate_targets_frame = earth_or_body_frame;
+    if( earth_or_body_frame == BODY_FRAME ) {
+        pitch_rate_target_bf = desired_rate;
+    }else{
+        pitch_rate_target_ef = desired_rate;
+    }
+}
+
+// set_yaw_rate_target - to be called by upper controllers to set yaw rate targets in the earth frame
+void set_yaw_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) {
+    rate_targets_frame = earth_or_body_frame;
+    if( earth_or_body_frame == BODY_FRAME ) {
+        yaw_rate_target_bf = desired_rate;
+    }else{
+        yaw_rate_target_ef = desired_rate;
+    }
+}
+
+// update_rate_contoller_targets - converts earth frame rates to body frame rates for rate controllers
+void
+update_rate_contoller_targets()
+{
+    if( rate_targets_frame == EARTH_FRAME ) {
+        // 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;
+    }
+}
  
 // run roll, pitch and yaw rate controllers and send output to motors
 // targets for these controllers comes from stabilize controllers
@@ -236,13 +270,13 @@ run_rate_controllers()
 {
 #if FRAME_CONFIG == HELI_FRAME          // helicopters only use rate controllers for yaw and only when not using an external gyro
     if(!motors.ext_gyro_enabled) {
-        g.rc_4.servo_out = get_rate_yaw(yaw_rate_target_bf) + yaw_rate_trim_ef;
+        g.rc_4.servo_out = get_rate_yaw(yaw_rate_target_bf);
     }
 #else
     // 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;
+    g.rc_1.servo_out = get_rate_roll(roll_rate_target_bf);
+    g.rc_2.servo_out = get_rate_pitch(pitch_rate_target_bf);
+    g.rc_4.servo_out = get_rate_yaw(yaw_rate_target_bf);
 #endif
 }
 

ArduCopter/config.h

@@ -526,6 +526,11 @@
 // Attitude Control
 //
 
+// definitions for earth frame and body frame
+// used to specify frame to rate controllers
+#define EARTH_FRAME     0
+#define BODY_FRAME      1
+
 // Alt Hold Mode
 #ifndef ALT_HOLD_YAW
  # define ALT_HOLD_YAW           YAW_HOLD

ArduCopter/system.pde

@@ -456,9 +456,15 @@ static void set_mode(byte mode)
     switch(control_mode)
     {
     case ACRO:
-        yaw_mode                = YAW_HOLD;
+        yaw_mode                = YAW_ACRO;
         roll_pitch_mode = ROLL_PITCH_ACRO;
         throttle_mode   = THROTTLE_MANUAL;
+        // reset acro axis targets to current attitude
+        if( g.axis_enabled ) {
+            roll_axis = ahrs.roll_sensor;
+            pitch_axis = ahrs.pitch_sensor;
+            nav_yaw = ahrs.yaw_sensor;
+        }
         break;
 
     case STABILIZE: