Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

MPU6000: Fixed Scaling on Accelerometers Rev C vs Rev D

Browse Source 
Rev C have non standard scaling factor that is 1/2 of the data sheet
Rev D chips conform to the specification
This commit is contained in:
Craig Elder 2012-05-08 18:25:09 -07:00
parent 1bb79eb2b5
commit b824a87b90
1 changed files with 34 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -40,6 +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
// Configuration bits MPU 3000 and MPU 6000 (not revised)?
@ -68,17 +69,26 @@
#define BIT_I2C_IF_DIS 0x10
#define BIT_INT_STATUS_DATA 0x01
#define MPU6000_REV_C 0x14
#define MPU6000_REV_D 0x58
uint8_t AP_InertialSensor_MPU6000::_cs_pin;
/* pch: by the data sheet, the gyro scale should be 16.4LSB per DPS
* Given the radians conversion factor (0.174532), the gyro scale factor
* is waaaay off - output values are way too sensitive.
* Previously a divisor of 128 was appropriate.
* After tridge's changes to ::read, 50.0 seems about right based
* on making some 360 deg rotations on my desk.
* This issue requires more investigation.
/*
RS-MPU-6000A-00.pdf, page 33, section 4.25 lists LSB sensitivity of
gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
*/
const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532 / 16.4);
/*
RS-MPU-6000A-00.pdf, page 31, section 4.23 lists LSB sensitivity of
accel as 4096 LSB/mg at scale factor of +/- 8g (AFS_SEL==2)
See note below about accel scaling of engineering sample MPU6k
variants however
*/
const float AP_InertialSensor_MPU6000::_accel_scale = 9.81 / 4096.0;
/* pch: I believe the accel and gyro indicies are correct
@ -94,6 +104,8 @@ const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3;
static volatile uint8_t _new_data;
static uint8_t _product_id;
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000( uint8_t cs_pin )
{
_cs_pin = cs_pin; /* can't use initializer list, is static */
@ -308,7 +320,21 @@ void AP_InertialSensor_MPU6000::hardware_init()
delay(1);
register_write(MPUREG_GYRO_CONFIG,BITS_FS_2000DPS); // Gyro scale 2000º/s
delay(1);
register_write(MPUREG_ACCEL_CONFIG,0x08); // Accel scele 4g (4096LSB/g)
_product_id = register_read(MPUREG_PRODUCT_ID); // read the product ID rev c has 1/2 the sensitivity of rev d
//Serial.printf("Product_ID= 0x%x\n", (unsigned) _product_id);
if (_product_id == MPU6000_REV_C) {
// Accel scale 8g (4096 LSB/g)
// Rev C has different scaling than rev D
register_write(MPUREG_ACCEL_CONFIG,1<<3);
} else {
// Accel scale 8g (4096 LSB/g)
register_write(MPUREG_ACCEL_CONFIG,2<<3);
}
delay(1);
// INT CFG => Interrupt on Data Ready