From bd2af289f5d86234ee2d679694716fe895176bde Mon Sep 17 00:00:00 2001 From: Philipp Oettershagen Date: Tue, 29 May 2018 01:44:01 +0200 Subject: [PATCH] Subsystem_info status flags & checks: Code style fixes and cleanup to avoid strcmp() as suggested by @LorenzMeier --- src/modules/commander/PreflightCheck.cpp | 12 ++++---- src/modules/sensors/voted_sensors_update.cpp | 32 +++++++++----------- src/modules/sensors/voted_sensors_update.h | 2 +- 3 files changed, 21 insertions(+), 25 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 6a626d2864..e91c20c35a 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -163,8 +163,8 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &sta } out: - if(instance==0) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, present, !optional, success, status); - if(instance==1) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, present, !optional, success, status); + if (instance==0) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, present, !optional, success, status); + if (instance==1) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, present, !optional, success, status); DevMgr::releaseHandle(h); return success; @@ -336,8 +336,8 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & #endif out: - if(instance==0) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, present, !optional, success, status); - if(instance==1) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, present, !optional, success, status); + if (instance==0) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, present, !optional, success, status); + if (instance==1) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, present, !optional, success, status); DevMgr::releaseHandle(h); return success; @@ -388,8 +388,8 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, u } out: - if(instance==0) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, present, !optional, success, status); - if(instance==1) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, present, !optional, success, status); + if (instance==0) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, present, !optional, success, status); + if (instance==1) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, present, !optional, success, status); DevMgr::releaseHandle(h); return success; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 19991465d3..b241542ff9 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -893,7 +893,7 @@ void VotedSensorsUpdate::baro_poll(vehicle_air_data_s &airdata) } } -bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_name) +bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_name, const uint64_t type) { if (sensor.last_failover_count != sensor.voter.failover_count()) { @@ -931,22 +931,18 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n PX4_ERR("%s sensor switch from #%i", sensor_name, failover_index); - if (ctr_valid < 2) { // subsystem_info only contains flags for the first two sensors - if (ctr_valid == 0) { // There are no valid sensors left! - if (strcmp(sensor_name, "Gyro") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO; } + if (ctr_valid < 2) { + if (ctr_valid == 0) { + // Zero valid sensors remain! Set even the primary sensor health to false + _info.subsystem_type = type; - if (strcmp(sensor_name, "Accel") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC; } + } else if (ctr_valid == 1) { + // One valid sensor remains, set secondary sensor health to false + if (type == subsystem_info_s::SUBSYSTEM_TYPE_GYRO) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO2; } - if (strcmp(sensor_name, "Mag") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG; } + if (type == subsystem_info_s::SUBSYSTEM_TYPE_ACC) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; } - if (strcmp(sensor_name, "Baro") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE; } - - } else if (ctr_valid == 1) { // A single valid sensor remains, set secondary sensor health to false - if (strcmp(sensor_name, "Gyro") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO2; } - - if (strcmp(sensor_name, "Accel") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; } - - if (strcmp(sensor_name, "Mag") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG2; } + if (type == subsystem_info_s::SUBSYSTEM_TYPE_MAG) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG2; } } _info.present = true; @@ -1095,10 +1091,10 @@ void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw, vehicle_air_data_s void VotedSensorsUpdate::check_failover() { - check_failover(_accel, "Accel"); - check_failover(_gyro, "Gyro"); - check_failover(_mag, "Mag"); - check_failover(_baro, "Baro"); + check_failover(_accel, "Accel", subsystem_info_s::SUBSYSTEM_TYPE_ACC); + check_failover(_gyro, "Gyro", subsystem_info_s::SUBSYSTEM_TYPE_GYRO); + check_failover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG); + check_failover(_baro, "Baro", subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE); } void VotedSensorsUpdate::set_relative_timestamps(sensor_combined_s &raw) diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 2b0833196b..f905690506 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -206,7 +206,7 @@ private: * Check & handle failover of a sensor * @return true if a switch occured (could be for a non-critical reason) */ - bool check_failover(SensorData &sensor, const char *sensor_name); + bool check_failover(SensorData &sensor, const char *sensor_name, const uint64_t type); /** * Apply a gyro calibration.