From 3d1da597dd1e51b73aa2bd7e586d9735c6e36a12 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 9 Sep 2022 09:41:52 +0200 Subject: [PATCH] commander: run arming checks on _offboard_control_mode_sub update --- src/modules/commander/Commander.cpp | 13 ++++++++++++- src/modules/commander/Commander.hpp | 2 ++ 2 files changed, 14 insertions(+), 1 deletion(-) 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 *