fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 09:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

TECS: improve initialization

Browse Source 
-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-12 15:16:38 +01:00
committed by Matthias Grob
parent 1f5fc3e849
commit 60e2c6a5cb
4 changed files with 52 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+30 -34
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;
}
src/lib/tecs/TECS.hpp
+18 -7
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.