diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 9a55bb9c54..071aec8840 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -419,9 +419,6 @@ private: int32_t nav_delay_time_max; // used for delaying the navigation commands (eg land,takeoff etc.) uint32_t nav_delay_time_start; - // Flip - Vector3f flip_orig_attitude; // original vehicle attitude before flip - // throw mode state struct { ThrowModeStage stage; @@ -817,8 +814,6 @@ private: bool brake_init(bool ignore_checks); void brake_run(); void brake_timeout_to_loiter_ms(uint32_t timeout_ms); - bool flip_init(bool ignore_checks); - void flip_run(); bool guided_nogps_init(bool ignore_checks); void guided_nogps_run(); void land_run_vertical_control(bool pause_descent = false); @@ -1064,6 +1059,8 @@ private: Copter::FlightMode_DRIFT flightmode_drift{*this}; + Copter::FlightMode_FLIP flightmode_flip{*this}; + Copter::FlightMode_GUIDED flightmode_guided{*this}; Copter::FlightMode_LAND flightmode_land{*this}; diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h index 14cebac3c3..0e8dfd752e 100644 --- a/ArduCopter/FlightMode.h +++ b/ArduCopter/FlightMode.h @@ -633,3 +633,33 @@ protected: private: }; + + + +class FlightMode_FLIP : public FlightMode { + +public: + + FlightMode_FLIP(Copter &copter) : + Copter::FlightMode(copter) + { } + + virtual bool init(bool ignore_checks) override; + virtual void run() override; // should be called at 100hz or more + + virtual bool requires_GPS() const override { return false; } + virtual bool has_manual_throttle() const override { return false; } + virtual bool allows_arming(bool from_gcs) const override { return false; }; + virtual bool is_autopilot() const override { return false; } + +protected: + + const char *name() const override { return "FLIP"; } + const char *name4() const override { return "FLIP"; } + +private: + + // Flip + Vector3f flip_orig_attitude; // original vehicle attitude before flip + +}; diff --git a/ArduCopter/control_flip.cpp b/ArduCopter/control_flip.cpp index 217dd2da1c..2fd692b400 100644 --- a/ArduCopter/control_flip.cpp +++ b/ArduCopter/control_flip.cpp @@ -30,6 +30,7 @@ #define FLIP_PITCH_BACK 1 // used to set flip_dir #define FLIP_PITCH_FORWARD -1 // used to set flip_dir +// FIXME? these should be instance variables? FlipState flip_state; // current state of flip control_mode_t flip_orig_control_mode; // flight mode when flip was initated uint32_t flip_start_time; // time since flip began @@ -37,15 +38,15 @@ int8_t flip_roll_dir; // roll direction (-1 = roll left, 1 = roll int8_t flip_pitch_dir; // pitch direction (-1 = pitch forward, 1 = pitch back) // flip_init - initialise flip controller -bool Copter::flip_init(bool ignore_checks) +bool Copter::FlightMode_FLIP::init(bool ignore_checks) { // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes - if (control_mode != ACRO && control_mode != STABILIZE && control_mode != ALT_HOLD) { + if (_copter.control_mode != ACRO && _copter.control_mode != STABILIZE && _copter.control_mode != ALT_HOLD) { return false; } // if in acro or stabilize ensure throttle is above zero - if (ap.throttle_zero && (control_mode == ACRO || control_mode == STABILIZE)) { + if (ap.throttle_zero && (_copter.control_mode == ACRO || _copter.control_mode == STABILIZE)) { return false; } @@ -60,7 +61,7 @@ bool Copter::flip_init(bool ignore_checks) } // capture original flight mode so that we can return to it after completion - flip_orig_control_mode = control_mode; + flip_orig_control_mode = _copter.control_mode; // initialise state flip_state = Flip_Start; @@ -83,8 +84,9 @@ bool Copter::flip_init(bool ignore_checks) Log_Write_Event(DATA_FLIP_START); // capture current attitude which will be used during the Flip_Recovery stage - flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -aparm.angle_max, aparm.angle_max); - flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -aparm.angle_max, aparm.angle_max); + float angle_max = _copter.aparm.angle_max; + flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -angle_max, angle_max); + flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -angle_max, angle_max); flip_orig_attitude.z = ahrs.yaw_sensor; return true; @@ -92,7 +94,7 @@ bool Copter::flip_init(bool ignore_checks) // flip_run - runs the flip controller // should be called at 100hz or more -void Copter::flip_run() +void Copter::FlightMode_FLIP::run() { float throttle_out; float recovery_angle; @@ -191,9 +193,9 @@ void Copter::flip_run() // check for successful recovery if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) { // restore original flight mode - if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) { + if (!_copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) { // this should never happen but just in case - set_mode(STABILIZE, MODE_REASON_UNKNOWN); + _copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN); } // log successful completion Log_Write_Event(DATA_FLIP_END); @@ -202,12 +204,12 @@ void Copter::flip_run() case Flip_Abandon: // restore original flight mode - if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) { + if (!_copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) { // this should never happen but just in case - set_mode(STABILIZE, MODE_REASON_UNKNOWN); + _copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN); } // log abandoning flip - Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED); + _copter.Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED); break; } diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 11c0b7e9e7..f5c47f8863 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -117,7 +117,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) break; case FLIP: - success = flip_init(ignore_checks); + success = flightmode_flip.init(ignore_checks); + if (success) { + flightmode = &flightmode_flip; + } break; #if AUTOTUNE_ENABLED == ENABLED @@ -218,10 +221,6 @@ void Copter::update_flight_mode() switch (control_mode) { - case FLIP: - flip_run(); - break; - #if AUTOTUNE_ENABLED == ENABLED case AUTOTUNE: autotune_run(); @@ -382,9 +381,6 @@ void Copter::notify_flight_mode() // set flight mode string switch (control_mode) { - case FLIP: - notify.set_flight_mode_str("FLIP"); - break; case AUTOTUNE: notify.set_flight_mode_str("ATUN"); break;