From 882edaf78771b20d99c62138518ea26f306fce61 Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@mac.com>
Date: Tue, 22 Apr 2014 22:32:03 -0700
Subject: [PATCH] Copter: add pitch axis flipping
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Added Pitch axis flipping. You must be pushing forward or back 3° to make it flip in Pitch axis or it will flip in the default roll axis.
Added some extra throttle control to maintain altitude
removed the CH7 ability to exit Flip mode. It will exit only if completing flip or user aborts with control sticks.
---
 ArduCopter/control_flip.pde | 97 ++++++++++++++++++++++++++++++-------
 ArduCopter/defines.h        |  2 +
 ArduCopter/switches.pde     |  2 -
 3 files changed, 81 insertions(+), 20 deletions(-)

diff --git a/ArduCopter/control_flip.pde b/ArduCopter/control_flip.pde
index 4c4e6168c..c429e705d 100644
--- a/ArduCopter/control_flip.pde
+++ b/ArduCopter/control_flip.pde
@@ -18,19 +18,23 @@
  *          Flip_Recover (while copter is between -90deg and original target angle) : use earth frame angle controller to return vehicle to original attitude
  */
 
-#define FLIP_THR_INC        170     // throttle increase during Flip_Start stage (under 45deg lean angle)
-#define FLIP_THR_DEC        120     // throttle decrease during Flip_Roll stage (between 45deg ~ -90deg roll)
-#define FLIP_ROLL_RATE      40000   // roll rate request in centi-degrees / sec (i.e. 400 deg/sec)
+#define FLIP_THR_INC        200     // throttle increase during Flip_Start stage (under 45deg lean angle)
+#define FLIP_THR_DEC        240     // throttle decrease during Flip_Roll stage (between 45deg ~ -90deg roll)
+#define FLIP_ROTATION_RATE  40000   // rotation rate request in centi-degrees / sec (i.e. 400 deg/sec)
 #define FLIP_TIMEOUT_MS     2500    // timeout after 2.5sec.  Vehicle will switch back to original flight mode
 #define FLIP_RECOVERY_ANGLE 500     // consider successful recovery when roll is back within 5 degrees of original
 
 #define FLIP_ROLL_RIGHT      1      // used to set flip_dir
 #define FLIP_ROLL_LEFT      -1      // used to set flip_dir
 
+#define FLIP_PITCH_BACK      1      // used to set flip_dir
+#define FLIP_PITCH_FORWARD  -1      // used to set flip_dir
+
 FlipState flip_state;               // current state of flip
 uint8_t   flip_orig_control_mode;   // flight mode when flip was initated
 uint32_t  flip_start_time;          // time since flip began
-int8_t    flip_dir;                 // roll direction (-1 = roll left, 1 = roll right)
+int8_t    flip_roll_dir;            // roll direction (-1 = roll left, 1 = roll right)
+int8_t    flip_pitch_dir;           // pitch direction (-1 = pitch forward, 1 = pitch back)
 
 // flip_init - initialise flip controller
 static bool flip_init(bool ignore_checks)
@@ -61,12 +65,20 @@ static bool flip_init(bool ignore_checks)
     // initialise state
     flip_state = Flip_Start;
     flip_start_time = millis();
-
-    // choose direction based on pilot's roll stick
+    
+    flip_roll_dir = flip_pitch_dir = 0;
+
+    // choose direction based on pilot's roll and pitch sticks
+    if (g.rc_2.control_in > 300) {
+        flip_pitch_dir = FLIP_PITCH_BACK;
+    }else if(g.rc_2.control_in < -300) {
+        flip_pitch_dir = FLIP_PITCH_FORWARD;
+    }else{
     if (g.rc_1.control_in >= 0) {
-        flip_dir = FLIP_ROLL_RIGHT;
+            flip_roll_dir = FLIP_ROLL_RIGHT;
     }else{
-        flip_dir = FLIP_ROLL_LEFT;
+            flip_roll_dir = FLIP_ROLL_LEFT;
+    }
     }
 
     // log start of flip
