From ab15ba870177d968b26fa6baee05f93e1e74621d Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Wed, 23 Apr 2025 14:28:14 +0300 Subject: [PATCH] FixedWingModeManager: make sure configuration handler publishes after an external mode has been running. Also don't publish if we are not controlling anything, e.g. in rotary wing mode Signed-off-by: RomanBapst --- .../ControllerConfigurationHandler.cpp | 5 +++++ .../ControllerConfigurationHandler.hpp | 2 ++ .../fw_mode_manager/FixedWingModeManager.cpp | 15 ++++++++++++--- 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_mode_manager/ControllerConfigurationHandler.cpp b/src/modules/fw_mode_manager/ControllerConfigurationHandler.cpp index 6f28e89a13..fcfabb1679 100644 --- a/src/modules/fw_mode_manager/ControllerConfigurationHandler.cpp +++ b/src/modules/fw_mode_manager/ControllerConfigurationHandler.cpp @@ -132,3 +132,8 @@ void CombinedControllerConfigurationHandler::setEnforceLowHeightCondition(bool l { _longitudinal_configuration_current_cycle.enforce_low_height_condition = low_height_condition; } + +void CombinedControllerConfigurationHandler::resetLastPublishTime() +{ + _time_last_longitudinal_publish = _time_last_lateral_publish = 0; +} diff --git a/src/modules/fw_mode_manager/ControllerConfigurationHandler.hpp b/src/modules/fw_mode_manager/ControllerConfigurationHandler.hpp index 743b2feb5f..5d7abe5d62 100644 --- a/src/modules/fw_mode_manager/ControllerConfigurationHandler.hpp +++ b/src/modules/fw_mode_manager/ControllerConfigurationHandler.hpp @@ -75,6 +75,8 @@ public: void setLateralAccelMax(float lateral_accel_max); + void resetLastPublishTime(); + private: bool booleanValueChanged(bool new_value, bool current_value) { diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index fc34d14187..85a5b7f104 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -1840,8 +1840,15 @@ FixedWingModeManager::Run() /* only run controller if position changed and we are not running an external mode*/ - if (!((_nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1) - && (_nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8)) && _local_pos_sub.update(&_local_pos)) { + const bool is_external_nav_state = (_nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1) + && (_nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8); + + if (is_external_nav_state) { + // this will cause the configuration handler to publish immediately the next time an internal flight + // mode is active + _ctrl_configuration_handler.resetLastPublishTime(); + + } else if (_local_pos_sub.update(&_local_pos)) { const hrt_abstime now = _local_pos.timestamp; @@ -2095,7 +2102,9 @@ FixedWingModeManager::Run() } } - _ctrl_configuration_handler.update(now); + if (_control_mode_current != FW_POSCTRL_MODE_OTHER) { + _ctrl_configuration_handler.update(now); + } // only publish status in full FW mode if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING