Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

sensors: check uORB::SubscriptionData validity before use

Browse Source
This commit is contained in:
Daniel Agar
2022-02-07 16:03:10 -05:00
parent 21b78f9d05
commit 86f81680fb
4 changed files with 31 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+3 -3
View File
@@ -344,7 +344,7 @@ int Sensors::parameters_update()
// sensor_accel
uORB::SubscriptionData<sensor_accel_s> 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_s> 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_s> 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();
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
+5 -2
View File
@@ -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);
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+21 -12
View File
@@ -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<vehicle_imu_status_s> 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_s> 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_s> 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;
src/modules/sensors/voted_sensors_update.cpp
+2 -1
View File
@@ -87,7 +87,8 @@ void VotedSensorsUpdate::parametersUpdate()
uORB::SubscriptionData<vehicle_imu_s> 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);