From 469efb00f64e001b28dee9126b8967473cf34712 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Sat, 3 Sep 2016 21:51:37 -0300 Subject: [PATCH] AP_InertialSensor: save id for gyro and accel instances 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. --- .../AP_InertialSensor/AP_InertialSensor.cpp | 95 ++++++++++++++++++- .../AP_InertialSensor/AP_InertialSensor.h | 12 ++- .../AP_InertialSensor_BMI160.cpp | 6 +- .../AP_InertialSensor_HIL.cpp | 4 +- .../AP_InertialSensor_L3G4200D.cpp | 4 +- .../AP_InertialSensor_LSM9DS0.cpp | 4 +- .../AP_InertialSensor_MPU6000.cpp | 4 +- .../AP_InertialSensor_MPU9250.cpp | 4 +- .../AP_InertialSensor_PX4.cpp | 4 +- .../AP_InertialSensor_QURT.cpp | 6 +- .../AP_InertialSensor_SITL.cpp | 4 +- .../AP_InertialSensor_qflight.cpp | 4 +- 12 files changed, 125 insertions(+), 26 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 1f0b1e62c1..666499c440 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -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; iget_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, diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp index f178fddaa2..8b6477c7bf 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp @@ -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(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index e9bde0553a..4a1833ea7f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -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)); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index 347e2f4744..e01aad67a1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -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); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 94a39df5cb..96b7b31403 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -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); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 927424fd84..9535524636 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -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)); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index 61b886a72e..7a4fc704d6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -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; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp index f528c131b2..4783aab534 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp @@ -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(20); init_mpu9250(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp index f4c12610a1..0dc4555cb4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp @@ -36,8 +36,8 @@ bool AP_InertialSensor_SITL::init_sensor(void) // grab the used instances for (uint8_t i=0; iupdate_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)); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp index 138586a184..30d785aae5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp @@ -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;