From 55809e809c57a533240411841d003585d200cac4 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 9 Nov 2021 15:15:17 +0100 Subject: [PATCH] FW Position Control: add Eco mode in TECS and separate Eco climbrate Separate settings in Eco mode (beside airspeed): - FW_T_CLMB_R_SP_E (slower climb for more efficiency) - FW_T_ALT_TC_E (larger to reduce motor action due to altitude errors) - FW_T_SPDWEIGHT_E (more weight to airspeed to reduce motor action due to altitude errors) Signed-off-by: Silvan Fuhrer --- msg/tecs_status.msg | 1 + src/lib/tecs/TECS.cpp | 15 +++++++- src/lib/tecs/TECS.hpp | 13 ++++++- .../FixedwingPositionControl.cpp | 12 +++++- .../FixedwingPositionControl.hpp | 5 ++- .../fw_pos_control_l1_params.c | 37 +++++++++++++++++++ 6 files changed, 77 insertions(+), 6 deletions(-) diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index 37bf9bb5e0..bb79cd54f8 100644 --- a/msg/tecs_status.msg +++ b/msg/tecs_status.msg @@ -6,6 +6,7 @@ uint8 TECS_MODE_LAND = 3 uint8 TECS_MODE_LAND_THROTTLELIM = 4 uint8 TECS_MODE_BAD_DESCENT = 5 uint8 TECS_MODE_CLIMBOUT = 6 +uint8 TECS_MODE_ECO = 7 float32 altitude_sp diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index d50fb5bf90..a6a353dfb3 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -586,7 +586,7 @@ 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 target_climbrate, float target_sinkrate, float hgt_rate_sp) + float target_climbrate, float target_sinkrate, float hgt_rate_sp, bool eco_mode_enabled) { // Calculate the time since last update (seconds) uint64_t now = hrt_absolute_time(); @@ -623,6 +623,8 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set // Detect an uncommanded descent caused by an unachievable airspeed demand _detect_uncommanded_descent(); + _eco_mode_enabled = eco_mode_enabled; + // Calculate the demanded true airspeed _update_speed_setpoint(); @@ -652,6 +654,9 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set } else if (_climbout_mode_active) { _tecs_mode = ECL_TECS_MODE_CLIMBOUT; + } else if (_eco_mode_enabled) { + _tecs_mode = ECL_TECS_MODE_ECO; + } else { // This is the default operation mode _tecs_mode = ECL_TECS_MODE_NORMAL; @@ -662,6 +667,11 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set void TECS::_update_speed_height_weights() { + if (_eco_mode_enabled) { + _pitch_speed_weight = _pitch_speed_weight_eco; + _height_error_gain = _height_error_gain_eco; + } + // Calculate the weight applied to control of specific kinetic energy error _SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f); @@ -699,6 +709,9 @@ TECS::tecs_status_publish(const hrt_abstime &now) case TECS::ECL_TECS_MODE_CLIMBOUT: tecs_status.mode = tecs_status_s::TECS_MODE_CLIMBOUT; break; + + case TECS::ECL_TECS_MODE_ECO: + tecs_status.mode = tecs_status_s::TECS_MODE_ECO; } tecs_status.altitude_sp = _hgt_setpoint; diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index a2caaf3c3f..1a74671290 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -94,7 +94,8 @@ 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 target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN); + float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN, + bool eco_mode_enabled = false); void reset_state() { _states_initialized = false; } @@ -102,7 +103,8 @@ public: ECL_TECS_MODE_NORMAL = 0, ECL_TECS_MODE_UNDERSPEED, ECL_TECS_MODE_BAD_DESCENT, - ECL_TECS_MODE_CLIMBOUT + ECL_TECS_MODE_CLIMBOUT, + ECL_TECS_MODE_ECO }; void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; } @@ -140,6 +142,9 @@ public: void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; } + // eco mode settings + void set_speed_weight_eco(float weight_eco) { _pitch_speed_weight_eco = weight_eco; } + void set_height_error_time_constant_eco(float time_const_eco) { _height_error_gain_eco = 1.0f / math::max(time_const_eco, 0.1f); } // getters float get_throttle_setpoint() { return _last_throttle_setpoint; } @@ -210,6 +215,9 @@ private: float _speed_derivative_time_const{0.01f}; ///< speed derivative filter time constant (s) float _SEB_rate_ff{1.0f}; + float _height_error_gain_eco{0.2f}; ///< in eco mode: height error inverse time constant [1/s] + float _pitch_speed_weight_eco{1.0f}; ///< in eco mode: speed control weighting used by pitch demand calculation + // complimentary filter states float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec) float _vert_pos_state{0.0f}; ///< complimentary filter state - height (m) @@ -280,6 +288,7 @@ private: bool _airspeed_enabled{false}; ///< true when airspeed use has been enabled bool _states_initialized{false}; ///< true when TECS states have been iniitalized bool _in_air{false}; ///< true when the vehicle is flying + bool _eco_mode_enabled{false}; // flight phase int _flight_phase{tecs_status_s::TECS_FLIGHT_PHASE_LEVEL}; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index b7849330c7..c015a1564c 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -131,6 +131,9 @@ FixedwingPositionControl::parameters_update() _tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get()); _tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get()); + _tecs.set_speed_weight_eco(_param_fw_t_spdweight_eco.get()); + _tecs.set_height_error_time_constant_eco(_param_fw_t_h_error_tc_eco.get()); + // Landing slope /* check if negative value for 2/3 of flare altitude is set for throttle cut */ @@ -662,7 +665,8 @@ FixedwingPositionControl::getManualHeightRateSetpoint() } else if (_manual_control_setpoint_altitude < - deadBand) { /* pitching up */ float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor; - const float climb_rate_target = _param_climbrate_target.get(); + const float climb_rate_target = _speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO ? + _param_fw_t_clmb_r_sp_eco.get() : _param_climbrate_target.get(); height_rate_setpoint = pitch * climb_rate_target; } @@ -2261,6 +2265,9 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo } } + const float climb_rate_target = _speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO ? + _param_fw_t_clmb_r_sp_eco.get() : _param_climbrate_target.get(); + _tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()), _current_altitude, alt_sp, airspeed_sp, _airspeed, _eas2tas, @@ -2269,7 +2276,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo throttle_min, throttle_max, throttle_cruise, pitch_min_rad - radians(_param_fw_psp_off.get()), pitch_max_rad - radians(_param_fw_psp_off.get()), - _param_climbrate_target.get(), _param_sinkrate_target.get(), hgt_rate_sp); + climb_rate_target, _param_sinkrate_target.get(), hgt_rate_sp, + _speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO); } void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index d52d0f95d5..fd4ffb58ea 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -471,7 +471,10 @@ private: (ParamInt) _fw_spd_mode_set, (ParamFloat) fw_alt_min, (ParamFloat) fw_alt_err_u, - (ParamFloat) fw_alt_err_o + (ParamFloat) fw_alt_err_o, + (ParamFloat) _param_fw_t_h_error_tc_eco, + (ParamFloat) _param_fw_t_spdweight_eco, + (ParamFloat) _param_fw_t_clmb_r_sp_eco ) }; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index a3815e74a9..22cbd515b9 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -928,3 +928,40 @@ PARAM_DEFINE_FLOAT(FW_ALT_ERR_O, 20.f); * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_ALT_MIN, 50.f); + +/** + * Eco Mode: Speed <--> Altitude priority + * + * @min 0.0 + * @max 2.0 + * @decimal 1 + * @increment 1.0 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT_E, 1.8f); + +/** + * Eco Mode: Altitude error time constant. + * + * Normally larger than FW_T_ALT_TC to have looser altitude control and in turn less throttle changes. + * + * @min 2.0 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_ALT_TC_E, 10.0f); + +/** + * Eco target climbrate. + * + * The rate at which the vehicle will climb in autonomous modes to achieve altitude setpoints in Eco mode. + * + * @unit m/s + * @min 0.5 + * @max 15 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP_E, 2.0f);