From 8c42b7178d2d2480a4bd3c6dacca7a0f370c1e19 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 9 Nov 2021 13:17:18 +0100 Subject: [PATCH] FW Position Control: add reset function for global variables used in auto modes Signed-off-by: Silvan Fuhrer --- .../fw_pos_control_l1/FixedwingPositionControl.cpp | 14 ++++++++++++++ .../fw_pos_control_l1/FixedwingPositionControl.hpp | 2 +- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 33063b8888..b2ed628795 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -496,6 +496,14 @@ FixedwingPositionControl::check_eco_dash_allowed() } } +void +FixedwingPositionControl::resetAutoSpeedAdaptions() +{ + _conditions_for_eco_dash_met = false; + _time_conditions_not_met = _local_pos.timestamp; + _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; +} + void FixedwingPositionControl::update_wind_mode() { @@ -2048,21 +2056,25 @@ FixedwingPositionControl::Run() case FW_POSCTRL_MODE_AUTO_ALTITUDE: { control_auto_fixed_bank_alt_hold(_local_pos.timestamp); + resetAutoSpeedAdaptions(); break; } case FW_POSCTRL_MODE_AUTO_CLIMBRATE: { control_auto_descend(_local_pos.timestamp); + resetAutoSpeedAdaptions(); break; } case FW_POSCTRL_MODE_MANUAL_POSITION: { control_manual_position(_local_pos.timestamp, curr_pos, ground_speed); + resetAutoSpeedAdaptions(); break; } case FW_POSCTRL_MODE_MANUAL_ALTITUDE: { control_manual_altitude(_local_pos.timestamp, curr_pos, ground_speed); + resetAutoSpeedAdaptions(); break; } @@ -2073,6 +2085,8 @@ FixedwingPositionControl::Run() reset_takeoff_state(); } + resetAutoSpeedAdaptions(); + _att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get()); break; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index a836a3360a..949f4fc7d7 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -388,8 +388,8 @@ private: void check_eco_dash_allowed(); void updateSpeedMode(); - void update_wind_mode(); + void resetAutoSpeedAdaptions(); /* * Call TECS : a wrapper function to call the TECS implementation