diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 4886eb3509..174fcbe540 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -200,6 +200,22 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) } #endif +#if FRAME_CONFIG != HELI_FRAME + // ensure vehicle doesn't leap off the ground if a user switches + // into a manual throttle mode from a non-manual-throttle mode + // (e.g. user arms in guided, raises throttle to 1300 (not enough to + // trigger auto takeoff), then switches into manual): + if (!ignore_checks && + ap.land_complete && + new_flightmode->has_manual_throttle() && + !copter.flightmode->has_manual_throttle() && + new_flightmode->get_pilot_desired_throttle() > copter.get_non_takeoff_throttle()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: throttle too high"); + Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode); + return false; + } +#endif + if (!ignore_checks && new_flightmode->requires_GPS() && !copter.position_ok()) { diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 5019e5ba0c..d7479519fd 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -79,7 +79,9 @@ public: protected: - virtual bool init(bool ignore_checks) = 0; + virtual bool init(bool ignore_checks) { + return true; + } virtual void run() = 0; virtual bool is_autopilot() const { return false; } @@ -210,7 +212,6 @@ public: // inherit constructor using Copter::Mode::Mode; - virtual bool init(bool ignore_checks) override; virtual void run() override; bool is_autopilot() const override { return false; } @@ -1040,7 +1041,6 @@ public: // inherit constructor using Copter::Mode::Mode; - virtual bool init(bool ignore_checks) override; virtual void run() override; bool requires_GPS() const override { return false; } diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index 0e91efc3d7..35499eaf9e 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -8,17 +8,6 @@ * Init and run calls for acro flight mode */ -bool Copter::ModeAcro::init(bool ignore_checks) -{ - // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high - if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() && - (get_pilot_desired_throttle() > copter.get_non_takeoff_throttle())) { - return false; - } - - return true; -} - void Copter::ModeAcro::run() { float target_roll, target_pitch, target_yaw; diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index 5d1c0b607c..8e95376ee6 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/ArduCopter/mode_stabilize.cpp @@ -4,18 +4,6 @@ * Init and run calls for stabilize flight mode */ -// stabilize_init - initialise stabilize controller -bool Copter::ModeStabilize::init(bool ignore_checks) -{ - // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high - if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() && - (get_pilot_desired_throttle() > get_non_takeoff_throttle())) { - return false; - } - - return true; -} - // stabilize_run - runs the main stabilize controller // should be called at 100hz or more void Copter::ModeStabilize::run()