From e84d97b387fdbdb041194fcf2fbe58028f9cc364 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Jul 2015 22:23:13 +0200 Subject: [PATCH] Only update TECS filter if auto is enabled. This forces the internal TECS logic to reset between auto runs --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 8856dd829d..2f915c711f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1054,8 +1054,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - if (!_mTecs.getEnabled() && !_vehicle_status.condition_landed) { - _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + /* update TECS filters, but only if in auto mode. */ + if (_control_mode.flag_control_auto_enabled && (_global_pos.timestamp > 0) && !_mTecs.getEnabled() && !_vehicle_status.condition_landed) { + _tecs.update_50hz(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); } /* define altitude error */