@@ -103,39 +115,80 @@ static void flip_stop()
 static void flip_run()
 {
     int16_t throttle_out;
+    float recovery_angle;
 
     // if pilot inputs roll > 40deg or timeout occurs abandon flip
-    if (!motors.armed() || (abs(g.rc_1.control_in) >= 4000) || ((millis() - flip_start_time) > FLIP_TIMEOUT_MS)) {
+    if (!motors.armed() || (abs(g.rc_1.control_in) >= 4000) || (abs(g.rc_2.control_in) >= 4000) || ((millis() - flip_start_time) > FLIP_TIMEOUT_MS)) {
         flip_state = Flip_Abandon;
     }
 
     // get pilot's desired throttle
     throttle_out = get_pilot_desired_throttle(g.rc_3.control_in);
 
-    // get roll rate
-    int32_t roll_angle = ahrs.roll_sensor * flip_dir;
+    // get corrected angle based on direction and axis of rotation
+    // we flip the sign of flip_angle to minimize the code repetition
+    int32_t flip_angle;
+
+    if (flip_roll_dir != 0) {
+        flip_angle = ahrs.roll_sensor * flip_roll_dir;
+    } else {
+        flip_angle = ahrs.pitch_sensor * flip_pitch_dir;
+    }
 
     // state machine
     switch (flip_state) {
 
     case Flip_Start:
-        // under 45 degrees request 400deg/sec roll
-        attitude_control.rate_bf_roll_pitch_yaw(FLIP_ROLL_RATE * flip_dir, 0.0, 0.0);
+        // under 45 degrees request 400deg/sec roll or pitch
+        attitude_control.rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
+
         // increase throttle
         throttle_out += FLIP_THR_INC;
+
         // beyond 45deg lean angle move to next stage
-        if (roll_angle >= 4500) {
+        if (flip_angle >= 4500) {
+            if (flip_roll_dir != 0) {
+                // we are rolling
             flip_state = Flip_Roll;
+            } else {
+                // we are pitching
+                flip_state = Flip_Pitch_A;
+        }
         }
         break;
 
     case Flip_Roll:
         // between 45deg ~ -90deg request 400deg/sec roll
-        attitude_control.rate_bf_roll_pitch_yaw(FLIP_ROLL_RATE * flip_dir, 0.0, 0.0);
+        attitude_control.rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, 0.0, 0.0);
         // decrease throttle
         throttle_out -= FLIP_THR_DEC;
+
         // beyond -90deg move on to recovery
-        if((roll_angle < 4500) && (roll_angle > -9000)) {
+        if ((flip_angle < 4500) && (flip_angle > -9000)) {
+            flip_state = Flip_Recover;
+        }
+        break;
+
+    case Flip_Pitch_A:
+        // between 45deg ~ -90deg request 400deg/sec pitch
+        attitude_control.rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
+        // decrease throttle
+        throttle_out -= FLIP_THR_DEC;
+
+        // check roll for inversion
+        if ((labs(ahrs.roll_sensor) > 9000) && (flip_angle > 4500)) {
+            flip_state = Flip_Pitch_B;
+        }
+        break;
+
+    case Flip_Pitch_B:
+        // between 45deg ~ -90deg request 400deg/sec pitch
+        attitude_control.rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
+        // decrease throttle
+        throttle_out -= FLIP_THR_DEC;
+
+        // check roll for inversion
+        if ((labs(ahrs.roll_sensor) < 9000) && (flip_angle > -4500)) {
             flip_state = Flip_Recover;
         }
         break;
@@ -144,11 +197,19 @@ static void flip_run()
         // use originally captured earth-frame angle targets to recover
         attitude_control.angle_ef_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false);
 
-        // increase throttle to gain any lost alitude
+        // increase throttle to gain any lost altitude
         throttle_out += FLIP_THR_INC;
 
+        if (flip_roll_dir != 0) {
+            // we are rolling
+            recovery_angle = fabs(flip_orig_attitude.x - (float)ahrs.roll_sensor);
+        } else {
+            // we are pitching
+            recovery_angle = fabs(flip_orig_attitude.y - (float)ahrs.pitch_sensor);
+        }
+
         // check for successful recovery
-        if (fabs(flip_orig_attitude.x - (float)ahrs.roll_sensor) <= FLIP_RECOVERY_ANGLE) {
+        if (fabs(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
             // restore original flight mode
             if (!set_mode(flip_orig_control_mode)) {
                 // this should never happen but just in case
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index e09f213ec..e3bd52b6b 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -202,6 +202,8 @@ enum RTLState {
 enum FlipState {
     Flip_Start,
     Flip_Roll,
+    Flip_Pitch_A,
+    Flip_Pitch_B,
     Flip_Recover,
     Flip_Abandon
 };
diff --git a/ArduCopter/switches.pde b/ArduCopter/switches.pde
index 2029d1ce2..7795743a3 100644
--- a/ArduCopter/switches.pde
+++ b/ArduCopter/switches.pde
@@ -165,8 +165,6 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
             // flip if switch is on, positive throttle and we're actually flying
             if(ch_flag == AUX_SWITCH_HIGH) {
                 set_mode(FLIP);
-            }else{
-                flip_stop();
             }
             break;
 
-- 
GitLab