From faabe2d4319c71b0a04d86e7d6d63f30a85a3257 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 4 Jun 2021 15:34:08 +0200 Subject: [PATCH] mavlink: make sure heartbeat reports flight termination --- src/modules/mavlink/streams/HEARTBEAT.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/modules/mavlink/streams/HEARTBEAT.hpp index 84bb4b71b2..d0851e9ed5 100644 --- a/src/modules/mavlink/streams/HEARTBEAT.hpp +++ b/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -59,6 +59,7 @@ public: private: explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink) {} + uORB::Subscription _acturator_armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; @@ -76,6 +77,8 @@ private: vehicle_control_mode_s vehicle_control_mode{}; _vehicle_control_mode_sub.copy(&vehicle_control_mode); + actuator_armed_s actuator_armed{}; + _acturator_armed_sub.copy(&actuator_armed); // uint8_t base_mode (MAV_MODE_FLAG) - System mode bitmap. uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; @@ -123,7 +126,8 @@ private: } // system_status overrides - if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) { + if (actuator_armed.force_failsafe || actuator_armed.lockdown || actuator_armed.manual_lockdown + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) { system_status = MAV_STATE_FLIGHT_TERMINATION; } else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) {