fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-14 13:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

lib/sensor_calibration: handle calibration slot change on parameter update

Browse Source
This commit is contained in:
Daniel Agar
2022-01-10 11:03:11 -05:00
parent ab547bb982
commit cc1fee525a
7 changed files with 44 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+14 -8
View File
@@ -340,26 +340,32 @@ int Sensors::parameters_update()
// ensure calibration slots are active for the number of sensors currently available
// this to done to eliminate differences in the active set of parameters before and after sensor calibration
for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
// sensor_accel
uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
uORB::SubscriptionData<sensor_mag_s> sensor_mag_sub{ORB_ID(sensor_mag), i};
if (sensor_accel_sub.get().device_id != 0) {
calibration::Accelerometer cal{sensor_accel_sub.get().device_id};
calibration::Accelerometer cal;
cal.set_calibration_index(i);
cal.ParametersUpdate();
cal.ParametersLoad();
}
// sensor_gyro
uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
if (sensor_gyro_sub.get().device_id != 0) {
calibration::Gyroscope cal{sensor_gyro_sub.get().device_id};
calibration::Gyroscope cal;
cal.set_calibration_index(i);
cal.ParametersUpdate();
cal.ParametersLoad();
}
// sensor_mag
uORB::SubscriptionData<sensor_mag_s> sensor_mag_sub{ORB_ID(sensor_mag), i};
if (sensor_mag_sub.get().device_id != 0) {
calibration::Magnetometer cal{sensor_mag_sub.get().device_id};
calibration::Magnetometer cal;
cal.set_calibration_index(i);
cal.ParametersUpdate();
cal.ParametersLoad();
}
}