diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8e4d86e9b6..7c49f588b7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -344,7 +344,7 @@ int Sensors::parameters_update() // sensor_accel uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; - if (sensor_accel_sub.get().device_id != 0) { + if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) { calibration::Accelerometer cal; cal.set_calibration_index(i); cal.ParametersLoad(); @@ -353,7 +353,7 @@ int Sensors::parameters_update() // sensor_gyro uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; - if (sensor_gyro_sub.get().device_id != 0) { + if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) { calibration::Gyroscope cal; cal.set_calibration_index(i); cal.ParametersLoad(); @@ -362,7 +362,7 @@ int Sensors::parameters_update() // sensor_mag uORB::SubscriptionData sensor_mag_sub{ORB_ID(sensor_mag), i}; - if (sensor_mag_sub.get().device_id != 0) { + if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) { calibration::Magnetometer cal; cal.set_calibration_index(i); cal.ParametersLoad(); diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 7dd62009e4..d1baa48e3d 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -91,8 +91,10 @@ void VehicleAcceleration::CheckAndUpdateFilters() const float sample_rate_hz = imu_status.get().accel_rate_hz; - if ((imu_status.get().accel_device_id != 0) && (imu_status.get().accel_device_id == _calibration.device_id()) + if (imu_status.advertised() && (imu_status.get().timestamp != 0) + && (imu_status.get().accel_device_id != 0) && (imu_status.get().accel_device_id == _calibration.device_id()) && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) { + // check if sample rate error is greater than 1% if (!PX4_ISFINITE(_filter_sample_rate) || (fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz); @@ -161,7 +163,8 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force) const uint32_t device_id = sensor_accel_sub.get().device_id; - if ((device_id != 0) && (device_id == sensor_selection.accel_device_id)) { + if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().timestamp != 0) + && (device_id != 0) && (device_id == sensor_selection.accel_device_id)) { if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) { PX4_DEBUG("selected sensor changed %" PRIu32 " -> %" PRIu32 "", _calibration.device_id(), device_id); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 770d32f25b..3d8262fbf9 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -230,19 +230,20 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u // use vehicle_imu_status to do basic sensor selection validation for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { uORB::SubscriptionData imu_status{ORB_ID(vehicle_imu_status), i}; - bool imu_status_gyro_valid = false; - if ((imu_status.get().gyro_device_id != 0) && (time_now_us < imu_status.get().timestamp + 1_s)) { - imu_status_gyro_valid = true; - } + if (imu_status.advertised() + && (imu_status.get().timestamp != 0) && (time_now_us < imu_status.get().timestamp + 1_s) + && (imu_status.get().gyro_device_id != 0)) { + // vehicle_imu_status gyro valid - if ((device_id != 0) && (imu_status.get().gyro_device_id == device_id) && imu_status_gyro_valid) { - selected_device_id_valid = true; - } + if ((device_id != 0) && (imu_status.get().gyro_device_id == device_id)) { + selected_device_id_valid = true; + } - // record first valid IMU as a backup option - if ((device_id_first_valid_imu == 0) && imu_status_gyro_valid) { - device_id_first_valid_imu = imu_status.get().gyro_device_id; + // record first valid IMU as a backup option + if (device_id_first_valid_imu == 0) { + device_id_first_valid_imu = imu_status.get().gyro_device_id; + } } } @@ -259,7 +260,11 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { uORB::SubscriptionData sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i}; - if ((time_now_us < sensor_gyro_fifo_sub.get().timestamp + 1_s) && (sensor_gyro_fifo_sub.get().device_id != 0)) { + if (sensor_gyro_fifo_sub.advertised() + && (sensor_gyro_fifo_sub.get().timestamp != 0) + && (sensor_gyro_fifo_sub.get().device_id != 0) + && (time_now_us < sensor_gyro_fifo_sub.get().timestamp + 1_s)) { + // if no gyro was selected use the first valid sensor_gyro_fifo if (!device_id_valid) { device_id = sensor_gyro_fifo_sub.get().device_id; @@ -297,7 +302,11 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; - if ((time_now_us < sensor_gyro_sub.get().timestamp + 1_s) && (sensor_gyro_sub.get().device_id != 0)) { + if (sensor_gyro_sub.advertised() + && (sensor_gyro_sub.get().timestamp != 0) + && (sensor_gyro_sub.get().device_id != 0) + && (time_now_us < sensor_gyro_sub.get().timestamp + 1_s)) { + // if no gyro was selected use the first valid sensor_gyro if (!device_id_valid) { device_id = sensor_gyro_sub.get().device_id; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 934fb83c95..fd14defed3 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -87,7 +87,8 @@ void VotedSensorsUpdate::parametersUpdate() uORB::SubscriptionData imu{ORB_ID(vehicle_imu), uorb_index}; imu.update(); - if (imu.get().timestamp > 0 && imu.get().accel_device_id > 0 && imu.get().gyro_device_id > 0) { + if (imu.advertised() && (imu.get().timestamp != 0) + && (imu.get().accel_device_id != 0) && (imu.get().gyro_device_id != 0)) { // find corresponding configured accel priority int8_t accel_cal_index = calibration::FindCurrentCalibrationIndex("ACC", imu.get().accel_device_id);