From 25c537bae983d639fa0336f0845a2ad8c5092c2c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 13 Sep 2020 16:34:48 -0400 Subject: [PATCH] commander: don't impose additional timeouts in sensor checks - the sensors hub is responsible for sensor timeouts --- .../PreFlightCheck/checks/accelerometerCheck.cpp | 10 +++++----- .../Arming/PreFlightCheck/checks/baroCheck.cpp | 11 +++++------ .../Arming/PreFlightCheck/checks/gyroCheck.cpp | 12 ++++++------ .../PreFlightCheck/checks/magnetometerCheck.cpp | 8 ++++---- .../PreFlightCheck/checks/manualControlCheck.cpp | 2 +- .../Arming/PreFlightCheck/checks/powerCheck.cpp | 2 +- src/modules/commander/mag_calibration.cpp | 2 +- 7 files changed, 23 insertions(+), 24 deletions(-) diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp index dd2cd3d34b..c87d3ba0ee 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp @@ -50,15 +50,15 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s { const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK); bool calibration_valid = false; - bool accel_valid = true; + bool valid = true; if (exists) { uORB::SubscriptionData accel{ORB_ID(sensor_accel), instance}; - accel_valid = (hrt_elapsed_time(&accel.get().timestamp) < 1_s); + valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0); - if (!accel_valid) { + if (!valid) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel #%u", instance); } @@ -86,7 +86,7 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s } /* this is frickin' fatal */ - accel_valid = false; + valid = false; } } } @@ -97,7 +97,7 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s } } - const bool success = calibration_valid && accel_valid; + const bool success = calibration_valid && valid; if (instance == 0) { set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, exists, !optional, success, status); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp index 1b7542b531..e5e16dd69c 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp @@ -47,20 +47,19 @@ bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & const bool optional, int32_t &device_id, const bool report_fail) { const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK); - bool baro_valid = false; + bool valid = false; if (exists) { uORB::SubscriptionData baro{ORB_ID(sensor_baro), instance}; - baro_valid = (hrt_elapsed_time(&baro.get().timestamp) < 1_s); + valid = (baro.get().device_id != 0) && (baro.get().timestamp != 0); - if (!baro_valid) { + if (!valid) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro #%u", instance); } } - } else { if (!optional && report_fail) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro Sensor #%u missing", instance); @@ -68,8 +67,8 @@ bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & } if (instance == 0) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, !optional, baro_valid, status); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, !optional, valid, status); } - return baro_valid; + return valid; } diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp index 6c69b93f2b..fc8687206b 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp @@ -49,15 +49,15 @@ bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & { const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK); bool calibration_valid = false; - bool gyro_valid = false; + bool valid = false; if (exists) { uORB::SubscriptionData gyro{ORB_ID(sensor_gyro), instance}; - gyro_valid = (hrt_elapsed_time(&gyro.get().timestamp) < 1_s); + valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0); - if (!gyro_valid) { + if (!valid) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro #%u", instance); } @@ -80,11 +80,11 @@ bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & } if (instance == 0) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, exists, !optional, calibration_valid && gyro_valid, status); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, exists, !optional, calibration_valid && valid, status); } else if (instance == 1) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, exists, !optional, calibration_valid && gyro_valid, status); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, exists, !optional, calibration_valid && valid, status); } - return calibration_valid && gyro_valid; + return calibration_valid && valid; } diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp index cfaadfdccb..bb74ba5fbd 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp @@ -49,15 +49,15 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st { const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK); bool calibration_valid = false; - bool mag_valid = false; + bool valid = false; if (exists) { uORB::SubscriptionData magnetometer{ORB_ID(sensor_mag), instance}; - mag_valid = (hrt_elapsed_time(&magnetometer.get().timestamp) < 1_s); + valid = (magnetometer.get().device_id != 0) && (magnetometer.get().timestamp != 0); - if (!mag_valid) { + if (!valid) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass #%u", instance); } @@ -79,7 +79,7 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st } } - const bool success = calibration_valid && mag_valid; + const bool success = calibration_valid && valid; if (instance == 0) { set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp index b8b0e80c7f..babbfde815 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp @@ -47,7 +47,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo manual_control_setpoint_sub.update(); const manual_control_setpoint_s &manual_control_setpoint = manual_control_setpoint_sub.get(); - if (hrt_elapsed_time(&manual_control_setpoint.timestamp) < 1_s) { + if (manual_control_setpoint.timestamp != 0) { //check action switches if (manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp index 75f18b82a7..52d13e2541 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp @@ -67,7 +67,7 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta system_power_sub.update(); const system_power_s &system_power = system_power_sub.get(); - if (hrt_elapsed_time(&system_power.timestamp) < 1_s) { + if (system_power.timestamp != 0) { // Check avionics rail voltages (if USB isn't connected) if (!system_power.usb_connected) { float avionics_power_rail_voltage = system_power.voltage5v_v; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 4b98f4acd7..7cc13f63db 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -976,7 +976,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian sensor_mag_s mag{}; mag_sub.copy(&mag); - if (mag_sub.advertised() && (hrt_elapsed_time(&mag.timestamp) < 1_s)) { + if (mag_sub.advertised() && (mag.timestamp != 0)) { calibration::Magnetometer cal{mag.device_id, mag.is_external};