diff options
author | Andrew Tridgell <andrew@tridgell.net> | 2014-12-29 16:15:41 +1100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-30 20:55:57 +0100 |
commit | e5d3c80686ff27210887f60d8e024fa5635829b6 (patch) | |
tree | 7b57dd279ec1e8977de08230ecd228633f78e356 /src/drivers | |
parent | ca47952281cfe66732b08d3878eb6c8b1613abeb (diff) | |
download | px4-firmware-e5d3c80686ff27210887f60d8e024fa5635829b6.tar.gz px4-firmware-e5d3c80686ff27210887f60d8e024fa5635829b6.tar.bz2 px4-firmware-e5d3c80686ff27210887f60d8e024fa5635829b6.zip |
mpu6000: added factory self-test function
this follows the factory calibration self-test method in the datasheet
to see if the sensor still has the same calibration it had in the factory
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 226 |
1 files changed, 208 insertions, 18 deletions
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index eb9666dc5..2559c73e0 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -113,6 +113,10 @@ #define MPUREG_FIFO_COUNTL 0x73 #define MPUREG_FIFO_R_W 0x74 #define MPUREG_PRODUCT_ID 0x0C +#define MPUREG_TRIM1 0x0D +#define MPUREG_TRIM2 0x0E +#define MPUREG_TRIM3 0x0F +#define MPUREG_TRIM4 0x10 // Configuration bits MPU 3000 and MPU 6000 (not revised)? #define BIT_SLEEP 0x40 @@ -121,6 +125,9 @@ #define MPU_CLK_SEL_PLLGYROX 0x01 #define MPU_CLK_SEL_PLLGYROZ 0x03 #define MPU_EXT_SYNC_GYROX 0x02 +#define BITS_GYRO_ST_X 0x80 +#define BITS_GYRO_ST_Y 0x40 +#define BITS_GYRO_ST_Z 0x20 #define BITS_FS_250DPS 0x00 #define BITS_FS_500DPS 0x08 #define BITS_FS_1000DPS 0x10 @@ -196,6 +203,13 @@ public: void print_registers(); + /** + * Test behaviour against factory offsets + * + * @return 0 on success, 1 on failure + */ + int factory_self_test(); + protected: virtual int probe(); @@ -254,6 +268,10 @@ private: uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; + // use this to avoid processing measurements when in factory + // self test + volatile bool _in_factory_test; + /** * Start automatic measurement. */ @@ -375,6 +393,24 @@ private: /* do not allow to copy this class due to pointer data members */ MPU6000(const MPU6000&); MPU6000 operator=(const MPU6000&); + +#pragma pack(push, 1) + /** + * Report conversation within the MPU6000, including command byte and + * interrupt status. + */ + struct MPUReport { + uint8_t cmd; + uint8_t status; + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + }; +#pragma pack(pop) }; /* @@ -454,7 +490,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), - _checked_next(0) + _checked_next(0), + _in_factory_test(false) { // disable debug() calls _debug_enabled = false; @@ -889,6 +926,152 @@ MPU6000::gyro_self_test() return 0; } + +/* + perform a self-test comparison to factory trim values. This takes + about 200ms and will return OK if the current values are within 14% + of the expected values + */ +int +MPU6000::factory_self_test() +{ + _in_factory_test = true; + uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG); + uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG); + const uint16_t repeats = 100; + int ret = OK; + + // gyro self test has to be done at 250DPS + write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); + + struct MPUReport mpu_report; + float accel_baseline[3]; + float gyro_baseline[3]; + float accel[3]; + float gyro[3]; + float accel_ftrim[3]; + float gyro_ftrim[3]; + + // get baseline values without self-test enabled + set_frequency(MPU6000_HIGH_BUS_SPEED); + + memset(accel_baseline, 0, sizeof(accel_baseline)); + memset(gyro_baseline, 0, sizeof(gyro_baseline)); + memset(accel, 0, sizeof(accel)); + memset(gyro, 0, sizeof(gyro)); + + for (uint8_t i=0; i<repeats; i++) { + up_udelay(1000); + mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)); + + accel_baseline[0] += int16_t_from_bytes(mpu_report.accel_x); + accel_baseline[1] += int16_t_from_bytes(mpu_report.accel_y); + accel_baseline[2] += int16_t_from_bytes(mpu_report.accel_z); + gyro_baseline[0] += int16_t_from_bytes(mpu_report.gyro_x); + gyro_baseline[1] += int16_t_from_bytes(mpu_report.gyro_y); + gyro_baseline[2] += int16_t_from_bytes(mpu_report.gyro_z); + } + +#if 1 + write_reg(MPUREG_GYRO_CONFIG, + BITS_FS_250DPS | + BITS_GYRO_ST_X | + BITS_GYRO_ST_Y | + BITS_GYRO_ST_Z); + + // accel 8g, self-test enabled all axes + write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config | 0xE0); +#endif + + up_udelay(20000); + + // get values with self-test enabled + set_frequency(MPU6000_HIGH_BUS_SPEED); + + + for (uint8_t i=0; i<repeats; i++) { + up_udelay(1000); + mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)); + accel[0] += int16_t_from_bytes(mpu_report.accel_x); + accel[1] += int16_t_from_bytes(mpu_report.accel_y); + accel[2] += int16_t_from_bytes(mpu_report.accel_z); + gyro[0] += int16_t_from_bytes(mpu_report.gyro_x); + gyro[1] += int16_t_from_bytes(mpu_report.gyro_y); + gyro[2] += int16_t_from_bytes(mpu_report.gyro_z); + } + + for (uint8_t i=0; i<3; i++) { + accel_baseline[i] /= repeats; + gyro_baseline[i] /= repeats; + accel[i] /= repeats; + gyro[i] /= repeats; + } + + // extract factory trim values + uint8_t trims[4]; + trims[0] = read_reg(MPUREG_TRIM1); + trims[1] = read_reg(MPUREG_TRIM2); + trims[2] = read_reg(MPUREG_TRIM3); + trims[3] = read_reg(MPUREG_TRIM4); + uint8_t atrim[3]; + uint8_t gtrim[3]; + + atrim[0] = ((trims[0]>>3)&0x1C) | ((trims[3]>>4)&0x03); + atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03); + atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03); + gtrim[0] = trims[0] & 0x1F; + gtrim[1] = trims[1] & 0x1F; + gtrim[2] = trims[2] & 0x1F; + + // convert factory trims to right units + for (uint8_t i=0; i<3; i++) { + accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f); + gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1); + } + // Y gyro trim is negative + gyro_ftrim[1] *= -1; + + for (uint8_t i=0; i<3; i++) { + float diff = accel[i]-accel_baseline[i]; + float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i]; + ::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n", + (unsigned)i, + (int)(1000*accel_baseline[i]), + (int)(1000*accel[i]), + (int)(1000*diff), + (int)(1000*accel_ftrim[i]), + (int)err); + if (fabsf(err) > 14) { + ::printf("FAIL\n"); + ret = -EIO; + } + } + for (uint8_t i=0; i<3; i++) { + float diff = gyro[i]-gyro_baseline[i]; + float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; + ::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n", + (unsigned)i, + (int)(1000*gyro_baseline[i]), + (int)(1000*gyro[i]), + (int)(1000*(gyro[i]-gyro_baseline[i])), + (int)(1000*gyro_ftrim[i]), + (int)err); + if (fabsf(err) > 14) { + ::printf("FAIL\n"); + ret = -EIO; + } + } + + write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); + write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); + + _in_factory_test = false; + + return ret; +} + ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { @@ -1329,24 +1512,12 @@ MPU6000::check_registers(void) void MPU6000::measure() { -#pragma pack(push, 1) - /** - * Report conversation within the MPU6000, including command byte and - * interrupt status. - */ - struct MPUReport { - uint8_t cmd; - uint8_t status; - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; - } mpu_report; -#pragma pack(pop) + if (_in_factory_test) { + // don't publish any data while in factory test mode + return; + } + struct MPUReport mpu_report; struct Report { int16_t accel_x; int16_t accel_y; @@ -1635,6 +1806,7 @@ void test(bool); void reset(bool); void info(bool); void regdump(bool); +void factorytest(bool); void usage(); /** @@ -1826,6 +1998,21 @@ regdump(bool external_bus) exit(0); } +/** + * Dump the register information + */ +void +factorytest(bool external_bus) +{ + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr == nullptr) + errx(1, "driver not running"); + + (*g_dev_ptr)->factory_self_test(); + + exit(0); +} + void usage() { @@ -1892,5 +2079,8 @@ mpu6000_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) mpu6000::regdump(external_bus); + if (!strcmp(verb, "factorytest")) + mpu6000::factorytest(external_bus); + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } |