fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 21:03:34 +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-12-14 09:41:12 -05:00
committed by Daniel Agar
parent 37099f85b9
commit eb69f15d3a
5 changed files with 33 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/HealthFlags/HealthFlags.cpp
+10 -10
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" : " ",
src/modules/commander/Arming/HealthFlags/HealthFlags.h
+1
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);
src/modules/commander/Commander.cpp
+5 -1
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)