vtol_type: reset quadchute filter states if TECS is not running

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2020-08-11 13:30:24 +03:00
committed by Silvan Fuhrer
parent faeaa411d0
commit 01535a726a

View File

@@ -222,6 +222,12 @@ bool VtolType::can_transition_on_ground()
void VtolType::check_quadchute_condition()
{
if (!_tecs_running) {
// reset the filtered height rate and heigh rate setpoint if TECS is not running
_ra_hrate = 0.0f;
_ra_hrate_sp = 0.0f;
}
if (_v_control_mode->flag_armed && !_land_detected->landed) {
Eulerf euler = Quatf(_v_att->q);