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

Commander: Add function to print health flags consistently

Browse Source 
This enables us to have better situational awareness during development and when inspecting a system in the field as to which subsystem is currently faulty. These flags are from standard MAVLink and are not sufficient nor do they match well the actual critical path to a safe flight. This will be addressed in a second step with the addition of a new MAVLink message and new flags.
This commit is contained in:
Lorenz Meier
2020-03-17 00:17:11 +01:00
parent 9d5aebd235
commit bf3b55cac7
3 changed files with 44 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/HealthFlags/HealthFlags.cpp
+41
View File
@@ -40,6 +40,7 @@
*/
#include "HealthFlags.h"
#include <uORB/topics/subsystem_info.h>
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
{
@@ -78,3 +79,43 @@ void set_health_flags_healthy(uint64_t subsystem_type, bool healthy, vehicle_sta
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);
}
#define _print_sub(name, bit) PX4_INFO(name":\t\t%s\t%s", \
(status.onboard_control_sensors_enabled & bit) ? "EN" : " ", \
(status.onboard_control_sensors_present & bit) ? ((status.onboard_control_sensors_health & bit) ? "OK" : "ERR") : (status.onboard_control_sensors_enabled & bit) ? "OFF" : "")
void print_health_flags(vehicle_status_s &status)
{
PX4_INFO("DEVICE\t\tSTATUS");
PX4_INFO("----------------------------------");
_print_sub("GYRO", subsystem_info_s::SUBSYSTEM_TYPE_GYRO);
_print_sub("ACC", subsystem_info_s::SUBSYSTEM_TYPE_ACC);
_print_sub("MAG", subsystem_info_s::SUBSYSTEM_TYPE_MAG);
_print_sub("PRESS", subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE);
_print_sub("AIRSP", subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE);
_print_sub("GPS", subsystem_info_s::SUBSYSTEM_TYPE_GPS);
_print_sub("OPT", subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW);
_print_sub("VIO", subsystem_info_s::SUBSYSTEM_TYPE_CVPOSITION);
_print_sub("LASER", subsystem_info_s::SUBSYSTEM_TYPE_LASERPOSITION);
_print_sub("GTRUTH", subsystem_info_s::SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH);
_print_sub("RATES", subsystem_info_s::SUBSYSTEM_TYPE_ANGULARRATECONTROL);
_print_sub("ATT", subsystem_info_s::SUBSYSTEM_TYPE_ATTITUDESTABILIZATION);
_print_sub("YAW", subsystem_info_s::SUBSYSTEM_TYPE_YAWPOSITION);
_print_sub("ALTCTL", subsystem_info_s::SUBSYSTEM_TYPE_ALTITUDECONTROL);
_print_sub("POS", subsystem_info_s::SUBSYSTEM_TYPE_POSITIONCONTROL);
_print_sub("MOT", subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL);
_print_sub("RC ", subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER);
_print_sub("GYRO2", subsystem_info_s::SUBSYSTEM_TYPE_GYRO2);
_print_sub("ACC2", subsystem_info_s::SUBSYSTEM_TYPE_ACC2);
_print_sub("MAG2", subsystem_info_s::SUBSYSTEM_TYPE_MAG2);
_print_sub("GEOFENCE", subsystem_info_s::SUBSYSTEM_TYPE_GEOFENCE);
_print_sub("AHRS", subsystem_info_s::SUBSYSTEM_TYPE_AHRS);
_print_sub("TERRAIN", subsystem_info_s::SUBSYSTEM_TYPE_TERRAIN);
_print_sub("REVMOT", subsystem_info_s::SUBSYSTEM_TYPE_REVERSEMOTOR);
_print_sub("LOGGIN", subsystem_info_s::SUBSYSTEM_TYPE_LOGGING);
_print_sub("BATT", subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY);
_print_sub("PROX", subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY);
_print_sub("SATCOM", subsystem_info_s::SUBSYSTEM_TYPE_SATCOM);
_print_sub("PREARM", subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK);
_print_sub("OBSAVD", subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE);
}
src/modules/commander/Arming/HealthFlags/HealthFlags.h
+1
View File
@@ -47,3 +47,4 @@
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status);
void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status);
void set_health_flags_healthy(uint64_t subsystem_type, bool healthy, vehicle_status_s &status);
void print_health_flags(vehicle_status_s &status);
src/modules/commander/Commander.cpp
+2
View File
@@ -292,6 +292,8 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
PreFlightCheck::arm_requirements_t{}, status);
PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED");
print_health_flags(status);
return 0;
}