AP_InertialSensor: added product_id support

fill in parameter from first backend

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -326,6 +326,9 @@ AP_InertialSensor::_detect_backends(Sample_rate sample_rate)
         _accel_count == 0) {
         hal.scheduler->panic(PSTR("No INS backends available"));
     }
+
+    // set the product ID to the ID of the first backend
+    _product_id.set(_backends[0]->product_id());
 }
 
 void

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -5,7 +5,8 @@
 #include "AP_InertialSensor_Backend.h"
 
 AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
-    _imu(imu)
+    _imu(imu),
+    _product_id(AP_PRODUCT_ID_NONE)
 {}
 
 /*

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -48,6 +48,11 @@ public:
      */
     virtual bool gyro_sample_available() = 0;
 
+    /*
+      return the product ID
+     */
+    int16_t product_id(void) const { return _product_id; }
+
 protected:
     // access to frontend
     AP_InertialSensor &_imu;
@@ -58,6 +63,9 @@ protected:
     // rotate accel vector, scale and offset
     void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now);
 
+    // backend should fill in its product ID from AP_PRODUCT_ID_*
+    int16_t _product_id;
+
     // note that each backend is also expected to have a static detect()
     // function which instantiates an instance of the backend sensor
     // driver if the sensor is available

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp

@@ -172,6 +172,8 @@ bool AP_InertialSensor_Flymaple::_init_sensor(AP_InertialSensor::Sample_rate sam
     _gyro_instance = _imu.register_gyro();
     _accel_instance = _imu.register_accel();
 
+    _product_id = AP_PRODUCT_ID_FLYMAPLE;
+
     return true;
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp

@@ -51,6 +51,8 @@ bool AP_InertialSensor_HIL::_init_sensor(AP_InertialSensor::Sample_rate sample_r
     _imu.register_gyro();
     _imu.register_accel();
 
+    _product_id = AP_PRODUCT_ID_NONE;
+
     return true;
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp

@@ -238,6 +238,8 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(AP_InertialSensor::Sample_rate sam
     _gyro_instance = _imu.register_gyro();
     _accel_instance = _imu.register_accel();
 
+    _product_id = AP_PRODUCT_ID_L3G4200D;
+
     return true;
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h

@@ -26,6 +26,9 @@ public:
     static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
                                              AP_InertialSensor::Sample_rate sample_rate);
 
+    // return product ID
+    int16_t product_id(void) const { return AP_PRODUCT_ID_L3G4200D; }
+
 private:
     bool            _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
     void            _accumulate(void);

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -525,13 +525,13 @@ bool AP_InertialSensor_MPU6000::_hardware_init(AP_InertialSensor::Sample_rate sa
     hal.scheduler->delay(1);
 
     // read the product ID rev c has 1/2 the sensitivity of rev d
-    uint8_t product_id = _register_read(MPUREG_PRODUCT_ID);
+    _product_id = _register_read(MPUREG_PRODUCT_ID);
     //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
 
-    if ((product_id == MPU6000ES_REV_C4) || 
-        (product_id == MPU6000ES_REV_C5) ||
-        (product_id == MPU6000_REV_C4)   || 
-        (product_id == MPU6000_REV_C5)) {
+    if ((_product_id == MPU6000ES_REV_C4) || 
+        (_product_id == MPU6000ES_REV_C5) ||
+        (_product_id == MPU6000_REV_C4)   || 
+        (_product_id == MPU6000_REV_C5)) {
         // Accel scale 8g (4096 LSB/g)
         // Rev C has different scaling than rev D
         _register_write(MPUREG_ACCEL_CONFIG,1<<3);

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp

@@ -250,6 +250,8 @@ bool AP_InertialSensor_MPU9250::_init_sensor(AP_InertialSensor::Sample_rate samp
     _gyro_instance = _imu.register_gyro();
     _accel_instance = _imu.register_accel();
 
+    _product_id = AP_PRODUCT_ID_MPU9250;
+
     // start the timer process to read samples
     hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9250::_poll_data));
 

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp

@@ -86,6 +86,8 @@ bool AP_InertialSensor_Oilpan::_init_sensor(AP_InertialSensor::Sample_rate sampl
     _gyro_instance = _imu.register_gyro();
     _accel_instance = _imu.register_accel();
 
+    _product_id = AP_PRODUCT_ID_APM1_2560;
+
     return true;
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -94,6 +94,12 @@ bool AP_InertialSensor_PX4::_init_sensor(AP_InertialSensor::Sample_rate sample_r
 
     _set_filter_frequency(_imu.get_filter());
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+    _product_id = AP_PRODUCT_ID_PX4_V2;
+#else
+    _product_id = AP_PRODUCT_ID_PX4;
+#endif
+
     return true;
 }