diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 3684e50012..b8a535843a 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -246,9 +246,8 @@ bool PreFlightCheck::sensorAvailabilityCheck(const bool report_failure, const ui /* check all sensors, but fail only for mandatory ones */ for (uint8_t i = 0u; i < max_optional_count; i++) { const bool required = (i < max_mandatory_count) || isSensorRequired(i); - int32_t device_id = -1; - if (!sens_check(mavlink_log_pub, status, i, !required, device_id, report_fail)) { + if (!sens_check(mavlink_log_pub, status, i, !required, report_fail)) { if (required) { pass_check = false; } diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index 758520c289..4e80807c90 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -48,7 +48,7 @@ #include typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail); + const bool optional, const bool report_fail); typedef bool (*is_sens_req_func_t)(uint8_t instance); class PreFlightCheck @@ -105,20 +105,20 @@ private: vehicle_status_s &status, sens_check_func_t sens_check, is_sens_req_func_t isSensorRequired); static bool isMagRequired(uint8_t instance); static bool magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail); + const bool optional, const bool report_fail); static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status); static bool isAccelRequired(uint8_t instance); static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail); + const bool optional, const bool report_fail); static bool isGyroRequired(uint8_t instance); static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail); + const bool optional, const bool report_fail); static bool isBaroRequired(uint8_t instance); static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail); + const bool optional, const bool report_fail); static bool isDistSensRequired(uint8_t instance); static bool distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail); + const bool optional, const bool report_fail); static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status); static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional, const bool report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp index 7b2f3f2893..c15a537e0d 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp @@ -62,7 +62,7 @@ bool PreFlightCheck::isAccelRequired(const uint8_t instance) } bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail) + const bool optional, const bool report_fail) { const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK); bool calibration_valid = false; @@ -80,13 +80,11 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s } } - device_id = accel.get().device_id; - if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { calibration_valid = true; } else { - calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", device_id) >= 0); + calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel.get().device_id) >= 0); } if (!calibration_valid) { diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp index ff53c36530..ac9e37fdfe 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp @@ -60,7 +60,7 @@ bool PreFlightCheck::isBaroRequired(const uint8_t instance) } bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail) + const bool optional, const bool report_fail) { const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK); bool valid = false; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp index 87fb0f2497..71c1406675 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp @@ -48,7 +48,7 @@ bool PreFlightCheck::isDistSensRequired(const uint8_t instance) } bool PreFlightCheck::distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail) + const bool optional, const bool report_fail) { const bool exists = (orb_exists(ORB_ID(distance_sensor), instance) == PX4_OK); bool check_valid = false; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp index e6f559acdb..12e1242c0f 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp @@ -61,7 +61,7 @@ bool PreFlightCheck::isGyroRequired(const uint8_t instance) } bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail) + const bool optional, const bool report_fail) { const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK); bool calibration_valid = false; @@ -79,13 +79,11 @@ bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & } } - device_id = gyro.get().device_id; - if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { calibration_valid = true; } else { - calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", device_id) >= 0); + calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro.get().device_id) >= 0); } if (!calibration_valid) { diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp index e49444c3d3..21cc051eba 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp @@ -61,7 +61,7 @@ bool PreFlightCheck::isMagRequired(const uint8_t instance) } bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail) + const bool optional, const bool report_fail) { const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK); bool calibration_valid = false; @@ -80,13 +80,11 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st } } - device_id = magnetometer.get().device_id; - if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { calibration_valid = true; } else { - calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", device_id) >= 0); + calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", magnetometer.get().device_id) >= 0); } if (!calibration_valid) { @@ -98,7 +96,7 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { uORB::SubscriptionData estimator_status_sub{ORB_ID(estimator_status), i}; - if (estimator_status_sub.get().mag_device_id == static_cast(device_id)) { + if (estimator_status_sub.get().mag_device_id == static_cast(magnetometer.get().device_id)) { if (estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)) { is_mag_fault = true; break;