Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-16 05:38:28 -04:00
Code Issues Packages Projects Releases Wiki Activity
fc8068adfe
ardupilot/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
Víctor Mayoral Vilches 7cf1187473 AP_InertialSensor_MPU9150: Clock-based wait_for_sample() impl. 
Adapt the I2C driver to a system clock based wait_for_sample()
implementation.

The sample rate of the sensor has been corrected to 800 Hz (could be
further pushed up to 1KHz).

Filters for the mag have also been created but remain commented until
the code for the mag is ready.
2014-07-14 09:10:23 +10:00

1196 lines
34 KiB
C++
Raw Blame History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
-- Coded by Victor Mayoral Vilches --
The MPU9150 is a sensor composed by a MPU6050 with a AK8975 on the auxiliary bus.
Please check the following links for datasheets and documentation:
- http://www.invensense.com/mems/gyro/documents/PS-MPU-9150A-00v4_3.pdf
- http://www.invensense.com/mems/gyro/documents/RM-MPU-9150A-00v4_2.pdf
*/
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_ERLE
#include <AP_Math.h>
#include "AP_InertialSensor_MPU9150.h"
#include <stdio.h>
#include <unistd.h>
#include <time.h>
const extern AP_HAL::HAL& hal;
///////
/* Hardware registers needed by driver. */
struct gyro_reg_s {
uint8_t who_am_i;
uint8_t rate_div;
uint8_t lpf;
uint8_t prod_id;
uint8_t user_ctrl;
uint8_t fifo_en;
uint8_t gyro_cfg;
uint8_t accel_cfg;
uint8_t motion_thr;
uint8_t motion_dur;
uint8_t fifo_count_h;
uint8_t fifo_r_w;
uint8_t raw_gyro;
uint8_t raw_accel;
uint8_t temp;
uint8_t int_enable;
uint8_t dmp_int_status;
uint8_t int_status;
uint8_t pwr_mgmt_1;
uint8_t pwr_mgmt_2;
uint8_t int_pin_cfg;
uint8_t mem_r_w;
uint8_t accel_offs;
uint8_t i2c_mst;
uint8_t bank_sel;
uint8_t mem_start_addr;
uint8_t prgm_start_h;
uint8_t raw_compass;
uint8_t yg_offs_tc;
uint8_t s0_addr;
uint8_t s0_reg;
uint8_t s0_ctrl;
uint8_t s1_addr;
uint8_t s1_reg;
uint8_t s1_ctrl;
uint8_t s4_ctrl;
uint8_t s0_do;
uint8_t s1_do;
uint8_t i2c_delay_ctrl;
};
/* Information specific to a particular device. */
struct hw_s {
uint8_t addr;
uint16_t max_fifo;
uint8_t num_reg;
uint16_t temp_sens;
int16_t temp_offset;
uint16_t bank_size;
uint16_t compass_fsr;
};
/* Information for self-test. */
struct test_s {
uint32_t gyro_sens;
uint32_t accel_sens;
uint8_t reg_rate_div;
uint8_t reg_lpf;
uint8_t reg_gyro_fsr;
uint8_t reg_accel_fsr;
uint16_t wait_ms;
uint8_t packet_thresh;
float min_dps;
float max_dps;
float max_gyro_var;
float min_g;
float max_g;
float max_accel_var;
};
/* Gyro driver state variables. */
struct gyro_state_s {
const struct gyro_reg_s *reg;
const struct hw_s *hw;
const struct test_s *test;
};
/* Filter configurations. The values correspond to the DLPF_CFG register.
Note that the gyro and accel frequencies are slightly different.
(DLPF_CFG register, RM-MPU-9150A00.pdf, pg. 13)
*/
enum lpf_e {
INV_FILTER_256HZ_NOLPF2 = 0,
INV_FILTER_188HZ = 1,
INV_FILTER_98HZ = 2,
INV_FILTER_42HZ = 3,
INV_FILTER_20HZ = 4,
INV_FILTER_10HZ = 5,
INV_FILTER_5HZ = 6,
INV_FILTER_2100HZ_NOLPF = 7,
NUM_FILTER
};
/* Full scale ranges. FS_SEL register.
(RM-MPU-9150A00.pdf, pg. 14)
*/
enum gyro_fsr_e {
INV_FSR_250DPS = 0,
INV_FSR_500DPS = 1,
INV_FSR_1000DPS = 2,
INV_FSR_2000DPS = 3,
NUM_GYRO_FSR
};
/* Full scale ranges. AFS_SEL register.
(RM-MPU-9150A00.pdf, pg. 15)
*/
enum accel_fsr_e {
INV_FSR_2G = 0,
INV_FSR_4G = 1,
INV_FSR_8G = 2,
INV_FSR_16G = 4,
NUM_ACCEL_FSR
};
/* Clock sources. */
enum clock_sel_e {
INV_CLK_INTERNAL = 0,
INV_CLK_PLL,
NUM_CLK
};
#define MPU9150_ADDRESS 0x68
#define INV_X_GYRO (0x40)
#define INV_Y_GYRO (0x20)
#define INV_Z_GYRO (0x10)
#define INV_XYZ_GYRO (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO)
#define INV_XYZ_ACCEL (0x08)
#define INV_XYZ_COMPASS (0x01)
#define MPU_INT_STATUS_DATA_READY (0x0001)
#define MPU_INT_STATUS_DMP (0x0002)
#define MPU_INT_STATUS_PLL_READY (0x0004)
#define MPU_INT_STATUS_I2C_MST (0x0008)
#define MPU_INT_STATUS_FIFO_OVERFLOW (0x0010)
#define MPU_INT_STATUS_ZMOT (0x0020)
#define MPU_INT_STATUS_MOT (0x0040)
#define MPU_INT_STATUS_FREE_FALL (0x0080)
#define MPU_INT_STATUS_DMP_0 (0x0100)
#define MPU_INT_STATUS_DMP_1 (0x0200)
#define MPU_INT_STATUS_DMP_2 (0x0400)
#define MPU_INT_STATUS_DMP_3 (0x0800)
#define MPU_INT_STATUS_DMP_4 (0x1000)
#define MPU_INT_STATUS_DMP_5 (0x2000)
#define BIT_I2C_MST_VDDIO (0x80)
#define BIT_FIFO_EN (0x40)
#define BIT_DMP_EN (0x80)
#define BIT_FIFO_RST (0x04)
#define BIT_DMP_RST (0x08)
#define BIT_FIFO_OVERFLOW (0x10)
#define BIT_DATA_RDY_EN (0x01)
#define BIT_DMP_INT_EN (0x02)
#define BIT_MOT_INT_EN (0x40)
#define BITS_FSR (0x18)
#define BITS_LPF (0x07)
#define BITS_HPF (0x07)
#define BITS_CLK (0x07)
#define BIT_FIFO_SIZE_1024 (0x40)
#define BIT_FIFO_SIZE_2048 (0x80)
#define BIT_FIFO_SIZE_4096 (0xC0)
#define BIT_RESET (0x80)
#define BIT_SLEEP (0x40)
#define BIT_S0_DELAY_EN (0x01)
#define BIT_S2_DELAY_EN (0x04)
#define BITS_SLAVE_LENGTH (0x0F)
#define BIT_SLAVE_BYTE_SW (0x40)
#define BIT_SLAVE_GROUP (0x10)
#define BIT_SLAVE_EN (0x80)
#define BIT_I2C_READ (0x80)
#define BITS_I2C_MASTER_DLY (0x1F)
#define BIT_AUX_IF_EN (0x20)
#define BIT_ACTL (0x80)
#define BIT_LATCH_EN (0x20)
#define BIT_ANY_RD_CLR (0x10)
#define BIT_BYPASS_EN (0x02)
#define BITS_WOM_EN (0xC0)
#define BIT_LPA_CYCLE (0x20)
#define BIT_STBY_XA (0x20)
#define BIT_STBY_YA (0x10)
#define BIT_STBY_ZA (0x08)
#define BIT_STBY_XG (0x04)
#define BIT_STBY_YG (0x02)
#define BIT_STBY_ZG (0x01)
#define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
#define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
// AK8975_SECONDARY
#define SUPPORTS_AK89xx_HIGH_SENS (0x00)
#define AK89xx_FSR (9830)
#define MAX_COMPASS_SAMPLE_RATE (100)
// Gyroscope scale (uncertain where the 0.01745 value comes from)
#define MPU9150_GYRO_SCALE_2000 (0.0174532f / 16.4f)
#define MPU9150_GYRO_SCALE_1000 (0.0174532f / 32.8f)
#define MPU9150_GYRO_SCALE_500 (0.0174532f / 65.5f)
#define MPU9150_GYRO_SCALE_250 (0.0174532f / 131f)
// Accelerometer scale adjustment
#define MPU9150_ACCEL_SCALE_16G (GRAVITY_MSS / 2048.0f)
#define MPU9150_ACCEL_SCALE_8G (GRAVITY_MSS / 4096.0f)
#define MPU9150_ACCEL_SCALE_4G (GRAVITY_MSS / 8192.0f)
#define MPU9150_ACCEL_SCALE_2G (GRAVITY_MSS / 16384.0f)
const struct gyro_reg_s reg = {
/* .who_am_i */ 0x75,
/* .rate_div */ 0x19,
/* .lpf */ 0x1A,
/* .prod_id */ 0x0C,
/* .user_ctrl */ 0x6A,
/* .fifo_en */ 0x23,
/* .gyro_cfg */ 0x1B,
/* .accel_cfg */ 0x1C,
/* .motion_thr */ 0x1F,
/* .motion_dur */ 0x20,
/* .fifo_count_h */ 0x72,
/* .fifo_r_w */ 0x74,
/* .raw_gyro */ 0x43,
/* .raw_accel */ 0x3B,
/* .temp */ 0x41,
/* .int_enable */ 0x38,
/* .dmp_int_status*/ 0x39,
/* .int_status */ 0x3A,
/* .pwr_mgmt_1 */ 0x6B,
/* .pwr_mgmt_2 */ 0x6C,
/* .int_pin_cfg */ 0x37,
/* .mem_r_w */ 0x6F,
/* .accel_offs */ 0x06,
/* .i2c_mst */ 0x24,
/* .bank_sel */ 0x6D,
/* .mem_start_addr*/ 0x6E,
/* .prgm_start_h */ 0x70,
/* .raw_compass */ 0x49,
/* .yg_offs_tc */ 0x01,
/* .s0_addr */ 0x25,
/* .s0_reg */ 0x26,
/* .s0_ctrl */ 0x27,
/* .s1_addr */ 0x28,
/* .s1_reg */ 0x29,
/* .s1_ctrl */ 0x2A,
/* .s4_ctrl */ 0x34,
/* .s0_do */ 0x63,
/* .s1_do */ 0x64,
/* .i2c_delay_ctrl*/ 0x67
};
const struct hw_s hw = {
/* .addr */ 0x68,
/* .max_fifo */ 1024,
/* .num_reg */ 118,
/* .temp_sens */ 340,
/* .temp_offset */ -521,
/* .bank_size */ 256,
/* .compass_fsr */ AK89xx_FSR
};
const struct test_s test = {
/* .gyro_sens */ 32768/250,
/* .accel_sens */ 32768/16,
/* .reg_rate_div */ 0, /* 1kHz. */
/* .reg_lpf */ 1, /* 188Hz. */
/* .reg_gyro_fsr */ 0, /* 250dps. */
/* .reg_accel_fsr */ 0x18, /* 16g. */
/* .wait_ms */ 50,
/* .packet_thresh */ 5, /* 5% */
/* .min_dps */ 10.f,
/* .max_dps */ 105.f,
/* .max_gyro_var */ 0.14f,
/* .min_g */ 0.3f,
/* .max_g */ 0.95f,
/* .max_accel_var */ 0.14f
};
static struct gyro_state_s st = {
/* .reg */ &reg,
/* .hw */ &hw,
/* .test */ &test
};
/**
* @brief Constructor
*/
AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150() :
AP_InertialSensor(),
_accel_filter_x(800, 10),
_accel_filter_y(800, 10),
_accel_filter_z(800, 10),
_gyro_filter_x(800, 10),
_gyro_filter_y(800, 10),
_gyro_filter_z(800, 10)
// _mag_filter_x(800, 10),
// _mag_filter_y(800, 10),
// _mag_filter_z(800, 10)
{
}
/*
set the filter frequency
*/
void AP_InertialSensor_MPU9150::_set_filter_frequency(uint8_t filter_hz)
{
if (filter_hz == 0)
filter_hz = _default_filter_hz;
_accel_filter_x.set_cutoff_frequency(800, filter_hz);
_accel_filter_y.set_cutoff_frequency(800, filter_hz);
_accel_filter_z.set_cutoff_frequency(800, filter_hz);
_gyro_filter_x.set_cutoff_frequency(800, filter_hz);
_gyro_filter_y.set_cutoff_frequency(800, filter_hz);
_gyro_filter_z.set_cutoff_frequency(800, filter_hz);
}
/**
* @brief Init method
* @param[in] Sample_rate The sample rate, check the struct def.
* @return AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE if successful.
*/
uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate )
{
// Sensors pushed to the FIFO.
uint8_t sensors;
switch (sample_rate) {
case RATE_50HZ:
_default_filter_hz = 10;
_sample_period_usec = (1000*1000) / 50;
break;
case RATE_100HZ:
_default_filter_hz = 20;
_sample_period_usec = (1000*1000) / 100;
break;
case RATE_200HZ:
_default_filter_hz = 20;
_sample_period_usec = 5000;
break;
case RATE_400HZ:
default:
_default_filter_hz = 20;
_sample_period_usec = 2500;
break;
}
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// take i2c bus sempahore
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)){
return -1;
}
// Init the sensor
// Reset the device
hal.i2c->writeRegister(st.hw->addr,
st.reg->pwr_mgmt_1,
BIT_RESET);
hal.scheduler->delay(100);
// Wake up the chip
hal.i2c->writeRegister(st.hw->addr,
st.reg->pwr_mgmt_1,
0x00);
// Check product revision
// This registers are not documented in the register map.
uint8_t buff[6];
if (hal.i2c->readRegisters(st.hw->addr, st.reg->accel_offs, 6, buff) != 0) {
hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: couldn't read the registers to determine revision"));
goto failed;
}
uint8_t rev;
rev = ((buff[5] & 0x01) << 2) | ((buff[3] & 0x01) << 1) |
(buff[1] & 0x01);
// Do not do the checking, for some reason the MPU-9150 returns 0
// if (rev) {
// if (rev == 1){
// /* Congrats, these parts are better. */
// ;
// }
// else if (rev == 2){
// ;
// }
// else {
// hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: Unsupported software product rev.\n"));
// goto failed;
// }
// } else {
// hal.scheduler->panic(PSTR("Product ID read as 0 indicates device is either incompatible or an MPU3050.\n"));
// goto failed;
// }
// Set gyro full-scale range [250, 500, 1000, 2000]
if (mpu_set_gyro_fsr(2000)){
hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_gyro_fsr.\n"));
goto failed;
}
// Set the accel full-scale range
if (mpu_set_accel_fsr(2)){
hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_accel_fsr.\n"));
goto failed;
}
// Set digital low pass filter (using _default_filter_hz, 20 for 100 Hz of sample rate)
if (mpu_set_lpf(_default_filter_hz)){
hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_lpf.\n"));
goto failed;
}
// Set sampling rate (value must be between 4Hz and 1KHz)
if (mpu_set_sample_rate(800)){
hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_sample_rate.\n"));
goto failed;
}
// Select which sensors are pushed to FIFO.
sensors = INV_XYZ_ACCEL| INV_XYZ_GYRO;
if (mpu_configure_fifo(sensors)){
hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_configure_fifo.\n"));
goto failed;
}
// For now the compass is not used.
// TODO adjust the functions to the ArduPilot API
// setup_compass();
// if (mpu_set_compass_sample_rate(10, 400))
// return -1;
mpu_set_sensors(sensors);
// Set the filter frecuency (_mpu6000_filter configured to the default value, check AP_InertialSensor.cpp)
_set_filter_frequency(_mpu6000_filter);
// give back i2c semaphore
i2c_sem->give();
// start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9150::_accumulate));
return AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE;
failed:
// give back i2c semaphore
i2c_sem->give();
return -1;
}
/**
* @brief Set the gyro full-scale range.
* @param[in] fsr Desired full-scale range.
* @return 0 if successful.
*/
int16_t AP_InertialSensor_MPU9150::mpu_set_gyro_fsr(uint16_t fsr)
{
uint8_t data;
switch (fsr) {
case 250:
data = INV_FSR_250DPS << 3;
break;
case 500:
data = INV_FSR_500DPS << 3;
break;
case 1000:
data = INV_FSR_1000DPS << 3;
break;
case 2000:
data = INV_FSR_2000DPS << 3;
break;
default:
return -1;
}
hal.i2c->writeRegister(st.hw->addr,
st.reg->gyro_cfg,
data);
return 0;
}
/**
* @brief Set the accel full-scale range.
* @param[in] fsr Desired full-scale range.
* @return 0 if successful.
*/
int16_t AP_InertialSensor_MPU9150::mpu_set_accel_fsr(uint8_t fsr)
{
uint8_t data;
switch (fsr) {
case 2:
data = INV_FSR_2G << 3;
break;
case 4:
data = INV_FSR_4G << 3;
break;
case 8:
data = INV_FSR_8G << 3;
break;
case 16:
data = INV_FSR_16G << 3;
break;
default:
return -1;
}
hal.i2c->writeRegister(st.hw->addr,
st.reg->accel_cfg,
data);
return 0;
}
/**
* @brief Set digital low pass filter.
* The following LPF settings are supported: 188, 98, 42, 20, 10, 5.
* @param[in] lpf Desired LPF setting.
* @return 0 if successful.
*/
int16_t AP_InertialSensor_MPU9150::mpu_set_lpf(uint16_t lpf)
{
uint8_t data;
if (lpf >= 188){
data = INV_FILTER_188HZ;
}
else if (lpf >= 98){
data = INV_FILTER_98HZ;
}
else if (lpf >= 42){
data = INV_FILTER_42HZ;
}
else if (lpf >= 20){
data = INV_FILTER_20HZ;
}
else if (lpf >= 10){
data = INV_FILTER_10HZ;
}
else {
data = INV_FILTER_5HZ;
}
hal.i2c->writeRegister(st.hw->addr,
st.reg->lpf,
data);
return 0;
}
/**
* @brief Set sampling rate.
* Sampling rate must be between 4Hz and 1kHz.
* @param[in] rate Desired sampling rate (Hz).
* @return 0 if successful.
*/
int16_t AP_InertialSensor_MPU9150::mpu_set_sample_rate(uint16_t rate)
{
uint8_t data;
// uint16_t sample_rate;
if (rate < 4){
rate = 4;
}
else if (rate > 1000){
rate = 1000;
}
data = 1000 / rate - 1;
hal.i2c->writeRegister(st.hw->addr,
st.reg->rate_div,
data);
// sample_rate = 1000 / (1 + data);
// mpu_set_compass_sample_rate(min(sample_rate, MAX_COMPASS_SAMPLE_RATE), rate);
return 0;
}
/**
* @brief Set compass sampling rate.
* The compass on the auxiliary I2C bus is read by the MPU hardware at a
* maximum of 100Hz. The actual rate can be set to a fraction of the gyro
* sampling rate.
*
* \n WARNING: The new rate may be different than what was requested. Call
* mpu_get_compass_sample_rate to check the actual setting.
* @param[in] rate Desired compass sampling rate (Hz).
* @return 0 if successful.
*/
int16_t AP_InertialSensor_MPU9150::mpu_set_compass_sample_rate(uint16_t rate, uint16_t chip_sample_rate)
{
uint8_t div;
if (!rate || rate > MAX_COMPASS_SAMPLE_RATE){
return -1;
}
div = chip_sample_rate / rate - 1;
hal.i2c->writeRegister(st.hw->addr,
st.reg->s4_ctrl,
div);
return 0;
}
/**
* @brief Select which sensors are pushed to FIFO.
* @e sensors can contain a combination of the following flags:
* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
* \n INV_XYZ_GYRO
* \n INV_XYZ_ACCEL
* @param[in] sensors Mask of sensors to push to FIFO.
* @return 0 if successful.
*/
int16_t AP_InertialSensor_MPU9150::mpu_configure_fifo(uint8_t sensors)
{
int16_t result = 0;
/* Compass data isn't going into the FIFO. Stop trying. */
sensors &= ~INV_XYZ_COMPASS;
// Enable or disable the interrupts
// set_int_enable(1);
set_int_enable(0);
if (sensors) {
if (mpu_reset_fifo(sensors)) {
return -1;
}
}
return result;
}
/**
* @brief Enable/disable data ready interrupt.
* If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready
* interrupt is used.
* @param[in] enable 1 to enable interrupt.
* @return 0 if successful.
*/
int16_t AP_InertialSensor_MPU9150::set_int_enable(uint8_t enable)
{
uint8_t tmp;
if (enable){
tmp = BIT_DATA_RDY_EN;
}
else {
tmp = 0x00;
}
hal.i2c->writeRegister(st.hw->addr,
st.reg->int_enable,
tmp);
return 0;
}
/**
* @brief Reset FIFO read/write pointers.
* @return 0 if successful.
*/
int16_t AP_InertialSensor_MPU9150::mpu_reset_fifo(uint8_t sensors)
{
uint8_t data;
data = 0;
hal.i2c->writeRegister(st.hw->addr,st.reg->int_enable, data);
hal.i2c->writeRegister(st.hw->addr,st.reg->fifo_en, data);
hal.i2c->writeRegister(st.hw->addr,st.reg->user_ctrl, data);
data = BIT_FIFO_RST;
hal.i2c->writeRegister(st.hw->addr,st.reg->user_ctrl, data);
data = BIT_FIFO_EN;
// data = BIT_FIFO_EN | BIT_AUX_IF_EN;
hal.i2c->writeRegister(st.hw->addr,st.reg->user_ctrl, data);
hal.scheduler->delay(50);
// interrupts for the DMP
// data = BIT_DATA_RDY_EN;
data = 0;
hal.i2c->writeRegister(st.hw->addr,st.reg->int_enable, data);
// enable FIFO
hal.i2c->writeRegister(st.hw->addr,st.reg->fifo_en, sensors);
return 0;
}
#if 0
/* This initialization is similar to the one in ak8975.c.
TODO: Use the ArduPilot APIs (write, read, ...), remove the st.chip_cfg cache vars
*/
static int AP_InertialSensor_MPU9150::setup_compass(void)
{
uint8_t data[4], akm_addr;
mpu_set_bypass(1);
/* Find compass. Possible addresses range from 0x0C to 0x0F. */
for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
int result;
result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data);
if (!result && (data[0] == AKM_WHOAMI))
break;
}
if (akm_addr > 0x0F) {
/* TODO: Handle this case in all compass-related functions. */
log_e("Compass not found.\n");
return -1;
}
st.chip_cfg.compass_addr = akm_addr;
data[0] = AKM_POWER_DOWN;
if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
return -1;
delay_ms(1);
data[0] = AKM_FUSE_ROM_ACCESS;
if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
return -1;
delay_ms(1);
/* Get sensitivity adjustment data from fuse ROM. */
if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data))
return -1;
st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;
st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;
st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;
data[0] = AKM_POWER_DOWN;
if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
return -1;
delay_ms(1);
mpu_set_bypass(0);
/* Set up master mode, master clock, and ES bit. */
data[0] = 0x40;
if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
return -1;
/* Slave 0 reads from AKM data registers. */
data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr;
if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data))
return -1;
/* Compass reads start at this register. */
data[0] = AKM_REG_ST1;
if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data))
return -1;
/* Enable slave 0, 8-byte reads. */
data[0] = BIT_SLAVE_EN | 8;
if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data))
return -1;
/* Slave 1 changes AKM measurement mode. */
data[0] = st.chip_cfg.compass_addr;
if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data))
return -1;
/* AKM measurement mode register. */
data[0] = AKM_REG_CNTL;
if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data))
return -1;
/* Enable slave 1, 1-byte writes. */
data[0] = BIT_SLAVE_EN | 1;
if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data))
return -1;
/* Set slave 1 data. */
data[0] = AKM_SINGLE_MEASUREMENT;
if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data))
return -1;
/* Trigger slave 0 and slave 1 actions at each sample. */
data[0] = 0x03;
if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data))
return -1;
/* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */
data[0] = BIT_I2C_MST_VDDIO;
if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data))
return -1;
return 0;
}
#endif
/**
* @brief Turn specific sensors on/off.
* @e sensors can contain a combination of the following flags:
* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
* \n INV_XYZ_GYRO
* \n INV_XYZ_ACCEL
* \n INV_XYZ_COMPASS
* @param[in] sensors Mask of sensors to wake.
* @return 0 if successful.
*/
int16_t AP_InertialSensor_MPU9150::mpu_set_sensors(uint8_t sensors)
{
uint8_t data;
// uint8_t user_ctrl;
if (sensors & INV_XYZ_GYRO){
data = INV_CLK_PLL;
}
else if (sensors){
data = 0;
}
else {
data = BIT_SLEEP;
}
hal.i2c->writeRegister(st.hw->addr,st.reg->pwr_mgmt_1, data);
data = 0;
if (!(sensors & INV_X_GYRO)){
data |= BIT_STBY_XG;
}
if (!(sensors & INV_Y_GYRO)){
data |= BIT_STBY_YG;
}
if (!(sensors & INV_Z_GYRO)){
data |= BIT_STBY_ZG;
}
if (!(sensors & INV_XYZ_ACCEL)){
data |= BIT_STBY_XYZA;
}
hal.i2c->writeRegister(st.hw->addr,st.reg->pwr_mgmt_2, data);
#if 0
if (sensors && (sensors != INV_XYZ_ACCEL)){
/* Latched interrupts only used in LP accel mode. */
mpu_set_int_latched(0);
}
#endif
// // handle the compass, not implemented for now
// #ifdef AK89xx_SECONDARY
// #ifdef AK89xx_BYPASS
// if (sensors & INV_XYZ_COMPASS)
// mpu_set_bypass(1);
// else
// mpu_set_bypass(0);
// #else
// if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
// return -1;
// /* Handle AKM power management. */
// if (sensors & INV_XYZ_COMPASS) {
// data = AKM_SINGLE_MEASUREMENT;
// user_ctrl |= BIT_AUX_IF_EN;
// } else {
// data = AKM_POWER_DOWN;
// user_ctrl &= ~BIT_AUX_IF_EN;
// }
// if (st.chip_cfg.dmp_on)
// user_ctrl |= BIT_DMP_EN;
// else
// user_ctrl &= ~BIT_DMP_EN;
// if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data))
// return -1;
// /* Enable/disable I2C master mode. */
// if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
// return -1;
// #endif
hal.scheduler->delay(50);
return 0;
}
#if 0
/**
* TODO: Remove the st.chip_cfg cache variables
*
* @brief Enable latched interrupts.
* Any MPU register will clear the interrupt.
* @param[in] enable 1 to enable, 0 to disable.
* @return 0 if successful.
*/
int16_t AP_InertialSensor_MPU9150::mpu_set_int_latched(uint8_t enable)
{
uint8_t tmp;
if (st.chip_cfg.latched_int == enable){
return 0;
}
if (enable){
tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR;
}
else {
tmp = 0;
}
if (st.chip_cfg.bypass_mode){
tmp |= BIT_BYPASS_EN;
}
if (st.chip_cfg.active_low_int){
tmp |= BIT_ACTL;
}
hal.i2c->writeRegister(st.hw->addr,st.reg->int_pin_cfg, tmp);
st.chip_cfg.latched_int = enable;
return 0;
}
#endif
/**
* @brief Get one packet from the FIFO.
* If @e sensors does not contain a particular sensor, disregard the data
* returned to that pointer.
* \n @e sensors can contain a combination of the following flags:
* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
* \n INV_XYZ_GYRO
* \n INV_XYZ_ACCEL
* \n If the FIFO has no new data, @e sensors will be zero.
* \n If the FIFO is disabled, @e sensors will be zero and this function will
* return a non-zero error code.
* @param[out] gyro Gyro data in hardware units.
* @param[out] accel Accel data in hardware units.
* @param[out] timestamp Timestamp in milliseconds.
* @param[out] sensors Mask of sensors read from FIFO.
* @param[out] more Number of remaining packets.
* @return 0 if successful.
*/
int16_t AP_InertialSensor_MPU9150::mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t timestamp,
uint8_t *sensors, uint8_t *more)
{
/* Assumes maximum packet size is gyro (6) + accel (6). */
uint8_t data[12];
uint8_t packet_size = 0;
uint16_t fifo_count, index = 0;
uint8_t sensors_aux = INV_XYZ_ACCEL| INV_XYZ_GYRO;
sensors[0] = 0;
// We assume we want gyro and accel values
packet_size = 12;
// fifo_count_h register contains the number of samples in the FIFO
hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_count_h, 2, data);
fifo_count = (data[0] << 8) | data[1];
if (fifo_count < packet_size){
return 0;
}
// hal.console->printf_P(PTR("FIFO count: %hd\n", fifo_count));
if (fifo_count > (st.hw->max_fifo >> 1)) {
/* FIFO is 50% full, better check overflow bit. */
hal.i2c->readRegister(st.hw->addr, st.reg->int_status, data);
if (data[0] & BIT_FIFO_OVERFLOW) {
mpu_reset_fifo(sensors_aux);
return -2;
}
}
timestamp = hal.scheduler->millis();
// read the data
hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_r_w, packet_size, data);
more[0] = fifo_count / packet_size - 1;
sensors[0] = 0;
if (index != packet_size) {
accel[0] = (data[index+0] << 8) | data[index+1];
accel[1] = (data[index+2] << 8) | data[index+3];
accel[2] = (data[index+4] << 8) | data[index+5];
sensors[0] |= INV_XYZ_ACCEL;
index += 6;
}
if (index != packet_size) {
gyro[0] = (data[index+0] << 8) | data[index+1];
sensors[0] |= INV_X_GYRO;
index += 2;
}
if (index != packet_size) {
gyro[1] = (data[index+0] << 8) | data[index+1];
sensors[0] |= INV_Y_GYRO;
index += 2;
}
if (index != packet_size) {
gyro[2] = (data[index+0] << 8) | data[index+1];
sensors[0] |= INV_Z_GYRO;
index += 2;
}
return 0;
}
/**
* @brief Accumulate values from accels and gyros.
*
* This method is called periodically by the scheduler.
*
*/
void AP_InertialSensor_MPU9150::_accumulate(void){
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// take i2c bus sempahore
if (!i2c_sem->take_nonblocking()){
return;
}
// Read accelerometer FIFO to find out how many samples are available
/* Assumes maximum packet size is gyro (6) + accel (6). */
uint8_t data[12];
uint8_t packet_size = 12;
uint16_t fifo_count, index = 0;
int16_t accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z;
// fifo_count_h register contains the number of samples in the FIFO
hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_count_h, 2, data);
fifo_count = (data[0] << 8) | data[1];
if (fifo_count < packet_size){
// give back i2c semaphore
i2c_sem->give();
return;
}
// hal.console->printf_P(PTR("FIFO count: %hd\n", fifo_count));
if (fifo_count > (st.hw->max_fifo >> 1)) {
/* FIFO is 50% full, better check overflow bit. */
hal.i2c->readRegister(st.hw->addr, st.reg->int_status, data);
if (data[0] & BIT_FIFO_OVERFLOW) {
mpu_reset_fifo(INV_XYZ_ACCEL| INV_XYZ_GYRO);
i2c_sem->give();
return;
}
}
// read the samples
for (uint16_t i=0; i< fifo_count; i++) {
// read the data
// TODO check whether it's possible to read all the packages in a single call
hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_r_w, packet_size, data);
// TODO, remove all the checking since it's being configured this way.
if (index != packet_size) {
accel_x = (int16_t) (data[index+0] << 8) | data[index+1];
accel_y = (int16_t) (data[index+2] << 8) | data[index+3];
accel_z = (int16_t) (data[index+4] << 8) | data[index+5];
index += 6;
}
if (index != packet_size) {
gyro_x = (int16_t) (data[index+0] << 8) | data[index+1];
index += 2;
}
if (index != packet_size) {
gyro_y = (int16_t) (data[index+0] << 8) | data[index+1];
index += 2;
}
if (index != packet_size) {
gyro_z = (int16_t) (data[index+0] << 8) | data[index+1];
index += 2;
}
// reset the index
index = 0;
// TODO Revisit why AP_InertialSensor_L3G4200D uses a minus sign in the y and z component. Maybe this
// is because the sensor is placed in the bottom side of the board?
_accel_filtered = Vector3f(
_accel_filter_x.apply(accel_x),
_accel_filter_y.apply(accel_y),
_accel_filter_z.apply(accel_z));
_gyro_filtered = Vector3f(
_gyro_filter_x.apply(gyro_x),
_gyro_filter_y.apply(gyro_y),
_gyro_filter_z.apply(gyro_z));
_gyro_samples_available++;
}
// give back i2c semaphore
i2c_sem->give();
}
bool AP_InertialSensor_MPU9150::_sample_available(void)
{
uint64_t tnow = hal.scheduler->micros();
while (tnow - _last_sample_timestamp > _sample_period_usec) {
_have_sample_available = true;
_last_sample_timestamp += _sample_period_usec;
}
return _have_sample_available;
}
bool AP_InertialSensor_MPU9150::wait_for_sample(uint16_t timeout_ms)
{
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
uint64_t tnow = hal.scheduler->micros();
// we spin for the last timing_lag microseconds. Before that
// we yield the CPU to allow IO to happen
const uint16_t timing_lag = 400;
if (_last_sample_timestamp + _sample_period_usec > tnow+timing_lag) {
hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_period_usec - (tnow+timing_lag));
}
if (_sample_available()) {
return true;
}
}
return false;
}
bool AP_InertialSensor_MPU9150::update(void)
{
if (!wait_for_sample(1000)) {
return false;
}
Vector3f accel_scale = _accel_scale[0].get();
_previous_accel[0] = _accel[0];
// hal.scheduler->suspend_timer_procs();
_accel[0] = _accel_filtered;
_gyro[0] = _gyro_filtered;
// hal.scheduler->resume_timer_procs();
// add offsets and rotation
_accel[0].rotate(_board_orientation);
// Adjust for chip scaling to get m/s/s
////////////////////////////////////////////////
_accel[0] *= MPU9150_ACCEL_SCALE_2G/_gyro_samples_available;
// Now the calibration scale factor
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] -= _accel_offset[0];
_gyro[0].rotate(_board_orientation);
// Adjust for chip scaling to get radians/sec
_gyro[0] *= MPU9150_GYRO_SCALE_2000 / _gyro_samples_available;
_gyro[0] -= _gyro_offset[0];
////////////////////////////////////////////////
_gyro_samples_available = 0;
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
}
_have_sample_available = false;
return true;
}
// TODO review to make sure it matches
float AP_InertialSensor_MPU9150::get_gyro_drift_rate(void)
{
// 0.5 degrees/second/minute (a guess)
return ToRad(0.5/60);
}
// TODO review to make sure it matches
float AP_InertialSensor_MPU9150::get_delta_time(void) const
{
return _sample_period_usec * 1.0e-6f;
}
#endif // CONFIG_HAL_BOARD
Reference in New Issue View Git Blame Copy Permalink