Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: save id for gyro and accel instances

Browse Source 
This allows each sensor to be uniquely identified in the system by using
either the index inside the backend or for those that use the Device
interface, to use the bus type, location, and device id.

We leave 16-bit for each sensor to be able to change its own
identification in future, which allows them to be changed in an
incompatible manner forcing a re-calibration.
This commit is contained in:
Lucas De Marchi 2016-09-03 21:51:37 -03:00 committed by Andrew Tridgell
parent bbb9bfa95e
commit 469efb00f6
12 changed files with 125 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

95
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -376,6 +376,48 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
// @User: Advanced
AP_GROUPINFO("POS3", 29, AP_InertialSensor, _accel_pos[2], 0.0f),
// @Param: GYR_ID
// @DisplayName: Gyro ID
// @Description: Gyro sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("GYR_ID", 30, AP_InertialSensor, _gyro_id[0], 0),
// @Param: GYR2_ID
// @DisplayName: Gyro2 ID
// @Description: Gyro2 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("GYR2_ID", 31, AP_InertialSensor, _gyro_id[1], 0),
// @Param: GYR3_ID
// @DisplayName: Gyro3 ID
// @Description: Gyro3 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("GYR3_ID", 32, AP_InertialSensor, _gyro_id[2], 0),
// @Param: ACC_ID
// @DisplayName: Accelerometer ID
// @Description: Accelerometer sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("ACC_ID", 33, AP_InertialSensor, _accel_id[0], 0),
// @Param: ACC2_ID
// @DisplayName: Accelerometer2 ID
// @Description: Accelerometer2 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("ACC2_ID", 34, AP_InertialSensor, _accel_id[1], 0),
// @Param: ACC3_ID
// @DisplayName: Accelerometer3 ID
// @Description: Accelerometer3 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("ACC3_ID", 35, AP_InertialSensor, _accel_id[2], 0),
/*
NOTE: parameter indexes have gaps above. When adding new
parameters check for conflicts carefully
@ -457,24 +499,67 @@ AP_InertialSensor *AP_InertialSensor::get_instance()
/*
register a new gyro instance
*/
uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz)
uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz,
uint32_t id)
{
if (_gyro_count == INS_MAX_INSTANCES) {
AP_HAL::panic("Too many gyros");
}
_gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz;
bool saved = _gyro_id[_gyro_count].load();
if (saved && (uint32_t)_gyro_id[_gyro_count] != id) {
// inconsistent gyro id - mark it as needing calibration
_gyro_cal_ok[_gyro_count] = false;
}
_gyro_id[_gyro_count].set((int32_t) id);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (!saved) {
// assume this is the same sensor and save its ID to allow seamless
// transition from when we didn't have the IDs.
_gyro_id[_gyro_count].save();
}
#endif
return _gyro_count++;
}
/*
register a new accel instance
*/
uint8_t AP_InertialSensor::register_accel(uint16_t raw_sample_rate_hz)
uint8_t AP_InertialSensor::register_accel(uint16_t raw_sample_rate_hz,
uint32_t id)
{
if (_accel_count == INS_MAX_INSTANCES) {
AP_HAL::panic("Too many accels");
}
_accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz;
bool saved = _accel_id[_accel_count].load();
if (!saved) {
// inconsistent accel id
_accel_id_ok[_accel_count] = false;
} else if ((uint32_t)_accel_id[_accel_count] != id) {
// inconsistent accel id
_accel_id_ok[_accel_count] = false;
} else {
_accel_id_ok[_accel_count] = true;
}
_accel_id[_accel_count].set((int32_t) id);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_SITL
// assume this is the same sensor and save its ID to allow seamless
// transition from when we didn't have the IDs.
_accel_id_ok[_accel_count] = true;
_accel_id[_accel_count].save();
#endif
return _accel_count++;
}
@ -805,6 +890,9 @@ bool AP_InertialSensor::accel_calibrated_ok_all() const
// check each accelerometer has offsets saved
for (uint8_t i=0; i<get_accel_count(); i++) {
if (!_accel_id_ok[i]) {
return false;
}
// exactly 0.0 offset is extremely unlikely
if (_accel_offset[i].get().is_zero()) {
return false;
@ -990,6 +1078,7 @@ void AP_InertialSensor::_save_gyro_calibration()
{
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_gyro_offset[i].save();
_gyro_id[i].save();
}
}
@ -1443,6 +1532,8 @@ void AP_InertialSensor::_acal_save_calibrations()
_accel_calibrator[i].get_calibration(bias, gain);
_accel_offset[i].set_and_save(bias);
_accel_scale[i].set_and_save(gain);
_accel_id[i].save();
_accel_id_ok[i] = true;
} else {
_accel_offset[i].set_and_save(Vector3f(0,0,0));
_accel_scale[i].set_and_save(Vector3f(0,0,0));

12
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -66,8 +66,8 @@ public:
/// Register a new gyro/accel driver, allocating an instance
/// number
uint8_t register_gyro(uint16_t raw_sample_rate_hz);
uint8_t register_accel(uint16_t raw_sample_rate_hz);
uint8_t register_gyro(uint16_t raw_sample_rate_hz, uint32_t id);
uint8_t register_accel(uint16_t raw_sample_rate_hz, uint32_t id);
bool calibrate_trim(float &trim_roll, float &trim_pitch);
@ -321,6 +321,11 @@ private:
// product id
AP_Int16 _old_product_id;
// IDs to uniquely identify each sensor: shall remain
// the same across reboots
AP_Int32 _accel_id[INS_MAX_INSTANCES];
AP_Int32 _gyro_id[INS_MAX_INSTANCES];
// accelerometer scaling and offsets
AP_Vector3f _accel_scale[INS_MAX_INSTANCES];
AP_Vector3f _accel_offset[INS_MAX_INSTANCES];
@ -354,8 +359,9 @@ private:
enum Rotation _gyro_orientation[INS_MAX_INSTANCES];
enum Rotation _accel_orientation[INS_MAX_INSTANCES];
// calibrated_ok flags
// calibrated_ok/id_ok flags
bool _gyro_cal_ok[INS_MAX_INSTANCES];
bool _accel_id_ok[INS_MAX_INSTANCES];
// primary accel and gyro
uint8_t _primary_gyro;

6
libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
View File

@ -177,8 +177,10 @@ void AP_InertialSensor_BMI160::start()
_dev->get_semaphore()->give();
_accel_instance = _imu.register_accel(BMI160_ODR_TO_HZ(BMI160_ODR));
_gyro_instance = _imu.register_gyro(BMI160_ODR_TO_HZ(BMI160_ODR));
_accel_instance = _imu.register_accel(BMI160_ODR_TO_HZ(BMI160_ODR),
_dev->get_id());
_gyro_instance = _imu.register_gyro(BMI160_ODR_TO_HZ(BMI160_ODR),
_dev->get_id());
/* Call _poll_data() at 1kHz */
_dev->register_periodic_callback(1000,

4
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
View File

@ -27,8 +27,8 @@ AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu
bool AP_InertialSensor_HIL::_init_sensor(void)
{
// grab the used instances
_imu.register_gyro(1200);
_imu.register_accel(1200);
_imu.register_gyro(1200, 1);
_imu.register_accel(1200, 1);
_imu.set_hil_mode();

4
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -206,8 +206,8 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
*/
void AP_InertialSensor_L3G4200D::start(void)
{
_gyro_instance = _imu.register_gyro(800);
_accel_instance = _imu.register_accel(800);
_gyro_instance = _imu.register_gyro(800, _dev->get_id());
_accel_instance = _imu.register_accel(800, _dev->get_id());
// start the timer process to read samples
_dev->register_periodic_callback(1250, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, bool));

4
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -505,8 +505,8 @@ fail_whoami:
*/
void AP_InertialSensor_LSM9DS0::start(void)
{
_gyro_instance = _imu.register_gyro(760);
_accel_instance = _imu.register_accel(800);
_gyro_instance = _imu.register_gyro(760, _dev_gyro->get_id());
_accel_instance = _imu.register_accel(800, _dev_accel->get_id());
set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(_accel_instance, _rotation);

4
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -382,8 +382,8 @@ void AP_InertialSensor_MPU6000::start()
_dev->get_semaphore()->give();
// grab the used instances
_gyro_instance = _imu.register_gyro(1000);
_accel_instance = _imu.register_accel(1000);
_gyro_instance = _imu.register_gyro(1000, _dev->get_id());
_accel_instance = _imu.register_accel(1000, _dev->get_id());
// setup sensor rotations from probe()
set_gyro_orientation(_gyro_instance, _rotation);

4
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -320,8 +320,8 @@ void AP_InertialSensor_MPU9250::start()
_dev->get_semaphore()->give();
// grab the used instances
_gyro_instance = _imu.register_gyro(DEFAULT_SAMPLE_RATE);
_accel_instance = _imu.register_accel(DEFAULT_SAMPLE_RATE);
_gyro_instance = _imu.register_gyro(DEFAULT_SAMPLE_RATE, _dev->get_id());
_accel_instance = _imu.register_accel(DEFAULT_SAMPLE_RATE, _dev->get_id());
// start the timer process to read samples
_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_read_sample, bool));

4
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -112,7 +112,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
if (samplerate < 100 || samplerate > 10000) {
AP_HAL::panic("Invalid gyro sample rate");
}
_gyro_instance[i] = _imu.register_gyro(samplerate);
_gyro_instance[i] = _imu.register_gyro(samplerate, i);
_gyro_sample_time[i] = 1.0f / samplerate;
}
@ -152,7 +152,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
if (samplerate < 100 || samplerate > 10000) {
AP_HAL::panic("Invalid accel sample rate");
}
_accel_instance[i] = _imu.register_accel(samplerate);
_accel_instance[i] = _imu.register_accel(samplerate, i);
_accel_sample_time[i] = 1.0f / samplerate;
}

