From 882edaf78771b20d99c62138518ea26f306fce61 Mon Sep 17 00:00:00 2001 From: Jason Short 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 4c4e6168cd..c429e705d2 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 e09f213ec2..e3bd52b6b1 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 2029d1ce27..7795743a35 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;