2014-05-01 17:41:22 -03:00
/*
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/>.
*/
2015-08-11 03:28:43 -03:00
# include <AP_HAL/AP_HAL.h>
2014-07-06 23:03:57 -03:00
2014-05-01 17:41:22 -03:00
# include "AP_InertialSensor_MPU9250.h"
2015-09-28 10:50:30 -03:00
# include <assert.h>
2016-01-20 18:20:35 -04:00
# include <utility>
2016-11-09 04:39:28 -04:00
# include <stdio.h>
2015-09-28 10:50:30 -03:00
2016-11-03 21:06:19 -03:00
# include <AP_HAL/GPIO.h>
2016-01-20 18:20:35 -04:00
2016-11-21 01:49:16 -04:00
# define debug(fmt, args ...) do {printf("MPU9250: " fmt "\n", ## args); } while(0)
2016-01-20 18:20:35 -04:00
extern const AP_HAL : : HAL & hal ;
2014-05-01 17:41:22 -03:00
2015-04-28 02:29:41 -03:00
// MPU9250 accelerometer scaling for 16g range
# define MPU9250_ACCEL_SCALE_1G (GRAVITY_MSS / 2048.0f)
2014-05-01 17:41:22 -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
// MPU9250 registers
# define MPUREG_XA_OFFS_H 0x77 // X axis accelerometer offset (high byte)
# define MPUREG_XA_OFFS_L 0x78 // X axis accelerometer offset (low byte)
# define MPUREG_YA_OFFS_H 0x7A // Y axis accelerometer offset (high byte)
# define MPUREG_YA_OFFS_L 0x0B // Y axis accelerometer offset (low byte)
# define MPUREG_ZA_OFFS_H 0x0D // Z axis accelerometer offset (high byte)
# define MPUREG_ZA_OFFS_L 0x0E // Z axis accelerometer offset (low byte)
2016-11-09 04:39:28 -04:00
// MPU9250 registers
2014-05-01 17:41:22 -03:00
# 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
2016-11-21 01:49:16 -04:00
# define MPUREG_CONFIG_FIFO_MODE_STOP 0x40
2014-05-01 17:41:22 -03:00
# define MPUREG_GYRO_CONFIG 0x1B
// bit definitions for MPUREG_GYRO_CONFIG
# 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
2016-11-09 04:39:28 -04:00
# define MPUREG_ACCEL_CONFIG2 0x1D
2014-05-01 17:41:22 -03:00
# 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
2016-11-09 04:39:28 -04:00
# define BIT_TEMP_FIFO_EN 0x80
# define BIT_XG_FIFO_EN 0x40
# define BIT_YG_FIFO_EN 0x20
# define BIT_ZG_FIFO_EN 0x10
# define BIT_ACCEL_FIFO_EN 0x08
# define BIT_SLV2_FIFO_EN 0x04
# define BIT_SLV1_FIFO_EN 0x02
# define BIT_SLV0_FIFI_EN0 0x01
2014-05-01 17:41:22 -03:00
# define MPUREG_INT_PIN_CFG 0x37
# define BIT_INT_RD_CLEAR 0x10 // clear the interrupt when any read occurs
2014-08-12 07:00:31 -03:00
# define BIT_LATCH_INT_EN 0x20 // latch data ready pin
2015-09-28 14:23:21 -03:00
# define BIT_BYPASS_EN 0x02 // connect auxiliary I2C bus to the main I2C bus
2014-05-01 17:41:22 -03:00
# define MPUREG_INT_ENABLE 0x38
// bit definitions for MPUREG_INT_ENABLE
# 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
// bit definitions for MPUREG_INT_STATUS (same bit pattern as above because this register shows what interrupt actually fired)
# 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
// bit definitions for MPUREG_USER_CTRL
# 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
# define BIT_USER_CTRL_I2C_IF_DIS 0x10 // Disable primary I2C interface and enable hal.spi->interface
# 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
2015-03-12 23:26:49 -03:00
# define MPUREG_WHOAMI_MPU9250 0x71
# define MPUREG_WHOAMI_MPU9255 0x73
2014-05-01 17:41:22 -03:00
2015-09-28 10:50:30 -03:00
/* bit definitions for MPUREG_MST_CTRL */
# define MPUREG_I2C_MST_CTRL 0x24
# define I2C_MST_P_NSR 0x10
# define I2C_SLV0_EN 0x80
# define I2C_MST_CLOCK_400KHZ 0x0D
# define I2C_MST_CLOCK_258KHZ 0x08
# define MPUREG_I2C_SLV4_CTRL 0x34
# define MPUREG_I2C_MST_DELAY_CTRL 0x67
# define I2C_SLV0_DLY_EN 0x01
# define I2C_SLV1_DLY_EN 0x02
# define I2C_SLV2_DLY_EN 0x04
# define I2C_SLV3_DLY_EN 0x08
# define READ_FLAG 0x80
# define MPUREG_I2C_SLV0_ADDR 0x25
# define MPUREG_EXT_SENS_DATA_00 0x49
# define MPUREG_I2C_SLV0_DO 0x63
2014-05-01 17:41:22 -03:00
// Configuration bits MPU 3000, MPU 6000 and MPU9250
# 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
2016-11-09 04:39:28 -04:00
# define BITS_DLPF_FCHOICE_B 0x08
2015-08-03 17:31:32 -03:00
2016-11-23 05:33:55 -04:00
# define MPU_SAMPLE_SIZE 14
# define MPU_FIFO_DOWNSAMPLE_COUNT 8
# define MPU_FIFO_BUFFER_SIZE 16
2016-01-20 18:20:35 -04:00
# define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
# define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])
2014-05-01 17:41:22 -03:00
/*
* PS - MPU - 9250 A - 00. pdf , page 8 , lists LSB sensitivity of
* gyro as 16.4 LSB / DPS at scale factor of + / - 2000 dps ( FS_SEL = = 3 )
*/
2016-01-20 18:20:35 -04:00
static const float GYRO_SCALE = 0.0174532f / 16.4f ;
2014-05-01 17:41:22 -03:00
/*
* PS - MPU - 9250 A - 00. pdf , page 9 , lists LSB sensitivity of
* accel as 4096 LSB / mg at scale factor of + / - 8 g ( AFS_SEL = = 2 )
*
* See note below about accel scaling of engineering sample MPUXk
* variants however
*/
2016-01-20 18:20:35 -04:00
AP_InertialSensor_MPU9250 : : AP_InertialSensor_MPU9250 ( AP_InertialSensor & imu ,
2016-11-05 02:03:31 -03:00
AP_HAL : : OwnPtr < AP_HAL : : Device > dev ,
enum Rotation rotation )
2016-01-20 18:20:35 -04:00
: AP_InertialSensor_Backend ( imu )
2016-11-09 04:39:28 -04:00
, _temp_filter ( 1000 , 1 )
2016-11-05 02:03:31 -03:00
, _rotation ( rotation )
2016-01-20 18:20:35 -04:00
, _dev ( std : : move ( dev ) )
2015-10-02 14:55:38 -03:00
{
}
2016-01-20 18:20:35 -04:00
AP_InertialSensor_MPU9250 : : ~ AP_InertialSensor_MPU9250 ( )
2015-10-02 14:55:38 -03:00
{
2016-01-20 18:20:35 -04:00
delete _auxiliary_bus ;
2015-10-02 14:55:38 -03:00
}
2016-01-20 18:20:35 -04:00
AP_InertialSensor_Backend * AP_InertialSensor_MPU9250 : : probe ( AP_InertialSensor & imu ,
2016-11-05 02:03:31 -03:00
AP_HAL : : OwnPtr < AP_HAL : : I2CDevice > dev ,
enum Rotation rotation )
2015-10-02 14:55:38 -03:00
{
2016-11-07 23:01:53 -04:00
if ( ! dev ) {
return nullptr ;
}
2016-01-20 18:20:35 -04:00
AP_InertialSensor_MPU9250 * sensor =
2016-11-15 01:51:18 -04:00
new AP_InertialSensor_MPU9250 ( imu , std : : move ( dev ) , rotation ) ;
2016-01-20 18:20:35 -04:00
if ( ! sensor | | ! sensor - > _init ( ) ) {
delete sensor ;
return nullptr ;
}
sensor - > _id = HAL_INS_MPU9250_I2C ;
2015-10-02 14:55:38 -03:00
2016-01-20 18:20:35 -04:00
return sensor ;
2015-10-02 14:55:38 -03:00
}
2015-09-28 10:50:30 -03:00
2016-01-20 18:20:35 -04:00
AP_InertialSensor_Backend * AP_InertialSensor_MPU9250 : : probe ( AP_InertialSensor & imu ,
2016-11-05 02:03:31 -03:00
AP_HAL : : OwnPtr < AP_HAL : : SPIDevice > dev ,
enum Rotation rotation )
2015-10-02 14:55:38 -03:00
{
2016-11-07 23:01:53 -04:00
if ( ! dev ) {
return nullptr ;
}
2016-06-13 19:08:14 -03:00
AP_InertialSensor_MPU9250 * sensor ;
dev - > set_read_flag ( 0x80 ) ;
2016-11-15 01:51:18 -04:00
sensor = new AP_InertialSensor_MPU9250 ( imu , std : : move ( dev ) , rotation ) ;
2016-01-20 18:20:35 -04:00
if ( ! sensor | | ! sensor - > _init ( ) ) {
delete sensor ;
return nullptr ;
}
sensor - > _id = HAL_INS_MPU9250_SPI ;
2015-10-02 14:55:38 -03:00
2016-01-20 18:20:35 -04:00
return sensor ;
2015-10-02 14:55:38 -03:00
}
2016-01-20 18:20:35 -04:00
bool AP_InertialSensor_MPU9250 : : _init ( )
2015-10-02 14:55:38 -03:00
{
2016-01-20 18:20:35 -04:00
bool success = _hardware_init ( ) ;
return success ;
2015-09-28 14:23:21 -03:00
}
2016-11-09 04:39:28 -04:00
void AP_InertialSensor_MPU9250 : : _fifo_reset ( )
{
2016-11-23 02:02:39 -04:00
uint8_t user_ctrl = _last_stat_user_ctrl ;
user_ctrl & = ~ ( BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN ) ;
2016-11-21 01:49:16 -04:00
_dev - > set_speed ( AP_HAL : : Device : : SPEED_LOW ) ;
_register_write ( MPUREG_FIFO_EN , 0 ) ;
2016-11-09 04:54:53 -04:00
_register_write ( MPUREG_USER_CTRL , user_ctrl ) ;
_register_write ( MPUREG_USER_CTRL , user_ctrl | BIT_USER_CTRL_FIFO_RESET ) ;
_register_write ( MPUREG_USER_CTRL , user_ctrl | BIT_USER_CTRL_FIFO_EN ) ;
2016-11-21 01:49:16 -04:00
_register_write ( MPUREG_FIFO_EN , BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
2016-11-23 02:02:39 -04:00
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN , true ) ;
2016-11-21 01:49:16 -04:00
hal . scheduler - > delay_microseconds ( 1 ) ;
_dev - > set_speed ( AP_HAL : : Device : : SPEED_HIGH ) ;
2016-11-23 02:02:39 -04:00
_last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN ;
2016-11-09 04:39:28 -04:00
}
2016-01-20 18:20:35 -04:00
bool AP_InertialSensor_MPU9250 : : _has_auxiliary_bus ( )
2015-09-28 13:48:18 -03:00
{
2016-11-09 06:54:56 -04:00
return _dev - > bus_type ( ) ! = AP_HAL : : Device : : BUS_TYPE_I2C ;
2015-09-28 13:48:18 -03:00
}
2016-01-20 18:20:35 -04:00
void AP_InertialSensor_MPU9250 : : start ( )
2015-09-28 13:48:18 -03:00
{
2016-11-17 21:04:22 -04:00
if ( ! _dev - > get_semaphore ( ) - > take ( 0 ) ) {
2016-11-21 16:00:24 -04:00
return ;
2015-09-28 13:48:18 -03:00
}
2016-01-20 18:20:35 -04:00
// initially run the bus at low speed
_dev - > set_speed ( AP_HAL : : Device : : SPEED_LOW ) ;
2015-09-28 13:48:18 -03:00
2016-01-20 18:20:35 -04:00
// only used for wake-up in accelerometer only low power mode
_register_write ( MPUREG_PWR_MGMT_2 , 0x00 ) ;
hal . scheduler - > delay ( 1 ) ;
2015-09-28 13:48:18 -03:00
2016-11-09 04:39:28 -04:00
// always use FIFO
2016-11-23 02:02:39 -04:00
_fifo_reset ( ) ;
2015-09-28 13:48:18 -03:00
2016-11-21 01:49:16 -04:00
// grab the used instances
_gyro_instance = _imu . register_gyro ( 1000 , _dev - > get_bus_id_devtype ( DEVTYPE_GYR_MPU9250 ) ) ;
_accel_instance = _imu . register_accel ( 1000 , _dev - > get_bus_id_devtype ( DEVTYPE_ACC_MPU9250 ) ) ;
2016-11-15 01:51:18 -04:00
if ( enable_fast_sampling ( _accel_instance ) & & _dev - > bus_type ( ) = = AP_HAL : : Device : : BUS_TYPE_SPI ) {
_fast_sampling = true ;
2016-11-21 01:49:16 -04:00
hal . console - > printf ( " MPU9250: enabled fast sampling \n " ) ;
2016-11-15 01:51:18 -04:00
}
2016-11-09 04:39:28 -04:00
if ( _fast_sampling ) {
// setup for fast sampling
2016-11-21 01:49:16 -04:00
_register_write ( MPUREG_CONFIG , BITS_DLPF_CFG_256HZ_NOLPF2 | MPUREG_CONFIG_FIFO_MODE_STOP , true ) ;
2016-11-09 04:39:28 -04:00
} else {
2016-11-21 01:49:16 -04:00
_register_write ( MPUREG_CONFIG , BITS_DLPF_CFG_188HZ | MPUREG_CONFIG_FIFO_MODE_STOP , true ) ;
2016-11-09 04:39:28 -04:00
}
2016-01-20 18:20:35 -04:00
// set sample rate to 1kHz, and use the 2 pole filter to give the
// desired rate
2016-11-10 02:27:06 -04:00
_register_write ( MPUREG_SMPLRT_DIV , 0 , true ) ;
2016-01-20 18:20:35 -04:00
hal . scheduler - > delay ( 1 ) ;
2014-05-01 17:41:22 -03:00
2016-01-20 18:20:35 -04:00
// Gyro scale 2000º/s
2016-11-10 02:27:06 -04:00
_register_write ( MPUREG_GYRO_CONFIG , BITS_GYRO_FS_2000DPS , true ) ;
2016-01-20 18:20:35 -04:00
hal . scheduler - > delay ( 1 ) ;
2015-10-02 14:55:38 -03:00
2016-01-20 18:20:35 -04:00
// RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 16g
2016-11-10 02:27:06 -04:00
_register_write ( MPUREG_ACCEL_CONFIG , 3 < < 3 , true ) ;
2014-10-15 20:31:06 -03:00
2016-11-09 04:39:28 -04:00
if ( _fast_sampling ) {
// setup ACCEL_FCHOICE for 4kHz sampling
2016-11-10 02:27:06 -04:00
_register_write ( MPUREG_ACCEL_CONFIG2 , 0x08 , true ) ;
2016-11-09 04:39:28 -04:00
} else {
2016-11-10 02:27:06 -04:00
_register_write ( MPUREG_ACCEL_CONFIG2 , 0x00 , true ) ;
2016-11-09 04:39:28 -04:00
}
2016-01-20 18:20:35 -04:00
// configure interrupt to fire when new data arrives
_register_write ( MPUREG_INT_ENABLE , BIT_RAW_RDY_EN ) ;
2015-10-02 14:55:38 -03:00
2016-01-20 18:20:35 -04:00
// clear interrupt on any read, and hold the data ready pin high
// until we clear the interrupt
uint8_t value = _register_read ( MPUREG_INT_PIN_CFG ) ;
value | = BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN ;
_register_write ( MPUREG_INT_PIN_CFG , value ) ;
2014-05-01 17:41:22 -03:00
2016-01-20 18:20:35 -04:00
// now that we have initialised, we set the bus speed to high
_dev - > set_speed ( AP_HAL : : Device : : SPEED_HIGH ) ;
2015-07-02 20:27:55 -03:00
2016-01-20 18:20:35 -04:00
_dev - > get_semaphore ( ) - > give ( ) ;
2014-08-12 07:00:31 -03:00
2016-11-05 02:03:31 -03:00
set_gyro_orientation ( _gyro_instance , _rotation ) ;
set_accel_orientation ( _accel_instance , _rotation ) ;
2016-11-09 04:39:28 -04:00
// allocate fifo buffer
2016-11-23 05:33:55 -04:00
_fifo_buffer = ( uint8_t * ) hal . util - > dma_allocate ( MPU_FIFO_BUFFER_SIZE * MPU_SAMPLE_SIZE ) ;
2016-11-09 04:39:28 -04:00
if ( _fifo_buffer = = nullptr ) {
AP_HAL : : panic ( " MPU9250: Unable to allocate FIFO buffer " ) ;
}
2016-11-21 01:49:16 -04:00
2016-11-23 05:33:55 -04:00
// start the timer process to read samples
_dev - > register_periodic_callback ( 1000 , FUNCTOR_BIND_MEMBER ( & AP_InertialSensor_MPU9250 : : _read_sample , bool ) ) ;
2014-05-01 17:41:22 -03:00
}
2014-08-19 06:39:21 -03:00
/*
update the accel and gyro vectors
*/
2016-01-20 18:20:35 -04:00
bool AP_InertialSensor_MPU9250 : : update ( )
2014-05-01 17:41:22 -03:00
{
2015-11-15 20:05:20 -04:00
update_gyro ( _gyro_instance ) ;
update_accel ( _accel_instance ) ;
2014-05-01 17:41:22 -03:00
2016-11-09 04:39:28 -04:00
_publish_temperature ( _accel_instance , _temp_filtered ) ;
2014-05-01 17:41:22 -03:00
return true ;
}
2016-11-21 01:49:16 -04:00
/*
accumulate new samples
*/
void AP_InertialSensor_MPU9250 : : accumulate ( )
{
2016-11-23 05:33:55 -04:00
// nothing to do
2016-11-21 01:49:16 -04:00
}
2016-01-20 18:20:35 -04:00
AuxiliaryBus * AP_InertialSensor_MPU9250 : : get_auxiliary_bus ( )
2014-05-01 17:41:22 -03:00
{
2016-01-20 18:20:35 -04:00
if ( _auxiliary_bus ) {
return _auxiliary_bus ;
}
if ( _has_auxiliary_bus ( ) ) {
2016-11-04 06:24:24 -03:00
_auxiliary_bus = new AP_MPU9250_AuxiliaryBus ( * this , _dev - > get_bus_id ( ) ) ;
2014-08-19 06:39:21 -03:00
}
2016-01-20 18:20:35 -04:00
return _auxiliary_bus ;
2014-05-01 17:41:22 -03:00
}
2014-08-19 06:39:21 -03:00
/*
2016-01-20 18:20:35 -04:00
* Return true if the MPU9250 has new data available for reading .
2014-08-19 06:39:21 -03:00
*/
2016-01-20 18:20:35 -04:00
bool AP_InertialSensor_MPU9250 : : _data_ready ( )
2014-08-19 06:39:21 -03:00
{
2016-01-20 18:20:35 -04:00
uint8_t int_status = _register_read ( MPUREG_INT_STATUS ) ;
return _data_ready ( int_status ) ;
}
2014-08-12 07:00:31 -03:00
2016-01-20 18:20:35 -04:00
bool AP_InertialSensor_MPU9250 : : _data_ready ( uint8_t int_status )
{
return ( int_status & BIT_RAW_RDY_INT ) ! = 0 ;
}
2015-08-28 10:36:20 -03:00
2016-11-09 04:39:28 -04:00
2016-11-21 01:49:16 -04:00
bool AP_InertialSensor_MPU9250 : : _accumulate ( uint8_t * samples , uint8_t n_samples , int16_t raw_temp )
2016-11-09 04:39:28 -04:00
{
for ( uint8_t i = 0 ; i < n_samples ; i + + ) {
2016-11-23 05:33:55 -04:00
const uint8_t * data = samples + MPU_SAMPLE_SIZE * i ;
2016-11-09 04:39:28 -04:00
Vector3f accel , gyro ;
accel = Vector3f ( int16_val ( data , 1 ) ,
int16_val ( data , 0 ) ,
- int16_val ( data , 2 ) ) ;
accel * = MPU9250_ACCEL_SCALE_1G ;
2016-11-21 01:49:16 -04:00
int16_t t2 = int16_val ( data , 3 ) ;
if ( abs ( t2 - raw_temp ) > 400 ) {
debug ( " temp reset %d %d %d " , raw_temp , t2 , raw_temp - t2 ) ;
_fifo_reset ( ) ;
return false ;
}
float temp = t2 / 340 + 36.53 ;
2016-11-09 04:39:28 -04:00
gyro = Vector3f ( int16_val ( data , 5 ) ,
int16_val ( data , 4 ) ,
- int16_val ( data , 6 ) ) ;
gyro * = GYRO_SCALE ;
_rotate_and_correct_accel ( _accel_instance , accel ) ;
_rotate_and_correct_gyro ( _gyro_instance , gyro ) ;
_notify_new_accel_raw_sample ( _accel_instance , accel , AP_HAL : : micros64 ( ) ) ;
_notify_new_gyro_raw_sample ( _gyro_instance , gyro ) ;
_temp_filtered = _temp_filter . apply ( temp ) ;
}
2016-11-21 01:49:16 -04:00
return true ;
2016-11-09 04:39:28 -04:00
}
2016-11-21 01:49:16 -04:00
/*
when doing fast sampling the sensor gives us 8 k samples / second . Every 2 nd accel sample is a duplicate .
To filter this we first apply a 1 p low pass filter at 188 Hz , then we
average over 8 samples to bring the data rate down to 1 kHz . This
gives very good aliasing rejection at frequencies well above what
can be handled with 1 kHz sample rates .
*/
bool AP_InertialSensor_MPU9250 : : _accumulate_fast_sampling ( uint8_t * samples , uint8_t n_samples , int16_t raw_temp )
2016-01-20 18:20:35 -04:00
{
2016-11-21 01:49:16 -04:00
int32_t tsum = 0 ;
2016-11-09 22:39:17 -04:00
const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / MPU9250_ACCEL_SCALE_1G ;
bool clipped = false ;
2016-11-21 01:49:16 -04:00
bool ret = true ;
2016-11-09 22:39:17 -04:00
2016-11-09 04:39:28 -04:00
for ( uint8_t i = 0 ; i < n_samples ; i + + ) {
2016-11-23 05:33:55 -04:00
const uint8_t * data = samples + MPU_SAMPLE_SIZE * i ;
2016-11-21 01:49:16 -04:00
// use temperatue to detect FIFO corruption
int16_t t2 = int16_val ( data , 3 ) ;
if ( abs ( t2 - raw_temp ) > 400 ) {
debug ( " temp reset %d %d %d " , raw_temp , t2 , raw_temp - t2 ) ;
_fifo_reset ( ) ;
ret = false ;
break ;
2016-11-09 22:39:17 -04:00
}
2016-11-21 01:49:16 -04:00
tsum + = t2 ;
if ( ( _accum . count & 1 ) = = 0 ) {
// accels are at 4kHz not 8kHz
Vector3f a ( int16_val ( data , 1 ) ,
int16_val ( data , 0 ) ,
- int16_val ( data , 2 ) ) ;
if ( fabsf ( a . x ) > clip_limit | |
fabsf ( a . y ) > clip_limit | |
fabsf ( a . z ) > clip_limit ) {
clipped = true ;
}
_accum . accel + = _accum . accel_filter . apply ( a ) ;
}
Vector3f g ( int16_val ( data , 5 ) ,
2016-11-15 01:51:18 -04:00
int16_val ( data , 4 ) ,
- int16_val ( data , 6 ) ) ;
2016-11-21 01:49:16 -04:00
_accum . gyro + = _accum . gyro_filter . apply ( g ) ;
2016-11-15 01:51:18 -04:00
_accum . count + + ;
2016-11-23 05:33:55 -04:00
if ( _accum . count = = MPU_FIFO_DOWNSAMPLE_COUNT ) {
float ascale = MPU9250_ACCEL_SCALE_1G / ( MPU_FIFO_DOWNSAMPLE_COUNT / 2 ) ;
_accum . accel * = ascale ;
float gscale = GYRO_SCALE / MPU_FIFO_DOWNSAMPLE_COUNT ;
_accum . gyro * = gscale ;
_rotate_and_correct_accel ( _accel_instance , _accum . accel ) ;
_rotate_and_correct_gyro ( _gyro_instance , _accum . gyro ) ;
_notify_new_accel_raw_sample ( _accel_instance , _accum . accel , AP_HAL : : micros64 ( ) , false ) ;
_notify_new_gyro_raw_sample ( _gyro_instance , _accum . gyro ) ;
_accum . accel . zero ( ) ;
_accum . gyro . zero ( ) ;
_accum . count = 0 ;
}
2016-11-09 04:39:28 -04:00
}
2014-08-19 06:39:21 -03:00
2016-11-09 22:39:17 -04:00
if ( clipped ) {
increment_clip_count ( _accel_instance ) ;
}
2015-08-28 10:36:20 -03:00
2016-11-23 05:33:55 -04:00
if ( ret ) {
float temp = ( tsum / n_samples ) / 340.0 + 36.53 ;
_temp_filtered = _temp_filter . apply ( temp ) ;
2016-11-15 01:51:18 -04:00
}
2016-11-23 05:33:55 -04:00
2016-11-21 01:49:16 -04:00
return ret ;
2014-05-01 17:41:22 -03:00
}
2016-01-20 18:20:35 -04:00
2014-08-19 06:39:21 -03:00
/*
2016-01-20 18:20:35 -04:00
* read from the data registers and update filtered data
2014-08-19 06:39:21 -03:00
*/
2016-11-03 21:06:19 -03:00
bool AP_InertialSensor_MPU9250 : : _read_sample ( )
2016-01-20 18:20:35 -04:00
{
2016-11-09 04:39:28 -04:00
uint8_t n_samples ;
uint16_t bytes_read ;
uint8_t * rx = _fifo_buffer ;
2016-11-21 01:49:16 -04:00
int16_t raw_temp ;
uint8_t trx [ 2 ] ;
bool need_reset = false ;
2016-11-10 02:27:06 -04:00
2016-11-09 04:39:28 -04:00
if ( ! _block_read ( MPUREG_FIFO_COUNTH , rx , 2 ) ) {
2016-11-10 17:06:50 -04:00
goto check_registers ;
2016-01-20 18:20:35 -04:00
}
2016-11-09 04:39:28 -04:00
bytes_read = uint16_val ( rx , 0 ) ;
2016-11-23 05:33:55 -04:00
n_samples = bytes_read / MPU_SAMPLE_SIZE ;
2016-11-09 04:39:28 -04:00
if ( n_samples = = 0 ) {
/* Not enough data in FIFO */
2016-11-10 02:27:06 -04:00
goto check_registers ;
2016-01-20 18:20:35 -04:00
}
2016-11-21 01:49:16 -04:00
/*
fetch temperature in order to detect FIFO sync errors
*/
if ( ! _block_read ( MPUREG_TEMP_OUT_H , trx , 2 ) ) {
return true ;
}
raw_temp = int16_val ( trx , 0 ) ;
/*
testing has shown that if we have more than 32 samples in the
FIFO then some of those samples will be corrupt . It always is
the ones at the end of the FIFO , so clear those with a reset
once we ' ve read the first 24. Reading 24 gives us the normal
number of samples for fast sampling at 400 Hz
*/
if ( n_samples > 32 ) {
need_reset = true ;
n_samples = 24 ;
}
2016-11-12 01:43:29 -04:00
while ( n_samples > 0 ) {
2016-11-23 05:33:55 -04:00
uint8_t n = MIN ( MPU_FIFO_BUFFER_SIZE , n_samples ) ;
if ( ! _block_read ( MPUREG_FIFO_R_W , rx , n * MPU_SAMPLE_SIZE ) ) {
printf ( " MPU60x0: error in fifo read %u bytes \n " , n * MPU_SAMPLE_SIZE ) ;
2016-11-12 01:43:29 -04:00
goto check_registers ;
}
2016-11-09 04:39:28 -04:00
2016-11-12 01:43:29 -04:00
if ( _fast_sampling ) {
2016-11-21 01:49:16 -04:00
if ( ! _accumulate_fast_sampling ( rx , n , raw_temp ) ) {
2016-11-23 05:33:55 -04:00
debug ( " stop at %u of %u " , n_samples , bytes_read / MPU_SAMPLE_SIZE ) ;
2016-11-21 01:49:16 -04:00
break ;
}
2016-11-12 01:43:29 -04:00
} else {
2016-11-21 01:49:16 -04:00
if ( ! _accumulate ( rx , n , raw_temp ) ) {
break ;
}
2016-11-12 01:43:29 -04:00
}
n_samples - = n ;
2016-11-09 04:39:28 -04:00
}
2016-11-21 01:49:16 -04:00
if ( need_reset ) {
2016-11-23 05:33:55 -04:00
//debug("fifo reset %u", bytes_read/MPU_SAMPLE_SIZE);
2016-11-18 21:53:25 -04:00
_fifo_reset ( ) ;
}
2016-11-10 02:27:06 -04:00
check_registers :
if ( _reg_check_counter + + = = 10 ) {
_reg_check_counter = 0 ;
// check next register value for correctness
if ( ! _dev - > check_next_register ( ) ) {
_inc_gyro_error_count ( _gyro_instance ) ;
_inc_accel_error_count ( _accel_instance ) ;
}
}
2016-11-09 04:39:28 -04:00
2016-11-03 21:06:19 -03:00
return true ;
2016-01-20 18:20:35 -04:00
}
bool AP_InertialSensor_MPU9250 : : _block_read ( uint8_t reg , uint8_t * buf ,
uint32_t size )
{
return _dev - > read_registers ( reg , buf , size ) ;
}
2015-10-02 16:14:23 -03:00
uint8_t AP_InertialSensor_MPU9250 : : _register_read ( uint8_t reg )
2014-05-01 17:41:22 -03:00
{
2016-01-20 18:20:35 -04:00
uint8_t val = 0 ;
_dev - > read_registers ( reg , & val , 1 ) ;
2015-10-02 14:55:38 -03:00
return val ;
2014-05-01 17:41:22 -03:00
}
2016-11-10 02:27:06 -04:00
void AP_InertialSensor_MPU9250 : : _register_write ( uint8_t reg , uint8_t val , bool checked )
2015-07-02 17:33:22 -03:00
{
2016-11-10 02:27:06 -04:00
_dev - > write_register ( reg , val , checked ) ;
2014-05-01 17:41:22 -03:00
}
2016-01-20 18:20:35 -04:00
bool AP_InertialSensor_MPU9250 : : _hardware_init ( void )
{
2016-11-17 21:04:22 -04:00
if ( ! _dev - > get_semaphore ( ) - > take ( 0 ) ) {
2016-11-21 16:00:24 -04:00
return false ;
2014-05-01 17:41:22 -03:00
}
2016-11-10 02:27:06 -04:00
// setup for register checking
_dev - > setup_checked_registers ( 6 ) ;
2015-10-02 16:14:23 -03:00
// initially run the bus at low speed
2016-01-20 18:20:35 -04:00
_dev - > set_speed ( AP_HAL : : Device : : SPEED_LOW ) ;
2015-10-02 16:14:23 -03:00
2015-09-28 14:23:21 -03:00
uint8_t value = _register_read ( MPUREG_WHOAMI ) ;
if ( value ! = MPUREG_WHOAMI_MPU9250 & & value ! = MPUREG_WHOAMI_MPU9255 ) {
hal . console - > printf ( " MPU9250: unexpected WHOAMI 0x%x \n " , ( unsigned ) value ) ;
2015-10-02 16:14:23 -03:00
goto fail_whoami ;
}
// Chip reset
uint8_t tries ;
for ( tries = 0 ; tries < 5 ; tries + + ) {
2016-11-23 02:02:39 -04:00
_last_stat_user_ctrl = _register_read ( MPUREG_USER_CTRL ) ;
2015-10-02 16:14:23 -03:00
/* First disable the master I2C to avoid hanging the slaves on the
2016-01-20 18:20:35 -04:00
* aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus
* is used */
2016-11-23 02:02:39 -04:00
if ( _last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN ) {
_last_stat_user_ctrl & = ~ BIT_USER_CTRL_I2C_MST_EN ;
_register_write ( MPUREG_USER_CTRL , _last_stat_user_ctrl ) ;
2015-10-02 16:14:23 -03:00
hal . scheduler - > delay ( 10 ) ;
}
// reset device
_register_write ( MPUREG_PWR_MGMT_1 , BIT_PWR_MGMT_1_DEVICE_RESET ) ;
hal . scheduler - > delay ( 100 ) ;
2016-01-20 18:20:35 -04:00
/* bus-dependent initialization */
2016-11-04 06:24:24 -03:00
if ( _dev - > bus_type ( ) = = AP_HAL : : Device : : BUS_TYPE_SPI ) {
2016-01-20 18:20:35 -04:00
/* Disable I2C bus if SPI selected (Recommended in Datasheet to be
* done just after the device is reset ) */
2016-11-23 02:02:39 -04:00
_last_stat_user_ctrl | = BIT_USER_CTRL_I2C_IF_DIS ;
_register_write ( MPUREG_USER_CTRL , _last_stat_user_ctrl ) ;
2016-01-20 18:20:35 -04:00
}
2015-10-02 16:14:23 -03:00
// Wake up device and select GyroZ clock. Note that the
// MPU9250 starts up in sleep mode, and it can take some time
// for it to come out of sleep
_register_write ( MPUREG_PWR_MGMT_1 , BIT_PWR_MGMT_1_CLK_ZGYRO ) ;
hal . scheduler - > delay ( 5 ) ;
// check it has woken up
if ( _register_read ( MPUREG_PWR_MGMT_1 ) = = BIT_PWR_MGMT_1_CLK_ZGYRO ) {
break ;
}
hal . scheduler - > delay ( 10 ) ;
2016-01-20 18:20:35 -04:00
if ( _data_ready ( ) ) {
2015-10-02 16:14:23 -03:00
break ;
}
}
if ( tries = = 5 ) {
hal . console - > println ( " Failed to boot MPU9250 5 times " ) ;
goto fail_tries ;
}
2014-05-01 17:41:22 -03:00
2016-01-20 18:20:35 -04:00
_dev - > set_speed ( AP_HAL : : Device : : SPEED_HIGH ) ;
_dev - > get_semaphore ( ) - > give ( ) ;
2015-07-02 20:27:55 -03:00
2014-05-01 17:41:22 -03:00
return true ;
2015-10-02 16:14:23 -03:00
fail_tries :
fail_whoami :
2016-01-20 18:20:35 -04:00
_dev - > get_semaphore ( ) - > give ( ) ;
_dev - > set_speed ( AP_HAL : : Device : : SPEED_HIGH ) ;
2015-10-02 16:14:23 -03:00
return false ;
2014-05-01 17:41:22 -03:00
}
2015-09-28 10:50:30 -03:00
AP_MPU9250_AuxiliaryBusSlave : : AP_MPU9250_AuxiliaryBusSlave ( AuxiliaryBus & bus , uint8_t addr ,
uint8_t instance )
: AuxiliaryBusSlave ( bus , addr , instance )
, _mpu9250_addr ( MPUREG_I2C_SLV0_ADDR + _instance * 3 )
, _mpu9250_reg ( _mpu9250_addr + 1 )
, _mpu9250_ctrl ( _mpu9250_addr + 2 )
, _mpu9250_do ( MPUREG_I2C_SLV0_DO + _instance )
{
}
int AP_MPU9250_AuxiliaryBusSlave : : _set_passthrough ( uint8_t reg , uint8_t size ,
uint8_t * out )
{
2016-01-20 18:20:35 -04:00
auto & backend = AP_InertialSensor_MPU9250 : : from ( _bus . get_backend ( ) ) ;
2015-09-28 10:50:30 -03:00
uint8_t addr ;
/* Ensure the slave read/write is disabled before changing the registers */
backend . _register_write ( _mpu9250_ctrl , 0 ) ;
if ( out ) {
backend . _register_write ( _mpu9250_do , * out ) ;
addr = _addr ;
} else {
addr = _addr | READ_FLAG ;
}
backend . _register_write ( _mpu9250_addr , addr ) ;
backend . _register_write ( _mpu9250_reg , reg ) ;
backend . _register_write ( _mpu9250_ctrl , I2C_SLV0_EN | size ) ;
return 0 ;
}
int AP_MPU9250_AuxiliaryBusSlave : : passthrough_read ( uint8_t reg , uint8_t * buf ,
uint8_t size )
{
assert ( buf ) ;
if ( _registered ) {
hal . console - > println ( " Error: can't passthrough when slave is already configured " ) ;
return - 1 ;
}
int r = _set_passthrough ( reg , size ) ;
2016-01-20 18:20:35 -04:00
if ( r < 0 ) {
2015-09-28 10:50:30 -03:00
return r ;
2016-01-20 18:20:35 -04:00
}
2015-09-28 10:50:30 -03:00
/* wait the value to be read from the slave and read it back */
hal . scheduler - > delay ( 10 ) ;
2016-01-20 18:20:35 -04:00
auto & backend = AP_InertialSensor_MPU9250 : : from ( _bus . get_backend ( ) ) ;
backend . _block_read ( MPUREG_EXT_SENS_DATA_00 + _ext_sens_data , buf , size ) ;
2015-09-28 10:50:30 -03:00
/* disable new reads */
backend . _register_write ( _mpu9250_ctrl , 0 ) ;
return size ;
}
int AP_MPU9250_AuxiliaryBusSlave : : passthrough_write ( uint8_t reg , uint8_t val )
{
if ( _registered ) {
hal . console - > println ( " Error: can't passthrough when slave is already configured " ) ;
return - 1 ;
}
int r = _set_passthrough ( reg , 1 , & val ) ;
2016-01-20 18:20:35 -04:00
if ( r < 0 ) {
2015-09-28 10:50:30 -03:00
return r ;
2016-01-20 18:20:35 -04:00
}
2015-09-28 10:50:30 -03:00
/* wait the value to be written to the slave */
hal . scheduler - > delay ( 10 ) ;
2016-01-20 18:20:35 -04:00
auto & backend = AP_InertialSensor_MPU9250 : : from ( _bus . get_backend ( ) ) ;
2015-09-28 10:50:30 -03:00
/* disable new writes */
backend . _register_write ( _mpu9250_ctrl , 0 ) ;
2016-03-03 17:22:26 -04:00
return 1 ;
2015-09-28 10:50:30 -03:00
}
int AP_MPU9250_AuxiliaryBusSlave : : read ( uint8_t * buf )
{
if ( ! _registered ) {
hal . console - > println ( " Error: can't read before configuring slave " ) ;
return - 1 ;
}
2016-01-20 18:20:35 -04:00
auto & backend = AP_InertialSensor_MPU9250 : : from ( _bus . get_backend ( ) ) ;
2016-03-03 17:22:26 -04:00
if ( ! backend . _block_read ( MPUREG_EXT_SENS_DATA_00 + _ext_sens_data , buf , _sample_size ) ) {
return - 1 ;
}
2015-09-28 10:50:30 -03:00
2016-03-03 17:22:26 -04:00
return _sample_size ;
2015-09-28 10:50:30 -03:00
}
2014-05-01 17:41:22 -03:00
2016-01-20 18:20:35 -04:00
/* MPU9250 provides up to 5 slave devices, but the 5th is way too different to
* configure and is seldom used */
2016-11-04 06:24:24 -03:00
AP_MPU9250_AuxiliaryBus : : AP_MPU9250_AuxiliaryBus ( AP_InertialSensor_MPU9250 & backend , uint32_t devid )
: AuxiliaryBus ( backend , 4 , devid )
2016-01-20 18:20:35 -04:00
{
}
AP_HAL : : Semaphore * AP_MPU9250_AuxiliaryBus : : get_semaphore ( )
{
return AP_InertialSensor_MPU9250 : : from ( _ins_backend ) . _dev - > get_semaphore ( ) ;
}
AuxiliaryBusSlave * AP_MPU9250_AuxiliaryBus : : _instantiate_slave ( uint8_t addr , uint8_t instance )
{
/* Enable slaves on MPU9250 if this is the first time */
if ( _ext_sens_data = = 0 )
_configure_slaves ( ) ;
return new AP_MPU9250_AuxiliaryBusSlave ( * this , addr , instance ) ;
}
void AP_MPU9250_AuxiliaryBus : : _configure_slaves ( )
{
auto & backend = AP_InertialSensor_MPU9250 : : from ( _ins_backend ) ;
/* Enable the I2C master to slaves on the auxiliary I2C bus*/
2016-11-23 02:02:39 -04:00
if ( ! ( backend . _last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN ) ) {
backend . _last_stat_user_ctrl | = BIT_USER_CTRL_I2C_MST_EN ;
backend . _register_write ( MPUREG_USER_CTRL , backend . _last_stat_user_ctrl ) ;
}
2016-11-09 04:54:53 -04:00
2016-01-20 18:20:35 -04:00
/* stop condition between reads; clock at 400kHz */
backend . _register_write ( MPUREG_I2C_MST_CTRL ,
I2C_MST_CLOCK_400KHZ | I2C_MST_P_NSR ) ;
/* Hard-code divider for internal sample rate, 1 kHz, resulting in a
* sample rate of 100 Hz */
backend . _register_write ( MPUREG_I2C_SLV4_CTRL , 9 ) ;
/* All slaves are subject to the sample rate */
backend . _register_write ( MPUREG_I2C_MST_DELAY_CTRL ,
I2C_SLV0_DLY_EN | I2C_SLV1_DLY_EN |
I2C_SLV2_DLY_EN | I2C_SLV3_DLY_EN ) ;
}
int AP_MPU9250_AuxiliaryBus : : _configure_periodic_read ( AuxiliaryBusSlave * slave ,
uint8_t reg , uint8_t size )
{
if ( _ext_sens_data + size > MAX_EXT_SENS_DATA ) {
return - 1 ;
}
AP_MPU9250_AuxiliaryBusSlave * mpu_slave =
static_cast < AP_MPU9250_AuxiliaryBusSlave * > ( slave ) ;
mpu_slave - > _set_passthrough ( reg , size ) ;
mpu_slave - > _ext_sens_data = _ext_sens_data ;
_ext_sens_data + = size ;
return 0 ;
}
2016-11-03 21:06:19 -03:00