Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: Check the gyro/accel id has not been previously registered

Browse Source 
If the Gyro/Accel ID is already in the registered list, do not try to add it again.

This stops an issue seen on a CubeOrangePlus BG3 where, during the very first boot after a parameter wipe, software incorrectly registers a fourth IMU.
The Fourth IMU is registered because the AUX IMU is the same DevID as the third ICM45686.
This commit is contained in:
James O'Shannessy 2024-07-17 17:33:14 +10:00 committed by Andrew Tridgell
parent d1b6eea4ee
commit 764dd273ae
1 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -734,6 +734,14 @@ bool AP_InertialSensor::register_gyro(uint8_t &instance, uint16_t raw_sample_rat
return false;
}
// Loop over the existing instances and check if the instance already exists
for (uint8_t instance_to_check = 0; instance_to_check < _gyro_count; instance_to_check++) {
if ((uint32_t)_gyro_id(instance_to_check) == id) {
// if it does, then bail
return false;
}
}
_gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz;
_gyro_over_sampling[_gyro_count] = 1;
_gyro_raw_sampling_multiplier[_gyro_count] = INT16_MAX/radians(2000);
@ -794,6 +802,14 @@ bool AP_InertialSensor::register_accel(uint8_t &instance, uint16_t raw_sample_ra
return false;
}
// Loop over the existing instances and check if the instance already exists
for (uint8_t instance_to_check = 0; instance_to_check < _accel_count; instance_to_check++) {
if ((uint32_t)_accel_id(instance_to_check) == id) {
// if it does, then bail
return false;
}
}
_accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz;
_accel_over_sampling[_accel_count] = 1;
_accel_raw_sampling_multiplier[_accel_count] = INT16_MAX/(16*GRAVITY_MSS);