diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 4ef6e2dae2..167fb60456 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1708,6 +1708,8 @@ void Commander::run() manualControlCheck(); + offboardControlCheck(); + // data link checks which update the status dataLinkCheck(); @@ -2407,7 +2409,6 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) void Commander::updateControlMode() { _vehicle_control_mode = {}; - _offboard_control_mode_sub.update(); mode_util::getVehicleControlMode(_arm_state_machine.isArmed(), _vehicle_status.nav_state, _vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode); _vehicle_control_mode.timestamp = hrt_absolute_time(); @@ -2782,6 +2783,16 @@ void Commander::manualControlCheck() } } +void Commander::offboardControlCheck() +{ + if (_offboard_control_mode_sub.update()) { + if (_vehicle_status_flags.offboard_control_signal_lost) { + // Run arming checks immediately to allow for offboard mode activation + _status_changed = true; + } + } +} + void Commander::send_parachute_command() { vehicle_command_s vcmd{}; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 1ffd6832c7..f82bf86579 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -133,6 +133,8 @@ private: void manualControlCheck(); + void offboardControlCheck(); + /** * @brief Handle incoming vehicle command relavant to Commander *