Copter: Body frame ACRO upgrade

ArduCopter/ArduCopter.pde

@@ -1466,7 +1466,7 @@ void update_yaw_mode(void)
     case YAW_ACRO:
         // pilot controlled yaw using rate controller
         if(g.axis_enabled) {
-            get_yaw_rate_stabilized_ef(g.rc_4.control_in);
+            get_yaw_rate_stabilized_bf(g.rc_4.control_in);
         }else{
             get_acro_yaw(g.rc_4.control_in);
         }
@@ -1625,8 +1625,8 @@ void update_roll_pitch_mode(void)
 
 #if FRAME_CONFIG == HELI_FRAME
 		if(g.axis_enabled) {
-            get_roll_rate_stabilized_ef(g.rc_1.control_in);
-            get_pitch_rate_stabilized_ef(g.rc_2.control_in);
+            get_roll_rate_stabilized_bf(g.rc_1.control_in);
+            get_pitch_rate_stabilized_bf(g.rc_2.control_in);
         }else{
             // ACRO does not get SIMPLE mode ability
             if (motors.flybar_mode == 1) {
@@ -1639,8 +1639,8 @@ void update_roll_pitch_mode(void)
 		}
 #else  // !HELI_FRAME
 		if(g.axis_enabled) {
-            get_roll_rate_stabilized_ef(g.rc_1.control_in);
-            get_pitch_rate_stabilized_ef(g.rc_2.control_in);
+            get_roll_rate_stabilized_bf(g.rc_1.control_in);
+            get_pitch_rate_stabilized_bf(g.rc_2.control_in);
         }else{
             // ACRO does not get SIMPLE mode ability
             get_acro_roll(g.rc_1.control_in);

ArduCopter/Attitude.pde

@@ -99,6 +99,161 @@ get_acro_yaw(int32_t target_rate)
     set_yaw_rate_target(target_rate, BODY_FRAME);
 }
 
+// Roll with rate input and stabilized in the body frame
+static void
+get_roll_rate_stabilized_bf(int32_t stick_angle)
+{
+    static float angle_error = 0;
+
+    // Scale pitch leveling by stick input
+    if (!g.acro_trainer_enabled) {
+        roll_axis = (float)roll_axis*constrain_float((1-fabsf(stick_angle/4500.0)),0,1)*cos_pitch_x;
+    }
+
+
+    // convert the input to the desired body frame roll rate
+
+    roll_axis += stick_angle * g.acro_p;
+
+    // add automatic correction
+    int32_t correction_rate = g.pi_stabilize_roll.get_p(angle_error);
+
+    // Calculate integrated body frame rate error
+    angle_error += (roll_axis - (omega.x * DEGX100)) * G_Dt;
+
+    // don't let angle error grow too large
+    angle_error = constrain_float(angle_error, - MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
+
+    #if FRAME_CONFIG == HELI_FRAME
+        if (!motors.motor_runup_complete) {
+               angle_error = 0;
+        }
+    #else
+        if (!motors.armed() || g.rc_3.servo_out == 0) {
+            angle_error = 0;
+        }
+    #endif // HELI_FRAME
+
+    // set body frame targets for rate controller
+    set_roll_rate_target(roll_axis + correction_rate, BODY_FRAME);
+
+    // Calculate trainer mode earth frame rate command
+    int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
+
+    int32_t target_rate = 0;
+
+    if (g.acro_trainer_enabled) {
+        if (roll_angle > 4500){
+            target_rate =  g.pi_stabilize_roll.get_p(4500-roll_angle);
+        }else if (roll_angle < -4500) {
+            target_rate =  g.pi_stabilize_roll.get_p(-4500-roll_angle);
+        }
+    }
+    roll_angle   = constrain_int32(roll_angle, -3000, 3000);
+    target_rate -=  (roll_angle * g.acro_balance_roll)/100;
+
+    // add earth frame targets for rate controller
+    set_roll_rate_target(target_rate, BODY_EARTH_FRAME);
+}
+
+// Pitch with rate input and stabilized in the body frame
+static void
+get_pitch_rate_stabilized_bf(int32_t stick_angle)
+{
+    static float angle_error = 0;
+
+    // scale pitch leveling by stick input
+
+    if (!g.acro_trainer_enabled) {
+        pitch_axis = (float)pitch_axis*constrain_float((1-fabsf(stick_angle/4500.0)),0,1)*cos_pitch_x;
+    }
+
+    // convert the input to the desired body frame pitch rate
+    pitch_axis += stick_angle * g.acro_p;
+
+    // add automatic correction
+    int32_t correction_rate = g.pi_stabilize_pitch.get_p(angle_error);
+
+    // Calculate integrated body frame rate error
+    angle_error += (pitch_axis - (omega.y * DEGX100)) * G_Dt;
+
+    // don't let angle error grow too large
+    angle_error = constrain_float(angle_error, - MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT);
+
+    #if FRAME_CONFIG == HELI_FRAME
+        if (!motors.motor_runup_complete) {
+               angle_error = 0;
+        }
+    #else
+        if (!motors.armed() || g.rc_3.servo_out == 0) {
+            angle_error = 0;
+        }
+    #endif // HELI_FRAME
+
+    // set body frame targets for rate controller
+    set_pitch_rate_target(pitch_axis + correction_rate, BODY_FRAME);
+
+    // Calculate trainer mode earth frame rate command
+    int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
+
+    int32_t target_rate = 0;
+
+    if (g.acro_trainer_enabled) {
+        if (pitch_angle > 4500){
+            target_rate =  g.pi_stabilize_pitch.get_p(4500-pitch_angle);
+        }else if (pitch_angle < -4500) {
+            target_rate =  g.pi_stabilize_pitch.get_p(-4500-pitch_angle);
+        }
+    }
+    pitch_angle   = constrain_int32(pitch_angle, -3000, 3000);
+    target_rate -=  (pitch_angle * g.acro_balance_pitch)/100;
+
+    // add earth frame targets for rate controller
+    set_pitch_rate_target(target_rate, BODY_EARTH_FRAME);
+}
+
+// Yaw with rate input and stabilized in the body frame
+static void
+get_yaw_rate_stabilized_bf(int32_t stick_angle)
+{
+    static float angle_error = 0;
+
+    // scale yaw leveling by stick input
+
+    if (!g.acro_trainer_enabled) {
+        nav_yaw = (float)nav_yaw*constrain_float((1-fabsf(stick_angle/4500.0)),0,1)*cos_pitch_x;
+    }
+
+    // convert the input to the desired body frame yaw rate
+    nav_yaw += stick_angle * g.acro_p;
+
+    // add automatic correction
+    int32_t correction_rate = g.pi_stabilize_yaw.get_p(angle_error);
+
+    // Calculate integrated body frame rate error
+    angle_error += (nav_yaw - (omega.z * DEGX100)) * G_Dt;
+
+    // don't let angle error grow too large
+    angle_error = constrain_float(angle_error, - MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
+
+    #if FRAME_CONFIG == HELI_FRAME
+        if (!motors.motor_runup_complete) {
+               angle_error = 0;
+        }
+    #else
+        if (!motors.armed() || g.rc_3.servo_out == 0) {
+            angle_error = 0;
+        }
+    #endif // HELI_FRAME
+
+    // set body frame targets for rate controller
+    set_yaw_rate_target(nav_yaw + correction_rate, BODY_FRAME);
+
+    // add earth frame targets for rate controller
+    set_yaw_rate_target(0, BODY_EARTH_FRAME);
+}
+
+
 // Roll with rate input and stabilized in the earth frame
 static void
 get_roll_rate_stabilized_ef(int32_t stick_angle)
@@ -237,9 +392,14 @@ 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 * cos_pitch_x * yaw_rate_target_ef;
-        yaw_rate_target_bf 		= cos_pitch_x * cos_roll_x * yaw_rate_target_ef - sin_roll * pitch_rate_target_ef;
+        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 * cos_pitch_x * yaw_rate_target_ef;
+        yaw_rate_target_bf      = cos_pitch_x * cos_roll_x * yaw_rate_target_ef - sin_roll * pitch_rate_target_ef;
+    }else if( rate_targets_frame == BODY_EARTH_FRAME ) {
+        // add converted earth frame rates to body frame rates
+        roll_axis   = roll_rate_target_ef - sin_pitch * yaw_rate_target_ef;
+        pitch_axis  = cos_roll_x  * pitch_rate_target_ef + sin_roll * cos_pitch_x * yaw_rate_target_ef;
+        nav_yaw     = cos_pitch_x * cos_roll_x * yaw_rate_target_ef - sin_roll * pitch_rate_target_ef;
     }
 }
 

ArduCopter/config.h

@@ -565,8 +565,9 @@
 
 // definitions for earth frame and body frame
 // used to specify frame to rate controllers
-#define EARTH_FRAME     0
-#define BODY_FRAME      1
+#define EARTH_FRAME         0
+#define BODY_FRAME          1
+#define BODY_EARTH_FRAME    2
 
 
 // Flight mode roll, pitch, yaw, throttle and navigation definitions

ArduCopter/system.pde

@@ -364,9 +364,9 @@ static void set_mode(uint8_t mode)
         set_nav_mode(NAV_NONE);
         // 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;
+            roll_axis   = 0;
+            pitch_axis  = 0;
+            nav_yaw     = 0;
         }
         break;