From 6511866408855b6803161f5f1a702eaeb1453acd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 2 Nov 2022 08:50:09 +0100 Subject: [PATCH] fix commander: check that offboard_control_mode data is recent This is a regression from https://github.com/PX4/PX4-Autopilot/pull/20172 --- .../checks/offboardCheck.cpp | 16 +++++----------- .../checks/offboardCheck.hpp | 4 +--- 2 files changed, 6 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp index 1609da7af4..3a3e9e3d50 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp @@ -35,11 +35,6 @@ using namespace time_literals; -OffboardChecks::OffboardChecks() -{ - _offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s); -} - void OffboardChecks::checkAndReport(const Context &context, Report &reporter) { reporter.failsafeFlags().offboard_control_signal_lost = true; @@ -47,9 +42,10 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter) offboard_control_mode_s offboard_control_mode; if (_offboard_control_mode_sub.copy(&offboard_control_mode)) { - bool offboard_available = offboard_control_mode.position || offboard_control_mode.velocity - || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate - || offboard_control_mode.actuator; + bool data_is_recent = hrt_absolute_time() < offboard_control_mode.timestamp + _param_com_of_loss_t.get() * 1_s; + bool offboard_available = (offboard_control_mode.position || offboard_control_mode.velocity + || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate + || offboard_control_mode.actuator) && data_is_recent; if (offboard_control_mode.position && reporter.failsafeFlags().local_position_invalid) { offboard_available = false; @@ -62,9 +58,7 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter) offboard_available = false; } - _offboard_available.set_state_and_update(offboard_available, hrt_absolute_time()); - // This is a mode requirement, no need to report - reporter.failsafeFlags().offboard_control_signal_lost = !_offboard_available.get_state(); + reporter.failsafeFlags().offboard_control_signal_lost = !offboard_available; } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp index 5d29bc75c2..6b15e24999 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp @@ -37,19 +37,17 @@ #include #include -#include class OffboardChecks : public HealthAndArmingCheckBase { public: - OffboardChecks(); + OffboardChecks() = default; ~OffboardChecks() = default; void checkAndReport(const Context &context, Report &reporter) override; private: uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; - systemlib::Hysteresis _offboard_available{false}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamFloat) _param_com_of_loss_t