diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 05940d9cf6..1428a5f8d3 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -714,6 +714,10 @@ TECS::tecs_status_publish(const hrt_abstime &now) case TECS::ECL_TECS_MODE_ECO: tecs_status.mode = tecs_status_s::TECS_MODE_ECO; + break; + + default: + tecs_status.mode = tecs_status_s::TECS_MODE_NORMAL; } tecs_status.altitude_sp = _hgt_setpoint; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index a9c6e1a98e..07212efc1e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -448,6 +448,21 @@ FixedwingPositionControl::getSpeedMode() { FW_SPEED_MODE_COMMANDED new_mode = static_cast(_param_fw_spd_mode_set.get()); + const float altitude_amsl_min = max(_tecs.get_hgt_setpoint() - _param_fw_spdm_alt_er_u.get(), + _local_pos.ref_alt + _param_fw_alt_spdm_min.get()); + const float altitude_amsl_max = _tecs.get_hgt_setpoint() + _param_fw_spdm_alt_er_o.get(); + + const bool altitude_conditions_met = _current_altitude <= altitude_amsl_max + && _current_altitude >= altitude_amsl_min; + + if (!altitude_conditions_met) { + // reset timer + _time_conditions_not_met = _local_pos.timestamp; + } + + // add a 10s timeout after conditions where not met + const bool conditions_for_eco_dash_met = hrt_elapsed_time(&_time_conditions_not_met) > 10_s; + FW_SPEED_MODE speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; //default; switch (new_mode) { @@ -456,7 +471,7 @@ FixedwingPositionControl::getSpeedMode() break; case ECO_CRUISE: - if (_conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) { + 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 { @@ -466,7 +481,7 @@ FixedwingPositionControl::getSpeedMode() break; case ECO_FULL: - if (_conditions_for_eco_dash_met) { + if (conditions_for_eco_dash_met) { speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO; } else { @@ -476,7 +491,7 @@ FixedwingPositionControl::getSpeedMode() break; case DASH_CRUISE: - if (_conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) { + 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 { @@ -486,7 +501,7 @@ FixedwingPositionControl::getSpeedMode() break; case DASH_FULL: - if (_conditions_for_eco_dash_met) { + if (conditions_for_eco_dash_met) { speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_DASH; } else { @@ -500,29 +515,8 @@ FixedwingPositionControl::getSpeedMode() } void -FixedwingPositionControl::check_eco_dash_allowed() +FixedwingPositionControl::resetEcoDashAllowed() { - const float altitude_amsl_min = max(_tecs.get_hgt_setpoint() - _param_fw_spdm_alt_er_u.get(), - _local_pos.ref_alt + _param_fw_alt_spdm_min.get()); - const float altitude_amsl_max = _tecs.get_hgt_setpoint() + _param_fw_spdm_alt_er_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::resetAutoSpeedAdaptions() -{ - _conditions_for_eco_dash_met = false; _time_conditions_not_met = _local_pos.timestamp; } @@ -795,7 +789,6 @@ 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(); FW_SPEED_MODE spd_mode = getSpeedMode(); /* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */ @@ -2039,25 +2032,25 @@ FixedwingPositionControl::Run() case FW_POSCTRL_MODE_AUTO_ALTITUDE: { control_auto_fixed_bank_alt_hold(_local_pos.timestamp); - resetAutoSpeedAdaptions(); + resetEcoDashAllowed(); break; } case FW_POSCTRL_MODE_AUTO_CLIMBRATE: { control_auto_descend(_local_pos.timestamp); - resetAutoSpeedAdaptions(); + resetEcoDashAllowed(); break; } case FW_POSCTRL_MODE_MANUAL_POSITION: { control_manual_position(_local_pos.timestamp, curr_pos, ground_speed); - resetAutoSpeedAdaptions(); + resetEcoDashAllowed(); break; } case FW_POSCTRL_MODE_MANUAL_ALTITUDE: { control_manual_altitude(_local_pos.timestamp, curr_pos, ground_speed); - resetAutoSpeedAdaptions(); + resetEcoDashAllowed(); break; } @@ -2068,7 +2061,7 @@ FixedwingPositionControl::Run() reset_takeoff_state(); } - resetAutoSpeedAdaptions(); + resetEcoDashAllowed(); _att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get()); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 742f997eb6..5319838b22 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -282,7 +282,7 @@ private: DASH_FULL }; - bool _conditions_for_eco_dash_met{false}; + // bool _conditions_for_eco_dash_met{false}; hrt_abstime _time_conditions_not_met{0}; float _last_airspeed_setpoint{NAN}; @@ -376,10 +376,9 @@ private: void publishOrbitStatus(const position_setpoint_s pos_sp); - void check_eco_dash_allowed(); FW_SPEED_MODE getSpeedMode(); void update_wind_mode(); - void resetAutoSpeedAdaptions(); + void resetEcoDashAllowed(); /* * Call TECS : a wrapper function to call the TECS implementation 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 f159746d28..6c38f300e5 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 @@ -859,7 +859,7 @@ PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f); * Wind-based airspeed scaling factor in Eco * * Multiplying this factor with the current absolute wind estimate gives the airspeed offset - * added to the setpoint (which is FW_AIRSPD_MIN) in Eco mode. This helps to make the + * added to the setpoint (which is FW_AIRSPD_MIN) in Eco mode (see FW_SPD_MODE_SET). This helps to make the * system more robust against disturbances in high wind. * * setpoint = FW_AIRSPD_MIN + FW_LND_AIRSPD_SC * wind @@ -893,7 +893,7 @@ 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. + * Eco/Dash mode is disabled if the current altitude overshoots the setpoint by this value. * * @min 1.0 * @max 50.0 @@ -906,7 +906,7 @@ PARAM_DEFINE_FLOAT(FW_SPDM_ALT_ER_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. + * Eco/Dash mode is disabled if the current altitude undershoots the setpoint by this value. * * @min 1.0 * @max 50.0 @@ -917,7 +917,7 @@ PARAM_DEFINE_FLOAT(FW_SPDM_ALT_ER_U, 10.f); PARAM_DEFINE_FLOAT(FW_SPDM_ALT_ER_O, 20.f); /** - * Min altitude for Eco/Dash + * Minimum height above home for Eco/Dash * * Eco/Dash mode can be enabled if above this relative altitude to home. * @@ -930,7 +930,7 @@ PARAM_DEFINE_FLOAT(FW_SPDM_ALT_ER_O, 20.f); PARAM_DEFINE_FLOAT(FW_SPDM_ALT_MIN, 50.f); /** - * Eco Mode: Speed <--> Altitude priority + * Speed <--> Altitude priority in Eco mode * * @min 0.0 * @max 2.0