dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

PreFlightCheck: add data timeout detection for mag, baro accel and gyro

This commit is contained in:
bresch 2022-03-10 17:58:17 +01:00 committed by Mathieu Bresciani
parent 75aa11c955
commit f3f09c1344
4 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp
View File

@ -77,7 +77,8 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s
if (exists) {
uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
is_valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0);
is_valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0)
&& (hrt_elapsed_time(&accel.get().timestamp) < 1_s);
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
is_calibration_valid = true;

2
src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp
View File

@ -73,7 +73,7 @@ bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &
if (exists) {
uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
valid = (baro.get().device_id != 0) && (baro.get().timestamp != 0);
valid = (baro.get().device_id != 0) && (baro.get().timestamp != 0) && (hrt_elapsed_time(&baro.get().timestamp) < 1_s);
}
if (instance == 0) {

3
src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp
View File

@ -75,7 +75,8 @@ bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &
if (exists) {
uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
is_valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0);
is_valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0)
&& (hrt_elapsed_time(&gyro.get().timestamp) < 1_s);
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
is_calibration_valid = true;

7
src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp
View File

@ -76,19 +76,20 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st
if (exists) {
uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
is_valid = (magnetometer.get().device_id != 0) && (magnetometer.get().timestamp != 0);
const sensor_mag_s &mag_data = magnetometer.get();
is_valid = (mag_data.device_id != 0) && (mag_data.timestamp != 0) && (hrt_elapsed_time(&mag_data.timestamp) < 1_s);
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
is_calibration_valid = true;
} else {
is_calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", magnetometer.get().device_id) >= 0);
is_calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id) >= 0);
}
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (estimator_status_sub.get().mag_device_id == static_cast<uint32_t>(magnetometer.get().device_id)) {
if (estimator_status_sub.get().mag_device_id == static_cast<uint32_t>(mag_data.device_id)) {
if (estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)) {
is_mag_fault = true;
break;