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