diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f340346ed4..79cd19e84f 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1062,18 +1062,6 @@ static void fast_loop() // -------------------- read_AHRS(); - // Acrobatic control - if (ap.do_flip) { - if(abs(g.rc_1.control_in) < 4000) { - // calling roll_flip will override the desired roll rate and throttle output - roll_flip(); - }else{ - // force an exit from the loop if we are not hands off sticks. - ap.do_flip = false; - Log_Write_Event(DATA_EXIT_FLIP); - } - } - // run low level rate controllers that only require IMU data attitude_control.rate_controller_run(); @@ -1502,6 +1490,10 @@ static void update_flight_mode() case SPORT: sport_run(); break; + + case FLIP: + flip_run(); + break; } } diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index a772d4097d..44a0e45c52 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -146,8 +146,10 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) switch(tmp_function) { case AUX_SWITCH_FLIP: // flip if switch is on, positive throttle and we're actually flying - if((ch_flag == AUX_SWITCH_HIGH) && (g.rc_3.control_in >= 0) && !ap.land_complete) { - init_flip(); + if(ch_flag == AUX_SWITCH_HIGH) { + set_mode(FLIP); + }else{ + flip_stop(); } break; diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 1278e74ec7..c6b66f237f 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -127,7 +127,8 @@ #define OF_LOITER 10 // Hold a single location using optical flow sensor #define DRIFT 11 // DRIFT mode (Note: 12 is no longer used) #define SPORT 13 // earth frame rate control -#define NUM_MODES 14 +#define FLIP 14 // flip the vehicle on the roll axis +#define NUM_MODES 15 // CH_6 Tuning @@ -231,6 +232,14 @@ enum RTLState { Land }; +// Flip states +enum FlipState { + Flip_Start, + Flip_Roll, + Flip_Recover, + Flip_Abandon +}; + // LAND state #define LAND_STATE_FLY_TO_LOCATION 0 #define LAND_STATE_DESCENDING 1 @@ -295,9 +304,8 @@ enum RTLState { #define DATA_LAND_COMPLETE 18 #define DATA_NOT_LANDED 28 #define DATA_LOST_GPS 19 -#define DATA_BEGIN_FLIP 21 -#define DATA_END_FLIP 22 -#define DATA_EXIT_FLIP 23 +#define DATA_FLIP_START 21 +#define DATA_FLIP_END 22 #define DATA_SET_HOME 25 #define DATA_SET_SIMPLE_ON 26 #define DATA_SET_SIMPLE_OFF 27 @@ -374,6 +382,7 @@ enum RTLState { #define ERROR_SUBSYSTEM_FLIGHT_MODE 10 #define ERROR_SUBSYSTEM_GPS 11 #define ERROR_SUBSYSTEM_CRASH_CHECK 12 +#define ERROR_SUBSYSTEM_FLIP 13 // general error codes #define ERROR_CODE_ERROR_RESOLVED 0 #define ERROR_CODE_FAILED_TO_INITIALISE 1 @@ -390,6 +399,8 @@ enum RTLState { #define ERROR_CODE_MAIN_INS_DELAY 1 // subsystem specific error codes -- crash checker #define ERROR_CODE_CRASH_CHECK_CRASH 1 +// subsystem specific error codes -- flip +#define ERROR_CODE_FLIP_ABANDONED 2 // Arming Check Enable/Disable bits #define ARMING_CHECK_NONE 0x00 diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 32b19583e0..5432a48f77 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -446,6 +446,10 @@ static bool set_mode(uint8_t mode) control_yaw = ahrs.yaw_sensor; break; + case FLIP: + success = flip_init(ignore_checks); + break; + default: success = false; break; @@ -581,6 +585,9 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) case SPORT: port->print_P(PSTR("SPORT")); break; + case FLIP: + port->print_P(PSTR("FLIP")); + break; default: port->printf_P(PSTR("Mode(%u)"), (unsigned)mode); break;