Skip to content

Commit

Permalink
TECS: init control params correctly
Browse files Browse the repository at this point in the history 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
  • Loading branch information
@sfuhrer
sfuhrer committed Dec 7, 2023
1 parent 90e8a2f commit ac33e4e
Show file tree
Hide file tree
Showing 3 changed files with 77 additions and 26 deletions.
41 changes: 28 additions & 13 deletions src/lib/tecs/TECS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -641,9 +641,30 @@ 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, float throttle_min, float throttle_setpoint_max,
float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate,
float target_sinkrate)
{
initControlParams(target_climbrate, target_sinkrate, eas_to_tas, pitch_limit_max, pitch_limit_min, throttle_min,
throttle_setpoint_max, throttle_trim);

// Init subclasses
TECSAltitudeReferenceModel::AltitudeReferenceState current_state{.alt = altitude,
.alt_rate = altitude_rate};
Expand Down Expand Up @@ -683,17 +704,9 @@ 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<float>((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.
Expand All @@ -702,7 +715,9 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set

if (dt > DT_MAX || _update_timestamp == 0UL) {
// Update time intervall too large, can't guarantee sanity of state updates anymore. reset the control loop.
initialize(altitude, hgt_rate, equivalent_airspeed, eas_to_tas);
initialize(altitude, hgt_rate, equivalent_airspeed, eas_to_tas, throttle_min, throttle_setpoint_max,
throttle_trim, pitch_limit_min, pitch_limit_max, target_climbrate,
target_sinkrate);

} else {
// Update airspeedfilter submodule
Expand Down
12 changes: 11 additions & 1 deletion src/lib/tecs/TECS.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -570,6 +570,13 @@ class TECS
*/
void enable_airspeed(bool enabled) { _control_flag.airspeed_enabled = enabled; }

/**
* @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 Update the control loop calculations
*
Expand All @@ -583,7 +590,10 @@ class TECS
* @brief Initialize the control loop
*
*/
void initialize(float altitude, float altitude_rate, float equivalent_airspeed, float eas_to_tas);
void initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed,
float eas_to_tas, float throttle_min, float throttle_setpoint_max,
float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate,
float target_sinkrate);

void resetIntegrals()
{
Expand Down
50 changes: 38 additions & 12 deletions src/modules/fw_pos_control/FixedwingPositionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -782,15 +782,31 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
_was_in_air = true;
_time_went_in_air = now;

_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas,
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_performance_model.getTrimThrottle(_param_fw_thr_min.get(),
_param_fw_thr_max.get(), _performance_model.getCalibratedTrimAirspeed(), _air_density),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_climbrate_target.get(),
_param_sinkrate_target.get());
}

/* reset flag when airplane landed */
if (_landed) {
_was_in_air = false;
_completed_manual_takeoff = false;

_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas,
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_performance_model.getTrimThrottle(_param_fw_thr_min.get(),
_param_fw_thr_max.get(), _performance_model.getCalibratedTrimAirspeed(), _air_density),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_climbrate_target.get(),
_param_sinkrate_target.get());
}
}

Expand Down Expand Up @@ -2421,7 +2437,15 @@ 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);
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas,
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_performance_model.getTrimThrottle(_param_fw_thr_min.get(),
_param_fw_thr_max.get(), _performance_model.getCalibratedTrimAirspeed(), _air_density),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_climbrate_target.get(),
_param_sinkrate_target.get());

break;
}
Expand Down Expand Up @@ -2579,22 +2603,24 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
_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);

if (_landed || _reinitialize_tecs) {
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas,
throttle_min,
throttle_max,
throttle_trim_compensated,
pitch_min_rad - radians(_param_fw_psp_off.get()),
pitch_max_rad - radians(_param_fw_psp_off.get()),
desired_max_climbrate,
desired_max_sinkrate);
}

// 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;
Expand Down

0 comments on commit ac33e4e

Please sign in to comment.