diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index a724c4af4a..ba79c7f3e2 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -203,7 +203,13 @@ void Plane::ahrs_update() */ void Plane::update_speed_height(void) { - if (control_mode->does_auto_throttle()) { + bool should_run_tecs = control_mode->does_auto_throttle(); +#if HAL_QUADPLANE_ENABLED + if (quadplane.should_disable_TECS()) { + should_run_tecs = false; + } +#endif + if (should_run_tecs) { // Call TECS 50Hz update. Note that we call this regardless of // throttle suppressed, as this needs to be running for // takeoff detection @@ -543,7 +549,14 @@ void Plane::update_alt() } #endif - if (control_mode->does_auto_throttle() && !throttle_suppressed) { + bool should_run_tecs = control_mode->does_auto_throttle(); +#if HAL_QUADPLANE_ENABLED + if (quadplane.should_disable_TECS()) { + should_run_tecs = false; + } +#endif + + if (should_run_tecs && !throttle_suppressed) { float distance_beyond_land_wp = 0; if (flight_stage == AP_FixedWing::FlightStage::LAND && diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index bda00e3004..bc9a8e3853 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -4550,4 +4550,20 @@ bool QuadPlane::abort_landing(void) return true; } +/* + return true if we should disable TECS in the current flight state + this ensures that TECS resets when we change height in a VTOL mode + */ +bool QuadPlane::should_disable_TECS() const +{ + if (in_vtol_land_descent()) { + return true; + } + if (plane.control_mode == &plane.mode_guided && + plane.auto_state.vtol_loiter) { + return true; + } + return false; +} + #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index d87709562c..481d1b80d3 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -180,6 +180,12 @@ public: */ bool in_vtol_land_descent(void) const; + /* + should we disable the TECS controller? + only called when in an auto-throttle mode + */ + bool should_disable_TECS() const; + private: AP_AHRS &ahrs;