From e951123cffa380c771f376328eccab3f125f162f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 16 Mar 2020 00:30:55 +0100 Subject: [PATCH] Health Flags: Optimize flash usage Changing from a define to a function for reporting to safe space. --- .../Arming/HealthFlags/HealthFlags.cpp | 75 ++++++++++--------- .../Arming/HealthFlags/HealthFlags.h | 2 +- 2 files changed, 41 insertions(+), 36 deletions(-) diff --git a/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp b/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp index 53cbb95172..f9851cc1d0 100644 --- a/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp +++ b/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -80,42 +80,47 @@ void set_health_flags_healthy(uint64_t subsystem_type, bool healthy, vehicle_sta 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_sub(const char *name, const vehicle_status_s &status, uint32_t bit) +{ + PX4_INFO("%s:\t\t%s\t%s", name, + (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) +void print_health_flags(const 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); + _print_sub("GYRO", status, subsystem_info_s::SUBSYSTEM_TYPE_GYRO); + _print_sub("ACC", status, subsystem_info_s::SUBSYSTEM_TYPE_ACC); + _print_sub("MAG", status, subsystem_info_s::SUBSYSTEM_TYPE_MAG); + _print_sub("PRESS", status, subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE); + _print_sub("AIRSP", status, subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE); + _print_sub("GPS", status, subsystem_info_s::SUBSYSTEM_TYPE_GPS); + _print_sub("OPT", status, subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW); + _print_sub("VIO", status, subsystem_info_s::SUBSYSTEM_TYPE_CVPOSITION); + _print_sub("LASER", status, subsystem_info_s::SUBSYSTEM_TYPE_LASERPOSITION); + _print_sub("GTRUTH", status, subsystem_info_s::SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH); + _print_sub("RATES", status, subsystem_info_s::SUBSYSTEM_TYPE_ANGULARRATECONTROL); + _print_sub("ATT", status, subsystem_info_s::SUBSYSTEM_TYPE_ATTITUDESTABILIZATION); + _print_sub("YAW", status, subsystem_info_s::SUBSYSTEM_TYPE_YAWPOSITION); + _print_sub("ALTCTL", status, subsystem_info_s::SUBSYSTEM_TYPE_ALTITUDECONTROL); + _print_sub("POS", status, subsystem_info_s::SUBSYSTEM_TYPE_POSITIONCONTROL); + _print_sub("MOT", status, subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL); + _print_sub("RC ", status, subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER); + _print_sub("GYRO2", status, subsystem_info_s::SUBSYSTEM_TYPE_GYRO2); + _print_sub("ACC2", status, subsystem_info_s::SUBSYSTEM_TYPE_ACC2); + _print_sub("MAG2", status, subsystem_info_s::SUBSYSTEM_TYPE_MAG2); + _print_sub("GEOFENCE", status, subsystem_info_s::SUBSYSTEM_TYPE_GEOFENCE); + _print_sub("AHRS", status, subsystem_info_s::SUBSYSTEM_TYPE_AHRS); + _print_sub("TERRAIN", status, subsystem_info_s::SUBSYSTEM_TYPE_TERRAIN); + _print_sub("REVMOT", status, subsystem_info_s::SUBSYSTEM_TYPE_REVERSEMOTOR); + _print_sub("LOGGIN", status, subsystem_info_s::SUBSYSTEM_TYPE_LOGGING); + _print_sub("BATT", status, subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY); + _print_sub("PROX", status, subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY); + _print_sub("SATCOM", status, subsystem_info_s::SUBSYSTEM_TYPE_SATCOM); + _print_sub("PREARM", status, subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK); + _print_sub("OBSAVD", status, 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 d9a9cd5e5e..470702aa91 100644 --- a/src/modules/commander/Arming/HealthFlags/HealthFlags.h +++ b/src/modules/commander/Arming/HealthFlags/HealthFlags.h @@ -48,4 +48,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); +void print_health_flags(const vehicle_status_s &status);