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

Subsystem_info status flags & checks: Code style fixes and cleanup to avoid strcmp() as suggested by @LorenzMeier

Browse Source
This commit is contained in:
Philipp Oettershagen 2018-05-29 01:44:01 +02:00 committed by Beat Küng
parent e4d863b95f
commit bd2af289f5
3 changed files with 21 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/commander/PreflightCheck.cpp
View File

@ -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;

32
src/modules/sensors/voted_sensors_update.cpp
View File

@ -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)

2
src/modules/sensors/voted_sensors_update.h
View File

@ -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.