diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index cd765b71b5..f540a8a5e8 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -628,8 +628,24 @@ void TECSControl::resetIntegrals() _throttle_integ_state = 0.0f; } +void TECS::initControlParams(float target_climbrate, float target_sinkrate, float eas_to_tas, float pitch_limit_max, + float pitch_limit_min, float throttle_min, float throttle_setpoint_max, float throttle_trim) +{ + // Update parameters from input + // Reference model + _reference_param.target_climbrate = target_climbrate; + _reference_param.target_sinkrate = target_sinkrate; + // Control + _control_param.tas_min = eas_to_tas * _equivalent_airspeed_min; + _control_param.pitch_max = pitch_limit_max; + _control_param.pitch_min = pitch_limit_min; + _control_param.throttle_trim = throttle_trim; + _control_param.throttle_max = throttle_setpoint_max; + _control_param.throttle_min = throttle_min; +} + void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed, - const float eas_to_tas) + float eas_to_tas) { // Init subclasses TECSAltitudeReferenceModel::AltitudeReferenceState current_state{.alt = altitude, @@ -649,15 +665,6 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo .tas_rate = 0.0f}; _control.initialize(control_setpoint, control_input, _control_param, _control_flag); - - _debug_status.control = _control.getDebugOutput(); - _debug_status.true_airspeed_filtered = eas_to_tas * _airspeed_filter.getState().speed; - _debug_status.true_airspeed_derivative = eas_to_tas * _airspeed_filter.getState().speed_rate; - _debug_status.altitude_reference = _altitude_reference_model.getAltitudeReference().alt; - _debug_status.height_rate_reference = _altitude_reference_model.getAltitudeReference().alt_rate; - _debug_status.height_rate_direct = _altitude_reference_model.getHeightRateSetpointDirect(); - - _update_timestamp = hrt_absolute_time(); } void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed, @@ -670,17 +677,8 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set const hrt_abstime now(hrt_absolute_time()); const float dt = static_cast((now - _update_timestamp)) / 1_s; - // Update parameters from input - // Reference model - _reference_param.target_climbrate = target_climbrate; - _reference_param.target_sinkrate = target_sinkrate; - // Control - _control_param.tas_min = eas_to_tas * _equivalent_airspeed_min; - _control_param.pitch_max = pitch_limit_max; - _control_param.pitch_min = pitch_limit_min; - _control_param.throttle_trim = throttle_trim; - _control_param.throttle_max = throttle_setpoint_max; - _control_param.throttle_min = throttle_min; + initControlParams(target_climbrate, target_sinkrate, eas_to_tas, pitch_limit_max, pitch_limit_min, throttle_min, + throttle_setpoint_max, throttle_trim); if (dt < DT_MIN) { // Update intervall too small, do not update. Assume constant states/output in this case. @@ -697,7 +695,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set .equivalent_airspeed_rate = speed_deriv_forward / eas_to_tas}; _airspeed_filter.update(dt, airspeed_input, _airspeed_filter_param, _control_flag.airspeed_enabled); - const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState(); // Update Reference model submodule const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint, @@ -712,20 +709,19 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set const TECSControl::Input control_input{ .altitude = altitude, .altitude_rate = hgt_rate, - .tas = eas_to_tas * eas.speed, - .tas_rate = eas_to_tas * eas.speed_rate}; + .tas = eas_to_tas * _airspeed_filter.getState().speed, + .tas_rate = eas_to_tas * _airspeed_filter.getState().speed_rate}; _control.update(dt, control_setpoint, control_input, _control_param, _control_flag); - - // Update time stamps - _update_timestamp = now; - - _debug_status.control = _control.getDebugOutput(); - _debug_status.true_airspeed_filtered = eas_to_tas * eas.speed; - _debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate; - _debug_status.altitude_reference = control_setpoint.altitude_reference.alt; - _debug_status.height_rate_reference = control_setpoint.altitude_reference.alt_rate; - _debug_status.height_rate_direct = _altitude_reference_model.getHeightRateSetpointDirect(); } + + _debug_status.control = _control.getDebugOutput(); + _debug_status.true_airspeed_filtered = eas_to_tas * _airspeed_filter.getState().speed; + _debug_status.true_airspeed_derivative = eas_to_tas * _airspeed_filter.getState().speed_rate; + _debug_status.altitude_reference = _altitude_reference_model.getAltitudeReference().alt; + _debug_status.height_rate_reference = _altitude_reference_model.getAltitudeReference().alt_rate; + _debug_status.height_rate_direct = _altitude_reference_model.getHeightRateSetpointDirect(); + + _update_timestamp = now; } diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 6f82fe8c1e..8749ac0a21 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -88,8 +88,11 @@ public: * @brief Initialize filter * * @param[in] equivalent_airspeed is the equivalent airspeed in [m/s]. + * @param[in] equivalent_airspeed_trim is the equivalent airspeed trim (vehicle setting) in [m/s]. + * @param[in] airspeed_sensor_available boolean if the airspeed sensor is available. */ - void initialize(float equivalent_airspeed); + void initialize(float equivalent_airspeed, const float equivalent_airspeed_trim, + const bool airspeed_sensor_available); /** * @brief Update filter @@ -579,12 +582,6 @@ public: float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN); - /** - * @brief Initialize the control loop - * - */ - void initialize(float altitude, float altitude_rate, float equivalent_airspeed, float eas_to_tas); - void resetIntegrals() { _control.resetIntegrals(); @@ -648,6 +645,20 @@ public: float get_underspeed_ratio() { return _control.getRatioUndersped(); } private: + /** + * @brief Initialize the control parameters + * + */ + void initControlParams(float target_climbrate, float target_sinkrate, float eas_to_tas, float pitch_limit_max, + float pitch_limit_min, float throttle_min, float throttle_setpoint_max, float throttle_trim); + + /** + * @brief Initialize the control loop + * + */ + void initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed, + float eas_to_tas); + TECSControl _control; ///< Control submodule. TECSAirspeedFilter _airspeed_filter; ///< Airspeed filter submodule. TECSAltitudeReferenceModel _altitude_reference_model; ///< Setpoint reference model submodule. diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 8fb08925a0..16575f3107 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -777,21 +777,11 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) void FixedwingPositionControl::update_in_air_states(const hrt_abstime now) { - /* save time when airplane is in air */ - if (!_was_in_air && !_landed) { - _was_in_air = true; - _time_went_in_air = now; - - _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas); - } - /* reset flag when airplane landed */ if (_landed) { - _was_in_air = false; _completed_manual_takeoff = false; - - _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas); } + } void @@ -2420,9 +2410,6 @@ FixedwingPositionControl::Run() case FW_POSCTRL_MODE_OTHER: { _att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get()); - - _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas); - break; } @@ -2569,32 +2556,16 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva } if (!_tecs_is_running) { - // next time we run TECS we should reinitialize states - _reinitialize_tecs = true; return; } - // We need an altitude lock to calculate the TECS control - if (_local_pos.timestamp == 0) { - _reinitialize_tecs = true; - } - - if (_reinitialize_tecs) { - _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas); - _reinitialize_tecs = false; - } - - /* No underspeed protection in landing mode */ - _tecs.set_detect_underspeed_enabled(!disable_underspeed_detection); - - if (_landed) { - _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas); - } - /* update TECS vehicle state estimates */ const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min, throttle_max, airspeed_sp, _air_density); + /* No underspeed protection in landing mode */ + _tecs.set_detect_underspeed_enabled(!disable_underspeed_detection); + // HOTFIX: the airspeed rate estimate using acceleration in body-forward direction has shown to lead to high biases // when flying tight turns. It's in this case much safer to just set the estimated airspeed rate to 0. const float airspeed_rate_estimate = 0.f; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index da96aef75a..a458313a9b 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -270,12 +270,6 @@ private: bool _landed{true}; - // indicates whether the plane was in the air in the previous interation - bool _was_in_air{false}; - - // [us] time at which the plane went in the air - hrt_abstime _time_went_in_air{0}; - // MANUAL MODES // indicates whether we have completed a manual takeoff in a position control mode @@ -388,10 +382,7 @@ private: // total energy control system - airspeed / altitude control TECS _tecs; - bool _reinitialize_tecs{true}; bool _tecs_is_running{false}; - hrt_abstime _time_last_tecs_update{0}; // [us] - // VTOL / TRANSITION float _airspeed_after_transition{0.0f};