diff --git a/ArduCopter/control_flip.pde b/ArduCopter/control_flip.pde
index 4c4e6168cde56bbe488045e3d467451c1d9cb4c8..c429e705d2577fc7f8baf6763b172e41c26ae403 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 e09f213ec27f707a7b3ce671d96ceff1d6c48050..e3bd52b6b14e8c3955c149c108b4d4a46edac0da 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 2029d1ce2720ae93819666af4587a1cf4fc7a2e0..7795743a35b56e716dbe55eb98a2729ec72f099b 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;