IMU: expose IMU_PRODUCT_ID as EEPROM Variable

this will allow us to log the Product ID in tlogs

libraries/AP_Common/AP_Common.h

@@ -240,7 +240,16 @@ struct Location {
 #define AP_PRODUCT_ID_APM1_1280 	0x01 	// APM1 with 1280 CPUs
 #define AP_PRODUCT_ID_APM1_2560 	0x02 	// APM1 with 2560 CPUs
 #define AP_PRODUCT_ID_SITL		 	0x03 	// Software in the loop
-#define AP_PRODUCT_ID_APM2_REVC     0x14 	// APM2 with REV C MPU 6000
+#define AP_PRODUCT_ID_APM2_REVC4 	0x14 	// APM2 with MPU6000ES_REV_C4
-#define AP_PRODUCT_ID_APM2_REVD     0x58 	// APM2 with REV D MPU 6000
+#define AP_PRODUCT_ID_APM2_REV_C5	0x15 	// APM2 with MPU6000ES_REV_C5
+#define AP_PRODUCT_ID_APM2_REV_D6	0x16	// APM2 with MPU6000ES_REV_D6
+#define AP_PRODUCT_ID_APM2_REV_D7	0x17	// APM2 with MPU6000ES_REV_D7
+#define AP_PRODUCT_ID_APM2_REV_D8	0x18	// APM2 with MPU6000ES_REV_D8	
+#define AP_PRODUCT_ID_APM2_REV_C4	0x54	// APM2 with MPU6000_REV_C4 	
+#define AP_PRODUCT_ID_APM2_REV_C5	0x55	// APM2 with MPU6000_REV_C5 	
+#define AP_PRODUCT_ID_APM2_REV_D6	0x56	// APM2 with MPU6000_REV_D6 		
+#define AP_PRODUCT_ID_APM2_REV_D7	0x57	// APM2 with MPU6000_REV_D7 	
+#define AP_PRODUCT_ID_APM2_REV_D8	0x58	// APM2 with MPU6000_REV_D8 	
+#define AP_PRODUCT_ID_APM2_REV_D9	0x59	// APM2 with MPU6000_REV_D9 	
 
 #endif // _AP_COMMON_H

libraries/AP_IMU/AP_IMU_INS.cpp

@@ -27,7 +27,7 @@ AP_IMU_INS::init( Start_style style,
                   void (*flash_leds_cb)(bool on),
                   AP_PeriodicProcess * scheduler )
 {
-    _ins->init(scheduler);
+    _product_id = _ins->init(scheduler);
     // if we are warm-starting, load the calibration data from EEPROM and go
     //
     if (WARM_START == style) {

libraries/AP_IMU/AP_IMU_INS.h

@@ -28,7 +28,9 @@ public:
     ///
 	AP_IMU_INS(AP_InertialSensor *ins) :
         _ins(ins)
-	{}
+	{
+			_product_id = AP_PRODUCT_ID_NONE; // set during hardware init
+	}
 
 	/// Do warm or cold start.
 	///
@@ -66,6 +68,7 @@ public:
 	virtual void		az(const float v)		{ _sensor_cal[5] = v; }
     virtual float       get_gyro_drift_rate(void);
 	
+
 private:
     AP_InertialSensor   *_ins;          ///< INS provides an axis and unit correct sensor source.
 

libraries/AP_IMU/AP_IMU_Shim.h

@@ -11,7 +11,10 @@
 class AP_IMU_Shim : public IMU
 {
 public:
-	AP_IMU_Shim(void) {}
+	AP_IMU_Shim(void) {
+		_product_id = AP_PRODUCT_ID_NONE;
+	}
+	
 
 	/// @name IMU protocol
 	//@{
@@ -69,6 +72,7 @@ public:
 
 	float get_gyro_drift_rate(void) { return 0; }
 
+	
 private:
 	/// set true when new data is delivered
 	bool		_updated;

libraries/AP_IMU/IMU.cpp

@@ -4,6 +4,7 @@
 // this allows the sensor calibration to be saved to EEPROM
 const AP_Param::GroupInfo IMU::var_info[] PROGMEM = {
     AP_GROUPINFO("CAL", 0, IMU, _sensor_cal),
+	AP_GROUPINFO("PRODUCT_ID", 1, IMU, _product_id),
     AP_GROUPEND
 };
 

libraries/AP_IMU/IMU.h

@@ -120,6 +120,8 @@ protected:
 	/// number of microseconds that the accel and gyro values
 	/// were sampled over
 	uint32_t       _sample_time;
+	
+	AP_Int16 	   _product_id; // this is the product id returned from the INS init
 };
 
 #endif

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -40,7 +40,7 @@
 #define MPUREG_FIFO_COUNTH 0x72
 #define MPUREG_FIFO_COUNTL 0x73
 #define MPUREG_FIFO_R_W 0x74
-#define MPUREG_PRODUCT_ID 0x0C // product ID REV C = 0x14 REV D = 0x58
+#define MPUREG_PRODUCT_ID 			0x0C	// Product ID Register
 
 
 // Configuration bits MPU 3000 and MPU 6000 (not revised)?
@@ -68,10 +68,20 @@
 #define BIT_RAW_RDY_EN        0x01
 #define BIT_I2C_IF_DIS              0x10
 #define BIT_INT_STATUS_DATA   0x01
-
+											// Product ID Description for MPU6000
-#define MPU6000_REV_C 				0x14
+											// high 4 bits 	low 4 bits
-#define MPU6000_REV_D 				0x58
+											// Product Name	Product Revision
-
+#define MPU6000ES_REV_C4 			0x14 	// 0001			0100
+#define MPU6000ES_REV_C5 			0x15 	// 0001			0101
+#define MPU6000ES_REV_D6 			0x16	// 0001			0110
+#define MPU6000ES_REV_D7 			0x17	// 0001			0111
+#define MPU6000ES_REV_D8 			0x18	// 0001			1000	
+#define MPU6000_REV_C4 				0x54	// 0101			0100 
+#define MPU6000_REV_C5 				0x55	// 0101			0101
+#define MPU6000_REV_D6 				0x56	// 0101			0110	
+#define MPU6000_REV_D7 				0x57	// 0101			0111
+#define MPU6000_REV_D8 				0x58	// 0101			1000
+#define MPU6000_REV_D9 				0x59	// 0101			1001
 
 
 uint8_t AP_InertialSensor_MPU6000::_cs_pin;
@@ -326,7 +336,8 @@ void AP_InertialSensor_MPU6000::hardware_init()
 	
 	//Serial.printf("Product_ID= 0x%x\n", (unsigned) _product_id);
 	
-	if (_product_id == MPU6000_REV_C) {
+	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);
@@ -334,8 +345,6 @@ void AP_InertialSensor_MPU6000::hardware_init()
 		// Accel scale 8g (4096 LSB/g)
 		register_write(MPUREG_ACCEL_CONFIG,2<<3);
 	}
-		
-	
     delay(1);
 
     // INT CFG => Interrupt on Data Ready