From bf3b55cac7aa9239eb8426f09d2d603be227683c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2020 10:50:35 +0100 Subject: [PATCH] Commander: Add function to print health flags consistently 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. --- .../Arming/HealthFlags/HealthFlags.cpp | 41 +++++++++++++++++++ .../Arming/HealthFlags/HealthFlags.h | 1 + src/modules/commander/Commander.cpp | 2 + 3 files changed, 44 insertions(+) diff --git a/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp b/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp index 892ee91257..53cbb95172 100644 --- a/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp +++ b/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp @@ -40,6 +40,7 @@ */ #include "HealthFlags.h" +#include 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); +} diff --git a/src/modules/commander/Arming/HealthFlags/HealthFlags.h b/src/modules/commander/Arming/HealthFlags/HealthFlags.h index 978e1ca7fe..a91e1564de 100644 --- a/src/modules/commander/Arming/HealthFlags/HealthFlags.h +++ b/src/modules/commander/Arming/HealthFlags/HealthFlags.h @@ -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); diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index ea70ce1ef7..a492ae122f 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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; }