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

@@ -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
+# lower 32 bits are for the base flags, higher 32 bits are or the extended flags
-uint32 onboard_control_sensors_enabled
+uint64 onboard_control_sensors_present
-uint32 onboard_control_sensors_health
+uint64 onboard_control_sensors_enabled
+uint64 onboard_control_sensors_health
 
 
 uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0

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,
+	set_health_flags(subsystem_type, status.onboard_control_sensors_present & subsystem_type,
-			 status.onboard_control_sensors_enabled & (uint32_t)subsystem_type, healthy, status);
+			 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" : " ",

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);

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)

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_present = static_cast<uint32_t>(status.onboard_control_sensors_present & 0xFFFFFFFF) |
-			msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
+							      MAV_SYS_STATUS_EXTENSION_USED;
-			msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
+			msg.onboard_control_sensors_enabled = static_cast<uint32_t>(status.onboard_control_sensors_enabled & 0xFFFFFFFF) |
+							      MAV_SYS_STATUS_EXTENSION_USED;
+			msg.onboard_control_sensors_health = static_cast<uint32_t>(status.onboard_control_sensors_health & 0xFFFFFFFF) |
+							     MAV_SYS_STATUS_EXTENSION_USED;
+
+			msg.onboard_control_sensors_present_extended = static_cast<uint32_t>((status.onboard_control_sensors_present >> 32u) &
+					0xFFFFFFFF);
+			msg.onboard_control_sensors_enabled_extended = static_cast<uint32_t>((status.onboard_control_sensors_enabled >> 32u) &
+					0xFFFFFFFF);
+			msg.onboard_control_sensors_health_extended = static_cast<uint32_t>((status.onboard_control_sensors_health >> 32u) &
+					0xFFFFFFFF);
 
 			msg.load = cpuload.load * 1000.0f;