Plane: evaluate assistance requirements on mode change

this avoid the AHRS being told we are flying forward - because we are no longer in a VTOL mode - and instantly being told we are not flying forward - because we are providing assistance
This commit is contained in:
Peter Barker 2024-10-08 20:39:22 +11:00 committed by Andrew Tridgell
parent 39ab13cec4
commit 1a1edf92b7
1 changed files with 8 additions and 0 deletions

View File

@ -140,6 +140,14 @@ bool Mode::enter()
// Make sure the flight stage is correct for the new mode
plane.update_flight_stage();
#if HAL_QUADPLANE_ENABLED
if (quadplane.enabled()) {
float aspeed;
bool have_airspeed = quadplane.ahrs.airspeed_estimate(aspeed);
quadplane.assisted_flight = quadplane.assist.should_assist(aspeed, have_airspeed);
}
#endif
}
return enter_result;