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.

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; 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));

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -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;

libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp

@@ -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,

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();
 

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));

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);

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);

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));

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;
     }
 

libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp

@@ -30,8 +30,8 @@ AP_InertialSensor_Backend *AP_InertialSensor_QURT::detect(AP_InertialSensor &_im
 
 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();

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; 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));

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;