mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: MPU9250: use AP_HAL::Device abstraction
This makes MPU9250 be almost the same as MPU6000 driver. Work has been done here to make than similar so it's easier to spot the differences.
This commit is contained in:
parent
d2b267d026
commit
02a7fa5c2b
|
@ -361,7 +361,7 @@ AP_AK8963_SerialBus_MPU9250::AP_AK8963_SerialBus_MPU9250(AP_InertialSensor &ins,
|
||||||
{
|
{
|
||||||
// Only initialize members. Fails are handled by configure or while
|
// Only initialize members. Fails are handled by configure or while
|
||||||
// getting the semaphore
|
// getting the semaphore
|
||||||
_bus = ins.get_auxiliary_bus(HAL_INS_MPU9250, mpu9250_instance);
|
_bus = ins.get_auxiliary_bus(HAL_INS_MPU9250_SPI, mpu9250_instance);
|
||||||
if (!_bus)
|
if (!_bus)
|
||||||
AP_HAL::panic("Cannot get MPU9250 auxiliary bus");
|
AP_HAL::panic("Cannot get MPU9250 auxiliary bus");
|
||||||
|
|
||||||
|
|
|
@ -61,7 +61,7 @@
|
||||||
#define HAL_INS_FLYMAPLE 6
|
#define HAL_INS_FLYMAPLE 6
|
||||||
#define HAL_INS_L3G4200D 7
|
#define HAL_INS_L3G4200D 7
|
||||||
#define HAL_INS_VRBRAIN 8
|
#define HAL_INS_VRBRAIN 8
|
||||||
#define HAL_INS_MPU9250 9
|
#define HAL_INS_MPU9250_SPI 9
|
||||||
#define HAL_INS_L3GD20 10
|
#define HAL_INS_L3GD20 10
|
||||||
#define HAL_INS_LSM9DS0 11
|
#define HAL_INS_LSM9DS0 11
|
||||||
#define HAL_INS_RASPILOT 12
|
#define HAL_INS_RASPILOT 12
|
||||||
|
@ -200,7 +200,8 @@
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD
|
||||||
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
|
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
|
||||||
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
|
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
|
||||||
#define HAL_INS_DEFAULT HAL_INS_MPU9250
|
#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI
|
||||||
|
#define HAL_INS_MPU9250_NAME "mpu9250"
|
||||||
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
|
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
|
||||||
#define HAL_BARO_MS5611_NAME "ms5611"
|
#define HAL_BARO_MS5611_NAME "ms5611"
|
||||||
#define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250
|
#define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250
|
||||||
|
@ -274,7 +275,8 @@
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
|
||||||
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
|
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
|
||||||
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
|
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
|
||||||
#define HAL_INS_DEFAULT HAL_INS_MPU9250
|
#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI
|
||||||
|
#define HAL_INS_MPU9250_NAME "mpu9250"
|
||||||
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C
|
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C
|
||||||
#define HAL_BARO_MS5611_I2C_BUS 1
|
#define HAL_BARO_MS5611_I2C_BUS 1
|
||||||
#define HAL_BARO_MS5611_I2C_ADDR 0x77
|
#define HAL_BARO_MS5611_I2C_ADDR 0x77
|
||||||
|
@ -296,7 +298,8 @@
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2
|
||||||
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
|
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
|
||||||
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
|
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
|
||||||
#define HAL_INS_DEFAULT HAL_INS_MPU9250
|
#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI
|
||||||
|
#define HAL_INS_MPU9250_NAME "mpu9250"
|
||||||
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
|
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
|
||||||
#define HAL_BARO_MS5611_NAME "ms5611"
|
#define HAL_BARO_MS5611_NAME "ms5611"
|
||||||
#define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250
|
#define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250
|
||||||
|
@ -315,7 +318,8 @@
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
|
||||||
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
|
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
|
||||||
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
|
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
|
||||||
#define HAL_INS_DEFAULT HAL_INS_MPU9250
|
#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI
|
||||||
|
#define HAL_INS_MPU9250_NAME "mpu9250"
|
||||||
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
|
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
|
||||||
#define HAL_BARO_MS5611_NAME "ms5611"
|
#define HAL_BARO_MS5611_NAME "ms5611"
|
||||||
#define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250
|
#define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250
|
||||||
|
@ -368,7 +372,8 @@
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
|
||||||
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
|
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
|
||||||
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
|
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
|
||||||
#define HAL_INS_DEFAULT HAL_INS_MPU9250
|
#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI
|
||||||
|
#define HAL_INS_MPU9250_NAME "mpu9250"
|
||||||
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
|
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
|
||||||
#define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250
|
#define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250
|
||||||
#define HAL_GPIO_A_LED_PIN 24
|
#define HAL_GPIO_A_LED_PIN 24
|
||||||
|
|
|
@ -526,11 +526,11 @@ AP_InertialSensor::detect_backends(void)
|
||||||
_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_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
|
||||||
#elif HAL_INS_DEFAULT == HAL_INS_BH
|
#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_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
|
||||||
_add_backend(AP_InertialSensor_MPU9250::detect(*this, hal.spi->device(AP_HAL::SPIDevice_MPU9250)));
|
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
|
||||||
#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN
|
#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN
|
||||||
_add_backend(AP_InertialSensor_PX4::detect(*this));
|
_add_backend(AP_InertialSensor_PX4::detect(*this));
|
||||||
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250
|
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI
|
||||||
_add_backend(AP_InertialSensor_MPU9250::detect(*this, hal.spi->device(AP_HAL::SPIDevice_MPU9250)));
|
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
|
||||||
#elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE
|
#elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE
|
||||||
_add_backend(AP_InertialSensor_Flymaple::detect(*this));
|
_add_backend(AP_InertialSensor_Flymaple::detect(*this));
|
||||||
#elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0
|
#elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0
|
||||||
|
@ -544,9 +544,7 @@ AP_InertialSensor::detect_backends(void)
|
||||||
//_add_backend(AP_InertialSensor_LSM303D::detect);
|
//_add_backend(AP_InertialSensor_LSM303D::detect);
|
||||||
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
|
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
|
||||||
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C
|
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C
|
||||||
_add_backend(AP_InertialSensor_MPU9250::detect_i2c(*this,
|
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU9250_I2C_BUS, HAL_INS_MPU9250_I2C_ADDR));
|
||||||
HAL_INS_MPU9250_I2C_POINTER,
|
|
||||||
HAL_INS_MPU9250_I2C_ADDR));
|
|
||||||
#elif HAL_INS_DEFAULT == HAL_INS_QFLIGHT
|
#elif HAL_INS_DEFAULT == HAL_INS_QFLIGHT
|
||||||
_add_backend(AP_InertialSensor_QFLIGHT::detect(*this));
|
_add_backend(AP_InertialSensor_QFLIGHT::detect(*this));
|
||||||
#elif HAL_INS_DEFAULT == HAL_INS_QURT
|
#elif HAL_INS_DEFAULT == HAL_INS_QURT
|
||||||
|
|
|
@ -12,19 +12,18 @@
|
||||||
|
|
||||||
You should have received a copy of the GNU General Public License
|
You should have received a copy of the GNU General Public License
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
-- Coded by Victor Mayoral Vilches --
|
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
|
||||||
|
|
||||||
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||||
#include "AP_InertialSensor_MPU9250.h"
|
#include "AP_InertialSensor_MPU9250.h"
|
||||||
#include <AP_HAL_Linux/GPIO.h>
|
|
||||||
|
|
||||||
#include <assert.h>
|
#include <assert.h>
|
||||||
|
#include <utility>
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
#include <AP_HAL_Linux/GPIO.h>
|
||||||
|
|
||||||
|
extern const AP_HAL::HAL &hal;
|
||||||
|
|
||||||
// MPU9250 accelerometer scaling for 16g range
|
// MPU9250 accelerometer scaling for 16g range
|
||||||
#define MPU9250_ACCEL_SCALE_1G (GRAVITY_MSS / 2048.0f)
|
#define MPU9250_ACCEL_SCALE_1G (GRAVITY_MSS / 2048.0f)
|
||||||
|
@ -181,11 +180,18 @@ extern const AP_HAL::HAL& hal;
|
||||||
#define DEFAULT_SMPLRT_DIV MPUREG_SMPLRT_1000HZ
|
#define DEFAULT_SMPLRT_DIV MPUREG_SMPLRT_1000HZ
|
||||||
#define DEFAULT_SAMPLE_RATE (1000 / (DEFAULT_SMPLRT_DIV + 1))
|
#define DEFAULT_SAMPLE_RATE (1000 / (DEFAULT_SMPLRT_DIV + 1))
|
||||||
|
|
||||||
|
#define MPU9250_SAMPLE_SIZE 14
|
||||||
|
#define MPU9250_MAX_FIFO_SAMPLES 3
|
||||||
|
#define MAX_DATA_READ (MPU9250_MAX_FIFO_SAMPLES * MPU9250_SAMPLE_SIZE)
|
||||||
|
|
||||||
|
#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
|
* 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)
|
* gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
|
||||||
*/
|
*/
|
||||||
#define GYRO_SCALE (0.0174532f / 16.4f)
|
static const float GYRO_SCALE = 0.0174532f / 16.4f;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* PS-MPU-9250A-00.pdf, page 9, lists LSB sensitivity of
|
* PS-MPU-9250A-00.pdf, page 9, lists LSB sensitivity of
|
||||||
|
@ -195,263 +201,145 @@ extern const AP_HAL::HAL& hal;
|
||||||
* variants however
|
* variants however
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/*
|
AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu,
|
||||||
* 2 bytes for each in this order: ACC_X, ACC_Y, ACC_Z, TEMP, GYRO_X, GYRO_Y
|
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||||
* and GYRO_Z
|
enum bus_type type,
|
||||||
*/
|
uint8_t read_flag)
|
||||||
#define MPU9250_SAMPLE_SIZE 14
|
: AP_InertialSensor_Backend(imu)
|
||||||
|
, _read_flag(read_flag)
|
||||||
/* SPI bus driver implementation */
|
, _bus_type(type)
|
||||||
AP_MPU9250_BusDriver_SPI::AP_MPU9250_BusDriver_SPI(AP_HAL::SPIDeviceDriver *spi)
|
|
||||||
{
|
|
||||||
_spi = spi;
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_MPU9250_BusDriver_SPI::init()
|
|
||||||
{
|
|
||||||
// disable I2C as recommended by the datasheet
|
|
||||||
write8(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_MPU9250_BusDriver_SPI::read8(uint8_t reg, uint8_t *val)
|
|
||||||
{
|
|
||||||
uint8_t addr = reg | 0x80; // Set most significant bit
|
|
||||||
|
|
||||||
uint8_t tx[2];
|
|
||||||
uint8_t rx[2];
|
|
||||||
|
|
||||||
tx[0] = addr;
|
|
||||||
tx[1] = 0;
|
|
||||||
_spi->transaction(tx, rx, 2);
|
|
||||||
|
|
||||||
*val = rx[1];
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_MPU9250_BusDriver_SPI::read_block(uint8_t reg, uint8_t *val, uint8_t count)
|
|
||||||
{
|
|
||||||
assert(count < 32);
|
|
||||||
|
|
||||||
uint8_t addr = reg | 0x80; // Set most significant bit
|
|
||||||
uint8_t tx[32] = { addr, };
|
|
||||||
uint8_t rx[32];
|
|
||||||
|
|
||||||
_spi->transaction(tx, rx, count + 1);
|
|
||||||
memcpy(val, rx + 1, count);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_MPU9250_BusDriver_SPI::write8(uint8_t reg, uint8_t val)
|
|
||||||
{
|
|
||||||
uint8_t tx[2];
|
|
||||||
uint8_t rx[2];
|
|
||||||
|
|
||||||
tx[0] = reg;
|
|
||||||
tx[1] = val;
|
|
||||||
_spi->transaction(tx, rx, 2);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_MPU9250_BusDriver_SPI::set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed)
|
|
||||||
{
|
|
||||||
_spi->set_bus_speed(speed);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool AP_MPU9250_BusDriver_SPI::read_data_transaction(uint8_t *samples, uint8_t &n_samples)
|
|
||||||
{
|
|
||||||
/* one register address followed by seven 2-byte registers */
|
|
||||||
struct PACKED {
|
|
||||||
uint8_t cmd;
|
|
||||||
uint8_t int_status;
|
|
||||||
uint8_t v[MPU9250_SAMPLE_SIZE];
|
|
||||||
} rx, tx = { cmd : MPUREG_INT_STATUS | 0x80, };
|
|
||||||
|
|
||||||
_spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx));
|
|
||||||
|
|
||||||
if (!(rx.int_status & BIT_RAW_RDY_INT)) {
|
|
||||||
n_samples = 0;
|
|
||||||
#if MPU9250_DEBUG
|
|
||||||
hal.console->printf("MPU9250: No sample available.\n");
|
|
||||||
#endif
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
n_samples = 1;
|
|
||||||
memcpy(&samples[0], &rx.v[0], MPU9250_SAMPLE_SIZE);
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
AP_HAL::Semaphore* AP_MPU9250_BusDriver_SPI::get_semaphore()
|
|
||||||
{
|
|
||||||
return _spi->get_semaphore();
|
|
||||||
}
|
|
||||||
|
|
||||||
bool AP_MPU9250_BusDriver_SPI::has_auxiliary_bus()
|
|
||||||
{
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* I2C bus driver implementation */
|
|
||||||
AP_MPU9250_BusDriver_I2C::AP_MPU9250_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr)
|
|
||||||
: _addr(addr)
|
|
||||||
, _i2c(i2c)
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_MPU9250_BusDriver_I2C::init()
|
|
||||||
{
|
|
||||||
uint8_t value;
|
|
||||||
|
|
||||||
read8(MPUREG_INT_PIN_CFG, &value);
|
|
||||||
// enable I2C bypass, connecting auxiliary I2C bus to the main one
|
|
||||||
value |= BIT_BYPASS_EN;
|
|
||||||
write8(MPUREG_INT_PIN_CFG, value);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_MPU9250_BusDriver_I2C::read8(uint8_t reg, uint8_t *val)
|
|
||||||
{
|
|
||||||
_i2c->readRegister(_addr, reg, val);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_MPU9250_BusDriver_I2C::read_block(uint8_t reg, uint8_t *val, uint8_t count)
|
|
||||||
{
|
|
||||||
_i2c->readRegisters(_addr, reg, count, val);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_MPU9250_BusDriver_I2C::write8(uint8_t reg, uint8_t val)
|
|
||||||
{
|
|
||||||
_i2c->writeRegister(_addr, reg, val);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool AP_MPU9250_BusDriver_I2C::read_data_transaction(uint8_t *samples,
|
|
||||||
uint8_t &n_samples)
|
|
||||||
{
|
|
||||||
uint8_t ret = 0;
|
|
||||||
struct PACKED {
|
|
||||||
uint8_t int_status;
|
|
||||||
uint8_t v[MPU9250_SAMPLE_SIZE];
|
|
||||||
} buffer;
|
|
||||||
|
|
||||||
ret = _i2c->readRegisters(_addr, MPUREG_INT_STATUS, sizeof(buffer), (uint8_t *)&buffer);
|
|
||||||
if (ret != 0) {
|
|
||||||
hal.console->printf("MPU9250: error in I2C read\n");
|
|
||||||
n_samples = 0;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!(buffer.int_status & BIT_RAW_RDY_INT)) {
|
|
||||||
#if MPU9250_DEBUG
|
|
||||||
hal.console->printf("MPU9250: No sample available.\n");
|
|
||||||
#endif
|
|
||||||
n_samples = 0;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
memcpy(samples, buffer.v, MPU9250_SAMPLE_SIZE);
|
|
||||||
n_samples = 1;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
AP_HAL::Semaphore* AP_MPU9250_BusDriver_I2C::get_semaphore()
|
|
||||||
{
|
|
||||||
return _i2c->get_semaphore();
|
|
||||||
}
|
|
||||||
|
|
||||||
bool AP_MPU9250_BusDriver_I2C::has_auxiliary_bus()
|
|
||||||
{
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu, AP_MPU9250_BusDriver *bus) :
|
|
||||||
AP_InertialSensor_Backend(imu),
|
|
||||||
_bus(bus),
|
|
||||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
|
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
|
||||||
_default_rotation(ROTATION_ROLL_180_YAW_270)
|
, _default_rotation(ROTATION_ROLL_180_YAW_270)
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
|
||||||
/* no rotation needed */
|
, _default_rotation(ROTATION_NONE)
|
||||||
_default_rotation(ROTATION_NONE)
|
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
|
||||||
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
|
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
|
||||||
_default_rotation(ROTATION_YAW_270)
|
, _default_rotation(ROTATION_YAW_270)
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
|
||||||
_default_rotation(ROTATION_NONE)
|
, _default_rotation(ROTATION_NONE)
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
|
||||||
_default_rotation(ROTATION_NONE)
|
, _default_rotation(ROTATION_NONE)
|
||||||
#else /* rotate for bbone default (and other boards) */
|
#else
|
||||||
_default_rotation(ROTATION_ROLL_180_YAW_90)
|
/* rotate for PXF (and default for other boards) */
|
||||||
|
, _default_rotation(ROTATION_ROLL_180_YAW_90)
|
||||||
#endif
|
#endif
|
||||||
|
, _dev(std::move(dev))
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
AP_InertialSensor_MPU9250::~AP_InertialSensor_MPU9250()
|
||||||
detect the sensor
|
|
||||||
*/
|
|
||||||
AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &_imu, AP_HAL::SPIDeviceDriver *spi)
|
|
||||||
{
|
{
|
||||||
AP_MPU9250_BusDriver *bus = new AP_MPU9250_BusDriver_SPI(spi);
|
delete _auxiliary_bus;
|
||||||
if (!bus)
|
|
||||||
return NULL;
|
|
||||||
return _detect(_imu, bus, HAL_INS_MPU9250);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect_i2c(AP_InertialSensor &_imu,
|
AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &imu,
|
||||||
AP_HAL::I2CDriver *i2c,
|
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
|
||||||
uint8_t addr)
|
|
||||||
{
|
{
|
||||||
AP_MPU9250_BusDriver *bus = new AP_MPU9250_BusDriver_I2C(i2c, addr);
|
AP_InertialSensor_MPU9250 *sensor =
|
||||||
if (!bus)
|
new AP_InertialSensor_MPU9250(imu, std::move(dev), BUS_TYPE_I2C, 0);
|
||||||
return nullptr;
|
if (!sensor || !sensor->_init()) {
|
||||||
return _detect(_imu, bus, HAL_INS_MPU9250);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Common detection method - it takes ownership of the bus, freeing it if it's
|
|
||||||
* not possible to return an AP_InertialSensor_Backend */
|
|
||||||
AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::_detect(AP_InertialSensor &_imu,
|
|
||||||
AP_MPU9250_BusDriver *bus,
|
|
||||||
int16_t id)
|
|
||||||
{
|
|
||||||
AP_InertialSensor_MPU9250 *sensor = new AP_InertialSensor_MPU9250(_imu, bus);
|
|
||||||
if (sensor == NULL) {
|
|
||||||
delete bus;
|
|
||||||
return NULL;
|
|
||||||
}
|
|
||||||
if (!sensor->_init_sensor()) {
|
|
||||||
delete sensor;
|
delete sensor;
|
||||||
delete bus;
|
return nullptr;
|
||||||
return NULL;
|
|
||||||
}
|
}
|
||||||
|
sensor->_id = HAL_INS_MPU9250_I2C;
|
||||||
sensor->_id = id;
|
|
||||||
|
|
||||||
return sensor;
|
return sensor;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
|
||||||
initialise the sensor
|
AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &imu,
|
||||||
*/
|
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev)
|
||||||
bool AP_InertialSensor_MPU9250::_init_sensor()
|
|
||||||
{
|
{
|
||||||
_bus_sem = _bus->get_semaphore();
|
AP_InertialSensor_MPU9250 *sensor =
|
||||||
|
new AP_InertialSensor_MPU9250(imu, std::move(dev), BUS_TYPE_SPI, 0x80);
|
||||||
|
if (!sensor || !sensor->_init()) {
|
||||||
|
delete sensor;
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
sensor->_id = HAL_INS_MPU9250_SPI;
|
||||||
|
|
||||||
if (!_hardware_init())
|
return sensor;
|
||||||
return false;
|
}
|
||||||
|
|
||||||
_gyro_instance = _imu.register_gyro(DEFAULT_SAMPLE_RATE);
|
bool AP_InertialSensor_MPU9250::_init()
|
||||||
_accel_instance = _imu.register_accel(DEFAULT_SAMPLE_RATE);
|
{
|
||||||
|
hal.scheduler->suspend_timer_procs();
|
||||||
_product_id = AP_PRODUCT_ID_MPU9250;
|
bool success = _hardware_init();
|
||||||
|
hal.scheduler->resume_timer_procs();
|
||||||
// start the timer process to read samples
|
|
||||||
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_poll_data, void));
|
|
||||||
|
|
||||||
#if MPU9250_DEBUG
|
#if MPU9250_DEBUG
|
||||||
_dump_registers();
|
_dump_registers();
|
||||||
#endif
|
#endif
|
||||||
return true;
|
|
||||||
|
return success;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AP_InertialSensor_MPU9250::_has_auxiliary_bus()
|
||||||
|
{
|
||||||
|
return _bus_type != BUS_TYPE_I2C;
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_InertialSensor_MPU9250::start()
|
||||||
|
{
|
||||||
|
hal.scheduler->suspend_timer_procs();
|
||||||
|
|
||||||
|
if (!_dev->get_semaphore()->take(100)) {
|
||||||
|
AP_HAL::panic("MPU92500: Unable to get semaphore");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
|
// disable sensor filtering
|
||||||
|
_register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2);
|
||||||
|
|
||||||
|
// set sample rate to 1kHz, and use the 2 pole filter to give the
|
||||||
|
// desired rate
|
||||||
|
_register_write(MPUREG_SMPLRT_DIV, DEFAULT_SMPLRT_DIV);
|
||||||
|
hal.scheduler->delay(1);
|
||||||
|
|
||||||
|
// Gyro scale 2000º/s
|
||||||
|
_register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS);
|
||||||
|
hal.scheduler->delay(1);
|
||||||
|
|
||||||
|
_product_id = AP_PRODUCT_ID_MPU9250;
|
||||||
|
|
||||||
|
// RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 16g
|
||||||
|
_register_write(MPUREG_ACCEL_CONFIG,3<<3);
|
||||||
|
|
||||||
|
// 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();
|
||||||
|
|
||||||
|
// grab the used instances
|
||||||
|
_gyro_instance = _imu.register_gyro(DEFAULT_SAMPLE_RATE);
|
||||||
|
_accel_instance = _imu.register_accel(DEFAULT_SAMPLE_RATE);
|
||||||
|
|
||||||
|
hal.scheduler->resume_timer_procs();
|
||||||
|
|
||||||
|
// start the timer process to read samples
|
||||||
|
hal.scheduler->register_timer_process(
|
||||||
|
FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_poll_data, void));
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
update the accel and gyro vectors
|
update the accel and gyro vectors
|
||||||
*/
|
*/
|
||||||
bool AP_InertialSensor_MPU9250::update( void )
|
bool AP_InertialSensor_MPU9250::update()
|
||||||
{
|
{
|
||||||
update_gyro(_gyro_instance);
|
update_gyro(_gyro_instance);
|
||||||
update_accel(_accel_instance);
|
update_accel(_accel_instance);
|
||||||
|
@ -459,41 +347,50 @@ bool AP_InertialSensor_MPU9250::update( void )
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*================ HARDWARE FUNCTIONS ==================== */
|
AuxiliaryBus *AP_InertialSensor_MPU9250::get_auxiliary_bus()
|
||||||
|
|
||||||
/**
|
|
||||||
* Timer process to poll for new data from the MPU9250.
|
|
||||||
*/
|
|
||||||
void AP_InertialSensor_MPU9250::_poll_data(void)
|
|
||||||
{
|
{
|
||||||
if (!_bus_sem->take_nonblocking()) {
|
if (_auxiliary_bus) {
|
||||||
/*
|
return _auxiliary_bus;
|
||||||
the semaphore being busy is an expected condition when the
|
|
||||||
mainline code is calling wait_for_sample() which will
|
|
||||||
grab the semaphore. We return now and rely on the mainline
|
|
||||||
code grabbing the latest sample.
|
|
||||||
*/
|
|
||||||
return;
|
|
||||||
}
|
}
|
||||||
_read_data_transaction();
|
|
||||||
_bus_sem->give();
|
if (_has_auxiliary_bus()) {
|
||||||
|
_auxiliary_bus = new AP_MPU9250_AuxiliaryBus(*this);
|
||||||
|
}
|
||||||
|
|
||||||
|
return _auxiliary_bus;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
read from the data registers and update filtered data
|
* Return true if the MPU9250 has new data available for reading.
|
||||||
*/
|
*/
|
||||||
void AP_InertialSensor_MPU9250::_read_data_transaction()
|
bool AP_InertialSensor_MPU9250::_data_ready()
|
||||||
{
|
{
|
||||||
uint8_t n_samples;
|
uint8_t int_status = _register_read(MPUREG_INT_STATUS);
|
||||||
uint8_t rx[MPU9250_SAMPLE_SIZE];
|
return _data_ready(int_status);
|
||||||
|
}
|
||||||
|
|
||||||
Vector3f accel, gyro;
|
bool AP_InertialSensor_MPU9250::_data_ready(uint8_t int_status)
|
||||||
|
{
|
||||||
|
return (int_status & BIT_RAW_RDY_INT) != 0;
|
||||||
|
}
|
||||||
|
|
||||||
if (!_bus->read_data_transaction(rx, n_samples)) {
|
/*
|
||||||
|
* Timer process to poll for new data from the MPU6000.
|
||||||
|
*/
|
||||||
|
void AP_InertialSensor_MPU9250::_poll_data()
|
||||||
|
{
|
||||||
|
if (!_dev->get_semaphore()->take_nonblocking()) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
|
_read_sample();
|
||||||
|
|
||||||
|
_dev->get_semaphore()->give();
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_InertialSensor_MPU9250::_accumulate(uint8_t *rx)
|
||||||
|
{
|
||||||
|
Vector3f accel, gyro;
|
||||||
|
|
||||||
accel = Vector3f(int16_val(rx, 1),
|
accel = Vector3f(int16_val(rx, 1),
|
||||||
int16_val(rx, 0),
|
int16_val(rx, 0),
|
||||||
|
@ -508,45 +405,80 @@ void AP_InertialSensor_MPU9250::_read_data_transaction()
|
||||||
-int16_val(rx, 6));
|
-int16_val(rx, 6));
|
||||||
gyro *= GYRO_SCALE;
|
gyro *= GYRO_SCALE;
|
||||||
gyro.rotate(_default_rotation);
|
gyro.rotate(_default_rotation);
|
||||||
|
|
||||||
_rotate_and_correct_gyro(_gyro_instance, gyro);
|
_rotate_and_correct_gyro(_gyro_instance, gyro);
|
||||||
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
|
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
read an 8 bit register
|
* read from the data registers and update filtered data
|
||||||
*/
|
*/
|
||||||
|
void AP_InertialSensor_MPU9250::_read_sample()
|
||||||
|
{
|
||||||
|
/* one register address followed by seven 2-byte registers */
|
||||||
|
struct PACKED {
|
||||||
|
uint8_t int_status;
|
||||||
|
uint8_t d[14];
|
||||||
|
} rx;
|
||||||
|
|
||||||
|
if (!_block_read(MPUREG_INT_STATUS, (uint8_t *) &rx, sizeof(rx))) {
|
||||||
|
hal.console->printf("MPU9250: error reading sample\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!_data_ready(rx.int_status)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
_accumulate(rx.d);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AP_InertialSensor_MPU9250::_block_read(uint8_t reg, uint8_t *buf,
|
||||||
|
uint32_t size)
|
||||||
|
{
|
||||||
|
reg |= _read_flag;
|
||||||
|
return _dev->read_registers(reg, buf, size);
|
||||||
|
}
|
||||||
|
|
||||||
uint8_t AP_InertialSensor_MPU9250::_register_read(uint8_t reg)
|
uint8_t AP_InertialSensor_MPU9250::_register_read(uint8_t reg)
|
||||||
{
|
{
|
||||||
uint8_t val;
|
uint8_t val = 0;
|
||||||
_bus->read8(reg, &val);
|
|
||||||
|
reg |= _read_flag;
|
||||||
|
_dev->read_registers(reg, &val, 1);
|
||||||
|
|
||||||
return val;
|
return val;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
|
||||||
write an 8 bit register
|
|
||||||
*/
|
|
||||||
void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val)
|
void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val)
|
||||||
{
|
{
|
||||||
_bus->write8(reg, val);
|
_dev->write_register(reg, val);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
initialise the sensor configuration registers
|
useful when debugging SPI bus errors
|
||||||
*/
|
*/
|
||||||
|
void AP_InertialSensor_MPU9250::_register_write_check(uint8_t reg, uint8_t val)
|
||||||
|
{
|
||||||
|
uint8_t readed;
|
||||||
|
_register_write(reg, val);
|
||||||
|
readed = _register_read(reg);
|
||||||
|
if (readed != val){
|
||||||
|
hal.console->printf("Values doesn't match; written: %02x; read: %02x ", val, readed);
|
||||||
|
}
|
||||||
|
#if MPU9250_DEBUG
|
||||||
|
hal.console->printf("Values written: %02x; readed: %02x ", val, readed);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
bool AP_InertialSensor_MPU9250::_hardware_init(void)
|
bool AP_InertialSensor_MPU9250::_hardware_init(void)
|
||||||
{
|
{
|
||||||
// we need to suspend timers to prevent other SPI drivers grabbing
|
if (!_dev->get_semaphore()->take(100)) {
|
||||||
// the bus while we do the long initialisation
|
AP_HAL::panic("MPU6000: Unable to get semaphore");
|
||||||
hal.scheduler->suspend_timer_procs();
|
|
||||||
|
|
||||||
if (!_bus_sem->take(100)) {
|
|
||||||
hal.console->printf("MPU9250: Unable to get semaphore");
|
|
||||||
return false;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// initially run the bus at low speed
|
// initially run the bus at low speed
|
||||||
_bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
|
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
||||||
|
|
||||||
uint8_t value = _register_read(MPUREG_WHOAMI);
|
uint8_t value = _register_read(MPUREG_WHOAMI);
|
||||||
if (value != MPUREG_WHOAMI_MPU9250 && value != MPUREG_WHOAMI_MPU9255) {
|
if (value != MPUREG_WHOAMI_MPU9250 && value != MPUREG_WHOAMI_MPU9255) {
|
||||||
|
@ -560,7 +492,8 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void)
|
||||||
uint8_t user_ctrl = _register_read(MPUREG_USER_CTRL);
|
uint8_t user_ctrl = _register_read(MPUREG_USER_CTRL);
|
||||||
|
|
||||||
/* First disable the master I2C to avoid hanging the slaves on the
|
/* First disable the master I2C to avoid hanging the slaves on the
|
||||||
* auxiliary I2C bus */
|
* aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus
|
||||||
|
* is used */
|
||||||
if (user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
|
if (user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
|
||||||
_register_write(MPUREG_USER_CTRL, user_ctrl & ~BIT_USER_CTRL_I2C_MST_EN);
|
_register_write(MPUREG_USER_CTRL, user_ctrl & ~BIT_USER_CTRL_I2C_MST_EN);
|
||||||
hal.scheduler->delay(10);
|
hal.scheduler->delay(10);
|
||||||
|
@ -570,8 +503,12 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void)
|
||||||
_register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
|
_register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
|
||||||
hal.scheduler->delay(100);
|
hal.scheduler->delay(100);
|
||||||
|
|
||||||
// bus-dependent initialization
|
/* bus-dependent initialization */
|
||||||
_bus->init();
|
if (_bus_type == BUS_TYPE_SPI) {
|
||||||
|
/* Disable I2C bus if SPI selected (Recommended in Datasheet to be
|
||||||
|
* done just after the device is reset) */
|
||||||
|
_register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
|
||||||
|
}
|
||||||
|
|
||||||
// Wake up device and select GyroZ clock. Note that the
|
// Wake up device and select GyroZ clock. Note that the
|
||||||
// MPU9250 starts up in sleep mode, and it can take some time
|
// MPU9250 starts up in sleep mode, and it can take some time
|
||||||
|
@ -585,71 +522,31 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
hal.scheduler->delay(10);
|
hal.scheduler->delay(10);
|
||||||
uint8_t status = _register_read(MPUREG_INT_STATUS);
|
if (_data_ready()) {
|
||||||
if ((status & BIT_RAW_RDY_INT) != 0) {
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if MPU9250_DEBUG
|
#if MPU9250_DEBUG
|
||||||
_dump_registers();
|
_dump_registers();
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
if (tries == 5) {
|
if (tries == 5) {
|
||||||
hal.console->println("Failed to boot MPU9250 5 times");
|
hal.console->println("Failed to boot MPU9250 5 times");
|
||||||
goto fail_tries;
|
goto fail_tries;
|
||||||
}
|
}
|
||||||
|
|
||||||
_register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode
|
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
||||||
|
_dev->get_semaphore()->give();
|
||||||
// used no filter of 256Hz on the sensor, then filter using
|
|
||||||
// the 2-pole software filter
|
|
||||||
_register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2);
|
|
||||||
|
|
||||||
// set sample rate to 1kHz, and use the 2 pole filter to give the
|
|
||||||
// desired rate
|
|
||||||
_register_write(MPUREG_SMPLRT_DIV, DEFAULT_SMPLRT_DIV);
|
|
||||||
_register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s
|
|
||||||
|
|
||||||
// RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 16g
|
|
||||||
_register_write(MPUREG_ACCEL_CONFIG,3<<3);
|
|
||||||
|
|
||||||
// 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
|
|
||||||
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 initialized, we set the SPI bus speed to high
|
|
||||||
_bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
|
|
||||||
|
|
||||||
_bus_sem->give();
|
|
||||||
|
|
||||||
hal.scheduler->resume_timer_procs();
|
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
|
|
||||||
fail_tries:
|
fail_tries:
|
||||||
fail_whoami:
|
fail_whoami:
|
||||||
_bus_sem->give();
|
_dev->get_semaphore()->give();
|
||||||
hal.scheduler->resume_timer_procs();
|
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
||||||
_bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
AuxiliaryBus *AP_InertialSensor_MPU9250::get_auxiliary_bus()
|
|
||||||
{
|
|
||||||
if (_auxiliar_bus)
|
|
||||||
return _auxiliar_bus;
|
|
||||||
|
|
||||||
if (_bus->has_auxiliary_bus())
|
|
||||||
_auxiliar_bus = new AP_MPU9250_AuxiliaryBus(*this);
|
|
||||||
|
|
||||||
return _auxiliar_bus;
|
|
||||||
}
|
|
||||||
|
|
||||||
#if MPU9250_DEBUG
|
#if MPU9250_DEBUG
|
||||||
// dump all config registers - used for debug
|
// dump all config registers - used for debug
|
||||||
void AP_InertialSensor_MPU9250::_dump_registers(AP_MPU9250_BusDriver *bus)
|
void AP_InertialSensor_MPU9250::_dump_registers(AP_MPU9250_BusDriver *bus)
|
||||||
|
@ -666,60 +563,6 @@ void AP_InertialSensor_MPU9250::_dump_registers(AP_MPU9250_BusDriver *bus)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
AP_MPU9250_AuxiliaryBus::AP_MPU9250_AuxiliaryBus(AP_InertialSensor_MPU9250 &backend)
|
|
||||||
: AuxiliaryBus(backend, 4)
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
AP_HAL::Semaphore *AP_MPU9250_AuxiliaryBus::get_semaphore()
|
|
||||||
{
|
|
||||||
return AP_InertialSensor_MPU9250::from(_ins_backend)._bus_sem;
|
|
||||||
}
|
|
||||||
|
|
||||||
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()
|
|
||||||
{
|
|
||||||
AP_InertialSensor_MPU9250 &backend = AP_InertialSensor_MPU9250::from(_ins_backend);
|
|
||||||
|
|
||||||
/* Enable the I2C master to slaves on the auxiliary I2C bus*/
|
|
||||||
uint8_t user_ctrl = backend._register_read(MPUREG_USER_CTRL);
|
|
||||||
backend._register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN);
|
|
||||||
|
|
||||||
/* 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
AP_MPU9250_AuxiliaryBusSlave::AP_MPU9250_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr,
|
AP_MPU9250_AuxiliaryBusSlave::AP_MPU9250_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr,
|
||||||
uint8_t instance)
|
uint8_t instance)
|
||||||
: AuxiliaryBusSlave(bus, addr, instance)
|
: AuxiliaryBusSlave(bus, addr, instance)
|
||||||
|
@ -733,7 +576,7 @@ AP_MPU9250_AuxiliaryBusSlave::AP_MPU9250_AuxiliaryBusSlave(AuxiliaryBus &bus, ui
|
||||||
int AP_MPU9250_AuxiliaryBusSlave::_set_passthrough(uint8_t reg, uint8_t size,
|
int AP_MPU9250_AuxiliaryBusSlave::_set_passthrough(uint8_t reg, uint8_t size,
|
||||||
uint8_t *out)
|
uint8_t *out)
|
||||||
{
|
{
|
||||||
AP_InertialSensor_MPU9250 &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend());
|
auto &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend());
|
||||||
uint8_t addr;
|
uint8_t addr;
|
||||||
|
|
||||||
/* Ensure the slave read/write is disabled before changing the registers */
|
/* Ensure the slave read/write is disabled before changing the registers */
|
||||||
|
@ -764,14 +607,15 @@ int AP_MPU9250_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *buf,
|
||||||
}
|
}
|
||||||
|
|
||||||
int r = _set_passthrough(reg, size);
|
int r = _set_passthrough(reg, size);
|
||||||
if (r < 0)
|
if (r < 0) {
|
||||||
return r;
|
return r;
|
||||||
|
}
|
||||||
|
|
||||||
/* wait the value to be read from the slave and read it back */
|
/* wait the value to be read from the slave and read it back */
|
||||||
hal.scheduler->delay(10);
|
hal.scheduler->delay(10);
|
||||||
|
|
||||||
AP_InertialSensor_MPU9250 &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend());
|
auto &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend());
|
||||||
backend._bus->read_block(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, size);
|
backend._block_read(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, size);
|
||||||
|
|
||||||
/* disable new reads */
|
/* disable new reads */
|
||||||
backend._register_write(_mpu9250_ctrl, 0);
|
backend._register_write(_mpu9250_ctrl, 0);
|
||||||
|
@ -787,13 +631,14 @@ int AP_MPU9250_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t val)
|
||||||
}
|
}
|
||||||
|
|
||||||
int r = _set_passthrough(reg, 1, &val);
|
int r = _set_passthrough(reg, 1, &val);
|
||||||
if (r < 0)
|
if (r < 0) {
|
||||||
return r;
|
return r;
|
||||||
|
}
|
||||||
|
|
||||||
/* wait the value to be written to the slave */
|
/* wait the value to be written to the slave */
|
||||||
hal.scheduler->delay(10);
|
hal.scheduler->delay(10);
|
||||||
|
|
||||||
AP_InertialSensor_MPU9250 &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend());
|
auto &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend());
|
||||||
|
|
||||||
/* disable new writes */
|
/* disable new writes */
|
||||||
backend._register_write(_mpu9250_ctrl, 0);
|
backend._register_write(_mpu9250_ctrl, 0);
|
||||||
|
@ -808,10 +653,68 @@ int AP_MPU9250_AuxiliaryBusSlave::read(uint8_t *buf)
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
AP_InertialSensor_MPU9250 &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend());
|
auto &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend());
|
||||||
backend._bus->read_block(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, _sample_size);
|
backend._block_read(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, _sample_size);
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CONFIG_HAL_BOARD
|
/* 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)
|
||||||
|
: AuxiliaryBus(backend, 4)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
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*/
|
||||||
|
uint8_t user_ctrl = backend._register_read(MPUREG_USER_CTRL);
|
||||||
|
backend._register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN);
|
||||||
|
|
||||||
|
/* 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;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
|
@ -4,6 +4,8 @@
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
#include <AP_HAL/I2CDevice.h>
|
||||||
|
#include <AP_HAL/SPIDevice.h>
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <Filter/Filter.h>
|
#include <Filter/Filter.h>
|
||||||
#include <Filter/LowPassFilter2p.h>
|
#include <Filter/LowPassFilter2p.h>
|
||||||
|
@ -18,129 +20,87 @@ class AP_MPU9250_AuxiliaryBusSlave;
|
||||||
// enable debug to see a register dump on startup
|
// enable debug to see a register dump on startup
|
||||||
#define MPU9250_DEBUG 0
|
#define MPU9250_DEBUG 0
|
||||||
|
|
||||||
class AP_MPU9250_BusDriver
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
virtual ~AP_MPU9250_BusDriver() { };
|
|
||||||
virtual void init() = 0;
|
|
||||||
virtual void read8(uint8_t reg, uint8_t *val) = 0;
|
|
||||||
virtual void write8(uint8_t reg, uint8_t val) = 0;
|
|
||||||
virtual void read_block(uint8_t reg, uint8_t *val, uint8_t count) = 0;
|
|
||||||
virtual void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed) = 0;
|
|
||||||
virtual bool read_data_transaction(uint8_t* samples,
|
|
||||||
uint8_t &n_samples) = 0;
|
|
||||||
virtual AP_HAL::Semaphore* get_semaphore() = 0;
|
|
||||||
virtual bool has_auxiliary_bus() = 0;
|
|
||||||
};
|
|
||||||
|
|
||||||
class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend
|
class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend
|
||||||
{
|
{
|
||||||
friend AP_MPU9250_AuxiliaryBus;
|
friend AP_MPU9250_AuxiliaryBus;
|
||||||
friend AP_MPU9250_AuxiliaryBusSlave;
|
friend AP_MPU9250_AuxiliaryBusSlave;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
virtual ~AP_InertialSensor_MPU9250();
|
||||||
AP_InertialSensor_MPU9250(AP_InertialSensor &imu, AP_MPU9250_BusDriver *bus);
|
|
||||||
|
|
||||||
/* update accel and gyro state */
|
|
||||||
bool update();
|
|
||||||
|
|
||||||
AuxiliaryBus *get_auxiliary_bus();
|
|
||||||
|
|
||||||
static AP_InertialSensor_MPU9250 &from(AP_InertialSensor_Backend &backend) {
|
static AP_InertialSensor_MPU9250 &from(AP_InertialSensor_Backend &backend) {
|
||||||
return static_cast<AP_InertialSensor_MPU9250&>(backend);
|
return static_cast<AP_InertialSensor_MPU9250&>(backend);
|
||||||
}
|
}
|
||||||
|
|
||||||
// detect the sensor
|
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
|
||||||
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, AP_HAL::SPIDeviceDriver *spi);
|
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
|
||||||
static AP_InertialSensor_Backend *detect_i2c(AP_InertialSensor &imu,
|
|
||||||
AP_HAL::I2CDriver *i2c,
|
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
|
||||||
uint8_t addr);
|
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev);
|
||||||
|
|
||||||
|
/* update accel and gyro state */
|
||||||
|
bool update();
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Return an AuxiliaryBus if the bus driver allows it
|
||||||
|
*/
|
||||||
|
AuxiliaryBus *get_auxiliary_bus() override;
|
||||||
|
|
||||||
|
void start() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
static AP_InertialSensor_Backend *_detect(AP_InertialSensor &_imu,
|
enum bus_type {
|
||||||
AP_MPU9250_BusDriver *bus,
|
BUS_TYPE_I2C = 0,
|
||||||
int16_t id);
|
BUS_TYPE_SPI,
|
||||||
|
};
|
||||||
|
|
||||||
bool _init_sensor();
|
AP_InertialSensor_MPU9250(AP_InertialSensor &imu,
|
||||||
void _read_data_transaction();
|
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||||
|
enum bus_type bus_type,
|
||||||
|
uint8_t read_flag);
|
||||||
|
|
||||||
|
#if MPU9250_DEBUG
|
||||||
|
static void _dump_registers();
|
||||||
|
#endif
|
||||||
|
|
||||||
|
bool _init();
|
||||||
|
bool _hardware_init();
|
||||||
|
|
||||||
|
void _set_filter_register(uint16_t filter_hz);
|
||||||
|
bool _has_auxiliary_bus();
|
||||||
|
|
||||||
|
/* Read a single sample */
|
||||||
|
void _read_sample();
|
||||||
|
|
||||||
|
/* Check if there's data available by reading register */
|
||||||
bool _data_ready();
|
bool _data_ready();
|
||||||
void _poll_data(void);
|
bool _data_ready(uint8_t int_status);
|
||||||
uint8_t _register_read( uint8_t reg );
|
|
||||||
void _register_write( uint8_t reg, uint8_t val );
|
|
||||||
bool _hardware_init(void);
|
|
||||||
|
|
||||||
AP_MPU9250_BusDriver *_bus;
|
/* Poll for new data (non-blocking) */
|
||||||
AP_HAL::Semaphore *_bus_sem;
|
void _poll_data();
|
||||||
AP_MPU9250_AuxiliaryBus *_auxiliar_bus = nullptr;
|
|
||||||
|
|
||||||
// gyro and accel instances
|
/* 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 );
|
||||||
|
void _register_write_check(uint8_t reg, uint8_t val);
|
||||||
|
|
||||||
|
void _accumulate(uint8_t *sample);
|
||||||
|
|
||||||
|
// instance numbers of accel and gyro data
|
||||||
uint8_t _gyro_instance;
|
uint8_t _gyro_instance;
|
||||||
uint8_t _accel_instance;
|
uint8_t _accel_instance;
|
||||||
|
|
||||||
|
const uint8_t _read_flag;
|
||||||
|
const enum bus_type _bus_type;
|
||||||
|
|
||||||
// The default rotation for the IMU, its value depends on how the IMU is
|
// The default rotation for the IMU, its value depends on how the IMU is
|
||||||
// placed by default on the system
|
// placed by default on the system
|
||||||
enum Rotation _default_rotation;
|
enum Rotation _default_rotation;
|
||||||
|
|
||||||
#if MPU9250_DEBUG
|
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
|
||||||
static void _dump_registers();
|
AP_MPU9250_AuxiliaryBus *_auxiliary_bus;
|
||||||
#endif
|
|
||||||
};
|
|
||||||
|
|
||||||
class AP_MPU9250_BusDriver_SPI : public AP_MPU9250_BusDriver
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
AP_MPU9250_BusDriver_SPI(AP_HAL::SPIDeviceDriver *spi);
|
|
||||||
void init();
|
|
||||||
void read8(uint8_t reg, uint8_t *val);
|
|
||||||
void write8(uint8_t reg, uint8_t val);
|
|
||||||
void read_block(uint8_t reg, uint8_t *val, uint8_t count);
|
|
||||||
void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
|
|
||||||
bool read_data_transaction(uint8_t* samples, uint8_t &n_samples);
|
|
||||||
AP_HAL::Semaphore* get_semaphore();
|
|
||||||
bool has_auxiliary_bus();
|
|
||||||
|
|
||||||
private:
|
|
||||||
AP_HAL::SPIDeviceDriver *_spi;
|
|
||||||
};
|
|
||||||
|
|
||||||
class AP_MPU9250_BusDriver_I2C : public AP_MPU9250_BusDriver
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
AP_MPU9250_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr);
|
|
||||||
void init();
|
|
||||||
void read8(uint8_t reg, uint8_t *val);
|
|
||||||
void write8(uint8_t reg, uint8_t val);
|
|
||||||
void read_block(uint8_t reg, uint8_t *val, uint8_t count);
|
|
||||||
void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed) {};
|
|
||||||
bool read_data_transaction(uint8_t* samples, uint8_t &n_samples);
|
|
||||||
AP_HAL::Semaphore* get_semaphore();
|
|
||||||
bool has_auxiliary_bus();
|
|
||||||
|
|
||||||
private:
|
|
||||||
uint8_t _addr;
|
|
||||||
AP_HAL::I2CDriver *_i2c;
|
|
||||||
};
|
|
||||||
|
|
||||||
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);
|
|
||||||
|
|
||||||
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;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
class AP_MPU9250_AuxiliaryBusSlave : public AuxiliaryBusSlave
|
class AP_MPU9250_AuxiliaryBusSlave : public AuxiliaryBusSlave
|
||||||
|
@ -165,3 +125,24 @@ private:
|
||||||
|
|
||||||
uint8_t _ext_sens_data = 0;
|
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);
|
||||||
|
|
||||||
|
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