diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 173ddd9ac8..4ecef372d9 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -56,7 +56,7 @@ static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a fil * which is used by the airspeed complimentary filter. */ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward, - bool altitude_lock, bool in_air, float altitude, float vz) + bool altitude_lock, float altitude, float vz) { // calculate the time lapsed since the last update uint64_t now = hrt_absolute_time(); @@ -69,7 +69,7 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float reset_altitude = true; } - if (!altitude_lock || !in_air) { + if (!altitude_lock) { reset_altitude = true; } @@ -80,8 +80,6 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float _state_update_timestamp = now; _EAS = equivalent_airspeed; - _in_air = in_air; - // Set the velocity and position state to the the INS data _vert_vel_state = -vz; _vert_pos_state = altitude; @@ -97,11 +95,6 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float _tas_rate_raw = 0.0f; _tas_rate_filtered = 0.0f; } - - if (!_in_air) { - _states_initialized = false; - } - } void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equivalent_airspeed, float EAS2TAS) @@ -124,8 +117,8 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva _EAS = equivalent_airspeed; } - // If first time through or not flying, reset airspeed states - if (_speed_update_timestamp == 0 || !_in_air) { + // If first time through reset airspeed states + if (_speed_update_timestamp == 0) { _tas_rate_state = 0.0f; _tas_state = (_EAS * EAS2TAS); } @@ -493,16 +486,14 @@ void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rat void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altitude, float pitch_min_climbout, float EAS2TAS) { - if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initialized) { + if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_states_initialized) { // On first time through or when not using TECS of if there has been a large time slip, // states must be reset to allow filters to a clean start _vert_vel_state = 0.0f; _vert_pos_state = baro_altitude; _tas_rate_state = 0.0f; _tas_state = _EAS * EAS2TAS; - _throttle_integ_state = 0.0f; - _pitch_integ_state = 0.0f; - _last_throttle_setpoint = (_in_air ? throttle_trim : 0.0f);; + _last_throttle_setpoint = throttle_trim; _last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max); _pitch_setpoint_unc = _last_pitch_setpoint; _TAS_setpoint_last = _EAS * EAS2TAS; @@ -512,13 +503,13 @@ void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altit _STE_rate_error = 0.0f; _hgt_setpoint = baro_altitude; + resetIntegrals(); + if (_dt > DT_MAX || _dt < DT_MIN) { _dt = DT_DEFAULT; } - _alt_control_traj_generator.reset(0, 0, baro_altitude); - _velocity_control_traj_generator.reset(0.0f, 0.0f, baro_altitude); - + resetTrajectoryGenerators(baro_altitude); } else if (_climbout_mode_active) { // During climbout use the lower pitch angle limit specified by the @@ -580,11 +571,6 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set // Initialize selected states and variables as required _initialize_states(pitch, throttle_trim, baro_altitude, pitch_min_climbout, eas_to_tas); - // Don't run TECS control algorithms when not in flight - if (!_in_air) { - return; - } - _updateTrajectoryGenerationConstraints(); // Update the true airspeed state estimate diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index c3288b4856..f634b2f0b0 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -81,7 +81,6 @@ public: * Must be called at 50Hz or greater */ void update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward, bool altitude_lock, - bool in_air, float altitude, float vz); /** @@ -99,6 +98,23 @@ public: void reset_state() { _states_initialized = false; } + void resetIntegrals() + { + _throttle_integ_state = 0.0f; + _pitch_integ_state = 0.0f; + } + + /** + * @brief Resets the altitude and height rate control trajectory generators to the input altitude + * + * @param altitude Vehicle altitude (AMSL) [m] + */ + void resetTrajectoryGenerators(const float altitude) + { + _alt_control_traj_generator.reset(0, 0, altitude); + _velocity_control_traj_generator.reset(0.0f, 0.0f, altitude); + } + enum ECL_TECS_MODE { ECL_TECS_MODE_NORMAL = 0, ECL_TECS_MODE_UNDERSPEED, @@ -307,8 +323,7 @@ private: bool _uncommanded_descent_recovery{false}; ///< true when a continuous descent caused by an unachievable airspeed demand has been detected bool _climbout_mode_active{false}; ///< true when in climbout mode bool _airspeed_enabled{false}; ///< true when airspeed use has been enabled - bool _states_initialized{false}; ///< true when TECS states have been iniitalized - bool _in_air{false}; ///< true when the vehicle is flying + bool _states_initialized{false}; ///< true when TECS states have been iniitalized /** * Update the airspeed internal state using a second order complementary filter diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index c47ebf0e7d..cb94f783e0 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -855,11 +855,17 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now) _was_in_air = true; _time_went_in_air = now; _takeoff_ground_alt = _current_altitude; // XXX: is this really a good idea? + + _tecs.resetIntegrals(); + _tecs.resetTrajectoryGenerators(_current_altitude); } /* reset flag when airplane landed */ if (_landed) { _was_in_air = false; + + _tecs.resetIntegrals(); + _tecs.resetTrajectoryGenerators(_current_altitude); } } @@ -2176,7 +2182,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, float FixedwingPositionControl::get_tecs_pitch() { - if (_is_tecs_running) { + if (_tecs_is_running) { return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get()); } @@ -2187,7 +2193,7 @@ FixedwingPositionControl::get_tecs_pitch() float FixedwingPositionControl::get_tecs_thrust() { - if (_is_tecs_running) { + if (_tecs_is_running) { return min(_tecs.get_throttle_setpoint(), 1.f); } @@ -2567,14 +2573,13 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva bool climbout_mode, float climbout_pitch_min_rad, bool disable_underspeed_detection, float hgt_rate_sp) { - // do not run TECS if we are not in air - bool run_tecs = !_landed; + _tecs_is_running = true; // do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition // (it should also not run during VTOL blending because airspeed is too low still) if (_vehicle_status.is_vtol) { if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) { - run_tecs = false; + _tecs_is_running = false; } if (_vehicle_status.in_transition_mode) { @@ -2607,9 +2612,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva } } - _is_tecs_running = run_tecs; - - if (!run_tecs) { + if (!_tecs_is_running) { // next time we run TECS we should reinitialize states _reinitialize_tecs = true; return; @@ -2623,16 +2626,14 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva /* No underspeed protection in landing mode */ _tecs.set_detect_underspeed_enabled(!disable_underspeed_detection); - /* tell TECS to update its state, but let it know when it cannot actually control the plane */ - bool in_air_alt_control = (!_landed && - (_control_mode.flag_control_auto_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_altitude_enabled || - _control_mode.flag_control_climb_rate_enabled)); + if (_landed) { + _tecs.resetIntegrals(); + _tecs.resetTrajectoryGenerators(_current_altitude); + } /* update TECS vehicle state estimates */ - _tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_alt_control, - _current_altitude, _local_pos.vz); + _tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), _current_altitude, + _local_pos.vz); const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min, throttle_max); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index e7d9d78444..64cc8ecfd7 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -302,7 +302,7 @@ private: matrix::Vector3f _body_velocity{}; bool _reinitialize_tecs{true}; - bool _is_tecs_running{false}; + bool _tecs_is_running{false}; hrt_abstime _time_last_tecs_update{0}; // [us]