diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index ab121ff935d469..02a05938007da3 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -421,7 +421,11 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc #if FRAME_CONFIG == HELI_FRAME switch (ch_flag) { case AuxSwitchPos::HIGH: - copter.motors->set_inverted_flight(true); + if (copter.flightmode->allows_inverted()) { + copter.motors->set_inverted_flight(true); + } else { + gcs().send_text(MAV_SEVERITY_WARNING, "Inverted flight not available in %s mode", copter.flightmode->name()); + } break; case AuxSwitchPos::MIDDLE: // nothing diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 921f9fb32902e6..d9119381948110 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -457,6 +457,11 @@ void Copter::exit_mode(Mode *&old_flightmode, input_manager.set_stab_col_ramp(0.0); } } + + // Make sure inverted flight is disabled if not supported in the new mode + if (!new_flightmode->allows_inverted()) { + motors->set_inverted_flight(false); + } #endif //HELI_FRAME } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index dd06ee0149c296..173d6b47d1ba9b 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -128,6 +128,10 @@ class Mode { virtual bool allows_autotune() const { return false; } virtual bool allows_flip() const { return false; } +#if FRAME_CONFIG == HELI_FRAME + virtual bool allows_inverted() const { return false; }; +#endif + // return a string for this flightmode virtual const char *name() const = 0; virtual const char *name4() const = 0; @@ -1574,6 +1578,8 @@ class ModeStabilize_Heli : public ModeStabilize { bool init(bool ignore_checks) override; void run() override; + bool allows_inverted() const override { return true; }; + protected: private: