From eb69f15d3ad65866541f271b3a335b30ab13bd06 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Tue, 23 Nov 2021 18:07:31 +0100 Subject: [PATCH] health-flags: Increase health flags to 64 bit bit field to support extended sys status mavlink message Add SYS_STATUS flag for parachute --- msg/vehicle_status.msg | 7 ++++--- .../Arming/HealthFlags/HealthFlags.cpp | 20 +++++++++---------- .../Arming/HealthFlags/HealthFlags.h | 1 + src/modules/commander/Commander.cpp | 6 +++++- src/modules/mavlink/streams/SYS_STATUS.hpp | 16 ++++++++++++--- 5 files changed, 33 insertions(+), 17 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 61b476aa7d..8c9d7e2d1d 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -92,9 +92,10 @@ bool geofence_violated uint8 failure_detector_status # Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL] # see SYS_STATUS mavlink message for the following -uint32 onboard_control_sensors_present -uint32 onboard_control_sensors_enabled -uint32 onboard_control_sensors_health +# lower 32 bits are for the base flags, higher 32 bits are or the extended flags +uint64 onboard_control_sensors_present +uint64 onboard_control_sensors_enabled +uint64 onboard_control_sensors_health uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0 diff --git a/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp b/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp index 4a43e292b4..449d966fed 100644 --- a/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp +++ b/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp @@ -47,40 +47,40 @@ void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool enabled, ok); if (present) { - status.onboard_control_sensors_present |= (uint32_t)subsystem_type; + status.onboard_control_sensors_present |= subsystem_type; } else { - status.onboard_control_sensors_present &= ~(uint32_t)subsystem_type; + status.onboard_control_sensors_present &= ~subsystem_type; } if (enabled) { - status.onboard_control_sensors_enabled |= (uint32_t)subsystem_type; + status.onboard_control_sensors_enabled |= subsystem_type; } else { - status.onboard_control_sensors_enabled &= ~(uint32_t)subsystem_type; + status.onboard_control_sensors_enabled &= ~subsystem_type; } if (ok) { - status.onboard_control_sensors_health |= (uint32_t)subsystem_type; + status.onboard_control_sensors_health |= subsystem_type; } else { - status.onboard_control_sensors_health &= ~(uint32_t)subsystem_type; + status.onboard_control_sensors_health &= ~subsystem_type; } } void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status) { - set_health_flags(subsystem_type, present, status.onboard_control_sensors_enabled & (uint32_t)subsystem_type, healthy, + set_health_flags(subsystem_type, present, status.onboard_control_sensors_enabled & subsystem_type, healthy, status); } void set_health_flags_healthy(uint64_t subsystem_type, bool healthy, vehicle_status_s &status) { - set_health_flags(subsystem_type, status.onboard_control_sensors_present & (uint32_t)subsystem_type, - status.onboard_control_sensors_enabled & (uint32_t)subsystem_type, healthy, status); + set_health_flags(subsystem_type, status.onboard_control_sensors_present & subsystem_type, + status.onboard_control_sensors_enabled & subsystem_type, healthy, status); } -void _print_sub(const char *name, const vehicle_status_s &status, uint32_t bit) +void _print_sub(const char *name, const vehicle_status_s &status, uint64_t bit) { PX4_INFO("%s:\t\t%s\t%s", name, (status.onboard_control_sensors_enabled & bit) ? "EN" : " ", diff --git a/src/modules/commander/Arming/HealthFlags/HealthFlags.h b/src/modules/commander/Arming/HealthFlags/HealthFlags.h index e1cd5247fe..04e5db1c38 100644 --- a/src/modules/commander/Arming/HealthFlags/HealthFlags.h +++ b/src/modules/commander/Arming/HealthFlags/HealthFlags.h @@ -76,6 +76,7 @@ struct subsystem_info_s { static constexpr uint64_t SUBSYSTEM_TYPE_SATCOM = 1 << 27; static constexpr uint64_t SUBSYSTEM_TYPE_PREARM_CHECK = 1 << 28; static constexpr uint64_t SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 1 << 29; + static constexpr uint64_t SUBSYSTEM_TYPE_PARACHUTE = 1ULL << 32; }; void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status); diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 38e61755ed..a176d23c4b 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3481,9 +3481,12 @@ void Commander::data_link_check() } } + bool healthy = telemetry.parachute_system_healthy; + _datalink_last_heartbeat_parachute_system = telemetry.timestamp; _status_flags.parachute_system_present = true; - _status_flags.parachute_system_healthy = telemetry.parachute_system_healthy; + _status_flags.parachute_system_healthy = healthy; + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, true, true, healthy, _status); } if (telemetry.heartbeat_component_obstacle_avoidance) { @@ -3534,6 +3537,7 @@ void Commander::data_link_check() _status_flags.parachute_system_healthy = false; _parachute_system_lost = true; _status_changed = true; + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, false, true, false, _status); } // AVOIDANCE SYSTEM state check (only if it is enabled) diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index 4fe5d8e046..5d81e6d859 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -91,9 +91,19 @@ private: mavlink_sys_status_t msg{}; - msg.onboard_control_sensors_present = status.onboard_control_sensors_present; - msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; - msg.onboard_control_sensors_health = status.onboard_control_sensors_health; + msg.onboard_control_sensors_present = static_cast(status.onboard_control_sensors_present & 0xFFFFFFFF) | + MAV_SYS_STATUS_EXTENSION_USED; + msg.onboard_control_sensors_enabled = static_cast(status.onboard_control_sensors_enabled & 0xFFFFFFFF) | + MAV_SYS_STATUS_EXTENSION_USED; + msg.onboard_control_sensors_health = static_cast(status.onboard_control_sensors_health & 0xFFFFFFFF) | + MAV_SYS_STATUS_EXTENSION_USED; + + msg.onboard_control_sensors_present_extended = static_cast((status.onboard_control_sensors_present >> 32u) & + 0xFFFFFFFF); + msg.onboard_control_sensors_enabled_extended = static_cast((status.onboard_control_sensors_enabled >> 32u) & + 0xFFFFFFFF); + msg.onboard_control_sensors_health_extended = static_cast((status.onboard_control_sensors_health >> 32u) & + 0xFFFFFFFF); msg.load = cpuload.load * 1000.0f;