Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 01:04:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

Sensors: Reset gyro and accel calibration if one sensor goes missing.

Browse Source 
This is to ensure that if sensor IDs are fixed we do not end up in a state where the system is partially calibrated and the need for re-calibration is not properly communicated to the user.
This commit is contained in:
Lorenz Meier
2017-01-29 12:04:55 +01:00
parent d0d1a8e04f
commit 9cacdaafc7
1 changed files with 36 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/voted_sensors_update.cpp
+36
View File
@@ -196,6 +196,8 @@ void VotedSensorsUpdate::parameters_update()
unsigned mag_count = 0;
unsigned gyro_count = 0;
unsigned accel_count = 0;
unsigned gyro_cal_found_count = 0;
unsigned accel_cal_found_count = 0;
/* run through all gyro sensors */
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
@@ -226,6 +228,10 @@ void VotedSensorsUpdate::parameters_update()
continue;
}
if (s == 0 && device_id > 0) {
gyro_cal_found_count++;
}
/* if the calibration is for this device, apply it */
if (device_id == driver_device_id) {
struct gyro_calibration_s gscale = {};
@@ -263,6 +269,19 @@ void VotedSensorsUpdate::parameters_update()
}
}
// There are less gyros than calibrations
// reset the board calibration and fail the initial load
if (gyro_count < gyro_cal_found_count) {
// run through all stored calibrations and reset them
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
int device_id = 0;
(void)sprintf(str, "CAL_GYRO%u_ID", i);
(void)param_set(param_find(str), &device_id);
}
}
/* run through all accel sensors */
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
@@ -292,6 +311,10 @@ void VotedSensorsUpdate::parameters_update()
continue;
}
if (s == 0 && device_id > 0) {
accel_cal_found_count++;
}
/* if the calibration is for this device, apply it */
if (device_id == driver_device_id) {
struct accel_calibration_s ascale = {};
@@ -329,6 +352,19 @@ void VotedSensorsUpdate::parameters_update()
}
}
// There are less accels than calibrations
// reset the board calibration and fail the initial load
if (accel_count < accel_cal_found_count) {
// run through all stored calibrations and reset them
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
int device_id = 0;
(void)sprintf(str, "CAL_ACC%u_ID", i);
(void)param_set(param_find(str), &device_id);
}
}
/* run through all mag sensors */
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {