From 09e415c142ab14ee242677d348b2ff355ed1a91b Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 9 Nov 2021 09:39:37 +0100 Subject: [PATCH] FW Position Control: add logic for speed modes Eco and Dash Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 73 +++++++++++++++++++ .../FixedwingPositionControl.hpp | 24 +++++- .../fw_pos_control_l1_params.c | 56 ++++++++++++++ 3 files changed, 152 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index f538858cc8..c8cf86b2cf 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -399,6 +399,77 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const return constrain(airspeed_demand, adjusted_min_airspeed, _param_fw_airspd_max.get()); } +void +FixedwingPositionControl::updateSpeedMode() +{ + FW_SPEED_MODE_COMMANDED new_mode = static_cast(_fw_spd_mode_set.get()); + + switch (new_mode) { + case NORMAL: + _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; //default + break; + + case ECO_CRUISE: + if (_conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) { + _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO; + + } else { + _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; + } + + break; + + case ECO_FULL: + if (_conditions_for_eco_dash_met) { + _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO; + + } else { + _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; + } + + break; + + case DASH_CRUISE: + if (_conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) { + _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_DASH; + + } else { + _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; + } + + break; + + case DASH_FULL: + if (_conditions_for_eco_dash_met) { + _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_DASH; + + } else { + _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; + } + + break; + } +} + +void +FixedwingPositionControl::check_eco_dash_allowed() +{ + const float altitude_amsl_min = max(_tecs.get_hgt_setpoint() - fw_alt_err_u.get(), + _local_pos.ref_alt + fw_alt_min.get()); + const float altitude_amsl_max = _tecs.get_hgt_setpoint() + fw_alt_err_o.get(); + + const bool altitdue_conditions_met = _current_altitude <= altitude_amsl_max + && _current_altitude >= altitude_amsl_min; + + // add a 10s timeout after conditions where not met + _conditions_for_eco_dash_met = altitdue_conditions_met && hrt_elapsed_time(&_time_conditions_not_met) > 10_s; + + + if (!altitdue_conditions_met) { + // reset timer + _time_conditions_not_met = _local_pos.timestamp; + } +} void FixedwingPositionControl::update_wind_mode() @@ -710,6 +781,8 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c /* get circle mode */ const bool was_circle_mode = _l1_control.circle_mode(); + check_eco_dash_allowed(); + updateSpeedMode(); update_wind_mode(); /* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */ diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index f5d72bafd3..25c1b22cb4 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -268,6 +268,23 @@ private: FW_POSCTRL_MODE_OTHER } _control_mode_current{FW_POSCTRL_MODE_OTHER}; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode. + enum FW_SPEED_MODE { + FW_SPEED_MODE_NORMAL, + FW_SPEED_MODE_ECO, + FW_SPEED_MODE_DASH, + } _speed_mode_current{FW_SPEED_MODE_NORMAL}; + + enum FW_SPEED_MODE_COMMANDED { + NORMAL, + ECO_CRUISE, + ECO_FULL, + DASH_CRUISE, + DASH_FULL + } _speed_mode_setting{NORMAL}; + + bool _conditions_for_eco_dash_met{false}; + hrt_abstime _time_conditions_not_met{0}; + // wind state hrt_abstime _first_time_current_mode_detected{0}; ///< last time in normal wind mode @@ -363,6 +380,8 @@ private: void publishOrbitStatus(const position_setpoint_s pos_sp); + void check_eco_dash_allowed(); + void updateSpeedMode(); void update_wind_mode(); @@ -456,8 +475,11 @@ private: (ParamFloat) _param_fw_wind_thld_l, (ParamFloat) _param_fw_wind_arsp_of, + (ParamInt) _fw_spd_mode_set, + (ParamFloat) fw_alt_min, + (ParamFloat) fw_alt_err_u, + (ParamFloat) fw_alt_err_o ) - }; #endif // FIXEDWINGPOSITIONCONTROL_HPP_ 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 63305de15d..09d85b639c 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 @@ -904,3 +904,59 @@ PARAM_DEFINE_FLOAT(FW_WIND_THLD_L, 0.0f); * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_WIND_ARSP_OF, 1.0f); + +/** + * FW Speed mode setting + * + * Setting + * + * @min 0 + * @max 4 + * @value 0 Normal + * @value 1 Eco cruise + * @value 2 Eco full + * @value 3 Dash cruise + * @value 4 Dash full + * @group FW TECS + */ +PARAM_DEFINE_INT32(FW_SPD_MODE_SET, 0); + + +/** + * Max altitude undershoot for Eco/Dash + * + * Eco/Dash mode is disabled if the current altitude is more than this value below the setpoint. + * + * @min 1.0 + * @max 50.0 + * @decimal 1 + * @increment 0.1 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_ALT_ERR_U, 10.f); + +/** + * Max altitude overshoot for Eco/Dash + * + * Eco/Dash mode is disabled if the current altitude is more than this value above the setpoint. + * + * @min 1.0 + * @max 50.0 + * @decimal 1 + * @increment 0.1 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_ALT_ERR_O, 20.f); + +/** + * Min altitude for Eco/Dash + * + * Eco/Dash mode can be enabled if above this relative altitude to home. + * + * @min -200.0 + * @max 200.0 + * @decimal 1 + * @increment 0.1 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_ALT_MIN, 50.f);