Browse Source

vtol_type: reset quadchute filter states if TECS is not running

Signed-off-by: RomanBapst <bapstroman@gmail.com>
sbg
RomanBapst 5 years ago committed by Silvan Fuhrer
parent
commit
01535a726a
  1. 6
      src/modules/vtol_att_control/vtol_type.cpp

6
src/modules/vtol_att_control/vtol_type.cpp

@ -222,6 +222,12 @@ bool VtolType::can_transition_on_ground() @@ -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);

Loading…
Cancel
Save