fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

health-flags: Increase health flags to 64 bit bit field to support extended sys status mavlink message

Browse Source 
Add SYS_STATUS flag for parachute
This commit is contained in:
Thomas Debrunner 2021-11-23 18:07:31 +01:00 committed by Daniel Agar
parent 37099f85b9
commit eb69f15d3a
5 changed files with 33 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
msg/vehicle_status.msg
View File

@ -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

20
src/modules/commander/Arming/HealthFlags/HealthFlags.cpp
View File

@ -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" : " ",

1
src/modules/commander/Arming/HealthFlags/HealthFlags.h
View File

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

6
src/modules/commander/Commander.cpp
View File

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

16
src/modules/mavlink/streams/SYS_STATUS.hpp
View File

@ -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<uint32_t>(status.onboard_control_sensors_present & 0xFFFFFFFF) |
MAV_SYS_STATUS_EXTENSION_USED;
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;