dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

TECS: improve initialization 

-remove external init, and instead always (but only) init when dt is too large
-init the controller params correctly

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-12-11 12:07:00 +01:00 committed by Matthias Grob
parent 1f5fc3e849
commit 60e2c6a5cb
4 changed files with 52 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
src/lib/tecs/TECS.cpp
View File

@ -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<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.
@ -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;
}

25
src/lib/tecs/TECS.hpp
View File

@ -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.

37
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -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;

9
src/modules/fw_pos_control/FixedwingPositionControl.hpp
View File

@ -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};