Plane: fixed TECS state reset in VTOL auto

this fixes a bug where TECS maintains its slow integrator while in a
VTOL hover mode in AUTO or GUIDED.

Among other things this affects PAYLOAD_PLACE and
DO_VTOL_TRANSITION. In those states the height can change while
hovering outside the control of TECS. When TECS regains control in a
fwd transition then can lead to a very large height loss or gain until
the TECS integrator can catch up
This commit is contained in:
Andrew Tridgell 2023-07-03 07:04:30 +10:00 committed by Randy Mackay
parent 0f160ab331
commit 61ed17b6c5
3 changed files with 37 additions and 2 deletions

View File

@ -203,7 +203,13 @@ void Plane::ahrs_update()
*/ */
void Plane::update_speed_height(void) 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 // Call TECS 50Hz update. Note that we call this regardless of
// throttle suppressed, as this needs to be running for // throttle suppressed, as this needs to be running for
// takeoff detection // takeoff detection
@ -543,7 +549,14 @@ void Plane::update_alt()
} }
#endif #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; float distance_beyond_land_wp = 0;
if (flight_stage == AP_FixedWing::FlightStage::LAND && if (flight_stage == AP_FixedWing::FlightStage::LAND &&

View File

@ -4550,4 +4550,20 @@ bool QuadPlane::abort_landing(void)
return true; 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 #endif // HAL_QUADPLANE_ENABLED

View File

@ -180,6 +180,12 @@ public:
*/ */
bool in_vtol_land_descent(void) const; 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: private:
AP_AHRS &ahrs; AP_AHRS &ahrs;