2011-12-21 00:30:22 -04:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2011-11-12 23:20:25 -04:00
2012-10-11 21:27:19 -03:00
# include <AP_HAL.h>
2011-11-12 23:20:25 -04:00
# include "AP_InertialSensor_MPU6000.h"
2012-10-11 21:27:19 -03:00
extern const AP_HAL : : HAL & hal ;
2011-11-12 23:20:25 -04:00
2012-11-05 00:27:03 -04:00
// MPU6000 accelerometer scaling
# define MPU6000_ACCEL_SCALE_1G (GRAVITY / 4096.0)
2011-11-12 23:20:25 -04:00
// MPU 6000 registers
2012-08-17 03:19:56 -03:00
# define MPUREG_XG_OFFS_TC 0x00
# define MPUREG_YG_OFFS_TC 0x01
# define MPUREG_ZG_OFFS_TC 0x02
# define MPUREG_X_FINE_GAIN 0x03
# define MPUREG_Y_FINE_GAIN 0x04
# define MPUREG_Z_FINE_GAIN 0x05
# define MPUREG_XA_OFFS_H 0x06 // X axis accelerometer offset (high byte)
# define MPUREG_XA_OFFS_L 0x07 // X axis accelerometer offset (low byte)
# define MPUREG_YA_OFFS_H 0x08 // Y axis accelerometer offset (high byte)
# define MPUREG_YA_OFFS_L 0x09 // Y axis accelerometer offset (low byte)
# define MPUREG_ZA_OFFS_H 0x0A // Z axis accelerometer offset (high byte)
# define MPUREG_ZA_OFFS_L 0x0B // Z axis accelerometer offset (low byte)
# define MPUREG_PRODUCT_ID 0x0C // Product ID Register
# define MPUREG_XG_OFFS_USRH 0x13 // X axis gyro offset (high byte)
# define MPUREG_XG_OFFS_USRL 0x14 // X axis gyro offset (low byte)
# define MPUREG_YG_OFFS_USRH 0x15 // Y axis gyro offset (high byte)
# define MPUREG_YG_OFFS_USRL 0x16 // Y axis gyro offset (low byte)
# define MPUREG_ZG_OFFS_USRH 0x17 // Z axis gyro offset (high byte)
# define MPUREG_ZG_OFFS_USRL 0x18 // Z axis gyro offset (low byte)
# define MPUREG_SMPLRT_DIV 0x19 // sample rate. Fsample= 1Khz/(<this value>+1) = 200Hz
# define MPUREG_SMPLRT_1000HZ 0x00
# define MPUREG_SMPLRT_500HZ 0x01
# define MPUREG_SMPLRT_250HZ 0x03
# define MPUREG_SMPLRT_200HZ 0x04
# define MPUREG_SMPLRT_100HZ 0x09
# define MPUREG_SMPLRT_50HZ 0x13
# define MPUREG_CONFIG 0x1A
# define MPUREG_GYRO_CONFIG 0x1B
2012-07-28 02:14:43 -03:00
// bit definitions for MPUREG_GYRO_CONFIG
2012-08-17 03:19:56 -03:00
# define BITS_GYRO_FS_250DPS 0x00
# define BITS_GYRO_FS_500DPS 0x08
# define BITS_GYRO_FS_1000DPS 0x10
# define BITS_GYRO_FS_2000DPS 0x18
# define BITS_GYRO_FS_MASK 0x18 // only bits 3 and 4 are used for gyro full scale so use this to mask off other bits
# define BITS_GYRO_ZGYRO_SELFTEST 0x20
# define BITS_GYRO_YGYRO_SELFTEST 0x40
# define BITS_GYRO_XGYRO_SELFTEST 0x80
# define MPUREG_ACCEL_CONFIG 0x1C
# define MPUREG_MOT_THR 0x1F // detection threshold for Motion interrupt generation. Motion is detected when the absolute value of any of the accelerometer measurements exceeds this
# define MPUREG_MOT_DUR 0x20 // duration counter threshold for Motion interrupt generation. The duration counter ticks at 1 kHz, therefore MOT_DUR has a unit of 1 LSB = 1 ms
# define MPUREG_ZRMOT_THR 0x21 // detection threshold for Zero Motion interrupt generation.
# define MPUREG_ZRMOT_DUR 0x22 // duration counter threshold for Zero Motion interrupt generation. The duration counter ticks at 16 Hz, therefore ZRMOT_DUR has a unit of 1 LSB = 64 ms.
# define MPUREG_FIFO_EN 0x23
# define MPUREG_INT_PIN_CFG 0x37
# define BIT_INT_RD_CLEAR 0x10 // clear the interrupt when any read occurs
# define MPUREG_INT_ENABLE 0x38
2012-07-28 02:14:43 -03:00
// bit definitions for MPUREG_INT_ENABLE
2012-08-17 03:19:56 -03:00
# define BIT_RAW_RDY_EN 0x01
# define BIT_DMP_INT_EN 0x02 // enabling this bit (DMP_INT_EN) also enables RAW_RDY_EN it seems
# define BIT_UNKNOWN_INT_EN 0x04
# define BIT_I2C_MST_INT_EN 0x08
# define BIT_FIFO_OFLOW_EN 0x10
# define BIT_ZMOT_EN 0x20
# define BIT_MOT_EN 0x40
# define BIT_FF_EN 0x80
# define MPUREG_INT_STATUS 0x3A
2012-07-28 02:14:43 -03:00
// bit definitions for MPUREG_INT_STATUS (same bit pattern as above because this register shows what interrupt actually fired)
2012-08-17 03:19:56 -03:00
# define BIT_RAW_RDY_INT 0x01
# define BIT_DMP_INT 0x02
# define BIT_UNKNOWN_INT 0x04
# define BIT_I2C_MST_INT 0x08
# define BIT_FIFO_OFLOW_INT 0x10
# define BIT_ZMOT_INT 0x20
# define BIT_MOT_INT 0x40
# define BIT_FF_INT 0x80
# 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
2012-07-28 02:14:43 -03:00
// bit definitions for MPUREG_USER_CTRL
2012-08-17 03:19:56 -03:00
# define BIT_USER_CTRL_SIG_COND_RESET 0x01 // resets signal paths and results registers for all sensors (gyros, accel, temp)
# define BIT_USER_CTRL_I2C_MST_RESET 0x02 // reset I2C Master (only applicable if I2C_MST_EN bit is set)
# define BIT_USER_CTRL_FIFO_RESET 0x04 // Reset (i.e. clear) FIFO buffer
# define BIT_USER_CTRL_DMP_RESET 0x08 // Reset DMP
2012-10-11 21:27:19 -03:00
# define BIT_USER_CTRL_I2C_IF_DIS 0x10 // Disable primary I2C interface and enable hal.spi->interface
2012-08-17 03:19:56 -03:00
# define BIT_USER_CTRL_I2C_MST_EN 0x20 // Enable MPU to act as the I2C Master to external slave sensors
# define BIT_USER_CTRL_FIFO_EN 0x40 // Enable FIFO operations
# define BIT_USER_CTRL_DMP_EN 0x80 // Enable DMP operations
# define MPUREG_PWR_MGMT_1 0x6B
# define BIT_PWR_MGMT_1_CLK_INTERNAL 0x00 // clock set to internal 8Mhz oscillator
# define BIT_PWR_MGMT_1_CLK_XGYRO 0x01 // PLL with X axis gyroscope reference
# define BIT_PWR_MGMT_1_CLK_YGYRO 0x02 // PLL with Y axis gyroscope reference
# define BIT_PWR_MGMT_1_CLK_ZGYRO 0x03 // PLL with Z axis gyroscope reference
# define BIT_PWR_MGMT_1_CLK_EXT32KHZ 0x04 // PLL with external 32.768kHz reference
# define BIT_PWR_MGMT_1_CLK_EXT19MHZ 0x05 // PLL with external 19.2MHz reference
# define BIT_PWR_MGMT_1_CLK_STOP 0x07 // Stops the clock and keeps the timing generator in reset
# define BIT_PWR_MGMT_1_TEMP_DIS 0x08 // disable temperature sensor
# define BIT_PWR_MGMT_1_CYCLE 0x20 // put sensor into cycle mode. cycles between sleep mode and waking up to take a single sample of data from active sensors at a rate determined by LP_WAKE_CTRL
# define BIT_PWR_MGMT_1_SLEEP 0x40 // put sensor into low power sleep mode
# define BIT_PWR_MGMT_1_DEVICE_RESET 0x80 // reset entire device
# define MPUREG_PWR_MGMT_2 0x6C // allows the user to configure the frequency of wake-ups in Accelerometer Only Low Power Mode
# define MPUREG_BANK_SEL 0x6D // DMP bank selection register (used to indirectly access DMP registers)
# define MPUREG_MEM_START_ADDR 0x6E // DMP memory start address (used to indirectly write to dmp memory)
# define MPUREG_MEM_R_W 0x6F // DMP related register
# define MPUREG_DMP_CFG_1 0x70 // DMP related register
# define MPUREG_DMP_CFG_2 0x71 // DMP related register
# define MPUREG_FIFO_COUNTH 0x72
# define MPUREG_FIFO_COUNTL 0x73
# define MPUREG_FIFO_R_W 0x74
# define MPUREG_WHOAMI 0x75
2011-12-25 05:37:50 -04:00
2011-11-12 23:20:25 -04:00
// Configuration bits MPU 3000 and MPU 6000 (not revised)?
2012-08-17 03:19:56 -03:00
# 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
// 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
2012-05-08 22:25:09 -03:00
2012-07-28 02:14:43 -03:00
// DMP output rate constants
2012-08-17 03:19:56 -03:00
# define MPU6000_200HZ 0x00 // default value
# define MPU6000_100HZ 0x01
# define MPU6000_66HZ 0x02
# define MPU6000_50HZ 0x03
2012-07-28 02:14:43 -03:00
// DMP FIFO constants
2012-10-11 21:27:19 -03:00
// Default quaternion FIFO size (4*4) + Footer(2)
# define FIFO_PACKET_SIZE 18
// Rate of the gyro bias from gravity correction (200Hz/4) => 50Hz
# define GYRO_BIAS_FROM_GRAVITY_RATE 4
// Default gain for accel fusion (with gyros)
# define DEFAULT_ACCEL_FUSION_GAIN 0x80
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
/*
2012-11-19 11:37:42 -04:00
* RM - MPU - 6000 A - 00. pdf , page 33 , section 4.25 lists LSB sensitivity of
2012-08-17 03:19:56 -03:00
* gyro as 16.4 LSB / DPS at scale factor of + / - 2000 dps ( FS_SEL = = 3 )
2011-11-12 23:20:25 -04:00
*/
const float AP_InertialSensor_MPU6000 : : _gyro_scale = ( 0.0174532 / 16.4 ) ;
2012-05-08 22:25:09 -03:00
2011-11-12 23:20:25 -04:00
/* 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 } ;
2012-08-17 03:19:56 -03:00
const int8_t AP_InertialSensor_MPU6000 : : _gyro_data_sign [ 3 ] = { 1 , 1 , - 1 } ;
2011-11-12 23:20:25 -04:00
const uint8_t AP_InertialSensor_MPU6000 : : _accel_data_index [ 3 ] = { 1 , 0 , 2 } ;
2012-08-17 03:19:56 -03:00
const int8_t AP_InertialSensor_MPU6000 : : _accel_data_sign [ 3 ] = { 1 , 1 , - 1 } ;
2011-11-12 23:20:25 -04:00
const uint8_t AP_InertialSensor_MPU6000 : : _temp_data_index = 3 ;
2012-11-05 00:27:03 -04:00
int16_t AP_InertialSensor_MPU6000 : : _mpu6000_product_id = AP_PRODUCT_ID_NONE ;
2012-10-11 21:27:19 -03:00
// variables to calculate time period over which a group of samples were
// collected
// time period overwhich samples were collected (initialise to non-zero
// number but will be overwritten on 2nd read in any case)
static volatile uint32_t _delta_time_micros = 1 ;
// time we start collecting sample (reset on update)
static volatile uint32_t _delta_time_start_micros = 0 ;
// time latest sample was collected
static volatile uint32_t _last_sample_time_micros = 0 ;
2011-12-25 05:37:50 -04:00
2012-07-28 02:14:43 -03:00
// DMP related static variables
bool AP_InertialSensor_MPU6000 : : _dmp_initialised = false ;
2012-10-11 21:27:19 -03:00
// high byte of number of elements in fifo buffer
uint8_t AP_InertialSensor_MPU6000 : : _fifoCountH ;
// low byte of number of elements in fifo buffer
uint8_t AP_InertialSensor_MPU6000 : : _fifoCountL ;
// holds the 4 quaternions representing attitude taken directly from the DMP
Quaternion AP_InertialSensor_MPU6000 : : quaternion ;
/* Static SPI device driver */
AP_HAL : : SPIDeviceDriver * AP_InertialSensor_MPU6000 : : _spi = NULL ;
AP_HAL : : Semaphore * AP_InertialSensor_MPU6000 : : _spi_sem = NULL ;
2012-07-28 02:14:43 -03:00
2012-11-05 00:27:03 -04:00
/*
2012-11-19 11:37:42 -04:00
* RM - MPU - 6000 A - 00. pdf , page 31 , section 4.23 lists LSB sensitivity of
2012-11-05 00:27:03 -04:00
* accel as 4096 LSB / mg at scale factor of + / - 8 g ( AFS_SEL = = 2 )
*
* See note below about accel scaling of engineering sample MPU6k
* variants however
*/
2012-09-28 07:21:59 -03:00
AP_InertialSensor_MPU6000 : : AP_InertialSensor_MPU6000 ( )
2011-11-12 23:20:25 -04:00
{
2012-08-17 03:19:56 -03:00
_temp = 0 ;
_initialised = false ;
_dmp_initialised = false ;
2011-11-12 23:20:25 -04:00
}
2012-10-11 21:27:19 -03:00
uint16_t AP_InertialSensor_MPU6000 : : _init_sensor ( Sample_rate sample_rate )
2011-11-12 23:20:25 -04:00
{
2012-11-05 00:27:03 -04:00
if ( _initialised ) return _mpu6000_product_id ;
2012-07-28 02:14:43 -03:00
_initialised = true ;
2012-10-11 21:27:19 -03:00
hal . scheduler - > suspend_timer_procs ( ) ;
2012-11-29 07:56:13 -04:00
hardware_init ( sample_rate ) ;
2012-10-11 21:27:19 -03:00
hal . scheduler - > resume_timer_procs ( ) ;
2012-11-05 00:27:03 -04:00
return _mpu6000_product_id ;
2011-11-12 23:20:25 -04:00
}
// 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 )
{
2012-08-17 03:19:56 -03:00
int32_t sum [ 7 ] ;
uint16_t count ;
float count_scale ;
2012-11-05 00:27:03 -04:00
Vector3f gyro_offset = _gyro_offset . get ( ) ;
Vector3f accel_scale = _accel_scale . get ( ) ;
Vector3f accel_offset = _accel_offset . get ( ) ;
2012-08-17 03:19:56 -03:00
// wait for at least 1 sample
2012-12-05 21:14:04 -04:00
uint32_t tstart = hal . scheduler - > micros ( ) ;
while ( _count = = 0 ) {
if ( hal . scheduler - > micros ( ) - tstart > 50000 ) {
hal . console - > println_P (
PSTR ( " PANIC: AP_InertialSensor_MPU6000::update "
" waited 50ms for data from interrupt " ) ) ;
return false ;
}
}
2012-08-17 03:19:56 -03:00
// disable interrupts for mininum time
2012-10-11 21:27:19 -03:00
hal . scheduler - > begin_atomic ( ) ;
2012-08-17 03:19:56 -03:00
for ( int i = 0 ; i < 7 ; i + + ) {
sum [ i ] = _sum [ i ] ;
_sum [ i ] = 0 ;
}
count = _count ;
_count = 0 ;
2012-08-30 04:48:36 -03:00
// record sample time
_delta_time_micros = _last_sample_time_micros - _delta_time_start_micros ;
_delta_time_start_micros = _last_sample_time_micros ;
2012-10-11 21:27:19 -03:00
hal . scheduler - > end_atomic ( ) ;
2011-11-12 23:20:25 -04:00
2012-08-17 03:19:56 -03:00
count_scale = 1.0 / count ;
2011-11-12 23:20:25 -04:00
2012-11-20 03:25:36 -04:00
_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 ;
_gyro - = gyro_offset ;
_accel . x = accel_scale . x * _accel_data_sign [ 0 ] * sum [ _accel_data_index [ 0 ] ] * count_scale * MPU6000_ACCEL_SCALE_1G ;
_accel . y = accel_scale . y * _accel_data_sign [ 1 ] * sum [ _accel_data_index [ 1 ] ] * count_scale * MPU6000_ACCEL_SCALE_1G ;
_accel . z = accel_scale . z * _accel_data_sign [ 2 ] * sum [ _accel_data_index [ 2 ] ] * count_scale * MPU6000_ACCEL_SCALE_1G ;
_accel - = _accel_offset ;
2011-11-12 23:20:25 -04:00
2012-08-17 03:19:56 -03:00
_temp = _temp_to_celsius ( sum [ _temp_data_index ] * count_scale ) ;
2011-11-12 23:20:25 -04:00
2012-08-17 03:19:56 -03:00
return true ;
2011-11-12 23:20:25 -04:00
}
2012-03-03 03:31:31 -04:00
bool AP_InertialSensor_MPU6000 : : new_data_available ( void )
{
return _count ! = 0 ;
}
2012-08-17 03:19:56 -03:00
float AP_InertialSensor_MPU6000 : : temperature ( ) {
return _temp ;
}
2011-11-12 23:20:25 -04:00
/*================ HARDWARE FUNCTIONS ==================== */
2012-12-06 17:38:40 -04:00
static int16_t spi_transfer_16 ( AP_HAL : : SPIDeviceDriver * _spi )
{
uint8_t byte_H , byte_L ;
byte_H = _spi - > transfer ( 0 ) ;
byte_L = _spi - > transfer ( 0 ) ;
return ( ( ( int16_t ) byte_H ) < < 8 ) | byte_L ;
}
2011-12-25 05:37:50 -04:00
/*
2012-10-11 21:27:19 -03:00
* this is called from the data_interrupt which fires when the MPU6000 has new
* sensor data available and add it to _sum [ ] to ensure this is the case ,
* these other devices must perform their spi reads after being called by the
* AP_TimerProcess .
2011-12-25 05:37:50 -04:00
*/
2012-09-06 02:08:23 -03:00
void AP_InertialSensor_MPU6000 : : read ( uint32_t )
2011-11-12 23:20:25 -04:00
{
2012-12-05 21:14:04 -04:00
static int semfail_ctr = 0 ;
2012-10-11 21:27:19 -03:00
if ( _spi_sem ) {
2012-12-05 21:14:04 -04:00
bool got = _spi_sem - > get ( ( void * ) & _spi_sem ) ;
if ( ! got ) {
semfail_ctr + + ;
if ( semfail_ctr > 100 ) {
hal . console - > println_P ( PSTR ( " PANIC: failed to take _spi_sem "
" 100 times in AP_InertialSensor_MPU6000::read " ) ) ;
}
return ;
} else {
semfail_ctr = 0 ;
}
2012-10-11 21:27:19 -03:00
}
2012-12-05 21:14:04 -04:00
2011-12-25 05:37:50 -04:00
// now read the data
2012-10-11 21:27:19 -03:00
_spi - > cs_assert ( ) ;
uint8_t addr = MPUREG_ACCEL_XOUT_H | 0x80 ;
_spi - > transfer ( addr ) ;
2011-12-25 05:37:50 -04:00
for ( uint8_t i = 0 ; i < 7 ; i + + ) {
2012-12-06 17:38:40 -04:00
_sum [ i ] + = spi_transfer_16 ( _spi ) ;
2011-12-25 05:37:50 -04:00
}
2012-10-11 21:27:19 -03:00
_spi - > cs_release ( ) ;
2011-12-25 05:37:50 -04:00
_count + + ;
if ( _count = = 0 ) {
// rollover - v unlikely
memset ( ( void * ) _sum , 0 , sizeof ( _sum ) ) ;
}
2011-11-12 23:20:25 -04:00
2012-08-17 03:19:56 -03:00
// should also read FIFO data if enabled
if ( _dmp_initialised ) {
if ( FIFO_ready ( ) ) {
FIFO_getPacket ( ) ;
}
}
2012-10-11 21:27:19 -03:00
if ( _spi_sem ) {
2012-12-05 21:14:04 -04:00
bool released = _spi_sem - > release ( ( void * ) & _spi_sem ) ;
if ( ! released ) {
hal . console - > println_P ( PSTR ( " PANIC: _spi_sem release failed in "
" AP_InertialSensor_MPU6000::read " ) ) ;
}
2012-10-11 21:27:19 -03:00
}
2011-11-12 23:20:25 -04:00
}
uint8_t AP_InertialSensor_MPU6000 : : register_read ( uint8_t reg )
{
2012-08-17 03:19:56 -03:00
uint8_t return_value ;
uint8_t addr = reg | 0x80 ; // Set most significant bit
2011-11-12 23:20:25 -04:00
2012-10-11 21:27:19 -03:00
_spi - > cs_assert ( ) ;
2011-11-12 23:20:25 -04:00
2012-10-11 21:27:19 -03:00
_spi - > transfer ( addr ) ;
return_value = _spi - > transfer ( 0 ) ;
2011-11-12 23:20:25 -04:00
2012-10-11 21:27:19 -03:00
_spi - > cs_release ( ) ;
2011-11-12 23:20:25 -04:00
2012-08-17 03:19:56 -03:00
return return_value ;
2011-11-12 23:20:25 -04:00
}
void AP_InertialSensor_MPU6000 : : register_write ( uint8_t reg , uint8_t val )
{
2012-10-11 21:27:19 -03:00
_spi - > cs_assert ( ) ;
_spi - > transfer ( reg ) ;
_spi - > transfer ( val ) ;
_spi - > cs_release ( ) ;
2011-11-12 23:20:25 -04:00
}
2011-12-25 05:37:50 -04:00
// MPU6000 new data interrupt on INT6
void AP_InertialSensor_MPU6000 : : data_interrupt ( void )
{
2012-09-06 08:48:36 -03:00
// record time that data was available
2012-10-11 21:27:19 -03:00
_last_sample_time_micros = hal . scheduler - > micros ( ) ;
2012-12-04 23:11:05 -04:00
// queue our read process to run after any currently running timed
// processes complete
2012-10-11 21:27:19 -03:00
hal . scheduler - > defer_timer_process ( AP_InertialSensor_MPU6000 : : read ) ;
2011-12-25 05:37:50 -04:00
}
2012-11-29 07:56:13 -04:00
void AP_InertialSensor_MPU6000 : : hardware_init ( Sample_rate sample_rate )
2011-11-12 23:20:25 -04:00
{
2012-10-11 21:27:19 -03:00
_spi = hal . spi - > device ( AP_HAL : : SPIDevice_MPU6000 ) ;
_spi_sem = _spi - > get_semaphore ( ) ;
2011-11-12 23:20:25 -04:00
// Chip reset
2012-07-28 02:14:43 -03:00
register_write ( MPUREG_PWR_MGMT_1 , BIT_PWR_MGMT_1_DEVICE_RESET ) ;
2012-10-11 21:27:19 -03:00
hal . scheduler - > delay ( 100 ) ;
2011-11-12 23:20:25 -04:00
// Wake up device and select GyroZ clock (better performance)
2012-07-28 02:14:43 -03:00
register_write ( MPUREG_PWR_MGMT_1 , BIT_PWR_MGMT_1_CLK_ZGYRO ) ;
2012-10-11 21:27:19 -03:00
hal . scheduler - > delay ( 1 ) ;
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
register_write ( MPUREG_PWR_MGMT_2 , 0x00 ) ; // only used for wake-up in accelerometer only low power mode
2012-10-11 21:27:19 -03:00
hal . scheduler - > delay ( 1 ) ;
2012-07-28 02:14:43 -03:00
2011-11-12 23:20:25 -04:00
// Disable I2C bus (recommended on datasheet)
2012-07-28 02:14:43 -03:00
register_write ( MPUREG_USER_CTRL , BIT_USER_CTRL_I2C_IF_DIS ) ;
2012-10-11 21:27:19 -03:00
hal . scheduler - > delay ( 1 ) ;
2012-11-29 07:56:13 -04:00
2012-11-29 16:15:12 -04:00
uint8_t rate , filter , default_filter ;
2012-11-29 07:56:13 -04:00
// sample rate and filtering
// to minimise the effects of aliasing we choose a filter
// that is less than half of the sample rate
switch ( sample_rate ) {
case RATE_50HZ :
rate = MPUREG_SMPLRT_50HZ ;
2012-11-29 16:15:12 -04:00
default_filter = BITS_DLPF_CFG_20HZ ;
2012-11-29 07:56:13 -04:00
break ;
case RATE_100HZ :
rate = MPUREG_SMPLRT_100HZ ;
2012-11-29 16:15:12 -04:00
default_filter = BITS_DLPF_CFG_42HZ ;
2012-11-29 07:56:13 -04:00
break ;
case RATE_200HZ :
default :
rate = MPUREG_SMPLRT_200HZ ;
2012-12-09 01:27:33 -04:00
default_filter = BITS_DLPF_CFG_42HZ ;
2012-11-29 16:15:12 -04:00
break ;
}
// choose filtering frequency
switch ( _mpu6000_filter ) {
case 5 :
filter = BITS_DLPF_CFG_5HZ ;
break ;
case 10 :
filter = BITS_DLPF_CFG_10HZ ;
break ;
case 20 :
filter = BITS_DLPF_CFG_20HZ ;
break ;
case 42 :
filter = BITS_DLPF_CFG_42HZ ;
break ;
case 98 :
2012-11-29 07:56:13 -04:00
filter = BITS_DLPF_CFG_98HZ ;
break ;
2012-11-29 16:15:12 -04:00
case 0 :
default :
// the user hasn't specified a specific frequency,
// use the default value for the given sample rate
filter = default_filter ;
2012-11-29 07:56:13 -04:00
}
// set sample rate
register_write ( MPUREG_SMPLRT_DIV , rate ) ;
2012-10-11 21:27:19 -03:00
hal . scheduler - > delay ( 1 ) ;
2012-11-29 07:56:13 -04:00
// set low pass filter
register_write ( MPUREG_CONFIG , filter ) ;
2012-10-11 21:27:19 -03:00
hal . scheduler - > delay ( 1 ) ;
2012-11-29 07:56:13 -04:00
2012-07-28 02:14:43 -03:00
register_write ( MPUREG_GYRO_CONFIG , BITS_GYRO_FS_2000DPS ) ; // Gyro scale 2000º/s
2012-10-11 21:27:19 -03:00
hal . scheduler - > delay ( 1 ) ;
2012-08-17 03:19:56 -03:00
2012-11-05 00:27:03 -04:00
_mpu6000_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) _mpu6000_product_id);
2012-08-17 03:19:56 -03:00
2012-11-05 00:27:03 -04:00
if ( ( _mpu6000_product_id = = MPU6000ES_REV_C4 ) | | ( _mpu6000_product_id = = MPU6000ES_REV_C5 ) | |
( _mpu6000_product_id = = MPU6000_REV_C4 ) | | ( _mpu6000_product_id = = MPU6000_REV_C5 ) ) {
2012-08-17 03:19:56 -03:00
// 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 ) ;
}
2012-10-11 21:27:19 -03:00
hal . scheduler - > delay ( 1 ) ;
2011-12-25 05:37:50 -04:00
2012-08-17 03:19:56 -03:00
register_write ( MPUREG_INT_ENABLE , BIT_RAW_RDY_EN ) ; // configure interrupt to fire when new data arrives
2012-10-11 21:27:19 -03:00
hal . scheduler - > delay ( 1 ) ;
2012-08-17 03:19:56 -03:00
register_write ( MPUREG_INT_PIN_CFG , BIT_INT_RD_CLEAR ) ; // clear interrupt on any read
2012-10-11 21:27:19 -03:00
hal . scheduler - > delay ( 1 ) ;
2011-11-12 23:20:25 -04:00
2012-10-11 21:27:19 -03:00
if ( ! hal . gpio - > attach_interrupt ( 6 , data_interrupt , GPIO_RISING ) ) {
hal . console - > println_P (
PSTR ( " Critical Error: AP_InertialSensor_MPU6000 could not "
" attach data ready interrupt. " ) ) ;
}
2011-11-12 23:20:25 -04:00
}
float AP_InertialSensor_MPU6000 : : _temp_to_celsius ( uint16_t regval )
{
/* TODO */
return 20.0 ;
}
2012-03-08 03:10:27 -04:00
// return the MPU6k gyro drift rate in radian/s/s
// note that this is much better than the oilpan gyros
float AP_InertialSensor_MPU6000 : : get_gyro_drift_rate ( void )
{
// 0.5 degrees/second/minute
return ToRad ( 0.5 / 60 ) ;
}
2012-07-28 02:14:43 -03:00
2012-08-30 04:48:36 -03:00
// get number of samples read from the sensors
uint16_t AP_InertialSensor_MPU6000 : : num_samples_available ( )
{
return _count ;
}
2012-11-05 00:27:03 -04:00
// get_delta_time returns the time period in seconds overwhich the sensor data was collected
uint32_t AP_InertialSensor_MPU6000 : : get_delta_time_micros ( )
2012-08-30 04:48:36 -03:00
{
2012-11-05 00:27:03 -04:00
return _delta_time_micros ;
2012-08-30 04:48:36 -03:00
}
2012-07-28 02:14:43 -03:00
// Update gyro offsets with new values. Offsets provided in as scaled deg/sec values
2012-11-05 00:27:03 -04:00
void AP_InertialSensor_MPU6000 : : push_gyro_offsets_to_dmp ( )
2012-07-28 02:14:43 -03:00
{
2012-11-05 00:27:03 -04:00
Vector3f gyro_offsets = _gyro_offset . get ( ) ;
int16_t offsetX = gyro_offsets . x / _gyro_scale * _gyro_data_sign [ 0 ] ;
int16_t offsetY = gyro_offsets . y / _gyro_scale * _gyro_data_sign [ 1 ] ;
int16_t offsetZ = gyro_offsets . z / _gyro_scale * _gyro_data_sign [ 2 ] ;
2012-07-28 02:14:43 -03:00
2012-11-05 00:27:03 -04:00
set_dmp_gyro_offsets ( offsetX , offsetY , offsetZ ) ;
// remove ins level offsets to avoid double counting
gyro_offsets . x = 0 ;
gyro_offsets . y = 0 ;
gyro_offsets . z = 0 ;
_gyro_offset = gyro_offsets ;
2012-07-28 02:14:43 -03:00
}
// Update gyro offsets with new values. New offset values are substracted to actual offset values.
// offset values in gyro LSB units (as read from registers)
2012-11-05 00:27:03 -04:00
void AP_InertialSensor_MPU6000 : : set_dmp_gyro_offsets ( int16_t offsetX , int16_t offsetY , int16_t offsetZ )
2012-07-28 02:14:43 -03:00
{
2012-08-17 03:19:56 -03:00
int16_t aux_int ;
if ( offsetX ! = 0 ) {
// Read actual value
aux_int = ( register_read ( MPUREG_XG_OFFS_USRH ) < < 8 ) | register_read ( MPUREG_XG_OFFS_USRL ) ;
aux_int - = offsetX < < 1 ; // Adjust to internal units
// Write to MPU registers
register_write ( MPUREG_XG_OFFS_USRH , ( aux_int > > 8 ) & 0xFF ) ;
register_write ( MPUREG_XG_OFFS_USRL , aux_int & 0xFF ) ;
}
if ( offsetY ! = 0 ) {
aux_int = ( register_read ( MPUREG_YG_OFFS_USRH ) < < 8 ) | register_read ( MPUREG_YG_OFFS_USRL ) ;
aux_int - = offsetY < < 1 ; // Adjust to internal units
// Write to MPU registers
register_write ( MPUREG_YG_OFFS_USRH , ( aux_int > > 8 ) & 0xFF ) ;
register_write ( MPUREG_YG_OFFS_USRL , aux_int & 0xFF ) ;
}
if ( offsetZ ! = 0 ) {
aux_int = ( register_read ( MPUREG_ZG_OFFS_USRH ) < < 8 ) | register_read ( MPUREG_ZG_OFFS_USRL ) ;
aux_int - = offsetZ < < 1 ; // Adjust to internal units
// Write to MPU registers
register_write ( MPUREG_ZG_OFFS_USRH , ( aux_int > > 8 ) & 0xFF ) ;
register_write ( MPUREG_ZG_OFFS_USRL , aux_int & 0xFF ) ;
}
2012-07-28 02:14:43 -03:00
}
2012-11-05 00:27:03 -04:00
// Update accel offsets with new values. Offsets provided in as scaled values (1G)
void AP_InertialSensor_MPU6000 : : push_accel_offsets_to_dmp ( )
2012-07-28 02:14:43 -03:00
{
2012-11-05 00:27:03 -04:00
Vector3f accel_offset = _accel_offset . get ( ) ;
Vector3f accel_scale = _accel_scale . get ( ) ;
int16_t offsetX = accel_offset . x / ( accel_scale . x * _accel_data_sign [ 0 ] * MPU6000_ACCEL_SCALE_1G ) ;
int16_t offsetY = accel_offset . y / ( accel_scale . y * _accel_data_sign [ 1 ] * MPU6000_ACCEL_SCALE_1G ) ;
int16_t offsetZ = accel_offset . z / ( accel_scale . z * _accel_data_sign [ 2 ] * MPU6000_ACCEL_SCALE_1G ) ;
// strangely x and y are reversed
set_dmp_accel_offsets ( offsetY , offsetX , offsetZ ) ;
2012-07-28 02:14:43 -03:00
}
// set_accel_offsets - adds an offset to acceleromter readings
// This is useful for dynamic acceleration correction (for example centripetal force correction)
// and for the initial offset calibration
// Input, accel offsets for X,Y and Z in LSB units (as read from raw values)
2012-11-05 00:27:03 -04:00
void AP_InertialSensor_MPU6000 : : set_dmp_accel_offsets ( int16_t offsetX , int16_t offsetY , int16_t offsetZ )
2012-07-28 02:14:43 -03:00
{
2012-08-17 03:19:56 -03:00
int aux_int ;
uint8_t regs [ 2 ] ;
// Write accel offsets to DMP memory...
// TO-DO: why don't we write to main accel offset registries? i.e. MPUREG_XA_OFFS_H
aux_int = offsetX > > 1 ; // Transform to internal units
regs [ 0 ] = ( aux_int > > 8 ) & 0xFF ;
regs [ 1 ] = aux_int & 0xFF ;
dmp_register_write ( 0x01 , 0x08 , 2 , regs ) ; // key KEY_D_1_8 Accel X offset
aux_int = offsetY > > 1 ;
regs [ 0 ] = ( aux_int > > 8 ) & 0xFF ;
regs [ 1 ] = aux_int & 0xFF ;
dmp_register_write ( 0x01 , 0x0A , 2 , regs ) ; // key KEY_D_1_10 Accel Y offset
aux_int = offsetZ > > 1 ;
regs [ 0 ] = ( aux_int > > 8 ) & 0xFF ;
regs [ 1 ] = aux_int & 0xFF ;
dmp_register_write ( 0x01 , 0x02 , 2 , regs ) ; // key KEY_D_1_2 Accel Z offset
2012-07-28 02:14:43 -03:00
}
// dmp_register_write - method to write to dmp's registers
// the dmp is logically separated from the main mpu6000. To write a block of memory to the DMP's memory you
2012-08-17 03:19:56 -03:00
// write the "bank" and starting address into two of the main MPU's registers, then write the data one byte
2012-07-28 02:14:43 -03:00
// at a time into the MPUREG_MEM_R_W register
void AP_InertialSensor_MPU6000 : : dmp_register_write ( uint8_t bank , uint8_t address , uint8_t num_bytes , uint8_t data [ ] )
{
2012-08-17 03:19:56 -03:00
register_write ( MPUREG_BANK_SEL , bank ) ;
register_write ( MPUREG_MEM_START_ADDR , address ) ;
2012-10-11 21:27:19 -03:00
_spi - > cs_assert ( ) ;
_spi - > transfer ( MPUREG_MEM_R_W ) ;
2012-08-17 03:19:56 -03:00
for ( uint8_t i = 0 ; i < num_bytes ; i + + ) {
2012-10-11 21:27:19 -03:00
_spi - > transfer ( data [ i ] ) ;
2012-08-17 03:19:56 -03:00
}
2012-10-11 21:27:19 -03:00
_spi - > cs_release ( ) ;
2012-07-28 02:14:43 -03:00
}
// MPU6000 DMP initialization
2012-08-28 05:00:03 -03:00
// this should be called after hardware_init if you wish to enable the dmp
2012-07-28 02:14:43 -03:00
void AP_InertialSensor_MPU6000 : : dmp_init ( )
{
2012-08-17 03:19:56 -03:00
uint8_t regs [ 4 ] ; // for writing to dmp
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
// ensure we only initialise once
if ( _dmp_initialised ) {
return ;
}
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
// load initial values into DMP memory
dmp_load_mem ( ) ;
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
dmp_set_gyro_calibration ( ) ;
dmp_set_accel_calibration ( ) ;
dmp_apply_endian_accel ( ) ;
dmp_set_mpu_sensors ( ) ;
dmp_set_bias_none ( ) ;
dmp_set_fifo_interrupt ( ) ;
dmp_send_quaternion ( ) ; // By default we only send the quaternion to the FIFO (18 bytes packet size)
dmp_set_fifo_rate ( MPU6000_200HZ ) ; // 200Hz DMP output rate
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
register_write ( MPUREG_INT_ENABLE , BIT_RAW_RDY_EN | BIT_DMP_INT_EN ) ; // configure interrupts to fire only when new data arrives from DMP (in fifo buffer)
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
// Randy: no idea what this does
register_write ( MPUREG_DMP_CFG_1 , 0x03 ) ; //MPUREG_DMP_CFG_1, 0x03
register_write ( MPUREG_DMP_CFG_2 , 0x00 ) ; //MPUREG_DMP_CFG_2, 0x00
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
//inv_state_change_fifo
regs [ 0 ] = 0xFF ;
regs [ 1 ] = 0xFF ;
dmp_register_write ( 0x01 , 0xB2 , 0x02 , regs ) ; // D_1_178
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
// ?? FIFO ??
regs [ 0 ] = 0x09 ;
regs [ 1 ] = 0x23 ;
regs [ 2 ] = 0xA1 ;
regs [ 3 ] = 0x35 ;
dmp_register_write ( 0x01 , 0x90 , 0x04 , regs ) ; // D_1_144
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
//register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_RESET); //MPUREG_USER_CTRL, BIT_FIFO_RST
FIFO_reset ( ) ;
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
FIFO_ready ( ) ;
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
//register_write(MPUREG_USER_CTRL, 0x00); // MPUREG_USER_CTRL, 0. TO-DO: is all this setting of USER_CTRL really necessary?
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
register_write ( MPUREG_USER_CTRL , BIT_USER_CTRL_FIFO_RESET ) ; //MPUREG_USER_CTRL, BIT_FIFO_RST. TO-DO: replace this call with FIFO_reset()?
register_write ( MPUREG_USER_CTRL , 0x00 ) ; // MPUREG_USER_CTRL: 0
register_write ( MPUREG_USER_CTRL , BIT_USER_CTRL_DMP_EN | BIT_USER_CTRL_FIFO_EN | BIT_USER_CTRL_DMP_RESET ) ;
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
// Set the gain of the accel in the sensor fusion
dmp_set_sensor_fusion_accel_gain ( DEFAULT_ACCEL_FUSION_GAIN ) ; // default value
2012-07-28 02:14:43 -03:00
2012-08-17 03:19:56 -03:00
// dmp initialisation complete
_dmp_initialised = true ;
2012-09-28 06:34:23 -03:00
2012-07-28 02:14:43 -03:00
}
// dmp_reset - reset dmp (required for changes in gains or offsets to take effect)
void AP_InertialSensor_MPU6000 : : dmp_reset ( )
{
2012-08-17 03:19:56 -03:00
//uint8_t tmp = register_read(MPUREG_USER_CTRL);
//tmp |= BIT_USER_CTRL_DMP_RESET;
//register_write(MPUREG_USER_CTRL,tmp);
register_write ( MPUREG_USER_CTRL , BIT_USER_CTRL_FIFO_RESET ) ; //MPUREG_USER_CTRL, BIT_FIFO_RST. TO-DO: replace this call with FIFO_reset()?
register_write ( MPUREG_USER_CTRL , 0x00 ) ; // MPUREG_USER_CTRL: 0
register_write ( MPUREG_USER_CTRL , BIT_USER_CTRL_DMP_EN | BIT_USER_CTRL_FIFO_EN | BIT_USER_CTRL_DMP_RESET ) ;
2012-07-28 02:14:43 -03:00
}
// New data packet in FIFO?
bool AP_InertialSensor_MPU6000 : : FIFO_ready ( )
{
2012-08-17 03:19:56 -03:00
_fifoCountH = register_read ( MPUREG_FIFO_COUNTH ) ;
_fifoCountL = register_read ( MPUREG_FIFO_COUNTL ) ;
if ( _fifoCountL = = FIFO_PACKET_SIZE ) {
return 1 ;
}
else {
//We should not reach this point or maybe we have more than one packet (we should manage this!)
FIFO_reset ( ) ;
return 0 ;
}
2012-07-28 02:14:43 -03:00
}
// FIFO_reset - reset/clear FIFO buffer used to capture attitude information from DMP
void AP_InertialSensor_MPU6000 : : FIFO_reset ( )
{
2012-08-17 03:19:56 -03:00
uint8_t temp ;
temp = register_read ( MPUREG_USER_CTRL ) ;
temp = temp | BIT_USER_CTRL_FIFO_RESET ; // FIFO RESET BIT
register_write ( MPUREG_USER_CTRL , temp ) ;
2012-07-28 02:14:43 -03:00
}
// FIFO_getPacket - read an attitude packet from FIFO buffer
// TO-DO: interpret results instead of just dumping into a buffer
void AP_InertialSensor_MPU6000 : : FIFO_getPacket ( )
{
2012-08-17 03:19:56 -03:00
uint8_t i ;
int16_t q_long [ 4 ] ;
uint8_t addr = MPUREG_FIFO_R_W | 0x80 ; // Set most significant bit to indicate a read
2012-09-18 00:56:52 -03:00
uint8_t received_packet [ DMP_FIFO_BUFFER_SIZE ] ; // FIFO packet buffer
2012-10-11 21:27:19 -03:00
_spi - > cs_assert ( ) ;
_spi - > transfer ( addr ) ; // send address we want to read from
2012-08-17 03:19:56 -03:00
for ( i = 0 ; i < _fifoCountL ; i + + ) {
2012-10-11 21:27:19 -03:00
received_packet [ i ] = _spi - > transfer ( 0 ) ; // request value
2012-08-17 03:19:56 -03:00
}
2012-10-11 21:27:19 -03:00
_spi - > cs_release ( ) ;
2012-08-17 03:19:56 -03:00
// we are using 16 bits resolution
2012-09-18 00:56:52 -03:00
q_long [ 0 ] = ( int16_t ) ( ( ( ( uint16_t ) received_packet [ 0 ] ) < < 8 ) + ( ( uint16_t ) received_packet [ 1 ] ) ) ;
q_long [ 1 ] = ( int16_t ) ( ( ( ( uint16_t ) received_packet [ 4 ] ) < < 8 ) + ( ( uint16_t ) received_packet [ 5 ] ) ) ;
q_long [ 2 ] = ( int16_t ) ( ( ( ( uint16_t ) received_packet [ 8 ] ) < < 8 ) + ( ( uint16_t ) received_packet [ 9 ] ) ) ;
q_long [ 3 ] = ( int16_t ) ( ( ( ( uint16_t ) received_packet [ 12 ] ) < < 8 ) + ( ( uint16_t ) received_packet [ 13 ] ) ) ;
2012-08-17 03:19:56 -03:00
// Take care of sign
for ( i = 0 ; i < 4 ; i + + ) {
if ( q_long [ i ] > 32767 ) {
q_long [ i ] - = 65536 ;
}
}
quaternion . q1 = ( ( float ) q_long [ 0 ] ) / 16384.0f ; // convert from fixed point to float
quaternion . q2 = ( ( float ) q_long [ 2 ] ) / 16384.0f ; // convert from fixed point to float
quaternion . q3 = ( ( float ) q_long [ 1 ] ) / 16384.0f ; // convert from fixed point to float
quaternion . q4 = ( ( float ) - q_long [ 3 ] ) / 16384.0f ; // convert from fixed point to float
2012-07-28 02:14:43 -03:00
}
// dmp_set_gyro_calibration - apply default gyro calibration FS=2000dps and default orientation
void AP_InertialSensor_MPU6000 : : dmp_set_gyro_calibration ( )
{
2012-08-17 03:19:56 -03:00
uint8_t regs [ 4 ] ;
regs [ 0 ] = 0x4C ;
regs [ 1 ] = 0xCD ;
regs [ 2 ] = 0x6C ;
dmp_register_write ( 0x03 , 0x7B , 0x03 , regs ) ; //FCFG_1 inv_set_gyro_calibration
regs [ 0 ] = 0x36 ;
regs [ 1 ] = 0x56 ;
regs [ 2 ] = 0x76 ;
dmp_register_write ( 0x03 , 0xAB , 0x03 , regs ) ; //FCFG_3 inv_set_gyro_calibration
regs [ 0 ] = 0x02 ;
regs [ 1 ] = 0xCB ;
regs [ 2 ] = 0x47 ;
regs [ 3 ] = 0xA2 ;
dmp_register_write ( 0x00 , 0x68 , 0x04 , regs ) ; //D_0_104 inv_set_gyro_calibration
regs [ 0 ] = 0x00 ;
regs [ 1 ] = 0x05 ;
regs [ 2 ] = 0x8B ;
regs [ 3 ] = 0xC1 ;
dmp_register_write ( 0x02 , 0x18 , 0x04 , regs ) ; //D_0_24 inv_set_gyro_calibration
2012-07-28 02:14:43 -03:00
}
// dmp_set_accel_calibration - apply default accel calibration scale=8g and default orientation
void AP_InertialSensor_MPU6000 : : dmp_set_accel_calibration ( )
{
2012-08-17 03:19:56 -03:00
uint8_t regs [ 6 ] ;
regs [ 0 ] = 0x00 ;
regs [ 1 ] = 0x00 ;
regs [ 2 ] = 0x00 ;
regs [ 3 ] = 0x00 ;
dmp_register_write ( 0x01 , 0x0C , 0x04 , regs ) ; //D_1_152 inv_set_accel_calibration
regs [ 0 ] = 0x0C ;
regs [ 1 ] = 0xC9 ;
regs [ 2 ] = 0x2C ;
regs [ 3 ] = 0x97 ;
regs [ 4 ] = 0x97 ;
regs [ 5 ] = 0x97 ;
dmp_register_write ( 0x03 , 0x7F , 0x06 , regs ) ; //FCFG_2 inv_set_accel_calibration
regs [ 0 ] = 0x26 ;
regs [ 1 ] = 0x46 ;
regs [ 2 ] = 0x66 ;
dmp_register_write ( 0x03 , 0x89 , 0x03 , regs ) ; //FCFG_7 inv_set_accel_calibration
// accel range, 0x20,0x00 => 2g, 0x10,0x00=>4g regs= (1073741824/accel_scale*65536)
//regs[0]=0x20; // 2g
regs [ 0 ] = 0x08 ; // 8g
regs [ 1 ] = 0x00 ;
dmp_register_write ( 0x00 , 0x6C , 0x02 , regs ) ; //D_0_108 inv_set_accel_calibration
2012-07-28 02:14:43 -03:00
}
// dmp_apply_endian_accel - set byte order of accelerometer values?
void AP_InertialSensor_MPU6000 : : dmp_apply_endian_accel ( )
{
2012-08-17 03:19:56 -03:00
uint8_t regs [ 4 ] ;
regs [ 0 ] = 0x00 ;
regs [ 1 ] = 0x00 ;
regs [ 2 ] = 0x40 ;
regs [ 3 ] = 0x00 ;
dmp_register_write ( 0x01 , 0xEC , 0x04 , regs ) ; //D_1_236 inv_apply_endian_accel
2012-07-28 02:14:43 -03:00
}
// dmp_set_mpu_sensors - to configure for SIX_AXIS output
void AP_InertialSensor_MPU6000 : : dmp_set_mpu_sensors ( )
{
2012-08-17 03:19:56 -03:00
uint8_t regs [ 6 ] ;
regs [ 0 ] = 0x0C ;
regs [ 1 ] = 0xC9 ;
regs [ 2 ] = 0x2C ;
regs [ 3 ] = 0x97 ;
regs [ 4 ] = 0x97 ;
regs [ 5 ] = 0x97 ;
dmp_register_write ( 0x03 , 0x7F , 0x06 , regs ) ; //FCFG_2 inv_set_mpu_sensors(INV_SIX_AXIS_GYRO_ACCEL);
2012-07-28 02:14:43 -03:00
}
// dmp_set_bias_from_no_motion - turn on bias from no motion
void AP_InertialSensor_MPU6000 : : dmp_set_bias_from_no_motion ( )
{
2012-08-17 03:19:56 -03:00
uint8_t regs [ 4 ] ;
regs [ 0 ] = 0x0D ;
regs [ 1 ] = 0x35 ;
regs [ 2 ] = 0x5D ;
dmp_register_write ( 0x04 , 0x02 , 0x03 , regs ) ; //CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
regs [ 0 ] = 0x87 ;
regs [ 1 ] = 0x2D ;
regs [ 2 ] = 0x35 ;
regs [ 3 ] = 0x3D ;
dmp_register_write ( 0x04 , 0x09 , 0x04 , regs ) ; //FCFG_5 inv_set_bias_update( INV_BIAS_FROM_NO_MOTION );
2012-07-28 02:14:43 -03:00
}
// dmp_set_bias_none - turn off internal bias correction (we will use this and we handle the gyro bias correction externally)
void AP_InertialSensor_MPU6000 : : dmp_set_bias_none ( )
{
2012-08-17 03:19:56 -03:00
uint8_t regs [ 4 ] ;
regs [ 0 ] = 0x98 ;
regs [ 1 ] = 0x98 ;
regs [ 2 ] = 0x98 ;
dmp_register_write ( 0x04 , 0x02 , 0x03 , regs ) ; //CFG_MOTION_BIAS inv_turn_off_bias_from_no_motion
regs [ 0 ] = 0x87 ;
regs [ 1 ] = 0x2D ;
regs [ 2 ] = 0x35 ;
regs [ 3 ] = 0x3D ;
dmp_register_write ( 0x04 , 0x09 , 0x04 , regs ) ; //FCFG_5 inv_set_bias_update( INV_BIAS_FROM_NO_MOTION );
2012-07-28 02:14:43 -03:00
}
// dmp_set_fifo_interrupt
void AP_InertialSensor_MPU6000 : : dmp_set_fifo_interrupt ( )
{
2012-08-17 03:19:56 -03:00
uint8_t regs [ 1 ] ;
regs [ 0 ] = 0xFE ;
dmp_register_write ( 0x07 , 0x86 , 0x01 , regs ) ; //CFG_6 inv_set_fifo_interupt
2012-07-28 02:14:43 -03:00
}
// dmp_send_quaternion - send quaternion data to FIFO
void AP_InertialSensor_MPU6000 : : dmp_send_quaternion ( )
{
2012-08-17 03:19:56 -03:00
uint8_t regs [ 5 ] ;
regs [ 0 ] = 0xF1 ;
regs [ 1 ] = 0x20 ;
regs [ 2 ] = 0x28 ;
regs [ 3 ] = 0x30 ;
regs [ 4 ] = 0x38 ;
dmp_register_write ( 0x07 , 0x41 , 0x05 , regs ) ; //CFG_8 inv_send_quaternion
regs [ 0 ] = 0x30 ;
dmp_register_write ( 0x07 , 0x7E , 0x01 , regs ) ; //CFG_16 inv_set_footer
2012-07-28 02:14:43 -03:00
}
// dmp_send_gyro - send gyro data to FIFO
void AP_InertialSensor_MPU6000 : : dmp_send_gyro ( )
{
2012-08-17 03:19:56 -03:00
uint8_t regs [ 4 ] ;
regs [ 0 ] = 0xF1 ;
regs [ 1 ] = 0x28 ;
regs [ 2 ] = 0x30 ;
regs [ 3 ] = 0x38 ;
dmp_register_write ( 0x07 , 0x47 , 0x04 , regs ) ; //CFG_9 inv_send_gyro
2012-07-28 02:14:43 -03:00
}
// dmp_send_accel - send accel data to FIFO
void AP_InertialSensor_MPU6000 : : dmp_send_accel ( )
{
2012-08-17 03:19:56 -03:00
uint8_t regs [ 54 ] ;
regs [ 0 ] = 0xF1 ;
regs [ 1 ] = 0x28 ;
regs [ 2 ] = 0x30 ;
regs [ 3 ] = 0x38 ;
dmp_register_write ( 0x07 , 0x6C , 0x04 , regs ) ; //CFG_12 inv_send_accel
2012-07-28 02:14:43 -03:00
}
// This functions defines the rate at wich attitude data is send to FIFO
// Rate: 0 => SAMPLE_RATE(ex:200Hz), 1=> SAMPLE_RATE/2 (ex:100Hz), 2=> SAMPLE_RATE/3 (ex:66Hz)
// rate constant definitions in MPU6000.h
void AP_InertialSensor_MPU6000 : : dmp_set_fifo_rate ( uint8_t rate )
{
2012-08-17 03:19:56 -03:00
uint8_t regs [ 2 ] ;
regs [ 0 ] = 0x00 ;
regs [ 1 ] = rate ;
dmp_register_write ( 0x02 , 0x16 , 0x02 , regs ) ; //D_0_22 inv_set_fifo_rate
2012-07-28 02:14:43 -03:00
}
// This function defines the weight of the accel on the sensor fusion
// default value is 0x80
// The official invensense name is inv_key_0_96 (??)
void AP_InertialSensor_MPU6000 : : dmp_set_sensor_fusion_accel_gain ( uint8_t gain )
{
2012-08-17 03:19:56 -03:00
//inv_key_0_96
register_write ( MPUREG_BANK_SEL , 0x00 ) ;
register_write ( MPUREG_MEM_START_ADDR , 0x60 ) ;
2012-10-11 21:27:19 -03:00
_spi - > cs_assert ( ) ;
_spi - > transfer ( MPUREG_MEM_R_W ) ;
_spi - > transfer ( 0x00 ) ;
_spi - > transfer ( gain ) ; // Original : 0x80 To test: 0x40, 0x20 (too less)
_spi - > transfer ( 0x00 ) ;
_spi - > transfer ( 0x00 ) ;
_spi - > cs_release ( ) ;
2012-07-28 02:14:43 -03:00
}
// Load initial memory values into DMP memory banks
void AP_InertialSensor_MPU6000 : : dmp_load_mem ( )
{
2012-08-17 03:19:56 -03:00
for ( int i = 0 ; i < 7 ; i + + ) {
register_write ( MPUREG_BANK_SEL , i ) ; //MPUREG_BANK_SEL
for ( uint8_t j = 0 ; j < 16 ; j + + ) {
uint8_t start_addy = j * 0x10 ;
register_write ( MPUREG_MEM_START_ADDR , start_addy ) ;
2012-10-11 21:27:19 -03:00
_spi - > cs_assert ( ) ;
_spi - > transfer ( MPUREG_MEM_R_W ) ;
2012-08-17 03:19:56 -03:00
for ( int k = 0 ; k < 16 ; k + + ) {
uint8_t byteToSend = pgm_read_byte ( ( const prog_char * ) & ( dmpMem [ i ] [ j ] [ k ] ) ) ;
2012-10-11 21:27:19 -03:00
_spi - > transfer ( ( uint8_t ) byteToSend ) ;
2012-08-17 03:19:56 -03:00
}
2012-10-11 21:27:19 -03:00
_spi - > cs_release ( ) ;
2012-08-17 03:19:56 -03:00
}
}
register_write ( MPUREG_BANK_SEL , 7 ) ; //MPUREG_BANK_SEL
for ( uint8_t j = 0 ; j < 8 ; j + + ) {
uint8_t start_addy = j * 0x10 ;
register_write ( MPUREG_MEM_START_ADDR , start_addy ) ;
2012-10-11 21:27:19 -03:00
_spi - > cs_assert ( ) ;
_spi - > transfer ( MPUREG_MEM_R_W ) ;
2012-08-17 03:19:56 -03:00
for ( int k = 0 ; k < 16 ; k + + ) {
uint8_t byteToSend = pgm_read_byte ( ( const prog_char * ) & ( dmpMem [ 7 ] [ j ] [ k ] ) ) ;
2012-10-11 21:27:19 -03:00
_spi - > transfer ( ( uint8_t ) byteToSend ) ;
2012-08-17 03:19:56 -03:00
}
2012-10-11 21:27:19 -03:00
_spi - > cs_release ( ) ;
2012-08-17 03:19:56 -03:00
}
register_write ( MPUREG_MEM_START_ADDR , 0x80 ) ;
2012-10-11 21:27:19 -03:00
_spi - > cs_assert ( ) ;
_spi - > transfer ( MPUREG_MEM_R_W ) ;
2012-08-17 03:19:56 -03:00
for ( int k = 0 ; k < 9 ; k + + ) {
uint8_t byteToSend = pgm_read_byte ( ( const prog_char * ) & ( dmpMem [ 7 ] [ 8 ] [ k ] ) ) ;
2012-10-11 21:27:19 -03:00
_spi - > transfer ( ( uint8_t ) byteToSend ) ;
2012-08-17 03:19:56 -03:00
}
2012-10-11 21:27:19 -03:00
_spi - > cs_release ( ) ;
2012-07-28 02:14:43 -03:00
}
// ========= DMP MEMORY ================================
2012-07-28 04:33:04 -03:00
const uint8_t dmpMem [ 8 ] [ 16 ] [ 16 ] PROGMEM = {
2012-07-28 02:14:43 -03:00
{
2012-08-17 03:19:56 -03:00
{
0xFB , 0x00 , 0x00 , 0x3E , 0x00 , 0x0B , 0x00 , 0x36 , 0x00 , 0x01 , 0x00 , 0x02 , 0x00 , 0x03 , 0x00 , 0x00
}
,
{
0x00 , 0x65 , 0x00 , 0x54 , 0xFF , 0xEF , 0x00 , 0x00 , 0xFA , 0x80 , 0x00 , 0x0B , 0x12 , 0x82 , 0x00 , 0x01
}
,
{
0x00 , 0x02 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x28 , 0x00 , 0x00 , 0xFF , 0xFF , 0x45 , 0x81 , 0xFF , 0xFF , 0xFA , 0x72 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x03 , 0xE8 , 0x00 , 0x00 , 0x00 , 0x01 , 0x00 , 0x01 , 0x7F , 0xFF , 0xFF , 0xFE , 0x80 , 0x01
}
,
{
0x00 , 0x1B , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x3E , 0x03 , 0x30 , 0x40 , 0x00 , 0x00 , 0x00 , 0x02 , 0xCA , 0xE3 , 0x09 , 0x3E , 0x80 , 0x00 , 0x00
}
,
{
0x20 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x40 , 0x00 , 0x00 , 0x00 , 0x60 , 0x00 , 0x00 , 0x00
}
,
{
0x41 , 0xFF , 0x00 , 0x00 , 0x00 , 0x00 , 0x0B , 0x2A , 0x00 , 0x00 , 0x16 , 0x55 , 0x00 , 0x00 , 0x21 , 0x82
}
,
{
0xFD , 0x87 , 0x26 , 0x50 , 0xFD , 0x80 , 0x00 , 0x00 , 0x00 , 0x1F , 0x00 , 0x00 , 0x00 , 0x05 , 0x80 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0x02 , 0x00 , 0x00 , 0x00 , 0x03 , 0x00 , 0x00
}
,
{
0x40 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x04 , 0x6F , 0x00 , 0x02 , 0x65 , 0x32 , 0x00 , 0x00 , 0x5E , 0xC0
}
,
{
0x40 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0xFB , 0x8C , 0x6F , 0x5D , 0xFD , 0x5D , 0x08 , 0xD9 , 0x00 , 0x7C , 0x73 , 0x3B , 0x00 , 0x6C , 0x12 , 0xCC
}
,
{
0x32 , 0x00 , 0x13 , 0x9D , 0x32 , 0x00 , 0xD0 , 0xD6 , 0x32 , 0x00 , 0x08 , 0x00 , 0x40 , 0x00 , 0x01 , 0xF4
}
,
{
0xFF , 0xE6 , 0x80 , 0x79 , 0x02 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0xD0 , 0xD6 , 0x00 , 0x00 , 0x27 , 0x10
}
}
2012-07-28 02:14:43 -03:00
,
{
2012-08-17 03:19:56 -03:00
{
0xFB , 0x00 , 0x00 , 0x00 , 0x40 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x01 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0xFA , 0x36 , 0xFF , 0xBC , 0x30 , 0x8E , 0x00 , 0x05 , 0xFB , 0xF0 , 0xFF , 0xD9 , 0x5B , 0xC8
}
,
{
0xFF , 0xD0 , 0x9A , 0xBE , 0x00 , 0x00 , 0x10 , 0xA9 , 0xFF , 0xF4 , 0x1E , 0xB2 , 0x00 , 0xCE , 0xBB , 0xF7
}
,
{
0x00 , 0x00 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0x04 , 0x00 , 0x02 , 0x00 , 0x02 , 0x02 , 0x00 , 0x00 , 0x0C
}
,
{
0xFF , 0xC2 , 0x80 , 0x00 , 0x00 , 0x01 , 0x80 , 0x00 , 0x00 , 0xCF , 0x80 , 0x00 , 0x40 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x06 , 0x00 , 0x00 , 0x00 , 0x00 , 0x14
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x03 , 0x3F , 0x68 , 0xB6 , 0x79 , 0x35 , 0x28 , 0xBC , 0xC6 , 0x7E , 0xD1 , 0x6C
}
,
{
0x80 , 0x00 , 0x00 , 0x00 , 0x40 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0xB2 , 0x6A , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x3F , 0xF0 , 0x00 , 0x00 , 0x00 , 0x30
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x25 , 0x4D , 0x00 , 0x2F , 0x70 , 0x6D , 0x00 , 0x00 , 0x05 , 0xAE , 0x00 , 0x0C , 0x02 , 0xD0
}
}
2012-07-28 02:14:43 -03:00
,
{
2012-08-17 03:19:56 -03:00
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x65 , 0x00 , 0x54 , 0xFF , 0xEF , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x01 , 0x00 , 0x00 , 0x44 , 0x00 , 0x00 , 0x00 , 0x00 , 0x0C , 0x00 , 0x00 , 0x00 , 0x01 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x65 , 0x00 , 0x00 , 0x00 , 0x54 , 0x00 , 0x00 , 0xFF , 0xEF , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x40 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x40 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0x02 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x1B , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x40 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x1B , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
,
{
0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00
}
}
2012-07-28 02:14:43 -03:00
,
{
2012-08-17 03:19:56 -03:00
{
0xD8 , 0xDC , 0xBA , 0xA2 , 0xF1 , 0xDE , 0xB2 , 0xB8 , 0xB4 , 0xA8 , 0x81 , 0x91 , 0xF7 , 0x4A , 0x90 , 0x7F
}
,
{
0x91 , 0x6A , 0xF3 , 0xF9 , 0xDB , 0xA8 , 0xF9 , 0xB0 , 0xBA , 0xA0 , 0x80 , 0xF2 , 0xCE , 0x81 , 0xF3 , 0xC2
}
,
{
0xF1 , 0xC1 , 0xF2 , 0xC3 , 0xF3 , 0xCC , 0xA2 , 0xB2 , 0x80 , 0xF1 , 0xC6 , 0xD8 , 0x80 , 0xBA , 0xA7 , 0xDF
}
,
{
0xDF , 0xDF , 0xF2 , 0xA7 , 0xC3 , 0xCB , 0xC5 , 0xB6 , 0xF0 , 0x87 , 0xA2 , 0x94 , 0x24 , 0x48 , 0x70 , 0x3C
}
,
{
0x95 , 0x40 , 0x68 , 0x34 , 0x58 , 0x9B , 0x78 , 0xA2 , 0xF1 , 0x83 , 0x92 , 0x2D , 0x55 , 0x7D , 0xD8 , 0xB1
}
,
{
0xB4 , 0xB8 , 0xA1 , 0xD0 , 0x91 , 0x80 , 0xF2 , 0x70 , 0xF3 , 0x70 , 0xF2 , 0x7C , 0x80 , 0xA8 , 0xF1 , 0x01
}
,
{
0xB0 , 0x98 , 0x87 , 0xD9 , 0x43 , 0xD8 , 0x86 , 0xC9 , 0x88 , 0xBA , 0xA1 , 0xF2 , 0x0E , 0xB8 , 0x97 , 0x80
}
,
{
0xF1 , 0xA9 , 0xDF , 0xDF , 0xDF , 0xAA , 0xDF , 0xDF , 0xDF , 0xF2 , 0xAA , 0xC5 , 0xCD , 0xC7 , 0xA9 , 0x0C
}
,
{
0xC9 , 0x2C , 0x97 , 0x97 , 0x97 , 0x97 , 0xF1 , 0xA9 , 0x89 , 0x26 , 0x46 , 0x66 , 0xB0 , 0xB4 , 0xBA , 0x80
}
,
{
0xAC , 0xDE , 0xF2 , 0xCA , 0xF1 , 0xB2 , 0x8C , 0x02 , 0xA9 , 0xB6 , 0x98 , 0x00 , 0x89 , 0x0E , 0x16 , 0x1E
}
,
{
0xB8 , 0xA9 , 0xB4 , 0x99 , 0x2C , 0x54 , 0x7C , 0xB0 , 0x8A , 0xA8 , 0x96 , 0x36 , 0x56 , 0x76 , 0xF1 , 0xB9
}
,
{
0xAF , 0xB4 , 0xB0 , 0x83 , 0xC0 , 0xB8 , 0xA8 , 0x97 , 0x11 , 0xB1 , 0x8F , 0x98 , 0xB9 , 0xAF , 0xF0 , 0x24
}
,
{
0x08 , 0x44 , 0x10 , 0x64 , 0x18 , 0xF1 , 0xA3 , 0x29 , 0x55 , 0x7D , 0xAF , 0x83 , 0xB5 , 0x93 , 0xAF , 0xF0
}
,
{
0x00 , 0x28 , 0x50 , 0xF1 , 0xA3 , 0x86 , 0x9F , 0x61 , 0xA6 , 0xDA , 0xDE , 0xDF , 0xD9 , 0xFA , 0xA3 , 0x86
}
,
{
0x96 , 0xDB , 0x31 , 0xA6 , 0xD9 , 0xF8 , 0xDF , 0xBA , 0xA6 , 0x8F , 0xC2 , 0xC5 , 0xC7 , 0xB2 , 0x8C , 0xC1
}
,
{
0xB8 , 0xA2 , 0xDF , 0xDF , 0xDF , 0xA3 , 0xDF , 0xDF , 0xDF , 0xD8 , 0xD8 , 0xF1 , 0xB8 , 0xA8 , 0xB2 , 0x86
}
}
2012-07-28 02:14:43 -03:00
,
{
2012-08-17 03:19:56 -03:00
{
0xB4 , 0x98 , 0x0D , 0x35 , 0x5D , 0xB8 , 0xAA , 0x98 , 0xB0 , 0x87 , 0x2D , 0x35 , 0x3D , 0xB2 , 0xB6 , 0xBA
}
,
{
0xAF , 0x8C , 0x96 , 0x19 , 0x8F , 0x9F , 0xA7 , 0x0E , 0x16 , 0x1E , 0xB4 , 0x9A , 0xB8 , 0xAA , 0x87 , 0x2C
}
,
{
0x54 , 0x7C , 0xB9 , 0xA3 , 0xDE , 0xDF , 0xDF , 0xA3 , 0xB1 , 0x80 , 0xF2 , 0xC4 , 0xCD , 0xC9 , 0xF1 , 0xB8
}
,
{
0xA9 , 0xB4 , 0x99 , 0x83 , 0x0D , 0x35 , 0x5D , 0x89 , 0xB9 , 0xA3 , 0x2D , 0x55 , 0x7D , 0xB5 , 0x93 , 0xA3
}
,
{
0x0E , 0x16 , 0x1E , 0xA9 , 0x2C , 0x54 , 0x7C , 0xB8 , 0xB4 , 0xB0 , 0xF1 , 0x97 , 0x83 , 0xA8 , 0x11 , 0x84
}
,
{
0xA5 , 0x09 , 0x98 , 0xA3 , 0x83 , 0xF0 , 0xDA , 0x24 , 0x08 , 0x44 , 0x10 , 0x64 , 0x18 , 0xD8 , 0xF1 , 0xA5
}
,
{
0x29 , 0x55 , 0x7D , 0xA5 , 0x85 , 0x95 , 0x02 , 0x1A , 0x2E , 0x3A , 0x56 , 0x5A , 0x40 , 0x48 , 0xF9 , 0xF3
}
,
{
0xA3 , 0xD9 , 0xF8 , 0xF0 , 0x98 , 0x83 , 0x24 , 0x08 , 0x44 , 0x10 , 0x64 , 0x18 , 0x97 , 0x82 , 0xA8 , 0xF1
}
,
{
0x11 , 0xF0 , 0x98 , 0xA2 , 0x24 , 0x08 , 0x44 , 0x10 , 0x64 , 0x18 , 0xDA , 0xF3 , 0xDE , 0xD8 , 0x83 , 0xA5
}
,
{
0x94 , 0x01 , 0xD9 , 0xA3 , 0x02 , 0xF1 , 0xA2 , 0xC3 , 0xC5 , 0xC7 , 0xD8 , 0xF1 , 0x84 , 0x92 , 0xA2 , 0x4D
}
,
{
0xDA , 0x2A , 0xD8 , 0x48 , 0x69 , 0xD9 , 0x2A , 0xD8 , 0x68 , 0x55 , 0xDA , 0x32 , 0xD8 , 0x50 , 0x71 , 0xD9
}
,
{
0x32 , 0xD8 , 0x70 , 0x5D , 0xDA , 0x3A , 0xD8 , 0x58 , 0x79 , 0xD9 , 0x3A , 0xD8 , 0x78 , 0x93 , 0xA3 , 0x4D
}
,
{
0xDA , 0x2A , 0xD8 , 0x48 , 0x69 , 0xD9 , 0x2A , 0xD8 , 0x68 , 0x55 , 0xDA , 0x32 , 0xD8 , 0x50 , 0x71 , 0xD9
}
,
{
0x32 , 0xD8 , 0x70 , 0x5D , 0xDA , 0x3A , 0xD8 , 0x58 , 0x79 , 0xD9 , 0x3A , 0xD8 , 0x78 , 0xA8 , 0x8A , 0x9A
}
,
{
0xF0 , 0x28 , 0x50 , 0x78 , 0x9E , 0xF3 , 0x88 , 0x18 , 0xF1 , 0x9F , 0x1D , 0x98 , 0xA8 , 0xD9 , 0x08 , 0xD8
}
,
{
0xC8 , 0x9F , 0x12 , 0x9E , 0xF3 , 0x15 , 0xA8 , 0xDA , 0x12 , 0x10 , 0xD8 , 0xF1 , 0xAF , 0xC8 , 0x97 , 0x87
}
}
2012-07-28 02:14:43 -03:00
,
{
2012-08-17 03:19:56 -03:00
{
0x34 , 0xB5 , 0xB9 , 0x94 , 0xA4 , 0x21 , 0xF3 , 0xD9 , 0x22 , 0xD8 , 0xF2 , 0x2D , 0xF3 , 0xD9 , 0x2A , 0xD8
}
,
{
0xF2 , 0x35 , 0xF3 , 0xD9 , 0x32 , 0xD8 , 0x81 , 0xA4 , 0x60 , 0x60 , 0x61 , 0xD9 , 0x61 , 0xD8 , 0x6C , 0x68
}
,
{
0x69 , 0xD9 , 0x69 , 0xD8 , 0x74 , 0x70 , 0x71 , 0xD9 , 0x71 , 0xD8 , 0xB1 , 0xA3 , 0x84 , 0x19 , 0x3D , 0x5D
}
,
{
0xA3 , 0x83 , 0x1A , 0x3E , 0x5E , 0x93 , 0x10 , 0x30 , 0x81 , 0x10 , 0x11 , 0xB8 , 0xB0 , 0xAF , 0x8F , 0x94
}
,
{
0xF2 , 0xDA , 0x3E , 0xD8 , 0xB4 , 0x9A , 0xA8 , 0x87 , 0x29 , 0xDA , 0xF8 , 0xD8 , 0x87 , 0x9A , 0x35 , 0xDA
}
,
{
0xF8 , 0xD8 , 0x87 , 0x9A , 0x3D , 0xDA , 0xF8 , 0xD8 , 0xB1 , 0xB9 , 0xA4 , 0x98 , 0x85 , 0x02 , 0x2E , 0x56
}
,
{
0xA5 , 0x81 , 0x00 , 0x0C , 0x14 , 0xA3 , 0x97 , 0xB0 , 0x8A , 0xF1 , 0x2D , 0xD9 , 0x28 , 0xD8 , 0x4D , 0xD9
}
,
{
0x48 , 0xD8 , 0x6D , 0xD9 , 0x68 , 0xD8 , 0xB1 , 0x84 , 0x0D , 0xDA , 0x0E , 0xD8 , 0xA3 , 0x29 , 0x83 , 0xDA
}
,
{
0x2C , 0x0E , 0xD8 , 0xA3 , 0x84 , 0x49 , 0x83 , 0xDA , 0x2C , 0x4C , 0x0E , 0xD8 , 0xB8 , 0xB0 , 0xA8 , 0x8A
}
,
{
0x9A , 0xF5 , 0x20 , 0xAA , 0xDA , 0xDF , 0xD8 , 0xA8 , 0x40 , 0xAA , 0xD0 , 0xDA , 0xDE , 0xD8 , 0xA8 , 0x60
}
,
{
0xAA , 0xDA , 0xD0 , 0xDF , 0xD8 , 0xF1 , 0x97 , 0x86 , 0xA8 , 0x31 , 0x9B , 0x06 , 0x99 , 0x07 , 0xAB , 0x97
}
,
{
0x28 , 0x88 , 0x9B , 0xF0 , 0x0C , 0x20 , 0x14 , 0x40 , 0xB8 , 0xB0 , 0xB4 , 0xA8 , 0x8C , 0x9C , 0xF0 , 0x04
}
,
{
0x28 , 0x51 , 0x79 , 0x1D , 0x30 , 0x14 , 0x38 , 0xB2 , 0x82 , 0xAB , 0xD0 , 0x98 , 0x2C , 0x50 , 0x50 , 0x78
}
,
{
0x78 , 0x9B , 0xF1 , 0x1A , 0xB0 , 0xF0 , 0x8A , 0x9C , 0xA8 , 0x29 , 0x51 , 0x79 , 0x8B , 0x29 , 0x51 , 0x79
}
,
{
0x8A , 0x24 , 0x70 , 0x59 , 0x8B , 0x20 , 0x58 , 0x71 , 0x8A , 0x44 , 0x69 , 0x38 , 0x8B , 0x39 , 0x40 , 0x68
}
,
{
0x8A , 0x64 , 0x48 , 0x31 , 0x8B , 0x30 , 0x49 , 0x60 , 0xA5 , 0x88 , 0x20 , 0x09 , 0x71 , 0x58 , 0x44 , 0x68
}
}
2012-07-28 02:14:43 -03:00
,
{
2012-08-17 03:19:56 -03:00
{
0x11 , 0x39 , 0x64 , 0x49 , 0x30 , 0x19 , 0xF1 , 0xAC , 0x00 , 0x2C , 0x54 , 0x7C , 0xF0 , 0x8C , 0xA8 , 0x04
}
,
{
0x28 , 0x50 , 0x78 , 0xF1 , 0x88 , 0x97 , 0x26 , 0xA8 , 0x59 , 0x98 , 0xAC , 0x8C , 0x02 , 0x26 , 0x46 , 0x66
}
,
{
0xF0 , 0x89 , 0x9C , 0xA8 , 0x29 , 0x51 , 0x79 , 0x24 , 0x70 , 0x59 , 0x44 , 0x69 , 0x38 , 0x64 , 0x48 , 0x31
}
,
{
0xA9 , 0x88 , 0x09 , 0x20 , 0x59 , 0x70 , 0xAB , 0x11 , 0x38 , 0x40 , 0x69 , 0xA8 , 0x19 , 0x31 , 0x48 , 0x60
}
,
{
0x8C , 0xA8 , 0x3C , 0x41 , 0x5C , 0x20 , 0x7C , 0x00 , 0xF1 , 0x87 , 0x98 , 0x19 , 0x86 , 0xA8 , 0x6E , 0x76
}
,
{
0x7E , 0xA9 , 0x99 , 0x88 , 0x2D , 0x55 , 0x7D , 0x9E , 0xB9 , 0xA3 , 0x8A , 0x22 , 0x8A , 0x6E , 0x8A , 0x56
}
,
{
0x8A , 0x5E , 0x9F , 0xB1 , 0x83 , 0x06 , 0x26 , 0x46 , 0x66 , 0x0E , 0x2E , 0x4E , 0x6E , 0x9D , 0xB8 , 0xAD
}
,
{
0x00 , 0x2C , 0x54 , 0x7C , 0xF2 , 0xB1 , 0x8C , 0xB4 , 0x99 , 0xB9 , 0xA3 , 0x2D , 0x55 , 0x7D , 0x81 , 0x91
}
,
{
0xAC , 0x38 , 0xAD , 0x3A , 0xB5 , 0x83 , 0x91 , 0xAC , 0x2D , 0xD9 , 0x28 , 0xD8 , 0x4D , 0xD9 , 0x48 , 0xD8
}
,
{
0x6D , 0xD9 , 0x68 , 0xD8 , 0x8C , 0x9D , 0xAE , 0x29 , 0xD9 , 0x04 , 0xAE , 0xD8 , 0x51 , 0xD9 , 0x04 , 0xAE
}
,
{
0xD8 , 0x79 , 0xD9 , 0x04 , 0xD8 , 0x81 , 0xF3 , 0x9D , 0xAD , 0x00 , 0x8D , 0xAE , 0x19 , 0x81 , 0xAD , 0xD9
}
,
{
0x01 , 0xD8 , 0xF2 , 0xAE , 0xDA , 0x26 , 0xD8 , 0x8E , 0x91 , 0x29 , 0x83 , 0xA7 , 0xD9 , 0xAD , 0xAD , 0xAD
}
,
{
0xAD , 0xF3 , 0x2A , 0xD8 , 0xD8 , 0xF1 , 0xB0 , 0xAC , 0x89 , 0x91 , 0x3E , 0x5E , 0x76 , 0xF3 , 0xAC , 0x2E
}
,
{
0x2E , 0xF1 , 0xB1 , 0x8C , 0x5A , 0x9C , 0xAC , 0x2C , 0x28 , 0x28 , 0x28 , 0x9C , 0xAC , 0x30 , 0x18 , 0xA8
}
,
{
0x98 , 0x81 , 0x28 , 0x34 , 0x3C , 0x97 , 0x24 , 0xA7 , 0x28 , 0x34 , 0x3C , 0x9C , 0x24 , 0xF2 , 0xB0 , 0x89
}
,
{
0xAC , 0x91 , 0x2C , 0x4C , 0x6C , 0x8A , 0x9B , 0x2D , 0xD9 , 0xD8 , 0xD8 , 0x51 , 0xD9 , 0xD8 , 0xD8 , 0x79
}
}
2012-07-28 02:14:43 -03:00
,
{
2012-08-17 03:19:56 -03:00
{
0xD9 , 0xD8 , 0xD8 , 0xF1 , 0x9E , 0x88 , 0xA3 , 0x31 , 0xDA , 0xD8 , 0xD8 , 0x91 , 0x2D , 0xD9 , 0x28 , 0xD8
}
,
{
0x4D , 0xD9 , 0x48 , 0xD8 , 0x6D , 0xD9 , 0x68 , 0xD8 , 0xB1 , 0x83 , 0x93 , 0x35 , 0x3D , 0x80 , 0x25 , 0xDA
}
,
{
0xD8 , 0xD8 , 0x85 , 0x69 , 0xDA , 0xD8 , 0xD8 , 0xB4 , 0x93 , 0x81 , 0xA3 , 0x28 , 0x34 , 0x3C , 0xF3 , 0xAB
}
,
{
0x8B , 0xF8 , 0xA3 , 0x91 , 0xB6 , 0x09 , 0xB4 , 0xD9 , 0xAB , 0xDE , 0xFA , 0xB0 , 0x87 , 0x9C , 0xB9 , 0xA3
}
,
{
0xDD , 0xF1 , 0xA3 , 0xA3 , 0xA3 , 0xA3 , 0x95 , 0xF1 , 0xA3 , 0xA3 , 0xA3 , 0x9D , 0xF1 , 0xA3 , 0xA3 , 0xA3
}
,
{
0xA3 , 0xF2 , 0xA3 , 0xB4 , 0x90 , 0x80 , 0xF2 , 0xA3 , 0xA3 , 0xA3 , 0xA3 , 0xA3 , 0xA3 , 0xA3 , 0xA3 , 0xA3
}
,
{
0xA3 , 0xB2 , 0xA3 , 0xA3 , 0xA3 , 0xA3 , 0xA3 , 0xA3 , 0xB0 , 0x87 , 0xB5 , 0x99 , 0xF1 , 0xA3 , 0xA3 , 0xA3
}
,
{
0x98 , 0xF1 , 0xA3 , 0xA3 , 0xA3 , 0xA3 , 0x97 , 0xA3 , 0xA3 , 0xA3 , 0xA3 , 0xF3 , 0x9B , 0xA3 , 0xA3 , 0xDC
}
,
{
0xB9 , 0xA7 , 0xF1 , 0x26 , 0x26 , 0x26 , 0xD8 , 0xD8 , 0xFF
}
}
2012-07-28 02:14:43 -03:00
} ;