From 75085dc5d610f03b53ccbd89fcd93048d748657b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jul 2015 10:16:48 +0200 Subject: [PATCH] Condition TECS properly on any altitude control mode --- .../fw_pos_control_l1_main.cpp | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 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 2f915c711f..33a78f3141 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 @@ -1037,7 +1037,7 @@ bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - float dt = FLT_MIN; // Using non zero value to a avoid division by zero + float dt = 0.01; // Using non zero value to a avoid division by zero if (_control_position_last_called > 0) { dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f; } @@ -1045,20 +1045,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; - math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; - calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); - float eas2tas = 1.0f; // XXX calculate actual number based on current measurements /* filter speed and altitude for controller */ math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - /* 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); + /* tell TECS to update its state, but let it know when it cannot actually control the plane */ + bool in_air_alt_control = (!_vehicle_status.condition_landed && + (_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_altitude_enabled)); + + /* update TECS filters */ + if (!_mTecs.getEnabled()) { + _tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb, + accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); } + math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; + calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); + /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;