6
libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp
View File

@ -28,10 +28,10 @@ AP_InertialSensor_Backend *AP_InertialSensor_QURT::detect(AP_InertialSensor &_im
return sensor;
}
bool AP_InertialSensor_QURT::init_sensor(void)
bool AP_InertialSensor_QURT::init_sensor(void)
{
gyro_instance = _imu.register_gyro(1000);
accel_instance = _imu.register_accel(1000);
gyro_instance = _imu.register_gyro(1000, 1);
accel_instance = _imu.register_accel(1000, 1);
mpu9250_mag_buffer = new ObjectBuffer<mpu9x50_data>(20);
init_mpu9250();

4
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -36,8 +36,8 @@ bool AP_InertialSensor_SITL::init_sensor(void)
// grab the used instances
for (uint8_t i=0; i<INS_SITL_INSTANCES; i++) {
gyro_instance[i] = _imu.register_gyro(sitl->update_rate_hz);
accel_instance[i] = _imu.register_accel(sitl->update_rate_hz);
gyro_instance[i] = _imu.register_gyro(sitl->update_rate_hz, i);
accel_instance[i] = _imu.register_accel(sitl->update_rate_hz, i);
}
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void));

4
libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp
View File

@ -31,8 +31,8 @@ AP_InertialSensor_Backend *AP_InertialSensor_QFLIGHT::detect(AP_InertialSensor &
bool AP_InertialSensor_QFLIGHT::init_sensor(void)
{
gyro_instance = _imu.register_gyro(1000);
accel_instance = _imu.register_accel(1000);
gyro_instance = _imu.register_gyro(1000, 1);
accel_instance = _imu.register_accel(1000, 1);
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_QFLIGHT::timer_update, void));
return true;