#include #include "AP_InertialSensor_MPU6000.h" #include #include // MPU 6000 registers #define MPUREG_WHOAMI 0x75 // #define MPUREG_SMPLRT_DIV 0x19 // #define MPUREG_CONFIG 0x1A // #define MPUREG_GYRO_CONFIG 0x1B #define MPUREG_ACCEL_CONFIG 0x1C #define MPUREG_INT_PIN_CFG 0x37 #define MPUREG_INT_ENABLE 0x38 #define MPUREG_ACCEL_XOUT_H 0x3B // #define MPUREG_ACCEL_XOUT_L 0x3C // #define MPUREG_ACCEL_YOUT_H 0x3D // #define MPUREG_ACCEL_YOUT_L 0x3E // #define MPUREG_ACCEL_ZOUT_H 0x3F // #define MPUREG_ACCEL_ZOUT_L 0x40 // #define MPUREG_TEMP_OUT_H 0x41// #define MPUREG_TEMP_OUT_L 0x42// #define MPUREG_GYRO_XOUT_H 0x43 // #define MPUREG_GYRO_XOUT_L 0x44 // #define MPUREG_GYRO_YOUT_H 0x45 // #define MPUREG_GYRO_YOUT_L 0x46 // #define MPUREG_GYRO_ZOUT_H 0x47 // #define MPUREG_GYRO_ZOUT_L 0x48 // #define MPUREG_USER_CTRL 0x6A // #define MPUREG_PWR_MGMT_1 0x6B // #define MPUREG_PWR_MGMT_2 0x6C // // Configuration bits MPU 3000 and MPU 6000 (not revised)? #define BIT_SLEEP 0x40 #define BIT_H_RESET 0x80 #define BITS_CLKSEL 0x07 #define MPU_CLK_SEL_PLLGYROX 0x01 #define MPU_CLK_SEL_PLLGYROZ 0x03 #define MPU_EXT_SYNC_GYROX 0x02 #define BITS_FS_250DPS 0x00 #define BITS_FS_500DPS 0x08 #define BITS_FS_1000DPS 0x10 #define BITS_FS_2000DPS 0x18 #define BITS_FS_MASK 0x18 #define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 #define BITS_DLPF_CFG_188HZ 0x01 #define BITS_DLPF_CFG_98HZ 0x02 #define BITS_DLPF_CFG_42HZ 0x03 #define BITS_DLPF_CFG_20HZ 0x04 #define BITS_DLPF_CFG_10HZ 0x05 #define BITS_DLPF_CFG_5HZ 0x06 #define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 #define BITS_DLPF_CFG_MASK 0x07 #define BIT_INT_ANYRD_2CLEAR 0x10 #define BIT_RAW_RDY_EN 0x01 #define BIT_I2C_IF_DIS 0x10 int 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. */ const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532 / 16.4); const float AP_InertialSensor_MPU6000::_accel_scale = 9.81 / 4096.0; /* pch: I believe the accel and gyro indicies are correct * but somone else should please confirm. */ const uint8_t AP_InertialSensor_MPU6000::_gyro_data_index[3] = { 5, 4, 6 }; const int8_t AP_InertialSensor_MPU6000::_gyro_data_sign[3] = { 1, 1, -1 }; const uint8_t AP_InertialSensor_MPU6000::_accel_data_index[3] = { 1, 0, 2 }; const int8_t AP_InertialSensor_MPU6000::_accel_data_sign[3] = { 1, 1, -1 }; const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3; AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000( int cs_pin ) { _cs_pin = cs_pin; /* can't use initializer list, is static */ _gyro.x = 0; _gyro.y = 0; _gyro.z = 0; _accel.x = 0; _accel.y = 0; _accel.z = 0; _temp = 0; } void AP_InertialSensor_MPU6000::init( AP_PeriodicProcess * scheduler ) { hardware_init(); scheduler->register_process( &AP_InertialSensor_MPU6000::read ); } // accumulation in ISR - must be read with interrupts disabled // the sum of the values since last read static volatile int32_t _sum[7]; // how many values we've accumulated since last read static volatile uint16_t _count; /*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ bool AP_InertialSensor_MPU6000::update( void ) { int32_t sum[7]; uint16_t count; float count_scale; // wait for at least 1 sample while (_count == 0) /* nop */; // disable interrupts for mininum time cli(); for (int i=0; i<7; i++) { sum[i] = _sum[i]; _sum[i] = 0; } count = _count; _count = 0; sei(); count_scale = 1.0 / count; _gyro.x = _gyro_scale * _gyro_data_sign[0] * sum[_gyro_data_index[0]] * count_scale; _gyro.y = _gyro_scale * _gyro_data_sign[1] * sum[_gyro_data_index[1]] * count_scale; _gyro.z = _gyro_scale * _gyro_data_sign[2] * sum[_gyro_data_index[2]] * count_scale; _accel.x = _accel_scale * _accel_data_sign[0] * sum[_accel_data_index[0]] * count_scale; _accel.y = _accel_scale * _accel_data_sign[1] * sum[_accel_data_index[1]] * count_scale; _accel.z = _accel_scale * _accel_data_sign[2] * sum[_accel_data_index[2]] * count_scale; _temp = _temp_to_celsius(sum[_temp_data_index] * count_scale); return true; } float AP_InertialSensor_MPU6000::gx() { return _gyro.x; } float AP_InertialSensor_MPU6000::gy() { return _gyro.y; } float AP_InertialSensor_MPU6000::gz() { return _gyro.z; } void AP_InertialSensor_MPU6000::get_gyros( float * g ) { g[0] = _gyro.x; g[1] = _gyro.y; g[2] = _gyro.z; } float AP_InertialSensor_MPU6000::ax() { return _accel.x; } float AP_InertialSensor_MPU6000::ay() { return _accel.y; } float AP_InertialSensor_MPU6000::az() { return _accel.z; } void AP_InertialSensor_MPU6000::get_accels( float * a ) { a[0] = _accel.x; a[1] = _accel.y; a[2] = _accel.z; } void AP_InertialSensor_MPU6000::get_sensors( float * sensors ) { sensors[0] = _gyro.x; sensors[1] = _gyro.y; sensors[2] = _gyro.z; sensors[3] = _accel.x; sensors[4] = _accel.y; sensors[5] = _accel.z; } float AP_InertialSensor_MPU6000::temperature() { return _temp; } uint32_t AP_InertialSensor_MPU6000::sample_time() { uint32_t us = micros(); /* XXX rollover creates a major bug */ uint32_t delta = us - _last_sample_micros; reset_sample_time(); return delta; } void AP_InertialSensor_MPU6000::reset_sample_time() { _last_sample_micros = micros(); } /*================ HARDWARE FUNCTIONS ==================== */ static int16_t spi_transfer_16(void) { uint8_t byte_H, byte_L; byte_H = SPI.transfer(0); byte_L = SPI.transfer(0); return (((int16_t)byte_H)<<8) | byte_L; } void AP_InertialSensor_MPU6000::read() { // We start a multibyte SPI read of sensors byte addr = MPUREG_ACCEL_XOUT_H | 0x80; // Set most significant bit digitalWrite(_cs_pin, LOW); SPI.transfer(addr); for (uint8_t i=0; i<7; i++) { _sum[i] += spi_transfer_16(); } _count++; if (_count == 0) { // rollover - v unlikely memset((void*)_sum, 0, sizeof(_sum)); } digitalWrite(_cs_pin, HIGH); } uint8_t AP_InertialSensor_MPU6000::register_read( uint8_t reg ) { uint8_t dump; uint8_t return_value; uint8_t addr = reg | 0x80; // Set most significant bit digitalWrite(_cs_pin, LOW); dump = SPI.transfer(addr); return_value = SPI.transfer(0); digitalWrite(_cs_pin, HIGH); return return_value; } void AP_InertialSensor_MPU6000::register_write(uint8_t reg, uint8_t val) { uint8_t dump; digitalWrite(_cs_pin, LOW); dump = SPI.transfer(reg); dump = SPI.transfer(val); digitalWrite(_cs_pin, HIGH); } void AP_InertialSensor_MPU6000::hardware_init() { // MPU6000 chip select setup pinMode(_cs_pin, OUTPUT); digitalWrite(_cs_pin, HIGH); delay(1); // Chip reset register_write(MPUREG_PWR_MGMT_1, BIT_H_RESET); delay(100); // Wake up device and select GyroZ clock (better performance) register_write(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); delay(1); // Disable I2C bus (recommended on datasheet) register_write(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); delay(1); // SAMPLE RATE register_write(MPUREG_SMPLRT_DIV,0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz delay(1); // FS & DLPF FS=2000º/s, DLPF = 42Hz (low pass filter) register_write(MPUREG_CONFIG, BITS_DLPF_CFG_42HZ); 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) delay(1); // INT CFG => Interrupt on Data Ready register_write(MPUREG_INT_ENABLE,BIT_RAW_RDY_EN); // INT: Raw data ready delay(1); register_write(MPUREG_INT_PIN_CFG,BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read delay(1); // Oscillator set // register_write(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); delay(1); } float AP_InertialSensor_MPU6000::_temp_to_celsius ( uint16_t regval ) { /* TODO */ return 20.0; }