diff --git a/libraries/AP_Common/AP_Common.h b/libraries/AP_Common/AP_Common.h
index cb3e5c56076b33d472697f7d87e474547ea55ed6..ca07a1470e72725fa7c82b309b96f57cbbf72c10 100644
--- a/libraries/AP_Common/AP_Common.h
+++ b/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_REVD 0x58 // APM2 with REV D MPU 6000
+#define AP_PRODUCT_ID_APM2_REVC4 0x14 // APM2 with MPU6000ES_REV_C4
+#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
diff --git a/libraries/AP_IMU/AP_IMU_INS.cpp b/libraries/AP_IMU/AP_IMU_INS.cpp
index ad6b7013676c479f4e965497de2aef355f87646c..1ca10a45f2cef1fb955187ffbc272c542e972ea1 100644
--- a/libraries/AP_IMU/AP_IMU_INS.cpp
+++ b/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) {
diff --git a/libraries/AP_IMU/AP_IMU_INS.h b/libraries/AP_IMU/AP_IMU_INS.h
index 4fb82af0690b3008413beaa14bbf27df6b92fca5..a0ac5bdfbbe01922ae49e10b4708a99df1ba41a7 100644
--- a/libraries/AP_IMU/AP_IMU_INS.h
+++ b/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.
///
@@ -65,6 +67,7 @@ public:
virtual void ay(const float v) { _sensor_cal[4] = v; }
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.
diff --git a/libraries/AP_IMU/AP_IMU_Shim.h b/libraries/AP_IMU/AP_IMU_Shim.h
index ffdaffbb77b80a2e100f4231ca94a5ea78fd4a7d..0a7e88c673179c41ce26179f9b0ede9f64ce49d5 100644
--- a/libraries/AP_IMU/AP_IMU_Shim.h
+++ b/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;
diff --git a/libraries/AP_IMU/IMU.cpp b/libraries/AP_IMU/IMU.cpp
index 222664c5f76d50714995983ef0a4aac2673712c6..838314857c8e31e2cd8cd67f003848558dddb8cf 100644
--- a/libraries/AP_IMU/IMU.cpp
+++ b/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
};
diff --git a/libraries/AP_IMU/IMU.h b/libraries/AP_IMU/IMU.h
index 485f2a4e7899bbe0e3ba58894d3ee3a3460cb7ff..d3b1041c000fab44d00510b7b439f8de41f6cde2 100644
--- a/libraries/AP_IMU/IMU.h
+++ b/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
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index bad66f888d77ade8a9b987328aeee6acbb089d19..ed0b4f9d4888b6a85d2d4861345e758cd1937702 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/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
-
-#define MPU6000_REV_C 0x14
-#define MPU6000_REV_D 0x58
-
+ // Product ID Description for MPU6000
+ // high 4 bits low 4 bits
+ // 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