mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: unify MPU6000 and MPU9250 drivers
make a single AP_InertialSensor_Invensense driver. This avoids a lot of duplication and will save time as new varients are added
This commit is contained in:
Andrew Tridgell
parent
c846cc249d
commit
65b9b86099
|
|
@ -15,8 +15,7 @@
|
|||
#include "AP_InertialSensor_HIL.h"
|
||||
#include "AP_InertialSensor_L3G4200D.h"
|
||||
#include "AP_InertialSensor_LSM9DS0.h"
|
||||
#include "AP_InertialSensor_MPU6000.h"
|
||||
#include "AP_InertialSensor_MPU9250.h"
|
||||
#include "AP_InertialSensor_Invensense.h"
|
||||
#include "AP_InertialSensor_PX4.h"
|
||||
#include "AP_InertialSensor_QURT.h"
|
||||
#include "AP_InertialSensor_SITL.h"
|
||||
|
|
@ -671,69 +670,59 @@ AP_InertialSensor::detect_backends(void)
|
|||
#elif HAL_INS_DEFAULT == HAL_INS_HIL
|
||||
_add_backend(AP_InertialSensor_HIL::detect(*this));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI && defined(HAL_INS_DEFAULT_ROTATION)
|
||||
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME),
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME),
|
||||
HAL_INS_DEFAULT_ROTATION));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI
|
||||
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C && defined(HAL_INS_DEFAULT_ROTATION)
|
||||
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR),
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR),
|
||||
HAL_INS_DEFAULT_ROTATION));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C
|
||||
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_BH
|
||||
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
|
||||
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN
|
||||
|
||||
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PX4V1) {
|
||||
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
|
||||
|
||||
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK) {
|
||||
if (!_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180))) {
|
||||
// handle pixfalcon with mpu9250 instead of mpu6000
|
||||
_fast_sampling_mask.set_default(1);
|
||||
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
|
||||
}
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
|
||||
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
|
||||
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
|
||||
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME),
|
||||
ROTATION_ROLL_180, ROTATION_ROLL_180_YAW_270));
|
||||
|
||||
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
|
||||
// older Pixhawk2 boards have the MPU6000 instead of MPU9250
|
||||
_fast_sampling_mask.set_default(1);
|
||||
if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180))) {
|
||||
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_EXT_NAME), ROTATION_PITCH_180));
|
||||
}
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180));
|
||||
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
|
||||
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_G_NAME),
|
||||
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME),
|
||||
ROTATION_ROLL_180_YAW_270, ROTATION_ROLL_180_YAW_90));
|
||||
if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270))) {
|
||||
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_270));
|
||||
}
|
||||
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
|
||||
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXRACER) {
|
||||
_fast_sampling_mask.set_default(3);
|
||||
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_ROLL_180_YAW_90));
|
||||
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_ROLL_180_YAW_90));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
|
||||
|
||||
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PHMINI) {
|
||||
// PHMINI uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line
|
||||
_fast_sampling_mask.set_default(3);
|
||||
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180));
|
||||
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
|
||||
|
||||
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PH2SLIM) {
|
||||
_fast_sampling_mask.set_default(1);
|
||||
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
|
||||
}
|
||||
// also add any PX4 backends (eg. canbus sensors)
|
||||
_add_backend(AP_InertialSensor_PX4::detect(*this));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI && defined(HAL_INS_DEFAULT_ROTATION)
|
||||
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), HAL_INS_DEFAULT_ROTATION));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), HAL_INS_DEFAULT_ROTATION));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI
|
||||
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0
|
||||
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
|
||||
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
|
||||
|
|
@ -741,19 +730,19 @@ AP_InertialSensor::detect_backends(void)
|
|||
#elif HAL_INS_DEFAULT == HAL_INS_L3G4200D
|
||||
_add_backend(AP_InertialSensor_L3G4200D::probe(*this, hal.i2c_mgr->get_device(HAL_INS_L3G4200D_I2C_BUS, HAL_INS_L3G4200D_I2C_ADDR)));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_RASPILOT
|
||||
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
|
||||
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
|
||||
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
|
||||
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME),
|
||||
ROTATION_NONE, ROTATION_YAW_90));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C
|
||||
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU9250_I2C_BUS, HAL_INS_MPU9250_I2C_ADDR)));
|
||||
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU9250_I2C_BUS, HAL_INS_MPU9250_I2C_ADDR)));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_QFLIGHT
|
||||
_add_backend(AP_InertialSensor_QFLIGHT::detect(*this));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_QURT
|
||||
_add_backend(AP_InertialSensor_QURT::detect(*this));
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_BBBMINI
|
||||
AP_InertialSensor_Backend *backend = AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME));
|
||||
AP_InertialSensor_Backend *backend = AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME));
|
||||
if (backend) {
|
||||
_add_backend(backend);
|
||||
hal.console->printf("MPU9250: Onboard IMU detected\n");
|
||||
|
|
@ -761,7 +750,7 @@ AP_InertialSensor::detect_backends(void)
|
|||
hal.console->printf("MPU9250: Onboard IMU not detected\n");
|
||||
}
|
||||
|
||||
backend = AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME_EXT));
|
||||
backend = AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME_EXT));
|
||||
if (backend) {
|
||||
_add_backend(backend);
|
||||
hal.console->printf("MPU9250: External IMU detected\n");
|
||||
|
|
|
|||
|
|
@ -1,37 +1,56 @@
|
|||
/*
|
||||
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/>.
|
||||
*/
|
||||
/*
|
||||
driver for all supported Invensense IMUs, including MPU6000, MPU9250
|
||||
and ICM-20608
|
||||
*/
|
||||
|
||||
#include <assert.h>
|
||||
#include <utility>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "AP_InertialSensor_MPU6000.h"
|
||||
#include "AP_InertialSensor_Invensense.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
#include <AP_HAL_Linux/GPIO.h>
|
||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
|
||||
#define MPU6000_DRDY_PIN BBB_P8_14
|
||||
#define Invensense_DRDY_PIN BBB_P8_14
|
||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
|
||||
#define MPU6000_DRDY_PIN RPI_GPIO_24
|
||||
#define Invensense_DRDY_PIN RPI_GPIO_24
|
||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
|
||||
#define MPU6000_DRDY_PIN MINNOW_GPIO_I2S_CLK
|
||||
#define Invensense_DRDY_PIN MINNOW_GPIO_I2S_CLK
|
||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
|
||||
#define MPU6000_EXT_SYNC_ENABLE 1
|
||||
#define Invensense_EXT_SYNC_ENABLE 1
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#define debug(fmt, args ...) do {printf("MPU6000: " fmt "\n", ## args); } while(0)
|
||||
#define debug(fmt, args ...) do {printf("MPU: " fmt "\n", ## args); } while(0)
|
||||
|
||||
/*
|
||||
EXT_SYNC allows for frame synchronisation with an external device
|
||||
such as a camera. When enabled the LSB of AccelZ holds the FSYNC bit
|
||||
*/
|
||||
#ifndef MPU6000_EXT_SYNC_ENABLE
|
||||
#define MPU6000_EXT_SYNC_ENABLE 0
|
||||
#ifndef Invensense_EXT_SYNC_ENABLE
|
||||
#define Invensense_EXT_SYNC_ENABLE 0
|
||||
#endif
|
||||
|
||||
// MPU 6000 registers
|
||||
// common registers
|
||||
#define MPUREG_XG_OFFS_TC 0x00
|
||||
#define MPUREG_YG_OFFS_TC 0x01
|
||||
#define MPUREG_ZG_OFFS_TC 0x02
|
||||
|
|
@ -176,7 +195,7 @@ extern const AP_HAL::HAL& hal;
|
|||
#define MPUREG_FIFO_R_W 0x74
|
||||
#define MPUREG_WHOAMI 0x75
|
||||
|
||||
// ICM2608 specific registers
|
||||
// ICM20608 specific registers
|
||||
#define ICMREG_ACCEL_CONFIG2 0x1D
|
||||
#define ICM_ACC_DLPF_CFG_1046HZ_NOLPF 0x00
|
||||
#define ICM_ACC_DLPF_CFG_218HZ 0x01
|
||||
|
|
@ -197,7 +216,9 @@ extern const AP_HAL::HAL& hal;
|
|||
|
||||
// WHOAMI values
|
||||
#define MPU_WHOAMI_6000 0x68
|
||||
#define ICM_WHOAMI_20608 0xaf
|
||||
#define MPU_WHOAMI_20608 0xaf
|
||||
#define MPU_WHOAMI_MPU9250 0x71
|
||||
#define MPU_WHOAMI_MPU9255 0x73
|
||||
|
||||
#define BIT_READ_FLAG 0x80
|
||||
#define BIT_I2C_SLVX_EN 0x80
|
||||
|
|
@ -213,7 +234,7 @@ extern const AP_HAL::HAL& hal;
|
|||
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
|
||||
#define BITS_DLPF_CFG_MASK 0x07
|
||||
|
||||
// Product ID Description for MPU6000
|
||||
// Product ID Description for MPU6000. Used to detect buggy chips
|
||||
// high 4 bits low 4 bits
|
||||
// Product Name Product Revision
|
||||
#define MPU6000ES_REV_C4 0x14 // 0001 0100
|
||||
|
|
@ -249,9 +270,9 @@ static const float GYRO_SCALE = (0.0174532f / 16.4f);
|
|||
* variants however
|
||||
*/
|
||||
|
||||
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
enum Rotation rotation)
|
||||
AP_InertialSensor_Invensense::AP_InertialSensor_Invensense(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
enum Rotation rotation)
|
||||
: AP_InertialSensor_Backend(imu)
|
||||
, _temp_filter(1000, 1)
|
||||
, _rotation(rotation)
|
||||
|
|
@ -259,7 +280,7 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu,
|
|||
{
|
||||
}
|
||||
|
||||
AP_InertialSensor_MPU6000::~AP_InertialSensor_MPU6000()
|
||||
AP_InertialSensor_Invensense::~AP_InertialSensor_Invensense()
|
||||
{
|
||||
if (_fifo_buffer != nullptr) {
|
||||
hal.util->dma_free(_fifo_buffer, MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE);
|
||||
|
|
@ -267,50 +288,58 @@ AP_InertialSensor_MPU6000::~AP_InertialSensor_MPU6000()
|
|||
delete _auxiliary_bus;
|
||||
}
|
||||
|
||||
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::probe(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
enum Rotation rotation)
|
||||
AP_InertialSensor_Backend *AP_InertialSensor_Invensense::probe(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_InertialSensor_MPU6000 *sensor =
|
||||
new AP_InertialSensor_MPU6000(imu, std::move(dev), rotation);
|
||||
AP_InertialSensor_Invensense *sensor =
|
||||
new AP_InertialSensor_Invensense(imu, std::move(dev), rotation);
|
||||
if (!sensor || !sensor->_init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
}
|
||||
sensor->_id = HAL_INS_MPU60XX_I2C;
|
||||
if (sensor->_mpu_type == Invensense_MPU9250) {
|
||||
sensor->_id = HAL_INS_MPU9250_I2C;
|
||||
} else {
|
||||
sensor->_id = HAL_INS_MPU60XX_I2C;
|
||||
}
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
|
||||
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::probe(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
|
||||
enum Rotation rotation)
|
||||
AP_InertialSensor_Backend *AP_InertialSensor_Invensense::probe(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_InertialSensor_MPU6000 *sensor;
|
||||
AP_InertialSensor_Invensense *sensor;
|
||||
|
||||
dev->set_read_flag(0x80);
|
||||
|
||||
sensor = new AP_InertialSensor_MPU6000(imu, std::move(dev), rotation);
|
||||
sensor = new AP_InertialSensor_Invensense(imu, std::move(dev), rotation);
|
||||
if (!sensor || !sensor->_init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
}
|
||||
sensor->_id = HAL_INS_MPU60XX_SPI;
|
||||
if (sensor->_mpu_type == Invensense_MPU9250) {
|
||||
sensor->_id = HAL_INS_MPU9250_SPI;
|
||||
} else {
|
||||
sensor->_id = HAL_INS_MPU60XX_SPI;
|
||||
}
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_MPU6000::_init()
|
||||
bool AP_InertialSensor_Invensense::_init()
|
||||
{
|
||||
#ifdef MPU6000_DRDY_PIN
|
||||
_drdy_pin = hal.gpio->channel(MPU6000_DRDY_PIN);
|
||||
#ifdef Invensense_DRDY_PIN
|
||||
_drdy_pin = hal.gpio->channel(Invensense_DRDY_PIN);
|
||||
_drdy_pin->mode(HAL_GPIO_INPUT);
|
||||
#endif
|
||||
|
||||
|
|
@ -319,7 +348,7 @@ bool AP_InertialSensor_MPU6000::_init()
|
|||
return success;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_MPU6000::_fifo_reset()
|
||||
void AP_InertialSensor_Invensense::_fifo_reset()
|
||||
{
|
||||
uint8_t user_ctrl = _last_stat_user_ctrl;
|
||||
user_ctrl &= ~(BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN);
|
||||
|
|
@ -335,12 +364,12 @@ void AP_InertialSensor_MPU6000::_fifo_reset()
|
|||
_last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN;
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_MPU6000::_has_auxiliary_bus()
|
||||
bool AP_InertialSensor_Invensense::_has_auxiliary_bus()
|
||||
{
|
||||
return _dev->bus_type() != AP_HAL::Device::BUS_TYPE_I2C;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_MPU6000::start()
|
||||
void AP_InertialSensor_Invensense::start()
|
||||
{
|
||||
if (!_dev->get_semaphore()->take(0)) {
|
||||
return;
|
||||
|
|
@ -357,8 +386,21 @@ void AP_InertialSensor_MPU6000::start()
|
|||
_fifo_reset();
|
||||
|
||||
// grab the used instances
|
||||
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU6000));
|
||||
_accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(DEVTYPE_ACC_MPU6000));
|
||||
enum DevTypes gdev, adev;
|
||||
switch (_mpu_type) {
|
||||
case Invensense_MPU9250:
|
||||
gdev = DEVTYPE_GYR_MPU9250;
|
||||
adev = DEVTYPE_ACC_MPU9250;
|
||||
break;
|
||||
case Invensense_MPU6000:
|
||||
case Invensense_ICM20608:
|
||||
default:
|
||||
gdev = DEVTYPE_GYR_MPU6000;
|
||||
adev = DEVTYPE_ACC_MPU6000;
|
||||
break;
|
||||
}
|
||||
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(gdev));
|
||||
_accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(adev));
|
||||
|
||||
// setup ODR and on-sensor filtering
|
||||
_set_filter_register();
|
||||
|
|
@ -374,9 +416,8 @@ void AP_InertialSensor_MPU6000::start()
|
|||
|
||||
// read the product ID rev c has 1/2 the sensitivity of rev d
|
||||
uint8_t product_id = _register_read(MPUREG_PRODUCT_ID);
|
||||
//Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
|
||||
|
||||
if (!_is_icm_device &&
|
||||
if (_mpu_type == Invensense_MPU6000 &&
|
||||
((product_id == MPU6000ES_REV_C4) ||
|
||||
(product_id == MPU6000ES_REV_C5) ||
|
||||
(product_id == MPU6000_REV_C4) ||
|
||||
|
|
@ -392,7 +433,7 @@ void AP_InertialSensor_MPU6000::start()
|
|||
}
|
||||
hal.scheduler->delay(1);
|
||||
|
||||
if (_is_icm_device) {
|
||||
if (_mpu_type == Invensense_ICM20608) {
|
||||
// this avoids a sensor bug, see description above
|
||||
_register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
|
||||
}
|
||||
|
|
@ -417,18 +458,18 @@ void AP_InertialSensor_MPU6000::start()
|
|||
// allocate fifo buffer
|
||||
_fifo_buffer = (uint8_t *)hal.util->dma_allocate(MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE);
|
||||
if (_fifo_buffer == nullptr) {
|
||||
AP_HAL::panic("MPU6000: Unable to allocate FIFO buffer");
|
||||
AP_HAL::panic("Invensense: Unable to allocate FIFO buffer");
|
||||
}
|
||||
|
||||
// start the timer process to read samples
|
||||
_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, bool));
|
||||
_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensense::_poll_data, bool));
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
publish any pending data
|
||||
*/
|
||||
bool AP_InertialSensor_MPU6000::update()
|
||||
bool AP_InertialSensor_Invensense::update()
|
||||
{
|
||||
update_accel(_accel_instance);
|
||||
update_gyro(_gyro_instance);
|
||||
|
|
@ -441,31 +482,31 @@ bool AP_InertialSensor_MPU6000::update()
|
|||
/*
|
||||
accumulate new samples
|
||||
*/
|
||||
void AP_InertialSensor_MPU6000::accumulate()
|
||||
void AP_InertialSensor_Invensense::accumulate()
|
||||
{
|
||||
// nothing to do
|
||||
}
|
||||
|
||||
AuxiliaryBus *AP_InertialSensor_MPU6000::get_auxiliary_bus()
|
||||
AuxiliaryBus *AP_InertialSensor_Invensense::get_auxiliary_bus()
|
||||
{
|
||||
if (_auxiliary_bus) {
|
||||
return _auxiliary_bus;
|
||||
}
|
||||
|
||||
if (_has_auxiliary_bus()) {
|
||||
_auxiliary_bus = new AP_MPU6000_AuxiliaryBus(*this, _dev->get_bus_id());
|
||||
_auxiliary_bus = new AP_Invensense_AuxiliaryBus(*this, _dev->get_bus_id());
|
||||
}
|
||||
|
||||
return _auxiliary_bus;
|
||||
}
|
||||
|
||||
/*
|
||||
* Return true if the MPU6000 has new data available for reading.
|
||||
* Return true if the Invensense has new data available for reading.
|
||||
*
|
||||
* We use the data ready pin if it is available. Otherwise, read the
|
||||
* status register.
|
||||
*/
|
||||
bool AP_InertialSensor_MPU6000::_data_ready()
|
||||
bool AP_InertialSensor_Invensense::_data_ready()
|
||||
{
|
||||
if (_drdy_pin) {
|
||||
return _drdy_pin->read() != 0;
|
||||
|
|
@ -475,22 +516,22 @@ bool AP_InertialSensor_MPU6000::_data_ready()
|
|||
}
|
||||
|
||||
/*
|
||||
* Timer process to poll for new data from the MPU6000. Called from bus thread with semaphore held
|
||||
* Timer process to poll for new data from the Invensense. Called from bus thread with semaphore held
|
||||
*/
|
||||
bool AP_InertialSensor_MPU6000::_poll_data()
|
||||
bool AP_InertialSensor_Invensense::_poll_data()
|
||||
{
|
||||
_read_fifo();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
|
||||
bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_samples)
|
||||
{
|
||||
for (uint8_t i = 0; i < n_samples; i++) {
|
||||
const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
|
||||
Vector3f accel, gyro;
|
||||
bool fsync_set = false;
|
||||
|
||||
#if MPU6000_EXT_SYNC_ENABLE
|
||||
#if Invensense_EXT_SYNC_ENABLE
|
||||
fsync_set = (int16_val(data, 2) & 1U) != 0;
|
||||
#endif
|
||||
|
||||
|
|
@ -531,7 +572,7 @@ bool AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
|
|||
gives very good aliasing rejection at frequencies well above what
|
||||
can be handled with 1kHz sample rates.
|
||||
*/
|
||||
bool AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
|
||||
bool AP_InertialSensor_Invensense::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
|
||||
{
|
||||
int32_t tsum = 0;
|
||||
const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / _accel_scale;
|
||||
|
|
@ -602,7 +643,7 @@ bool AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
|
|||
return ret;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_MPU6000::_read_fifo()
|
||||
void AP_InertialSensor_Invensense::_read_fifo()
|
||||
{
|
||||
uint8_t n_samples;
|
||||
uint16_t bytes_read;
|
||||
|
|
@ -686,7 +727,7 @@ check_registers:
|
|||
/*
|
||||
fetch temperature in order to detect FIFO sync errors
|
||||
*/
|
||||
bool AP_InertialSensor_MPU6000::_check_raw_temp(int16_t t2)
|
||||
bool AP_InertialSensor_Invensense::_check_raw_temp(int16_t t2)
|
||||
{
|
||||
if (abs(t2 - _raw_temp) < 400) {
|
||||
// cached copy OK
|
||||
|
|
@ -699,20 +740,20 @@ bool AP_InertialSensor_MPU6000::_check_raw_temp(int16_t t2)
|
|||
return (abs(t2 - _raw_temp) < 400);
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_MPU6000::_block_read(uint8_t reg, uint8_t *buf,
|
||||
bool AP_InertialSensor_Invensense::_block_read(uint8_t reg, uint8_t *buf,
|
||||
uint32_t size)
|
||||
{
|
||||
return _dev->read_registers(reg, buf, size);
|
||||
}
|
||||
|
||||
uint8_t AP_InertialSensor_MPU6000::_register_read(uint8_t reg)
|
||||
uint8_t AP_InertialSensor_Invensense::_register_read(uint8_t reg)
|
||||
{
|
||||
uint8_t val = 0;
|
||||
_dev->read_registers(reg, &val, 1);
|
||||
return val;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_MPU6000::_register_write(uint8_t reg, uint8_t val, bool checked)
|
||||
void AP_InertialSensor_Invensense::_register_write(uint8_t reg, uint8_t val, bool checked)
|
||||
{
|
||||
_dev->write_register(reg, val, checked);
|
||||
}
|
||||
|
|
@ -720,22 +761,21 @@ void AP_InertialSensor_MPU6000::_register_write(uint8_t reg, uint8_t val, bool c
|
|||
/*
|
||||
set the DLPF filter frequency. Assumes caller has taken semaphore
|
||||
*/
|
||||
void AP_InertialSensor_MPU6000::_set_filter_register(void)
|
||||
void AP_InertialSensor_Invensense::_set_filter_register(void)
|
||||
{
|
||||
uint8_t config;
|
||||
|
||||
#if MPU6000_EXT_SYNC_ENABLE
|
||||
#if Invensense_EXT_SYNC_ENABLE
|
||||
// add in EXT_SYNC bit if enabled
|
||||
config = (MPUREG_CONFIG_EXT_SYNC_AZ << MPUREG_CONFIG_EXT_SYNC_SHIFT);
|
||||
#else
|
||||
config = 0;
|
||||
#endif
|
||||
|
||||
|
||||
if (enable_fast_sampling(_accel_instance)) {
|
||||
_fast_sampling = (_is_icm_device && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI);
|
||||
_fast_sampling = (_mpu_type != Invensense_MPU6000 && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI);
|
||||
if (_fast_sampling) {
|
||||
hal.console->printf("MPU6000: enabled fast sampling\n");
|
||||
hal.console->printf("MPU[%u]: enabled fast sampling\n", _accel_instance);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -750,7 +790,7 @@ void AP_InertialSensor_MPU6000::_set_filter_register(void)
|
|||
config |= MPUREG_CONFIG_FIFO_MODE_STOP;
|
||||
_register_write(MPUREG_CONFIG, config, true);
|
||||
|
||||
if (_is_icm_device) {
|
||||
if (_mpu_type != Invensense_MPU6000) {
|
||||
if (_fast_sampling) {
|
||||
// setup for 4kHz accels
|
||||
_register_write(ICMREG_ACCEL_CONFIG2, ICM_ACC_FCHOICE_B, true);
|
||||
|
|
@ -761,17 +801,20 @@ void AP_InertialSensor_MPU6000::_set_filter_register(void)
|
|||
}
|
||||
|
||||
/*
|
||||
check whoami for MPU6000 or ICM-20608
|
||||
check whoami for sensor type
|
||||
*/
|
||||
bool AP_InertialSensor_MPU6000::_check_whoami(void)
|
||||
bool AP_InertialSensor_Invensense::_check_whoami(void)
|
||||
{
|
||||
uint8_t whoami = _register_read(MPUREG_WHOAMI);
|
||||
switch (whoami) {
|
||||
case MPU_WHOAMI_6000:
|
||||
_is_icm_device = false;
|
||||
_mpu_type = Invensense_MPU6000;
|
||||
case MPU_WHOAMI_MPU9250:
|
||||
case MPU_WHOAMI_MPU9255:
|
||||
_mpu_type = Invensense_MPU9250;
|
||||
return true;
|
||||
case ICM_WHOAMI_20608:
|
||||
_is_icm_device = true;
|
||||
case MPU_WHOAMI_20608:
|
||||
_mpu_type = Invensense_ICM20608;
|
||||
return true;
|
||||
}
|
||||
// not a value WHOAMI result
|
||||
|
|
@ -779,7 +822,7 @@ bool AP_InertialSensor_MPU6000::_check_whoami(void)
|
|||
}
|
||||
|
||||
|
||||
bool AP_InertialSensor_MPU6000::_hardware_init(void)
|
||||
bool AP_InertialSensor_Invensense::_hardware_init(void)
|
||||
{
|
||||
if (!_dev->get_semaphore()->take(0)) {
|
||||
return false;
|
||||
|
|
@ -823,7 +866,7 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
|
|||
}
|
||||
|
||||
// Wake up device and select GyroZ clock. Note that the
|
||||
// MPU6000 starts up in sleep mode, and it can take some time
|
||||
// Invensense 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);
|
||||
|
|
@ -843,11 +886,11 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
|
|||
_dev->get_semaphore()->give();
|
||||
|
||||
if (tries == 5) {
|
||||
hal.console->println("Failed to boot MPU6000 5 times");
|
||||
hal.console->println("Failed to boot Invensense 5 times");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_is_icm_device) {
|
||||
if (_mpu_type == Invensense_ICM20608) {
|
||||
// this avoids a sensor bug, see description above
|
||||
_register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
|
||||
}
|
||||
|
|
@ -855,40 +898,40 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
|
|||
return true;
|
||||
}
|
||||
|
||||
AP_MPU6000_AuxiliaryBusSlave::AP_MPU6000_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr,
|
||||
AP_Invensense_AuxiliaryBusSlave::AP_Invensense_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr,
|
||||
uint8_t instance)
|
||||
: AuxiliaryBusSlave(bus, addr, instance)
|
||||
, _mpu6000_addr(MPUREG_I2C_SLV0_ADDR + _instance * 3)
|
||||
, _mpu6000_reg(_mpu6000_addr + 1)
|
||||
, _mpu6000_ctrl(_mpu6000_addr + 2)
|
||||
, _mpu6000_do(MPUREG_I2C_SLV0_DO + _instance)
|
||||
, _mpu_addr(MPUREG_I2C_SLV0_ADDR + _instance * 3)
|
||||
, _mpu_reg(_mpu_addr + 1)
|
||||
, _mpu_ctrl(_mpu_addr + 2)
|
||||
, _mpu_do(MPUREG_I2C_SLV0_DO + _instance)
|
||||
{
|
||||
}
|
||||
|
||||
int AP_MPU6000_AuxiliaryBusSlave::_set_passthrough(uint8_t reg, uint8_t size,
|
||||
int AP_Invensense_AuxiliaryBusSlave::_set_passthrough(uint8_t reg, uint8_t size,
|
||||
uint8_t *out)
|
||||
{
|
||||
auto &backend = AP_InertialSensor_MPU6000::from(_bus.get_backend());
|
||||
auto &backend = AP_InertialSensor_Invensense::from(_bus.get_backend());
|
||||
uint8_t addr;
|
||||
|
||||
/* Ensure the slave read/write is disabled before changing the registers */
|
||||
backend._register_write(_mpu6000_ctrl, 0);
|
||||
backend._register_write(_mpu_ctrl, 0);
|
||||
|
||||
if (out) {
|
||||
backend._register_write(_mpu6000_do, *out);
|
||||
backend._register_write(_mpu_do, *out);
|
||||
addr = _addr;
|
||||
} else {
|
||||
addr = _addr | BIT_READ_FLAG;
|
||||
}
|
||||
|
||||
backend._register_write(_mpu6000_addr, addr);
|
||||
backend._register_write(_mpu6000_reg, reg);
|
||||
backend._register_write(_mpu6000_ctrl, BIT_I2C_SLVX_EN | size);
|
||||
backend._register_write(_mpu_addr, addr);
|
||||
backend._register_write(_mpu_reg, reg);
|
||||
backend._register_write(_mpu_ctrl, BIT_I2C_SLVX_EN | size);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int AP_MPU6000_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *buf,
|
||||
int AP_Invensense_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *buf,
|
||||
uint8_t size)
|
||||
{
|
||||
assert(buf);
|
||||
|
|
@ -906,18 +949,18 @@ int AP_MPU6000_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *buf,
|
|||
/* wait the value to be read from the slave and read it back */
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
auto &backend = AP_InertialSensor_MPU6000::from(_bus.get_backend());
|
||||
auto &backend = AP_InertialSensor_Invensense::from(_bus.get_backend());
|
||||
if (!backend._block_read(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, size)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* disable new reads */
|
||||
backend._register_write(_mpu6000_ctrl, 0);
|
||||
backend._register_write(_mpu_ctrl, 0);
|
||||
|
||||
return size;
|
||||
}
|
||||
|
||||
int AP_MPU6000_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t val)
|
||||
int AP_Invensense_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t val)
|
||||
{
|
||||
if (_registered) {
|
||||
hal.console->println("Error: can't passthrough when slave is already configured");
|
||||
|
|
@ -932,22 +975,22 @@ int AP_MPU6000_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t val)
|
|||
/* wait the value to be written to the slave */
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
auto &backend = AP_InertialSensor_MPU6000::from(_bus.get_backend());
|
||||
auto &backend = AP_InertialSensor_Invensense::from(_bus.get_backend());
|
||||
|
||||
/* disable new writes */
|
||||
backend._register_write(_mpu6000_ctrl, 0);
|
||||
backend._register_write(_mpu_ctrl, 0);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
int AP_MPU6000_AuxiliaryBusSlave::read(uint8_t *buf)
|
||||
int AP_Invensense_AuxiliaryBusSlave::read(uint8_t *buf)
|
||||
{
|
||||
if (!_registered) {
|
||||
hal.console->println("Error: can't read before configuring slave");
|
||||
return -1;
|
||||
}
|
||||
|
||||
auto &backend = AP_InertialSensor_MPU6000::from(_bus.get_backend());
|
||||
auto &backend = AP_InertialSensor_Invensense::from(_bus.get_backend());
|
||||
if (!backend._block_read(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, _sample_size)) {
|
||||
return -1;
|
||||
}
|
||||
|
|
@ -955,31 +998,31 @@ int AP_MPU6000_AuxiliaryBusSlave::read(uint8_t *buf)
|
|||
return _sample_size;
|
||||
}
|
||||
|
||||
/* MPU6000 provides up to 5 slave devices, but the 5th is way too different to
|
||||
/* Invensense provides up to 5 slave devices, but the 5th is way too different to
|
||||
* configure and is seldom used */
|
||||
AP_MPU6000_AuxiliaryBus::AP_MPU6000_AuxiliaryBus(AP_InertialSensor_MPU6000 &backend, uint32_t devid)
|
||||
AP_Invensense_AuxiliaryBus::AP_Invensense_AuxiliaryBus(AP_InertialSensor_Invensense &backend, uint32_t devid)
|
||||
: AuxiliaryBus(backend, 4, devid)
|
||||
{
|
||||
}
|
||||
|
||||
AP_HAL::Semaphore *AP_MPU6000_AuxiliaryBus::get_semaphore()
|
||||
AP_HAL::Semaphore *AP_Invensense_AuxiliaryBus::get_semaphore()
|
||||
{
|
||||
return static_cast<AP_InertialSensor_MPU6000&>(_ins_backend)._dev->get_semaphore();
|
||||
return static_cast<AP_InertialSensor_Invensense&>(_ins_backend)._dev->get_semaphore();
|
||||
}
|
||||
|
||||
AuxiliaryBusSlave *AP_MPU6000_AuxiliaryBus::_instantiate_slave(uint8_t addr, uint8_t instance)
|
||||
AuxiliaryBusSlave *AP_Invensense_AuxiliaryBus::_instantiate_slave(uint8_t addr, uint8_t instance)
|
||||
{
|
||||
/* Enable slaves on MPU6000 if this is the first time */
|
||||
/* Enable slaves on Invensense if this is the first time */
|
||||
if (_ext_sens_data == 0) {
|
||||
_configure_slaves();
|
||||
}
|
||||
|
||||
return new AP_MPU6000_AuxiliaryBusSlave(*this, addr, instance);
|
||||
return new AP_Invensense_AuxiliaryBusSlave(*this, addr, instance);
|
||||
}
|
||||
|
||||
void AP_MPU6000_AuxiliaryBus::_configure_slaves()
|
||||
void AP_Invensense_AuxiliaryBus::_configure_slaves()
|
||||
{
|
||||
auto &backend = AP_InertialSensor_MPU6000::from(_ins_backend);
|
||||
auto &backend = AP_InertialSensor_Invensense::from(_ins_backend);
|
||||
|
||||
/* Enable the I2C master to slaves on the auxiliary I2C bus*/
|
||||
if (!(backend._last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN)) {
|
||||
|
|
@ -1001,15 +1044,15 @@ void AP_MPU6000_AuxiliaryBus::_configure_slaves()
|
|||
BIT_I2C_SLV2_DLY_EN | BIT_I2C_SLV3_DLY_EN);
|
||||
}
|
||||
|
||||
int AP_MPU6000_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave,
|
||||
int AP_Invensense_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave,
|
||||
uint8_t reg, uint8_t size)
|
||||
{
|
||||
if (_ext_sens_data + size > MAX_EXT_SENS_DATA) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
AP_MPU6000_AuxiliaryBusSlave *mpu_slave =
|
||||
static_cast<AP_MPU6000_AuxiliaryBusSlave*>(slave);
|
||||
AP_Invensense_AuxiliaryBusSlave *mpu_slave =
|
||||
static_cast<AP_Invensense_AuxiliaryBusSlave*>(slave);
|
||||
mpu_slave->_set_passthrough(reg, size);
|
||||
mpu_slave->_ext_sens_data = _ext_sens_data;
|
||||
_ext_sens_data += size;
|
||||
|
|
@ -1,4 +1,11 @@
|
|||
#pragma once
|
||||
/*
|
||||
driver for the invensense range of IMUs, including:
|
||||
|
||||
MPU6000
|
||||
MPU9250
|
||||
ICM-20608
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
|
@ -15,19 +22,19 @@
|
|||
#include "AP_InertialSensor_Backend.h"
|
||||
#include "AuxiliaryBus.h"
|
||||
|
||||
class AP_MPU6000_AuxiliaryBus;
|
||||
class AP_MPU6000_AuxiliaryBusSlave;
|
||||
class AP_Invensense_AuxiliaryBus;
|
||||
class AP_Invensense_AuxiliaryBusSlave;
|
||||
|
||||
class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
|
||||
class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend
|
||||
{
|
||||
friend AP_MPU6000_AuxiliaryBus;
|
||||
friend AP_MPU6000_AuxiliaryBusSlave;
|
||||
friend AP_Invensense_AuxiliaryBus;
|
||||
friend AP_Invensense_AuxiliaryBusSlave;
|
||||
|
||||
public:
|
||||
virtual ~AP_InertialSensor_MPU6000();
|
||||
virtual ~AP_InertialSensor_Invensense();
|
||||
|
||||
static AP_InertialSensor_MPU6000 &from(AP_InertialSensor_Backend &backend) {
|
||||
return static_cast<AP_InertialSensor_MPU6000&>(backend);
|
||||
static AP_InertialSensor_Invensense &from(AP_InertialSensor_Backend &backend) {
|
||||
return static_cast<AP_InertialSensor_Invensense&>(backend);
|
||||
}
|
||||
|
||||
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
|
||||
|
|
@ -49,8 +56,14 @@ public:
|
|||
|
||||
void start() override;
|
||||
|
||||
enum Invensense_Type {
|
||||
Invensense_MPU6000=0,
|
||||
Invensense_MPU9250,
|
||||
Invensense_ICM20608,
|
||||
};
|
||||
|
||||
private:
|
||||
AP_InertialSensor_MPU6000(AP_InertialSensor &imu,
|
||||
AP_InertialSensor_Invensense(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
enum Rotation rotation);
|
||||
|
||||
|
|
@ -99,10 +112,10 @@ private:
|
|||
|
||||
AP_HAL::DigitalSource *_drdy_pin;
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
|
||||
AP_MPU6000_AuxiliaryBus *_auxiliary_bus;
|
||||
AP_Invensense_AuxiliaryBus *_auxiliary_bus;
|
||||
|
||||
// is this an ICM-20608?
|
||||
bool _is_icm_device;
|
||||
// which sensor type this is
|
||||
enum Invensense_Type _mpu_type;
|
||||
|
||||
// are we doing more than 1kHz sampling?
|
||||
bool _fast_sampling;
|
||||
|
|
@ -126,9 +139,9 @@ private:
|
|||
} _accum;
|
||||
};
|
||||
|
||||
class AP_MPU6000_AuxiliaryBusSlave : public AuxiliaryBusSlave
|
||||
class AP_Invensense_AuxiliaryBusSlave : public AuxiliaryBusSlave
|
||||
{
|
||||
friend class AP_MPU6000_AuxiliaryBus;
|
||||
friend class AP_Invensense_AuxiliaryBus;
|
||||
|
||||
public:
|
||||
int passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size) override;
|
||||
|
|
@ -137,27 +150,27 @@ public:
|
|||
int read(uint8_t *buf) override;
|
||||
|
||||
protected:
|
||||
AP_MPU6000_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr, uint8_t instance);
|
||||
AP_Invensense_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr, uint8_t instance);
|
||||
int _set_passthrough(uint8_t reg, uint8_t size, uint8_t *out = nullptr);
|
||||
|
||||
private:
|
||||
const uint8_t _mpu6000_addr;
|
||||
const uint8_t _mpu6000_reg;
|
||||
const uint8_t _mpu6000_ctrl;
|
||||
const uint8_t _mpu6000_do;
|
||||
const uint8_t _mpu_addr;
|
||||
const uint8_t _mpu_reg;
|
||||
const uint8_t _mpu_ctrl;
|
||||
const uint8_t _mpu_do;
|
||||
|
||||
uint8_t _ext_sens_data = 0;
|
||||
};
|
||||
|
||||
class AP_MPU6000_AuxiliaryBus : public AuxiliaryBus
|
||||
class AP_Invensense_AuxiliaryBus : public AuxiliaryBus
|
||||
{
|
||||
friend class AP_InertialSensor_MPU6000;
|
||||
friend class AP_InertialSensor_Invensense;
|
||||
|
||||
public:
|
||||
AP_HAL::Semaphore *get_semaphore() override;
|
||||
|
||||
protected:
|
||||
AP_MPU6000_AuxiliaryBus(AP_InertialSensor_MPU6000 &backend, uint32_t devid);
|
||||
AP_Invensense_AuxiliaryBus(AP_InertialSensor_Invensense &backend, uint32_t devid);
|
||||
|
||||
AuxiliaryBusSlave *_instantiate_slave(uint8_t addr, uint8_t instance) override;
|
||||
int _configure_periodic_read(AuxiliaryBusSlave *slave, uint8_t reg,
|
||||
|
|
@ -1,892 +0,0 @@
|
|||
/*
|
||||
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/>.
|
||||
*/
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "AP_InertialSensor_MPU9250.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <utility>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <AP_HAL/GPIO.h>
|
||||
|
||||
#define debug(fmt, args ...) do {printf("MPU9250: " fmt "\n", ## args); } while(0)
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
// MPU9250 accelerometer scaling for 16g range
|
||||
#define MPU9250_ACCEL_SCALE_1G (GRAVITY_MSS / 2048.0f)
|
||||
|
||||
#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)
|
||||
|
||||
// MPU9250 registers
|
||||
#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_CONFIG_FIFO_MODE_STOP 0x40
|
||||
#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
|
||||
#define MPUREG_ACCEL_CONFIG2 0x1D
|
||||
#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 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
|
||||
#define MPUREG_INT_PIN_CFG 0x37
|
||||
# define BIT_INT_RD_CLEAR 0x10 // clear the interrupt when any read occurs
|
||||
# define BIT_LATCH_INT_EN 0x20 // latch data ready pin
|
||||
# define BIT_BYPASS_EN 0x02 // connect auxiliary I2C bus to the main I2C bus
|
||||
#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
|
||||
#define MPUREG_WHOAMI_MPU9250 0x71
|
||||
#define MPUREG_WHOAMI_MPU9255 0x73
|
||||
|
||||
/* 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
|
||||
|
||||
// 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
|
||||
#define BITS_DLPF_FCHOICE_B 0x08
|
||||
|
||||
#define MPU_SAMPLE_SIZE 14
|
||||
#define MPU_FIFO_DOWNSAMPLE_COUNT 8
|
||||
#define MPU_FIFO_BUFFER_LEN 16
|
||||
|
||||
#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])
|
||||
|
||||
/*
|
||||
* PS-MPU-9250A-00.pdf, page 8, lists LSB sensitivity of
|
||||
* gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
|
||||
*/
|
||||
static const float GYRO_SCALE = 0.0174532f / 16.4f;
|
||||
|
||||
/*
|
||||
* PS-MPU-9250A-00.pdf, page 9, lists LSB sensitivity of
|
||||
* accel as 4096 LSB/mg at scale factor of +/- 8g (AFS_SEL==2)
|
||||
*
|
||||
* See note below about accel scaling of engineering sample MPUXk
|
||||
* variants however
|
||||
*/
|
||||
|
||||
AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
enum Rotation rotation)
|
||||
: AP_InertialSensor_Backend(imu)
|
||||
, _temp_filter(1000, 1)
|
||||
, _dev(std::move(dev))
|
||||
, _rotation(rotation)
|
||||
{
|
||||
}
|
||||
|
||||
AP_InertialSensor_MPU9250::~AP_InertialSensor_MPU9250()
|
||||
{
|
||||
if (_fifo_buffer != nullptr) {
|
||||
hal.util->dma_free(_fifo_buffer, MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE);
|
||||
}
|
||||
delete _auxiliary_bus;
|
||||
}
|
||||
|
||||
AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_InertialSensor_MPU9250 *sensor =
|
||||
new AP_InertialSensor_MPU9250(imu, std::move(dev), rotation);
|
||||
if (!sensor || !sensor->_init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
}
|
||||
sensor->_id = HAL_INS_MPU9250_I2C;
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
|
||||
AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_InertialSensor_MPU9250 *sensor;
|
||||
|
||||
dev->set_read_flag(0x80);
|
||||
|
||||
sensor = new AP_InertialSensor_MPU9250(imu, std::move(dev), rotation);
|
||||
if (!sensor || !sensor->_init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
}
|
||||
sensor->_id = HAL_INS_MPU9250_SPI;
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_MPU9250::_init()
|
||||
{
|
||||
bool success = _hardware_init();
|
||||
return success;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_MPU9250::_fifo_reset()
|
||||
{
|
||||
uint8_t user_ctrl = _last_stat_user_ctrl;
|
||||
user_ctrl &= ~(BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN);
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
||||
_register_write(MPUREG_FIFO_EN, 0);
|
||||
_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);
|
||||
_register_write(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
|
||||
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN, true);
|
||||
hal.scheduler->delay_microseconds(1);
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
||||
_last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN;
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_MPU9250::_has_auxiliary_bus()
|
||||
{
|
||||
return _dev->bus_type() != AP_HAL::Device::BUS_TYPE_I2C;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_MPU9250::start()
|
||||
{
|
||||
if (!_dev->get_semaphore()->take(0)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// initially run the bus at low speed
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
||||
|
||||
// only used for wake-up in accelerometer only low power mode
|
||||
_register_write(MPUREG_PWR_MGMT_2, 0x00);
|
||||
hal.scheduler->delay(1);
|
||||
|
||||
// always use FIFO
|
||||
_fifo_reset();
|
||||
|
||||
// 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));
|
||||
|
||||
if (enable_fast_sampling(_accel_instance) && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
|
||||
_fast_sampling = true;
|
||||
hal.console->printf("MPU9250: enabled fast sampling\n");
|
||||
}
|
||||
|
||||
if (_fast_sampling) {
|
||||
// setup for fast sampling
|
||||
_register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2 | MPUREG_CONFIG_FIFO_MODE_STOP, true);
|
||||
} else {
|
||||
_register_write(MPUREG_CONFIG, BITS_DLPF_CFG_188HZ | MPUREG_CONFIG_FIFO_MODE_STOP, true);
|
||||
}
|
||||
|
||||
// set sample rate to 1kHz, and use the 2 pole filter to give the
|
||||
// desired rate
|
||||
_register_write(MPUREG_SMPLRT_DIV, 0, true);
|
||||
hal.scheduler->delay(1);
|
||||
|
||||
// Gyro scale 2000º/s
|
||||
_register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS, true);
|
||||
hal.scheduler->delay(1);
|
||||
|
||||
// RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 16g
|
||||
_register_write(MPUREG_ACCEL_CONFIG,3<<3, true);
|
||||
|
||||
if (_fast_sampling) {
|
||||
// setup ACCEL_FCHOICE for 4kHz sampling
|
||||
_register_write(MPUREG_ACCEL_CONFIG2, 0x08, true);
|
||||
} else {
|
||||
_register_write(MPUREG_ACCEL_CONFIG2, 0x00, true);
|
||||
}
|
||||
|
||||
// configure interrupt to fire when new data arrives
|
||||
_register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);
|
||||
|
||||
// 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);
|
||||
|
||||
// now that we have initialised, we set the bus speed to high
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
||||
|
||||
_dev->get_semaphore()->give();
|
||||
|
||||
set_gyro_orientation(_gyro_instance, _rotation);
|
||||
set_accel_orientation(_accel_instance, _rotation);
|
||||
|
||||
// allocate fifo buffer
|
||||
_fifo_buffer = (uint8_t *)hal.util->dma_allocate(MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE);
|
||||
if (_fifo_buffer == nullptr) {
|
||||
AP_HAL::panic("MPU9250: Unable to allocate FIFO buffer");
|
||||
}
|
||||
|
||||
// start the timer process to read samples
|
||||
_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_read_sample, bool));
|
||||
}
|
||||
|
||||
/*
|
||||
update the accel and gyro vectors
|
||||
*/
|
||||
bool AP_InertialSensor_MPU9250::update()
|
||||
{
|
||||
update_gyro(_gyro_instance);
|
||||
update_accel(_accel_instance);
|
||||
|
||||
_publish_temperature(_accel_instance, _temp_filtered);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
accumulate new samples
|
||||
*/
|
||||
void AP_InertialSensor_MPU9250::accumulate()
|
||||
{
|
||||
// nothing to do
|
||||
}
|
||||
|
||||
|
||||
AuxiliaryBus *AP_InertialSensor_MPU9250::get_auxiliary_bus()
|
||||
{
|
||||
if (_auxiliary_bus) {
|
||||
return _auxiliary_bus;
|
||||
}
|
||||
|
||||
if (_has_auxiliary_bus()) {
|
||||
_auxiliary_bus = new AP_MPU9250_AuxiliaryBus(*this, _dev->get_bus_id());
|
||||
}
|
||||
|
||||
return _auxiliary_bus;
|
||||
}
|
||||
|
||||
/*
|
||||
* Return true if the MPU9250 has new data available for reading.
|
||||
*/
|
||||
bool AP_InertialSensor_MPU9250::_data_ready()
|
||||
{
|
||||
uint8_t int_status = _register_read(MPUREG_INT_STATUS);
|
||||
return _data_ready(int_status);
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_MPU9250::_data_ready(uint8_t int_status)
|
||||
{
|
||||
return (int_status & BIT_RAW_RDY_INT) != 0;
|
||||
}
|
||||
|
||||
|
||||
bool AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples)
|
||||
{
|
||||
for (uint8_t i = 0; i < n_samples; i++) {
|
||||
const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
|
||||
Vector3f accel, gyro;
|
||||
|
||||
accel = Vector3f(int16_val(data, 1),
|
||||
int16_val(data, 0),
|
||||
-int16_val(data, 2));
|
||||
accel *= MPU9250_ACCEL_SCALE_1G;
|
||||
|
||||
int16_t t2 = int16_val(data, 3);
|
||||
if (!_check_raw_temp(t2)) {
|
||||
debug("temp reset %d %d %d", _raw_temp, t2, _raw_temp-t2);
|
||||
_fifo_reset();
|
||||
return false;
|
||||
}
|
||||
float temp = t2/340.0f + 36.53f;
|
||||
|
||||
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);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
when doing fast sampling the sensor gives us 8k samples/second. Every 2nd accel sample is a duplicate.
|
||||
|
||||
To filter this we first apply a 1p low pass filter at 188Hz, then we
|
||||
average over 8 samples to bring the data rate down to 1kHz. This
|
||||
gives very good aliasing rejection at frequencies well above what
|
||||
can be handled with 1kHz sample rates.
|
||||
*/
|
||||
bool AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
|
||||
{
|
||||
int32_t tsum = 0;
|
||||
const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / MPU9250_ACCEL_SCALE_1G;
|
||||
bool clipped = false;
|
||||
bool ret = true;
|
||||
|
||||
for (uint8_t i = 0; i < n_samples; i++) {
|
||||
const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
|
||||
|
||||
// use temperatue to detect FIFO corruption
|
||||
int16_t t2 = int16_val(data, 3);
|
||||
if (!_check_raw_temp(t2)) {
|
||||
debug("temp reset %d %d %d", _raw_temp, t2, _raw_temp - t2);
|
||||
_fifo_reset();
|
||||
ret = false;
|
||||
break;
|
||||
}
|
||||
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),
|
||||
int16_val(data, 4),
|
||||
-int16_val(data, 6));
|
||||
|
||||
_accum.gyro += _accum.gyro_filter.apply(g);
|
||||
_accum.count++;
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
if (clipped) {
|
||||
increment_clip_count(_accel_instance);
|
||||
}
|
||||
|
||||
if (ret) {
|
||||
float temp = (static_cast<float>(tsum)/n_samples)/340.0f + 36.53f;
|
||||
_temp_filtered = _temp_filter.apply(temp);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
fetch temperature in order to detect FIFO sync errors
|
||||
*/
|
||||
bool AP_InertialSensor_MPU9250::_check_raw_temp(int16_t t2)
|
||||
{
|
||||
if (abs(t2 - _raw_temp) < 400) {
|
||||
// cached copy OK
|
||||
return true;
|
||||
}
|
||||
uint8_t trx[2];
|
||||
if (_block_read(MPUREG_TEMP_OUT_H, trx, 2)) {
|
||||
_raw_temp = int16_val(trx, 0);
|
||||
}
|
||||
return (abs(t2 - _raw_temp) < 400);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* read from the data registers and update filtered data
|
||||
*/
|
||||
bool AP_InertialSensor_MPU9250::_read_sample()
|
||||
{
|
||||
uint8_t n_samples;
|
||||
uint16_t bytes_read;
|
||||
uint8_t *rx = _fifo_buffer;
|
||||
bool need_reset = false;
|
||||
|
||||
if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) {
|
||||
goto check_registers;
|
||||
}
|
||||
|
||||
bytes_read = uint16_val(rx, 0);
|
||||
n_samples = bytes_read / MPU_SAMPLE_SIZE;
|
||||
|
||||
if (n_samples == 0) {
|
||||
/* Not enough data in FIFO */
|
||||
goto check_registers;
|
||||
}
|
||||
|
||||
/*
|
||||
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 400Hz
|
||||
*/
|
||||
if (n_samples > 32) {
|
||||
need_reset = true;
|
||||
n_samples = 24;
|
||||
}
|
||||
|
||||
while (n_samples > 0) {
|
||||
uint8_t n = MIN(MPU_FIFO_BUFFER_LEN, n_samples);
|
||||
if (!_dev->set_chip_select(true)) {
|
||||
if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE)) {
|
||||
goto check_registers;
|
||||
}
|
||||
} else {
|
||||
// this ensures we keep things nicely setup for DMA
|
||||
uint8_t reg = MPUREG_FIFO_R_W | 0x80;
|
||||
if (!_dev->transfer(®, 1, nullptr, 0)) {
|
||||
_dev->set_chip_select(false);
|
||||
goto check_registers;
|
||||
}
|
||||
memset(rx, 0, n * MPU_SAMPLE_SIZE);
|
||||
if (!_dev->transfer(rx, n * MPU_SAMPLE_SIZE, rx, n * MPU_SAMPLE_SIZE)) {
|
||||
hal.console->printf("MPU60x0: error in fifo read %u bytes\n", n * MPU_SAMPLE_SIZE);
|
||||
_dev->set_chip_select(false);
|
||||
goto check_registers;
|
||||
}
|
||||
_dev->set_chip_select(false);
|
||||
}
|
||||
if (_fast_sampling) {
|
||||
if (!_accumulate_fast_sampling(rx, n)) {
|
||||
debug("stop at %u of %u", n_samples, bytes_read/MPU_SAMPLE_SIZE);
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
if (!_accumulate(rx, n)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
n_samples -= n;
|
||||
}
|
||||
|
||||
if (need_reset) {
|
||||
//debug("fifo reset %u", bytes_read/MPU_SAMPLE_SIZE);
|
||||
_fifo_reset();
|
||||
}
|
||||
|
||||
check_registers:
|
||||
// check next register value for correctness
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
||||
if (!_dev->check_next_register()) {
|
||||
_inc_gyro_error_count(_gyro_instance);
|
||||
_inc_accel_error_count(_accel_instance);
|
||||
}
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_MPU9250::_block_read(uint8_t reg, uint8_t *buf,
|
||||
uint32_t size)
|
||||
{
|
||||
return _dev->read_registers(reg, buf, size);
|
||||
}
|
||||
|
||||
uint8_t AP_InertialSensor_MPU9250::_register_read(uint8_t reg)
|
||||
{
|
||||
uint8_t val = 0;
|
||||
_dev->read_registers(reg, &val, 1);
|
||||
return val;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val, bool checked)
|
||||
{
|
||||
_dev->write_register(reg, val, checked);
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_MPU9250::_hardware_init(void)
|
||||
{
|
||||
if (!_dev->get_semaphore()->take(0)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// setup for register checking
|
||||
_dev->setup_checked_registers(6, 20);
|
||||
|
||||
// initially run the bus at low speed
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
||||
|
||||
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);
|
||||
goto fail_whoami;
|
||||
}
|
||||
|
||||
// Chip reset
|
||||
uint8_t tries;
|
||||
for (tries = 0; tries < 5; tries++) {
|
||||
_last_stat_user_ctrl = _register_read(MPUREG_USER_CTRL);
|
||||
|
||||
/* First disable the master I2C to avoid hanging the slaves on the
|
||||
* aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus
|
||||
* is used */
|
||||
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);
|
||||
hal.scheduler->delay(10);
|
||||
}
|
||||
|
||||
// reset device
|
||||
_register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
|
||||
hal.scheduler->delay(100);
|
||||
|
||||
/* bus-dependent initialization */
|
||||
if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
|
||||
/* Disable I2C bus if SPI selected (Recommended in Datasheet to be
|
||||
* done just after the device is reset) */
|
||||
_last_stat_user_ctrl |= BIT_USER_CTRL_I2C_IF_DIS;
|
||||
_register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl);
|
||||
}
|
||||
|
||||
// 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);
|
||||
if (_data_ready()) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (tries == 5) {
|
||||
hal.console->println("Failed to boot MPU9250 5 times");
|
||||
goto fail_tries;
|
||||
}
|
||||
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
||||
_dev->get_semaphore()->give();
|
||||
|
||||
return true;
|
||||
|
||||
fail_tries:
|
||||
fail_whoami:
|
||||
_dev->get_semaphore()->give();
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
||||
return false;
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
auto &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend());
|
||||
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);
|
||||
if (r < 0) {
|
||||
return r;
|
||||
}
|
||||
|
||||
/* wait the value to be read from the slave and read it back */
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
auto &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend());
|
||||
backend._block_read(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, size);
|
||||
|
||||
/* 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);
|
||||
if (r < 0) {
|
||||
return r;
|
||||
}
|
||||
|
||||
/* wait the value to be written to the slave */
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
auto &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend());
|
||||
|
||||
/* disable new writes */
|
||||
backend._register_write(_mpu9250_ctrl, 0);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
int AP_MPU9250_AuxiliaryBusSlave::read(uint8_t *buf)
|
||||
{
|
||||
if (!_registered) {
|
||||
hal.console->println("Error: can't read before configuring slave");
|
||||
return -1;
|
||||
}
|
||||
|
||||
auto &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend());
|
||||
if (!backend._block_read(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, _sample_size)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return _sample_size;
|
||||
}
|
||||
|
||||
/* MPU9250 provides up to 5 slave devices, but the 5th is way too different to
|
||||
* configure and is seldom used */
|
||||
AP_MPU9250_AuxiliaryBus::AP_MPU9250_AuxiliaryBus(AP_InertialSensor_MPU9250 &backend, uint32_t devid)
|
||||
: AuxiliaryBus(backend, 4, devid)
|
||||
{
|
||||
}
|
||||
|
||||
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*/
|
||||
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);
|
||||
}
|
||||
|
||||
/* 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 100Hz */
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -1,159 +0,0 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/I2CDevice.h>
|
||||
#include <AP_HAL/SPIDevice.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <Filter/Filter.h>
|
||||
#include <Filter/LowPassFilter2p.h>
|
||||
|
||||
#include "AP_InertialSensor_Backend.h"
|
||||
#include "AP_InertialSensor.h"
|
||||
#include "AuxiliaryBus.h"
|
||||
|
||||
class AP_MPU9250_AuxiliaryBus;
|
||||
class AP_MPU9250_AuxiliaryBusSlave;
|
||||
|
||||
class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend
|
||||
{
|
||||
friend AP_MPU9250_AuxiliaryBus;
|
||||
friend AP_MPU9250_AuxiliaryBusSlave;
|
||||
|
||||
public:
|
||||
virtual ~AP_InertialSensor_MPU9250();
|
||||
|
||||
static AP_InertialSensor_MPU9250 &from(AP_InertialSensor_Backend &backend) {
|
||||
return static_cast<AP_InertialSensor_MPU9250&>(backend);
|
||||
}
|
||||
|
||||
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
/* update accel and gyro state */
|
||||
bool update() override;
|
||||
void accumulate() override;
|
||||
|
||||
/*
|
||||
* Return an AuxiliaryBus if the bus driver allows it
|
||||
*/
|
||||
AuxiliaryBus *get_auxiliary_bus() override;
|
||||
|
||||
void start() override;
|
||||
|
||||
private:
|
||||
AP_InertialSensor_MPU9250(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
enum Rotation rotation);
|
||||
|
||||
bool _init();
|
||||
bool _hardware_init();
|
||||
|
||||
void _set_filter_register(uint16_t filter_hz);
|
||||
bool _has_auxiliary_bus();
|
||||
|
||||
/* Read a single sample */
|
||||
bool _read_sample();
|
||||
|
||||
void _fifo_reset();
|
||||
|
||||
/* Check if there's data available by reading register */
|
||||
bool _data_ready();
|
||||
bool _data_ready(uint8_t int_status);
|
||||
|
||||
/* Read and write functions taking the differences between buses into
|
||||
* account */
|
||||
bool _block_read(uint8_t reg, uint8_t *buf, uint32_t size);
|
||||
uint8_t _register_read(uint8_t reg);
|
||||
void _register_write(uint8_t reg, uint8_t val, bool checked=false);
|
||||
|
||||
bool _accumulate(uint8_t *samples, uint8_t n_samples);
|
||||
bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples);
|
||||
|
||||
bool _check_raw_temp(int16_t t2);
|
||||
|
||||
// instance numbers of accel and gyro data
|
||||
uint8_t _gyro_instance;
|
||||
uint8_t _accel_instance;
|
||||
|
||||
float _temp_filtered;
|
||||
LowPassFilter2pFloat _temp_filter;
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
|
||||
AP_MPU9250_AuxiliaryBus *_auxiliary_bus;
|
||||
|
||||
enum Rotation _rotation;
|
||||
|
||||
// are we doing more than 1kHz sampling?
|
||||
bool _fast_sampling;
|
||||
|
||||
// Last status from register user control
|
||||
uint8_t _last_stat_user_ctrl;
|
||||
|
||||
// buffer for fifo read
|
||||
uint8_t *_fifo_buffer;
|
||||
|
||||
int16_t _raw_temp;
|
||||
|
||||
/*
|
||||
accumulators for fast sampling
|
||||
See description in _accumulate_fast_sampling()
|
||||
*/
|
||||
struct {
|
||||
Vector3f accel;
|
||||
Vector3f gyro;
|
||||
uint8_t count;
|
||||
LowPassFilterVector3f accel_filter{8000, 188};
|
||||
LowPassFilterVector3f gyro_filter{8000, 188};
|
||||
} _accum;
|
||||
};
|
||||
|
||||
class AP_MPU9250_AuxiliaryBusSlave : public AuxiliaryBusSlave
|
||||
{
|
||||
friend class AP_MPU9250_AuxiliaryBus;
|
||||
|
||||
public:
|
||||
int passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size) override;
|
||||
int passthrough_write(uint8_t reg, uint8_t val) override;
|
||||
|
||||
int read(uint8_t *buf) override;
|
||||
|
||||
protected:
|
||||
AP_MPU9250_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr, uint8_t instance);
|
||||
int _set_passthrough(uint8_t reg, uint8_t size, uint8_t *out = nullptr);
|
||||
|
||||
private:
|
||||
const uint8_t _mpu9250_addr;
|
||||
const uint8_t _mpu9250_reg;
|
||||
const uint8_t _mpu9250_ctrl;
|
||||
const uint8_t _mpu9250_do;
|
||||
|
||||
uint8_t _ext_sens_data = 0;
|
||||
};
|
||||
|
||||
class AP_MPU9250_AuxiliaryBus : public AuxiliaryBus
|
||||
{
|
||||
friend class AP_InertialSensor_MPU9250;
|
||||
|
||||
public:
|
||||
AP_HAL::Semaphore *get_semaphore() override;
|
||||
|
||||
protected:
|
||||
AP_MPU9250_AuxiliaryBus(AP_InertialSensor_MPU9250 &backend, uint32_t devid);
|
||||
|
||||
AuxiliaryBusSlave *_instantiate_slave(uint8_t addr, uint8_t instance);
|
||||
int _configure_periodic_read(AuxiliaryBusSlave *slave, uint8_t reg,
|
||||
uint8_t size);
|
||||
|
||||
private:
|
||||
void _configure_slaves();
|
||||
|
||||
static const uint8_t MAX_EXT_SENS_DATA = 24;
|
||||
uint8_t _ext_sens_data = 0;
|
||||
};
|
||||
Loading…
Reference in New Issue