From fe742f1c2349eeb8aa36e5d53d55996fd6c2f41b Mon Sep 17 00:00:00 2001 From: Craig Elder Date: Tue, 8 May 2012 19:29:59 -0700 Subject: [PATCH] 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 | 13 ++++++++-- libraries/AP_IMU/AP_IMU_INS.cpp | 2 +- libraries/AP_IMU/AP_IMU_INS.h | 5 +++- libraries/AP_IMU/AP_IMU_Shim.h | 6 ++++- libraries/AP_IMU/IMU.cpp | 1 + libraries/AP_IMU/IMU.h | 2 ++ .../AP_InertialSensor_MPU6000.cpp | 25 +++++++++++++------ 7 files changed, 41 insertions(+), 13 deletions(-) diff --git a/libraries/AP_Common/AP_Common.h b/libraries/AP_Common/AP_Common.h index cb3e5c5607..ca07a1470e 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 ad6b701367..1ca10a45f2 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 4fb82af069..a0ac5bdfbb 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 ffdaffbb77..0a7e88c673 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 222664c5f7..838314857c 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 485f2a4e78..d3b1041c00 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 bad66f888d..ed0b4f9d48 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