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

tecs: propagate altitude setpoint based on target climb/sink rate 

- avoids tecs always climbing and sinking and max rates and allows to fine tune these rates
- avoid numerical calculation of feedforward velocity using derivative, this
 was prone to jitter in dt

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2021-05-03 17:41:27 +03:00 committed by Silvan Fuhrer
parent 4b7416c05b
commit a5cc449e69
4 changed files with 41 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -174,51 +174,31 @@ void TECS::_update_speed_setpoint()
}
void TECS::_update_height_setpoint(float desired, float state)
void TECS::updateHeightRateSetpoint(float alt_sp_amsl_m, float target_climbrate_m_s, float target_sinkrate_m_s,
float alt_amsl)
{
// Detect first time through and initialize previous value to demand
if (PX4_ISFINITE(desired) && fabsf(_hgt_setpoint_in_prev) < 0.1f) {
_hgt_setpoint_in_prev = desired;
target_climbrate_m_s = math::min(target_climbrate_m_s, _max_climb_rate);
target_sinkrate_m_s = math::min(target_sinkrate_m_s, _max_sink_rate);
float feedforward_height_rate = 0.0f;
if (fabsf(alt_sp_amsl_m - _hgt_setpoint) < math::max(target_sinkrate_m_s, target_climbrate_m_s) * _dt) {
_hgt_setpoint = alt_sp_amsl_m;
} else if (alt_sp_amsl_m > _hgt_setpoint) {
_hgt_setpoint += target_climbrate_m_s * _dt;
feedforward_height_rate = target_climbrate_m_s;
} else if (alt_sp_amsl_m < _hgt_setpoint) {
_hgt_setpoint -= target_sinkrate_m_s * _dt;
feedforward_height_rate = -target_sinkrate_m_s;
}
// Apply a 2 point moving average to demanded height to reduce
// intersampling noise effects.
if (PX4_ISFINITE(desired)) {
_hgt_setpoint = 0.5f * (desired + _hgt_setpoint_in_prev);
} else {
_hgt_setpoint = _hgt_setpoint_in_prev;
}
_hgt_setpoint_in_prev = _hgt_setpoint;
// Apply a rate limit to respect vehicle performance limitations
if ((_hgt_setpoint - _hgt_setpoint_prev) > (_max_climb_rate * _dt)) {
_hgt_setpoint = _hgt_setpoint_prev + _max_climb_rate * _dt;
} else if ((_hgt_setpoint - _hgt_setpoint_prev) < (-_max_sink_rate * _dt)) {
_hgt_setpoint = _hgt_setpoint_prev - _max_sink_rate * _dt;
}
_hgt_setpoint_prev = _hgt_setpoint;
// Apply a first order noise filter
_hgt_setpoint_adj = 0.1f * _hgt_setpoint + 0.9f * _hgt_setpoint_adj_prev;
// Use a first order system to calculate a height rate setpoint from the current height error.
// Additionally, allow to add feedforward from heigh setpoint change
_hgt_rate_setpoint = (_hgt_setpoint_adj - state) * _height_error_gain + _height_setpoint_gain_ff *
(_hgt_setpoint_adj - _hgt_setpoint_adj_prev) / _dt;
_hgt_setpoint_adj_prev = _hgt_setpoint_adj;
// Limit the rate of change of height demand to respect vehicle performance limits
if (_hgt_rate_setpoint > _max_climb_rate) {
_hgt_rate_setpoint = _max_climb_rate;
} else if (_hgt_rate_setpoint < -_max_sink_rate) {
_hgt_rate_setpoint = -_max_sink_rate;
}
_hgt_rate_setpoint = (_hgt_setpoint - alt_amsl) * _height_error_gain + _height_setpoint_gain_ff *
feedforward_height_rate;
_hgt_rate_setpoint = math::constrain(_hgt_rate_setpoint, -_max_sink_rate, _max_climb_rate);
}
void TECS::_detect_underspeed()
@ -229,7 +209,7 @@ void TECS::_detect_underspeed()
}
if (((_tas_state < _TAS_min * 0.9f) && (_last_throttle_setpoint >= _throttle_setpoint_max * 0.95f))
|| ((_vert_pos_state < _hgt_setpoint_adj) && _underspeed_detected)) {
|| ((_vert_pos_state < _hgt_setpoint) && _underspeed_detected)) {
_underspeed_detected = true;
@ -241,7 +221,7 @@ void TECS::_detect_underspeed()
void TECS::_update_energy_estimates()
{
// Calculate specific energy demands in units of (m**2/sec**2)
_SPE_setpoint = _hgt_setpoint_adj * CONSTANTS_ONE_G; // potential energy
_SPE_setpoint = _hgt_setpoint * CONSTANTS_ONE_G; // potential energy
_SKE_setpoint = 0.5f * _TAS_setpoint_adj * _TAS_setpoint_adj; // kinetic energy
// Calculate total energy error
@ -477,10 +457,7 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt
_last_throttle_setpoint = (_in_air ? throttle_cruise : 0.0f);;
_last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max);
_pitch_setpoint_unc = _last_pitch_setpoint;
_hgt_setpoint_adj_prev = baro_altitude;
_hgt_setpoint_adj = _hgt_setpoint_adj_prev;
_hgt_setpoint_prev = _hgt_setpoint_adj_prev;
_hgt_setpoint_in_prev = _hgt_setpoint_adj_prev;
_hgt_setpoint = baro_altitude;
_TAS_setpoint_last = _EAS * EAS2TAS;
_TAS_setpoint_adj = _TAS_setpoint_last;
_underspeed_detected = false;
@ -499,15 +476,12 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt
// throttle lower limit is set to a value that prevents throttle reduction
_throttle_setpoint_min = _throttle_setpoint_max - 0.01f;
// height demand and associated states are set to track the measured height
_hgt_setpoint_adj_prev = baro_altitude;
_hgt_setpoint_adj = _hgt_setpoint_adj_prev;
_hgt_setpoint_prev = _hgt_setpoint_adj_prev;
// airspeed demand states are set to track the measured airspeed
_TAS_setpoint_last = _EAS * EAS2TAS;
_TAS_setpoint_adj = _EAS * EAS2TAS;
_hgt_setpoint = baro_altitude;
// disable speed and decent error condition checks
_underspeed_detected = false;
_uncommanded_descent_recovery = false;
@ -535,7 +509,8 @@ void TECS::_update_STE_rate_lim()
void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max)
float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max,
float target_climbrate, float target_sinkrate)
{
// Calculate the time since last update (seconds)
uint64_t now = hrt_absolute_time();
@ -573,8 +548,8 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
// Calculate the demanded true airspeed
_update_speed_setpoint();
// Calculate the demanded height
_update_height_setpoint(hgt_setpoint, baro_altitude);
// calculate heigh rate setpoint based on altitude demand
updateHeightRateSetpoint(hgt_setpoint, target_climbrate, target_sinkrate, baro_altitude);
// Calculate the specific energy values required by the control loop
_update_energy_estimates();

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

@ -84,7 +84,7 @@ public:
void update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
float throttle_min, float throttle_setpoint_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max);
float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate);
float get_throttle_setpoint() { return _last_throttle_setpoint; }
float get_pitch_setpoint() { return _last_pitch_setpoint; }
@ -139,7 +139,7 @@ public:
uint64_t timestamp() { return _pitch_update_timestamp; }
ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
float hgt_setpoint_adj() { return _hgt_setpoint_adj; }
float hgt_setpoint() { return _hgt_setpoint; }
float vert_pos_state() { return _vert_pos_state; }
float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
@ -188,11 +188,7 @@ public:
*/
void handle_alt_step(float delta_alt, float altitude)
{
// add height reset delta to all variables involved
// in filtering the demanded height
_hgt_setpoint_in_prev += delta_alt;
_hgt_setpoint_prev += delta_alt;
_hgt_setpoint_adj_prev += delta_alt;
_hgt_setpoint += delta_alt;
// reset height states
_vert_pos_state = altitude;
@ -259,10 +255,6 @@ private:
// height demand calculations
float _hgt_setpoint{0.0f}; ///< demanded height tracked by the TECS algorithm (m)
float _hgt_setpoint_in_prev{0.0f}; ///< previous value of _hgt_setpoint after noise filtering (m)
float _hgt_setpoint_prev{0.0f}; ///< previous value of _hgt_setpoint after noise filtering and rate limiting (m)
float _hgt_setpoint_adj{0.0f}; ///< demanded height used by the control loops after all filtering has been applied (m)
float _hgt_setpoint_adj_prev{0.0f}; ///< value of _hgt_setpoint_adj from previous frame (m)
float _hgt_rate_setpoint{0.0f}; ///< demanded climb rate tracked by the TECS algorithm
// vehicle physical limits
@ -318,9 +310,11 @@ private:
void _update_speed_setpoint();
/**
* Update the desired height
* Calculate desired height rate from altitude demand
*/
void _update_height_setpoint(float desired, float state);
void updateHeightRateSetpoint(float alt_sp_amsl_m, float target_climbrate_m_s, float target_sinkrate_m_s,
float alt_amsl);
/**
* Detect if the system is not capable of maintaining airspeed

5
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -386,7 +386,7 @@ FixedwingPositionControl::tecs_status_publish()
break;
}
t.altitude_sp = _tecs.hgt_setpoint_adj();
t.altitude_sp = _tecs.hgt_setpoint();
t.altitude_filtered = _tecs.vert_pos_state();
t.true_airspeed_sp = _tecs.TAS_setpoint_adj();
@ -1947,7 +1947,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad - radians(_param_fw_psp_off.get()),
pitch_max_rad - radians(_param_fw_psp_off.get()));
pitch_max_rad - radians(_param_fw_psp_off.get()),
_param_climbrate_target.get(), _param_sinkrate_target.get());
tecs_status_publish();
}

2
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -396,6 +396,8 @@ private:
(ParamFloat<px4::params::FW_T_STE_R_TC>) _param_ste_rate_time_const,
(ParamFloat<px4::params::FW_T_TAS_R_TC>) _param_tas_rate_time_const,
(ParamFloat<px4::params::FW_T_SEB_R_FF>) _param_seb_rate_ff,
(ParamFloat<px4::params::FW_T_CLIMB_R_SP>) _param_climbrate_target,
(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
(ParamFloat<px4::params::FW_THR_ALT_SCL>) _param_fw_thr_alt_scl,
(ParamFloat<px4::params::FW_THR_CRUISE>) _param_fw_thr_cruise,