mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
mpu9250: create dedicated i2c version and delete legacy driver
- update crazyflie and bbblue usage - eventually this should be merged with the SPI version after interface changes are made
This commit is contained in:
Daniel Agar
parent
c9a7894230
commit
3abe2e82d1
@ -24,7 +24,7 @@ px4_add_board(
|
||||
distance_sensor # all available distance sensor drivers
|
||||
gps
|
||||
#imu # all available imu drivers
|
||||
imu/mpu9250
|
||||
imu/invensense/mpu9250
|
||||
linux_pwm_out
|
||||
#magnetometer # all available magnetometer drivers
|
||||
magnetometer/hmc5883
|
||||
|
||||
@ -11,7 +11,8 @@ px4_add_board(
|
||||
barometer/lps25h
|
||||
distance_sensor/vl53l0x
|
||||
gps
|
||||
imu/mpu9250
|
||||
magnetometer/akm/ak8963
|
||||
imu/invensense/mpu9250
|
||||
optical_flow/pmw3901
|
||||
pwm_out
|
||||
MODULES
|
||||
@ -23,9 +24,9 @@ px4_add_board(
|
||||
events
|
||||
flight_mode_manager
|
||||
land_detector
|
||||
landing_target_estimator
|
||||
#landing_target_estimator
|
||||
load_mon
|
||||
local_position_estimator
|
||||
#local_position_estimator
|
||||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
|
||||
@ -6,10 +6,11 @@
|
||||
board_adc start
|
||||
|
||||
# Onboard I2C
|
||||
mpu9250 -I -R 12 start
|
||||
|
||||
# I2C bypass of mpu
|
||||
lps25h -I start
|
||||
if mpu9250_i2c -I -b 3 -a 0x69 -R 6 start; then
|
||||
sleep 1 # wait for mpu9250 to be configured with bypass enabled
|
||||
ak8963 -I -b 3 -R 4 start
|
||||
lps25h -I -b 3 start
|
||||
fi
|
||||
|
||||
# Optical flow deck
|
||||
vl53l0x start -X
|
||||
|
||||
@ -58,11 +58,11 @@ static constexpr wq_config_t SPI4{"wq:SPI4", 2336, -5};
|
||||
static constexpr wq_config_t SPI5{"wq:SPI5", 2336, -6};
|
||||
static constexpr wq_config_t SPI6{"wq:SPI6", 2336, -7};
|
||||
|
||||
static constexpr wq_config_t I2C0{"wq:I2C0", 1472, -8};
|
||||
static constexpr wq_config_t I2C1{"wq:I2C1", 1472, -9};
|
||||
static constexpr wq_config_t I2C2{"wq:I2C2", 1472, -10};
|
||||
static constexpr wq_config_t I2C3{"wq:I2C3", 1472, -11};
|
||||
static constexpr wq_config_t I2C4{"wq:I2C4", 1472, -12};
|
||||
static constexpr wq_config_t I2C0{"wq:I2C0", 2336, -8};
|
||||
static constexpr wq_config_t I2C1{"wq:I2C1", 2336, -9};
|
||||
static constexpr wq_config_t I2C2{"wq:I2C2", 2336, -10};
|
||||
static constexpr wq_config_t I2C3{"wq:I2C3", 2336, -11};
|
||||
static constexpr wq_config_t I2C4{"wq:I2C4", 2336, -12};
|
||||
|
||||
// PX4 att/pos controllers, highest priority after sensors.
|
||||
static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 1730, -13};
|
||||
|
||||
@ -51,7 +51,7 @@ load_mon start
|
||||
|
||||
bmp280 -I start
|
||||
|
||||
mpu9250 -I start
|
||||
mpu9250_i2c -I start
|
||||
# options: -R rotation
|
||||
|
||||
gps start -d /dev/ttyS2 -s -p ubx
|
||||
|
||||
@ -49,7 +49,7 @@ dataman start
|
||||
|
||||
bmp280 -I start
|
||||
|
||||
mpu9250 -I start
|
||||
mpu9250_i2c -I start
|
||||
# options: -R rotation
|
||||
|
||||
gps start -d /dev/ttyS2 -s -p ubx
|
||||
|
||||
@ -7,7 +7,7 @@ gps start -d /dev/tty-4
|
||||
param set MAV_TYPE 2
|
||||
sleep 1
|
||||
hmc5883 start
|
||||
mpu9250 start_without_mag
|
||||
mpu9250 start
|
||||
bmp280 start
|
||||
sleep 1
|
||||
rc_update start
|
||||
|
||||
@ -48,4 +48,19 @@ px4_add_module(
|
||||
drivers_gyroscope
|
||||
drivers_magnetometer
|
||||
px4_work_queue
|
||||
)
|
||||
)
|
||||
|
||||
px4_add_module(
|
||||
MODULE drivers__imu__invensense__mpu9250_i2c
|
||||
MAIN mpu9250_i2c
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
InvenSense_MPU9250_registers.hpp
|
||||
MPU9250_I2C.cpp
|
||||
MPU9250_I2C.hpp
|
||||
mpu9250_i2c_main.cpp
|
||||
DEPENDS
|
||||
drivers_accelerometer
|
||||
drivers_gyroscope
|
||||
px4_work_queue
|
||||
)
|
||||
|
||||
@ -54,6 +54,8 @@ static constexpr uint8_t Bit5 = (1 << 5);
|
||||
static constexpr uint8_t Bit6 = (1 << 6);
|
||||
static constexpr uint8_t Bit7 = (1 << 7);
|
||||
|
||||
static constexpr uint32_t I2C_ADDRESS_DEFAULT = 0x69; // 0b110100X
|
||||
static constexpr uint32_t I2C_SPEED = 400 * 1000;
|
||||
|
||||
static constexpr uint32_t SPI_SPEED = 1 * 1000 * 1000;
|
||||
static constexpr uint32_t SPI_SPEED_SENSOR = 10 * 1000 * 1000; // 20MHz for reading sensor and interrupt registers
|
||||
@ -106,7 +108,9 @@ enum class Register : uint8_t {
|
||||
enum CONFIG_BIT : uint8_t {
|
||||
FIFO_MODE = Bit6, // when the FIFO is full, additional writes will not be written to FIFO
|
||||
|
||||
DLPF_CFG_BYPASS_DLPF_8KHZ = 7, // Rate 8 kHz [2:0]
|
||||
// DLPF_CFG[2:0]
|
||||
DLPF_CFG_Fs_1KHZ = 1, // Rate 1 kHz, 184 Hz Bandwidth
|
||||
DLPF_CFG_BYPASS_DLPF_8KHZ = 7, // Rate 8 kHz, 3600 Hz Bandwidth
|
||||
};
|
||||
|
||||
// GYRO_CONFIG
|
||||
@ -118,7 +122,7 @@ enum GYRO_CONFIG_BIT : uint8_t {
|
||||
GYRO_FS_SEL_2000_DPS = Bit4 | Bit3, // 0b11000
|
||||
|
||||
// FCHOICE_B [1:0]
|
||||
FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b00 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz
|
||||
FCHOICE_B_BYPASS_DLPF = Bit1 | Bit0, // 0b00 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz
|
||||
};
|
||||
|
||||
// ACCEL_CONFIG
|
||||
@ -133,6 +137,9 @@ enum ACCEL_CONFIG_BIT : uint8_t {
|
||||
// ACCEL_CONFIG2
|
||||
enum ACCEL_CONFIG2_BIT : uint8_t {
|
||||
ACCEL_FCHOICE_B_BYPASS_DLPF = Bit3,
|
||||
|
||||
// [2:0] A_DLPFCFG
|
||||
A_DLPFCFG_BW_218HZ_DLPF = 1, // Rate 1 kHz, 218.1 Hz Bandwidth (DLPF filter Block)
|
||||
};
|
||||
|
||||
// FIFO_EN
|
||||
@ -177,7 +184,8 @@ enum I2C_SLV4_CTRL_BIT : uint8_t {
|
||||
|
||||
// INT_PIN_CFG
|
||||
enum INT_PIN_CFG_BIT : uint8_t {
|
||||
ACTL = Bit7,
|
||||
ACTL = Bit7,
|
||||
BYPASS_EN = Bit1, // interface pins(ES_CL and ES_DA) will go into ‘bypass mode’ when the i2c master interface is disabled
|
||||
};
|
||||
|
||||
// INT_ENABLE
|
||||
|
||||
@ -173,7 +173,7 @@ private:
|
||||
register_config_t _register_cfg[size_register_cfg] {
|
||||
// Register | Set bits, Clear bits
|
||||
{ Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 },
|
||||
{ Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF },
|
||||
{ Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_BYPASS_DLPF },
|
||||
{ Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 },
|
||||
{ Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF, 0 },
|
||||
{ Register::FIFO_EN, FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT | FIFO_EN_BIT::ACCEL, 0 },
|
||||
|
||||
591
src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp
Normal file
591
src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp
Normal file
@ -0,0 +1,591 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "MPU9250_I2C.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
||||
{
|
||||
return (msb << 8u) | lsb;
|
||||
}
|
||||
|
||||
MPU9250_I2C::MPU9250_I2C(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation,
|
||||
int bus_frequency, int address, spi_drdy_gpio_t drdy_gpio) :
|
||||
I2C(DRV_IMU_DEVTYPE_MPU9250, MODULE_NAME, bus, address, bus_frequency),
|
||||
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
|
||||
_drdy_gpio(drdy_gpio),
|
||||
_px4_accel(get_device_id(), rotation),
|
||||
_px4_gyro(get_device_id(), rotation)
|
||||
{
|
||||
if (drdy_gpio != 0) {
|
||||
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
|
||||
}
|
||||
|
||||
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
|
||||
}
|
||||
|
||||
MPU9250_I2C::~MPU9250_I2C()
|
||||
{
|
||||
perf_free(_bad_register_perf);
|
||||
perf_free(_bad_transfer_perf);
|
||||
perf_free(_fifo_empty_perf);
|
||||
perf_free(_fifo_overflow_perf);
|
||||
perf_free(_fifo_reset_perf);
|
||||
perf_free(_drdy_missed_perf);
|
||||
}
|
||||
|
||||
int MPU9250_I2C::init()
|
||||
{
|
||||
int ret = I2C::init();
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
DEVICE_DEBUG("I2C::init failed (%i)", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
return Reset() ? 0 : -1;
|
||||
}
|
||||
|
||||
bool MPU9250_I2C::Reset()
|
||||
{
|
||||
_state = STATE::RESET;
|
||||
DataReadyInterruptDisable();
|
||||
ScheduleClear();
|
||||
ScheduleNow();
|
||||
return true;
|
||||
}
|
||||
|
||||
void MPU9250_I2C::exit_and_cleanup()
|
||||
{
|
||||
DataReadyInterruptDisable();
|
||||
I2CSPIDriverBase::exit_and_cleanup();
|
||||
}
|
||||
|
||||
void MPU9250_I2C::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
|
||||
PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
|
||||
|
||||
perf_print_counter(_bad_register_perf);
|
||||
perf_print_counter(_bad_transfer_perf);
|
||||
perf_print_counter(_fifo_empty_perf);
|
||||
perf_print_counter(_fifo_overflow_perf);
|
||||
perf_print_counter(_fifo_reset_perf);
|
||||
perf_print_counter(_drdy_missed_perf);
|
||||
}
|
||||
|
||||
int MPU9250_I2C::probe()
|
||||
{
|
||||
const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
|
||||
|
||||
if (whoami != WHOAMI) {
|
||||
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void MPU9250_I2C::RunImpl()
|
||||
{
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
switch (_state) {
|
||||
case STATE::RESET:
|
||||
// PWR_MGMT_1: Device Reset
|
||||
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::H_RESET);
|
||||
_reset_timestamp = now;
|
||||
_failure_count = 0;
|
||||
_state = STATE::WAIT_FOR_RESET;
|
||||
ScheduleDelayed(100_ms);
|
||||
break;
|
||||
|
||||
case STATE::WAIT_FOR_RESET:
|
||||
|
||||
// The reset value is 0x00 for all registers other than the registers below
|
||||
// Document Number: RM-MPU-9250A-00 Page 9 of 55
|
||||
if ((RegisterRead(Register::WHO_AM_I) == WHOAMI)
|
||||
&& (RegisterRead(Register::PWR_MGMT_1) == 0x01)) {
|
||||
|
||||
// Wakeup and reset digital signal path
|
||||
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0);
|
||||
RegisterWrite(Register::SIGNAL_PATH_RESET,
|
||||
SIGNAL_PATH_RESET_BIT::GYRO_RESET | SIGNAL_PATH_RESET_BIT::ACCEL_RESET | SIGNAL_PATH_RESET_BIT::TEMP_RESET);
|
||||
RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::SIG_COND_RST);
|
||||
|
||||
// if reset succeeded then configure
|
||||
_state = STATE::CONFIGURE;
|
||||
ScheduleDelayed(100_ms);
|
||||
|
||||
} else {
|
||||
// RESET not complete
|
||||
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
|
||||
PX4_DEBUG("Reset failed, retrying");
|
||||
_state = STATE::RESET;
|
||||
ScheduleDelayed(100_ms);
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("Reset not complete, check again in 10 ms");
|
||||
ScheduleDelayed(10_ms);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE::CONFIGURE:
|
||||
if (Configure()) {
|
||||
// if configure succeeded then start reading from FIFO
|
||||
_state = STATE::FIFO_READ;
|
||||
|
||||
if (DataReadyInterruptConfigure()) {
|
||||
_data_ready_interrupt_enabled = true;
|
||||
|
||||
// backup schedule as a watchdog timeout
|
||||
ScheduleDelayed(100_ms);
|
||||
|
||||
} else {
|
||||
_data_ready_interrupt_enabled = false;
|
||||
ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
|
||||
}
|
||||
|
||||
FIFOReset();
|
||||
|
||||
} else {
|
||||
// CONFIGURE not complete
|
||||
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
|
||||
PX4_DEBUG("Configure failed, resetting");
|
||||
_state = STATE::RESET;
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("Configure failed, retrying");
|
||||
}
|
||||
|
||||
ScheduleDelayed(100_ms);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE::FIFO_READ: {
|
||||
if (_data_ready_interrupt_enabled) {
|
||||
// scheduled from interrupt if _drdy_fifo_read_samples was set
|
||||
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
|
||||
perf_count(_drdy_missed_perf);
|
||||
}
|
||||
|
||||
// push backup schedule back
|
||||
ScheduleDelayed(_fifo_empty_interval_us * 2);
|
||||
}
|
||||
|
||||
// always check current FIFO count
|
||||
bool success = false;
|
||||
const uint16_t fifo_count = FIFOReadCount();
|
||||
|
||||
if (fifo_count >= FIFO::SIZE) {
|
||||
FIFOReset();
|
||||
perf_count(_fifo_overflow_perf);
|
||||
|
||||
} else if (fifo_count == 0) {
|
||||
perf_count(_fifo_empty_perf);
|
||||
|
||||
} else {
|
||||
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
|
||||
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
|
||||
SAMPLES_PER_TRANSFER; // round down to nearest
|
||||
|
||||
if (samples > FIFO_MAX_SAMPLES) {
|
||||
// not technically an overflow, but more samples than we expected or can publish
|
||||
FIFOReset();
|
||||
perf_count(_fifo_overflow_perf);
|
||||
|
||||
} else if (samples >= 1) {
|
||||
if (FIFORead(now, samples)) {
|
||||
success = true;
|
||||
|
||||
if (_failure_count > 0) {
|
||||
_failure_count--;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!success) {
|
||||
_failure_count++;
|
||||
|
||||
// full reset if things are failing consistently
|
||||
if (_failure_count > 10) {
|
||||
Reset();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
|
||||
// check configuration registers periodically or immediately following any failure
|
||||
if (RegisterCheck(_register_cfg[_checked_register])) {
|
||||
_last_config_check_timestamp = now;
|
||||
_checked_register = (_checked_register + 1) % size_register_cfg;
|
||||
|
||||
} else {
|
||||
// register check failed, force reset
|
||||
perf_count(_bad_register_perf);
|
||||
Reset();
|
||||
}
|
||||
|
||||
} else {
|
||||
// periodically update temperature (~1 Hz)
|
||||
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
|
||||
UpdateTemperature();
|
||||
_temperature_update_timestamp = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void MPU9250_I2C::ConfigureAccel()
|
||||
{
|
||||
const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0]
|
||||
|
||||
switch (ACCEL_FS_SEL) {
|
||||
case ACCEL_FS_SEL_2G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
|
||||
_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
|
||||
break;
|
||||
|
||||
case ACCEL_FS_SEL_4G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
|
||||
_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
|
||||
break;
|
||||
|
||||
case ACCEL_FS_SEL_8G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
|
||||
_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
|
||||
break;
|
||||
|
||||
case ACCEL_FS_SEL_16G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
|
||||
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void MPU9250_I2C::ConfigureGyro()
|
||||
{
|
||||
const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] GYRO_FS_SEL[1:0]
|
||||
|
||||
float range_dps = 0.f;
|
||||
|
||||
switch (GYRO_FS_SEL) {
|
||||
case GYRO_FS_SEL_250_DPS:
|
||||
range_dps = 250.f;
|
||||
break;
|
||||
|
||||
case GYRO_FS_SEL_500_DPS:
|
||||
range_dps = 500.f;
|
||||
break;
|
||||
|
||||
case GYRO_FS_SEL_1000_DPS:
|
||||
range_dps = 1000.f;
|
||||
break;
|
||||
|
||||
case GYRO_FS_SEL_2000_DPS:
|
||||
range_dps = 2000.f;
|
||||
break;
|
||||
}
|
||||
|
||||
_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
|
||||
_px4_gyro.set_range(math::radians(range_dps));
|
||||
}
|
||||
|
||||
void MPU9250_I2C::ConfigureSampleRate(int sample_rate)
|
||||
{
|
||||
if (sample_rate == 0) {
|
||||
sample_rate = 1000; // default to 1000 Hz
|
||||
}
|
||||
|
||||
// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
|
||||
const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
|
||||
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
|
||||
|
||||
_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
|
||||
|
||||
// recompute FIFO empty interval (us) with actual gyro sample limit
|
||||
_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
|
||||
}
|
||||
|
||||
bool MPU9250_I2C::Configure()
|
||||
{
|
||||
// first set and clear all configured register bits
|
||||
for (const auto ®_cfg : _register_cfg) {
|
||||
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
|
||||
}
|
||||
|
||||
// now check that all are configured
|
||||
bool success = true;
|
||||
|
||||
for (const auto ®_cfg : _register_cfg) {
|
||||
if (!RegisterCheck(reg_cfg)) {
|
||||
success = false;
|
||||
}
|
||||
}
|
||||
|
||||
ConfigureAccel();
|
||||
ConfigureGyro();
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
int MPU9250_I2C::DataReadyInterruptCallback(int irq, void *context, void *arg)
|
||||
{
|
||||
static_cast<MPU9250_I2C *>(arg)->DataReady();
|
||||
return 0;
|
||||
}
|
||||
|
||||
void MPU9250_I2C::DataReady()
|
||||
{
|
||||
uint32_t expected = 0;
|
||||
|
||||
// at least the required number of samples in the FIFO
|
||||
if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
|
||||
&& _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
|
||||
|
||||
_drdy_count.store(0);
|
||||
ScheduleNow();
|
||||
}
|
||||
}
|
||||
|
||||
bool MPU9250_I2C::DataReadyInterruptConfigure()
|
||||
{
|
||||
// TODO
|
||||
return false;
|
||||
|
||||
if (_drdy_gpio == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Setup data ready on falling edge
|
||||
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
|
||||
}
|
||||
|
||||
bool MPU9250_I2C::DataReadyInterruptDisable()
|
||||
{
|
||||
if (_drdy_gpio == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
|
||||
}
|
||||
|
||||
bool MPU9250_I2C::RegisterCheck(const register_config_t ®_cfg)
|
||||
{
|
||||
bool success = true;
|
||||
|
||||
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
|
||||
|
||||
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
|
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
|
||||
success = false;
|
||||
}
|
||||
|
||||
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
|
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
|
||||
success = false;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
uint8_t MPU9250_I2C::RegisterRead(Register reg)
|
||||
{
|
||||
uint8_t cmd = static_cast<uint8_t>(reg);
|
||||
uint8_t value = 0;
|
||||
//set_frequency(SPI_SPEED); // low speed for regular registers
|
||||
transfer(&cmd, 1, &value, 1);
|
||||
return value;
|
||||
}
|
||||
|
||||
void MPU9250_I2C::RegisterWrite(Register reg, uint8_t value)
|
||||
{
|
||||
uint8_t cmd[2];
|
||||
cmd[0] = static_cast<uint8_t>(reg);
|
||||
cmd[1] = value;
|
||||
//set_frequency(SPI_SPEED); // low speed for regular registers
|
||||
transfer(cmd, sizeof(cmd), nullptr, 0);
|
||||
}
|
||||
|
||||
void MPU9250_I2C::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
|
||||
{
|
||||
const uint8_t orig_val = RegisterRead(reg);
|
||||
|
||||
uint8_t val = (orig_val & ~clearbits) | setbits;
|
||||
|
||||
if (orig_val != val) {
|
||||
RegisterWrite(reg, val);
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t MPU9250_I2C::FIFOReadCount()
|
||||
{
|
||||
// read FIFO count
|
||||
uint8_t cmd = static_cast<uint8_t>(Register::FIFO_COUNTH);
|
||||
uint8_t fifo_count_buf[2] {};
|
||||
//set_frequency(SPI_SPEED_SENSOR);
|
||||
|
||||
if (transfer(&cmd, 1, fifo_count_buf, 2) != PX4_OK) {
|
||||
perf_count(_bad_transfer_perf);
|
||||
return 0;
|
||||
}
|
||||
|
||||
return combine(fifo_count_buf[0], fifo_count_buf[1]);
|
||||
}
|
||||
|
||||
bool MPU9250_I2C::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples)
|
||||
{
|
||||
uint8_t cmd = static_cast<uint8_t>(Register::FIFO_R_W);
|
||||
FIFOTransferBuffer buffer{};
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
|
||||
//set_frequency(SPI_SPEED_SENSOR);
|
||||
|
||||
if (transfer(&cmd, 1, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
|
||||
perf_count(_bad_transfer_perf);
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
ProcessGyro(timestamp_sample, buffer.f, samples);
|
||||
return ProcessAccel(timestamp_sample, buffer.f, samples);
|
||||
}
|
||||
|
||||
void MPU9250_I2C::FIFOReset()
|
||||
{
|
||||
perf_count(_fifo_reset_perf);
|
||||
|
||||
// FIFO_EN: disable FIFO
|
||||
RegisterWrite(Register::FIFO_EN, 0);
|
||||
|
||||
// USER_CTRL: reset FIFO
|
||||
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
|
||||
|
||||
// reset while FIFO is disabled
|
||||
_drdy_count.store(0);
|
||||
_drdy_fifo_read_samples.store(0);
|
||||
|
||||
// FIFO_EN: enable both gyro and accel
|
||||
// USER_CTRL: re-enable FIFO
|
||||
for (const auto &r : _register_cfg) {
|
||||
if ((r.reg == Register::FIFO_EN) || (r.reg == Register::USER_CTRL)) {
|
||||
RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool MPU9250_I2C::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
|
||||
{
|
||||
sensor_accel_fifo_s accel{};
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.samples = 0;
|
||||
accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
|
||||
|
||||
bool bad_data = false;
|
||||
|
||||
for (int i = 0; i < samples; i = i + SAMPLES_PER_TRANSFER) {
|
||||
int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
|
||||
int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
|
||||
int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
|
||||
|
||||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.samples++;
|
||||
}
|
||||
|
||||
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
|
||||
|
||||
if (accel.samples > 0) {
|
||||
_px4_accel.updateFIFO(accel);
|
||||
}
|
||||
|
||||
return !bad_data;
|
||||
}
|
||||
|
||||
void MPU9250_I2C::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
|
||||
{
|
||||
sensor_gyro_fifo_s gyro{};
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
gyro.samples = samples;
|
||||
gyro.dt = FIFO_SAMPLE_DT;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
|
||||
const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
|
||||
const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
|
||||
|
||||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
|
||||
|
||||
_px4_gyro.updateFIFO(gyro);
|
||||
}
|
||||
|
||||
void MPU9250_I2C::UpdateTemperature()
|
||||
{
|
||||
// read current temperature
|
||||
uint8_t cmd = static_cast<uint8_t>(Register::TEMP_OUT_H);
|
||||
uint8_t temperature_buf[2] {};
|
||||
//set_frequency(SPI_SPEED_SENSOR);
|
||||
|
||||
if (transfer(&cmd, 1, temperature_buf, 2) != PX4_OK) {
|
||||
perf_count(_bad_transfer_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
const int16_t TEMP_OUT = combine(temperature_buf[0], temperature_buf[1]);
|
||||
const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
|
||||
|
||||
if (PX4_ISFINITE(TEMP_degC)) {
|
||||
_px4_accel.set_temperature(TEMP_degC);
|
||||
_px4_gyro.set_temperature(TEMP_degC);
|
||||
}
|
||||
}
|
||||
172
src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp
Normal file
172
src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp
Normal file
@ -0,0 +1,172 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file MPU9250_I2C.hpp
|
||||
*
|
||||
* Driver for the Invensense MPU9250 connected via I2C.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "InvenSense_MPU9250_registers.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/device/i2c.h>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/atomic.h>
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
|
||||
using namespace InvenSense_MPU9250;
|
||||
|
||||
class MPU9250_I2C : public device::I2C, public I2CSPIDriver<MPU9250_I2C>
|
||||
{
|
||||
public:
|
||||
MPU9250_I2C(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
|
||||
int address, spi_drdy_gpio_t drdy_gpio);
|
||||
~MPU9250_I2C() override;
|
||||
|
||||
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance);
|
||||
static void print_usage();
|
||||
|
||||
void RunImpl();
|
||||
|
||||
int init() override;
|
||||
void print_status() override;
|
||||
|
||||
private:
|
||||
void exit_and_cleanup() override;
|
||||
|
||||
// Sensor Configuration
|
||||
static constexpr float FIFO_SAMPLE_DT{1e6f / 1000.f};
|
||||
static constexpr uint32_t SAMPLES_PER_TRANSFER{1}; // ensure at least 1 new accel sample per transfer
|
||||
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 1000 Hz gyro
|
||||
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 1000 Hz accel
|
||||
|
||||
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
|
||||
|
||||
// Transfer data
|
||||
struct FIFOTransferBuffer {
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
|
||||
};
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
|
||||
struct register_config_t {
|
||||
Register reg;
|
||||
uint8_t set_bits{0};
|
||||
uint8_t clear_bits{0};
|
||||
};
|
||||
|
||||
int probe() override;
|
||||
|
||||
bool Reset();
|
||||
|
||||
bool Configure();
|
||||
void ConfigureAccel();
|
||||
void ConfigureGyro();
|
||||
void ConfigureSampleRate(int sample_rate);
|
||||
|
||||
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
|
||||
void DataReady();
|
||||
bool DataReadyInterruptConfigure();
|
||||
bool DataReadyInterruptDisable();
|
||||
|
||||
bool RegisterCheck(const register_config_t ®_cfg);
|
||||
|
||||
uint8_t RegisterRead(Register reg);
|
||||
void RegisterWrite(Register reg, uint8_t value);
|
||||
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
|
||||
|
||||
uint16_t FIFOReadCount();
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples);
|
||||
void FIFOReset();
|
||||
|
||||
bool ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
|
||||
void UpdateTemperature();
|
||||
|
||||
const spi_drdy_gpio_t _drdy_gpio;
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
|
||||
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
|
||||
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
|
||||
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
|
||||
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
|
||||
perf_counter_t _drdy_missed_perf{nullptr};
|
||||
|
||||
hrt_abstime _reset_timestamp{0};
|
||||
hrt_abstime _last_config_check_timestamp{0};
|
||||
hrt_abstime _temperature_update_timestamp{0};
|
||||
int _failure_count{0};
|
||||
|
||||
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
|
||||
px4::atomic<uint32_t> _drdy_count{0};
|
||||
bool _data_ready_interrupt_enabled{false};
|
||||
|
||||
enum class STATE : uint8_t {
|
||||
RESET,
|
||||
WAIT_FOR_RESET,
|
||||
CONFIGURE,
|
||||
FIFO_READ,
|
||||
};
|
||||
|
||||
STATE _state{STATE::RESET};
|
||||
|
||||
uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
|
||||
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
|
||||
|
||||
uint8_t _checked_register{0};
|
||||
static constexpr uint8_t size_register_cfg{9};
|
||||
register_config_t _register_cfg[size_register_cfg] {
|
||||
// Register | Set bits, Clear bits
|
||||
{ Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_Fs_1KHZ, 0 },
|
||||
{ Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS, 0 },
|
||||
{ Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 },
|
||||
{ Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::A_DLPFCFG_BW_218HZ_DLPF, 0 },
|
||||
{ Register::FIFO_EN, FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT | FIFO_EN_BIT::ACCEL, FIFO_EN_BIT::TEMP_OUT },
|
||||
{ Register::INT_PIN_CFG, INT_PIN_CFG_BIT::ACTL | INT_PIN_CFG_BIT::BYPASS_EN, 0 },
|
||||
{ Register::INT_ENABLE, INT_ENABLE_BIT::RAW_RDY_EN, 0 },
|
||||
{ Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS },
|
||||
{ Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP },
|
||||
};
|
||||
};
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -31,76 +31,48 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
#include "MPU9250_I2C.hpp"
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
#include "mpu9250.h"
|
||||
|
||||
void
|
||||
MPU9250::print_usage()
|
||||
void MPU9250_I2C::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("mpu9250", "driver");
|
||||
PRINT_MODULE_USAGE_NAME("mpu9520", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x39);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
I2CSPIDriverBase *MPU9250::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance)
|
||||
I2CSPIDriverBase *MPU9250_I2C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance)
|
||||
{
|
||||
device::Device *interface = nullptr;
|
||||
device::Device *mag_interface = nullptr;
|
||||
MPU9250_I2C *instance = new MPU9250_I2C(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
|
||||
cli.bus_frequency, cli.i2c_address, iterator.DRDYGPIO());
|
||||
|
||||
if (iterator.busType() == BOARD_I2C_BUS) {
|
||||
#ifdef USE_I2C
|
||||
interface = MPU9250_I2C_interface(iterator.bus(), PX4_I2C_OBDEV_MPU9250, cli.bus_frequency);
|
||||
// For i2c interfaces, connect to the magnetomer directly
|
||||
mag_interface = AK8963_I2C_interface(iterator.bus(), cli.bus_frequency);
|
||||
#endif // USE_I2C
|
||||
|
||||
} else if (iterator.busType() == BOARD_SPI_BUS) {
|
||||
interface = MPU9250_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode);
|
||||
}
|
||||
|
||||
if (interface == nullptr) {
|
||||
if (!instance) {
|
||||
PX4_ERR("alloc failed");
|
||||
delete mag_interface;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (interface->init() != OK) {
|
||||
delete interface;
|
||||
delete mag_interface;
|
||||
PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
|
||||
if (OK != instance->init()) {
|
||||
delete instance;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
MPU9250 *dev = new MPU9250(interface, mag_interface, cli.rotation, iterator.configuredBusOption(), iterator.bus());
|
||||
|
||||
if (dev == nullptr) {
|
||||
delete interface;
|
||||
delete mag_interface;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (OK != dev->init()) {
|
||||
delete dev;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return dev;
|
||||
return instance;
|
||||
}
|
||||
|
||||
extern "C" int
|
||||
mpu9250_main(int argc, char *argv[])
|
||||
extern "C" int mpu9250_i2c_main(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
using ThisDriver = MPU9250;
|
||||
BusCLIArguments cli{true, true};
|
||||
cli.default_spi_frequency = 20 * 1000 * 1000;
|
||||
cli.default_i2c_frequency = 400000;
|
||||
using ThisDriver = MPU9250_I2C;
|
||||
BusCLIArguments cli{true, false};
|
||||
cli.default_i2c_frequency = I2C_SPEED;
|
||||
cli.i2c_address = I2C_ADDRESS_DEFAULT;
|
||||
|
||||
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
|
||||
switch (ch) {
|
||||
@ -1,113 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mag_i2c.cpp
|
||||
*
|
||||
* I2C interface for AK8963
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
#include "mpu9250.h"
|
||||
#include "MPU9250_mag.h"
|
||||
|
||||
#ifdef USE_I2C
|
||||
|
||||
device::Device *AK8963_I2C_interface(int bus, int bus_frequency);
|
||||
|
||||
class AK8963_I2C : public device::I2C
|
||||
{
|
||||
public:
|
||||
AK8963_I2C(int bus, int bus_frequency);
|
||||
~AK8963_I2C() override = default;
|
||||
|
||||
int read(unsigned address, void *data, unsigned count) override;
|
||||
int write(unsigned address, void *data, unsigned count) override;
|
||||
|
||||
protected:
|
||||
int probe() override;
|
||||
|
||||
};
|
||||
|
||||
device::Device *
|
||||
AK8963_I2C_interface(int bus, int bus_frequency)
|
||||
{
|
||||
return new AK8963_I2C(bus, bus_frequency);
|
||||
}
|
||||
|
||||
AK8963_I2C::AK8963_I2C(int bus, int bus_frequency) :
|
||||
I2C(DRV_MAG_DEVTYPE_AK8963, "AK8963_I2C", bus, AK8963_I2C_ADDR, bus_frequency)
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
AK8963_I2C::write(unsigned reg_speed, void *data, unsigned count)
|
||||
{
|
||||
uint8_t cmd[2] {};
|
||||
|
||||
if (sizeof(cmd) < (count + 1)) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
cmd[0] = MPU9250_REG(reg_speed);
|
||||
cmd[1] = *(uint8_t *)data;
|
||||
return transfer(&cmd[0], count + 1, nullptr, 0);
|
||||
}
|
||||
|
||||
int
|
||||
AK8963_I2C::read(unsigned reg_speed, void *data, unsigned count)
|
||||
{
|
||||
uint8_t cmd = MPU9250_REG(reg_speed);
|
||||
return transfer(&cmd, 1, (uint8_t *)data, count);
|
||||
}
|
||||
|
||||
int
|
||||
AK8963_I2C::probe()
|
||||
{
|
||||
uint8_t whoami = 0;
|
||||
uint8_t expected = AK8963_DEVICE_ID;
|
||||
|
||||
if (PX4_OK != read(AK8963REG_WIA, &whoami, 1)) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
if (whoami != expected) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,49 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2016 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE drivers__mpu9250
|
||||
MAIN mpu9250
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
AK8963_I2C.cpp
|
||||
mpu9250.cpp
|
||||
mpu9250_i2c.cpp
|
||||
MPU9250_mag.cpp
|
||||
mpu9250_main.cpp
|
||||
mpu9250_spi.cpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
drivers_accelerometer
|
||||
drivers_gyroscope
|
||||
drivers_magnetometer
|
||||
)
|
||||
@ -1,428 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mag.cpp
|
||||
*
|
||||
* Driver for the ak8963 magnetometer within the Invensense mpu9250
|
||||
*
|
||||
* @author Robert Dickenson
|
||||
*
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/time.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include "MPU9250_mag.h"
|
||||
#include "mpu9250.h"
|
||||
|
||||
// If interface is non-null, then it will used for interacting with the device.
|
||||
// Otherwise, it will passthrough the parent MPU9250
|
||||
MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, enum Rotation rotation) :
|
||||
_interface(interface),
|
||||
_px4_mag(parent->_interface->get_device_id(), rotation),
|
||||
_parent(parent),
|
||||
_mag_overruns(perf_alloc(PC_COUNT, MODULE_NAME": mag overruns")),
|
||||
_mag_overflows(perf_alloc(PC_COUNT, MODULE_NAME": mag overflows")),
|
||||
_mag_errors(perf_alloc(PC_COUNT, MODULE_NAME": mag errors"))
|
||||
{
|
||||
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK8963);
|
||||
_px4_mag.set_external(_parent->is_external());
|
||||
_px4_mag.set_scale(MPU9250_MAG_RANGE_GA);
|
||||
}
|
||||
|
||||
MPU9250_mag::~MPU9250_mag()
|
||||
{
|
||||
perf_free(_mag_overruns);
|
||||
perf_free(_mag_overflows);
|
||||
perf_free(_mag_errors);
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250_mag::measure()
|
||||
{
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||||
|
||||
uint8_t st1 = 0;
|
||||
int ret = _interface->read(AK8963REG_ST1, &st1, sizeof(st1));
|
||||
|
||||
if (ret != OK) {
|
||||
perf_count(_mag_errors);
|
||||
_px4_mag.set_error_count(perf_event_count(_mag_errors));
|
||||
return;
|
||||
}
|
||||
|
||||
/* Check if data ready is set.
|
||||
* This is not described to be set in continuous mode according to the
|
||||
* MPU9250 datasheet. However, the datasheet of the 8963 recommends to
|
||||
* check data ready before doing the read and before triggering the
|
||||
* next measurement by reading ST2. */
|
||||
if (!(st1 & AK09916_ST1_DRDY)) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Monitor if data overrun flag is ever set. */
|
||||
if (st1 & 0x02) {
|
||||
perf_count(_mag_overruns);
|
||||
}
|
||||
|
||||
ak8963_regs data{};
|
||||
ret = _interface->read(AK8963REG_ST1, &data, sizeof(data));
|
||||
|
||||
if (ret != OK) {
|
||||
_px4_mag.set_error_count(perf_event_count(_mag_errors));
|
||||
return;
|
||||
}
|
||||
|
||||
/* Monitor magnetic sensor overflow flag. */
|
||||
if (data.ST2 & 0x08) {
|
||||
perf_count(_mag_overflows);
|
||||
}
|
||||
|
||||
_measure(timestamp_sample, data);
|
||||
}
|
||||
|
||||
bool MPU9250_mag::_measure(const hrt_abstime ×tamp_sample, const ak8963_regs &data)
|
||||
{
|
||||
/* Check if data ready is set.
|
||||
* This is not described to be set in continuous mode according to the
|
||||
* MPU9250 datasheet. However, the datasheet of the 8963 recommends to
|
||||
* check data ready before doing the read and before triggering the
|
||||
* next measurement by reading ST2.
|
||||
*
|
||||
* If _measure is used in passthrough mode, all the data is already
|
||||
* fetched, however, we should still not use the data if the data ready
|
||||
* is not set. This has lead to intermittent spikes when the data was
|
||||
* being updated while getting read.
|
||||
*/
|
||||
if (!(data.ST1 & AK09916_ST1_DRDY)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Monitor magnetic sensor overflow flag. */
|
||||
if (data.ST2 & 0x08) {
|
||||
perf_count(_mag_overflows);
|
||||
return false;
|
||||
}
|
||||
|
||||
_px4_mag.set_temperature(_parent->_last_temperature);
|
||||
|
||||
/*
|
||||
* Align axes - note the accel & gyro are also re-aligned so this
|
||||
* doesn't look obvious with the datasheet
|
||||
*/
|
||||
int16_t x = combine(data.HXH, data.HXL);
|
||||
int16_t y = -combine(data.HYH, data.HYL);
|
||||
int16_t z = -combine(data.HZH, data.HZL);
|
||||
_px4_mag.update(timestamp_sample, x * _ak8963_ASA[0], y * _ak8963_ASA[1], z * _ak8963_ASA[2]);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out)
|
||||
{
|
||||
uint8_t addr;
|
||||
|
||||
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // ensure slave r/w is disabled before changing the registers
|
||||
|
||||
if (out) {
|
||||
_parent->write_reg(MPUREG_I2C_SLV0_D0, *out);
|
||||
addr = AK8963_I2C_ADDR;
|
||||
|
||||
} else {
|
||||
addr = AK8963_I2C_ADDR | BIT_I2C_READ_FLAG;
|
||||
}
|
||||
|
||||
_parent->write_reg(MPUREG_I2C_SLV0_ADDR, addr);
|
||||
_parent->write_reg(MPUREG_I2C_SLV0_REG, reg);
|
||||
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, size | BIT_I2C_SLV0_EN);
|
||||
}
|
||||
|
||||
uint8_t
|
||||
MPU9250_mag::read_reg(unsigned int reg)
|
||||
{
|
||||
uint8_t buf{};
|
||||
|
||||
if (_interface == nullptr) {
|
||||
read_reg_through_mpu9250(reg, &buf);
|
||||
|
||||
} else {
|
||||
_interface->read(reg, &buf, 1);
|
||||
}
|
||||
|
||||
return buf;
|
||||
}
|
||||
|
||||
bool
|
||||
MPU9250_mag::ak8963_check_id(uint8_t &deviceid)
|
||||
{
|
||||
deviceid = read_reg(AK8963REG_WIA);
|
||||
|
||||
return (AK8963_DEVICE_ID == deviceid);
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250_mag::write_reg(unsigned reg, uint8_t value)
|
||||
{
|
||||
// general register transfer at low clock speed
|
||||
if (_interface == nullptr) {
|
||||
write_reg_through_mpu9250(reg, value);
|
||||
|
||||
} else {
|
||||
_interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250_mag::ak8963_reset()
|
||||
{
|
||||
// First initialize it to use the bus
|
||||
int rv = ak8963_setup();
|
||||
|
||||
if (rv == OK) {
|
||||
// Now reset the mag
|
||||
write_reg(AK8963REG_CNTL2, AK8963_RESET);
|
||||
|
||||
// Then re-initialize the bus/mag
|
||||
rv = ak8963_setup();
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
bool
|
||||
MPU9250_mag::ak8963_read_adjustments()
|
||||
{
|
||||
uint8_t response[3] {};
|
||||
|
||||
write_reg_through_mpu9250(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC);
|
||||
px4_usleep(200);
|
||||
|
||||
if (_interface != nullptr) {
|
||||
_interface->read(AK8963REG_ASAX, response, 3);
|
||||
|
||||
} else {
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
read_reg_through_mpu9250(AK8963REG_ASAX + i, response + i);
|
||||
}
|
||||
}
|
||||
|
||||
write_reg_through_mpu9250(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE);
|
||||
|
||||
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (0 != response[i] && 0xff != response[i]) {
|
||||
_ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f;
|
||||
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250_mag::ak8963_setup_master_i2c()
|
||||
{
|
||||
/* When _interface is null we are using SPI and must
|
||||
* use the parent interface to configure the device to act
|
||||
* in master mode (SPI to I2C bridge)
|
||||
*/
|
||||
if (_interface == nullptr) {
|
||||
_parent->modify_checked_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_EN);
|
||||
_parent->write_reg(MPUREG_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | BIT_I2C_MST_WAIT_FOR_ES | BITS_I2C_MST_CLOCK_400HZ);
|
||||
|
||||
} else {
|
||||
_parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0);
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250_mag::ak8963_setup()
|
||||
{
|
||||
int retries = 10;
|
||||
|
||||
do {
|
||||
ak8963_setup_master_i2c();
|
||||
write_reg(AK8963REG_CNTL2, AK8963_RESET);
|
||||
px4_usleep(100);
|
||||
|
||||
uint8_t id = 0;
|
||||
|
||||
if (ak8963_check_id(id)) {
|
||||
break;
|
||||
}
|
||||
|
||||
retries--;
|
||||
PX4_WARN("AK8963: bad id %d retries %d", id, retries);
|
||||
_parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST);
|
||||
px4_usleep(100);
|
||||
} while (retries > 0);
|
||||
|
||||
if (retries > 0) {
|
||||
retries = 10;
|
||||
|
||||
while (!ak8963_read_adjustments() && retries) {
|
||||
retries--;
|
||||
PX4_ERR("AK8963: failed to read adjustment data. Retries %d", retries);
|
||||
|
||||
_parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST);
|
||||
px4_usleep(100);
|
||||
ak8963_setup_master_i2c();
|
||||
write_reg(AK8963REG_CNTL2, AK8963_RESET);
|
||||
}
|
||||
}
|
||||
|
||||
if (retries == 0) {
|
||||
PX4_ERR("AK8963: failed to initialize, disabled!");
|
||||
_parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0);
|
||||
_parent->write_reg(MPUREG_I2C_MST_CTRL, 0);
|
||||
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
|
||||
|
||||
if (_interface == nullptr) {
|
||||
// Configure mpu' I2C Master interface to read ak8963 data into to fifo
|
||||
set_passthrough(AK8963REG_ST1, sizeof(ak8963_regs));
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void MPU9250_mag::write_imu_reg_verified(int reg, uint8_t val, uint8_t mask)
|
||||
{
|
||||
uint8_t b;
|
||||
int retry = 5;
|
||||
|
||||
while (retry) { // should not reach any retries in normal condition
|
||||
--retry;
|
||||
_parent->write_reg(reg, val);
|
||||
|
||||
b = _parent->read_reg(reg);
|
||||
|
||||
if ((b & mask) != val) {
|
||||
PX4_DEBUG("MPU9250_mag::write_imu_reg_verified failed. retrying...");
|
||||
continue;
|
||||
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void MPU9250_mag::read_reg_through_mpu9250(uint8_t reg, uint8_t *val)
|
||||
{
|
||||
// Read operation on the mag using the slave 4 registers.
|
||||
write_imu_reg_verified(MPUREG_I2C_SLV4_ADDR, AK8963_I2C_ADDR | BIT_I2C_READ_FLAG, 0xff);
|
||||
|
||||
// Set the mag register to read from.
|
||||
write_imu_reg_verified(MPUREG_I2C_SLV4_REG, reg, 0xff);
|
||||
|
||||
// Read the existing value of the SLV4 control register.
|
||||
uint8_t b = _parent->read_reg(MPUREG_I2C_SLV4_CTRL);
|
||||
|
||||
// Set the I2C_SLV4_EN bit in I2C_SL4_CTRL register without overwriting other
|
||||
// bits. Enable data transfer, a read transfer as configured above.
|
||||
b |= 0x80;
|
||||
// Trigger the data transfer
|
||||
_parent->write_reg(MPUREG_I2C_SLV4_CTRL, b);
|
||||
|
||||
// Continuously check I2C_MST_STATUS register value for the completion
|
||||
// of I2C transfer until timeout
|
||||
|
||||
int loop_ctrl = 1000; // wait up to 1000 * 1ms for completion
|
||||
|
||||
do {
|
||||
px4_usleep(1000);
|
||||
b = _parent->read_reg(MPUREG_I2C_MST_STATUS);
|
||||
} while (((b & 0x40) == 0x00) && (--loop_ctrl));
|
||||
|
||||
if (loop_ctrl == 0) {
|
||||
PX4_ERR("I2C transfer timed out");
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("mpu9250 SPI2IIC read delay: %dms", loop_ctrl);
|
||||
}
|
||||
|
||||
// Read the value received from the mag, and copy to the caller's out parameter.
|
||||
*val = _parent->read_reg(MPUREG_I2C_SLV4_DI);
|
||||
}
|
||||
|
||||
void MPU9250_mag::write_reg_through_mpu9250(uint8_t reg, uint8_t val)
|
||||
{
|
||||
// Configure a write operation to the mag using Slave 4.
|
||||
write_imu_reg_verified(MPUREG_I2C_SLV4_ADDR, AK8963_I2C_ADDR, 0xff);
|
||||
|
||||
// Set the mag register address to write to using Slave 4.
|
||||
write_imu_reg_verified(MPUREG_I2C_SLV4_REG, reg, 0xff);
|
||||
|
||||
// Set the value to write in the I2C_SLV4_DO register.
|
||||
write_imu_reg_verified(MPUREG_I2C_SLV4_DO, val, 0xff);
|
||||
|
||||
// Read the current value of the Slave 4 control register.
|
||||
uint8_t b = _parent->read_reg(MPUREG_I2C_SLV4_CTRL);
|
||||
|
||||
// Set I2C_SLV4_EN bit in I2C_SL4_CTRL register without overwriting other
|
||||
// bits.
|
||||
b |= 0x80;
|
||||
// Trigger the data transfer from the byte now stored in the SLV4_DO register.
|
||||
_parent->write_reg(MPUREG_I2C_SLV4_CTRL, b);
|
||||
|
||||
// Continuously check I2C_MST_STATUS regsiter value for the completion
|
||||
// of I2C transfer until timeout.
|
||||
|
||||
int loop_ctrl = 1000; // wait up to 1000 * 1ms for completion
|
||||
|
||||
do {
|
||||
px4_usleep(1000);
|
||||
b = _parent->read_reg(MPUREG_I2C_MST_STATUS);
|
||||
|
||||
} while (((b & 0x40) == 0x00) && (--loop_ctrl));
|
||||
|
||||
if (loop_ctrl == 0) {
|
||||
PX4_ERR("I2C transfer to mag timed out");
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("mpu9250 SPI2IIC write delay: %dms", loop_ctrl);
|
||||
}
|
||||
}
|
||||
@ -1,156 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <perf/perf_counter.h>
|
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
#include <drivers/device/Device.hpp>
|
||||
|
||||
/* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
|
||||
static constexpr float MPU9250_MAG_RANGE_GA{1.5e-3f};
|
||||
|
||||
/* ak8963 register address and bit definitions */
|
||||
#define AK8963_I2C_ADDR 0x0C
|
||||
#define AK8963_DEVICE_ID 0x48
|
||||
|
||||
#define AK8963REG_WIA 0x00
|
||||
#define AK8963REG_ST1 0x02
|
||||
#define AK8963REG_HXL 0x03
|
||||
#define AK8963REG_ASAX 0x10
|
||||
#define AK8963REG_CNTL1 0x0A
|
||||
#define AK8963REG_CNTL2 0x0B
|
||||
|
||||
#define AK8963_SINGLE_MEAS_MODE 0x01
|
||||
#define AK8963_CONTINUOUS_MODE1 0x02
|
||||
#define AK8963_CONTINUOUS_MODE2 0x06
|
||||
#define AK8963_POWERDOWN_MODE 0x00
|
||||
#define AK8963_SELFTEST_MODE 0x08
|
||||
#define AK8963_FUZE_MODE 0x0F
|
||||
#define AK8963_16BIT_ADC 0x10
|
||||
#define AK8963_14BIT_ADC 0x00
|
||||
#define AK8963_RESET 0x01
|
||||
#define AK8963_HOFL 0x08
|
||||
|
||||
/* ak09916 deviating register addresses and bit definitions */
|
||||
|
||||
#define AK09916_DEVICE_ID_A 0x48 // same as AK8963
|
||||
#define AK09916_DEVICE_ID_B 0x09 // additional ID byte ("INFO" on AK9063 without content specification.)
|
||||
|
||||
#define AK09916REG_HXL 0x11
|
||||
#define AK09916REG_HXH 0x12
|
||||
#define AK09916REG_HYL 0x13
|
||||
#define AK09916REG_HYH 0x14
|
||||
#define AK09916REG_HZL 0x15
|
||||
#define AK09916REG_HZH 0x16
|
||||
#define AK09916REG_ST1 0x10
|
||||
#define AK09916REG_ST2 0x18
|
||||
#define AK09916REG_CNTL2 0x31
|
||||
#define AK09916REG_CNTL3 0x32
|
||||
|
||||
|
||||
#define AK09916_CNTL2_POWERDOWN_MODE 0x00
|
||||
#define AK09916_CNTL2_SINGLE_MODE 0x01 /* default */
|
||||
#define AK09916_CNTL2_CONTINOUS_MODE_10HZ 0x02
|
||||
#define AK09916_CNTL2_CONTINOUS_MODE_20HZ 0x04
|
||||
#define AK09916_CNTL2_CONTINOUS_MODE_50HZ 0x06
|
||||
#define AK09916_CNTL2_CONTINOUS_MODE_100HZ 0x08
|
||||
#define AK09916_CNTL2_SELFTEST_MODE 0x10
|
||||
#define AK09916_CNTL3_SRST 0x01
|
||||
#define AK09916_ST1_DRDY 0x01
|
||||
#define AK09916_ST1_DOR 0x02
|
||||
|
||||
class MPU9250;
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct ak8963_regs {
|
||||
uint8_t ST1;
|
||||
uint8_t HXL;
|
||||
uint8_t HXH;
|
||||
uint8_t HYL;
|
||||
uint8_t HYH;
|
||||
uint8_t HZL;
|
||||
uint8_t HZH;
|
||||
uint8_t ST2;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
extern device::Device *AK8963_I2C_interface(int bus, int bus_frequency);
|
||||
|
||||
/**
|
||||
* Helper class implementing the magnetometer driver node.
|
||||
*/
|
||||
class MPU9250_mag
|
||||
{
|
||||
public:
|
||||
MPU9250_mag(MPU9250 *parent, device::Device *interface, enum Rotation rotation);
|
||||
~MPU9250_mag();
|
||||
|
||||
void set_passthrough(uint8_t reg, uint8_t size, uint8_t *out = nullptr);
|
||||
|
||||
int ak8963_reset();
|
||||
int ak8963_setup();
|
||||
int ak8963_setup_master_i2c();
|
||||
bool ak8963_check_id(uint8_t &id);
|
||||
bool ak8963_read_adjustments();
|
||||
|
||||
protected:
|
||||
device::Device *_interface;
|
||||
|
||||
friend class MPU9250;
|
||||
|
||||
void measure();
|
||||
bool _measure(const hrt_abstime ×tamp_sample, const ak8963_regs &data);
|
||||
|
||||
uint8_t read_reg(unsigned reg);
|
||||
void write_reg(unsigned reg, uint8_t value);
|
||||
void write_imu_reg_verified(int reg, uint8_t val, uint8_t mask);
|
||||
void read_reg_through_mpu9250(uint8_t reg, uint8_t *val);
|
||||
void write_reg_through_mpu9250(uint8_t reg, uint8_t val);
|
||||
|
||||
bool is_passthrough() { return _interface == nullptr; }
|
||||
|
||||
private:
|
||||
PX4Magnetometer _px4_mag;
|
||||
|
||||
MPU9250 *_parent;
|
||||
|
||||
float _ak8963_ASA[3] {1.f, 1.f, 1.f};
|
||||
|
||||
bool _mag_reading_data{false};
|
||||
|
||||
perf_counter_t _mag_overruns;
|
||||
perf_counter_t _mag_overflows;
|
||||
perf_counter_t _mag_errors;
|
||||
|
||||
};
|
||||
@ -1,672 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mpu9250.cpp
|
||||
*
|
||||
* Driver for the Invensense MPU9250 connected via I2C or SPI.
|
||||
*
|
||||
* @author Andrew Tridgell
|
||||
*
|
||||
* based on the mpu6000 driver
|
||||
*/
|
||||
|
||||
#include "mpu9250.h"
|
||||
|
||||
/*
|
||||
we set the timer interrupt to run a bit faster than the desired
|
||||
sample rate and then throw away duplicates by comparing
|
||||
accelerometer values. This time reduction is enough to cope with
|
||||
worst case timing jitter due to other timers
|
||||
*/
|
||||
#define MPU9250_TIMER_REDUCTION 200
|
||||
|
||||
/* Set accel range used */
|
||||
#define ACCEL_RANGE_G 16
|
||||
/*
|
||||
list of registers that will be checked in check_registers(). Note
|
||||
that MPUREG_PRODUCT_ID must be first in the list.
|
||||
*/
|
||||
const uint16_t MPU9250::_mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPUREG_WHOAMI,
|
||||
MPUREG_PWR_MGMT_1,
|
||||
MPUREG_PWR_MGMT_2,
|
||||
MPUREG_USER_CTRL,
|
||||
MPUREG_SMPLRT_DIV,
|
||||
MPUREG_CONFIG,
|
||||
MPUREG_GYRO_CONFIG,
|
||||
MPUREG_ACCEL_CONFIG,
|
||||
MPUREG_ACCEL_CONFIG2,
|
||||
MPUREG_INT_ENABLE,
|
||||
MPUREG_INT_PIN_CFG
|
||||
};
|
||||
|
||||
MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, enum Rotation rotation,
|
||||
I2CSPIBusOption bus_option, int bus) :
|
||||
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus),
|
||||
_interface(interface),
|
||||
_px4_accel(_interface->get_device_id(), rotation),
|
||||
_px4_gyro(_interface->get_device_id(), rotation),
|
||||
_mag(this, mag_interface, rotation),
|
||||
_dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
|
||||
_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")),
|
||||
_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": dupe"))
|
||||
{
|
||||
}
|
||||
|
||||
MPU9250::~MPU9250()
|
||||
{
|
||||
// delete the perf counter
|
||||
perf_free(_sample_perf);
|
||||
perf_free(_bad_registers);
|
||||
perf_free(_duplicates);
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250::init()
|
||||
{
|
||||
/*
|
||||
* If the MPU is using I2C we should reduce the sample rate to 200Hz and
|
||||
* make the integration autoreset faster so that we integrate just one
|
||||
* sample since the sampling rate is already low.
|
||||
*/
|
||||
const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C);
|
||||
|
||||
if (is_i2c) {
|
||||
_sample_rate = 200;
|
||||
}
|
||||
|
||||
int ret = probe();
|
||||
|
||||
if (ret != OK) {
|
||||
PX4_DEBUG("probe failed");
|
||||
return ret;
|
||||
}
|
||||
|
||||
_reset_wait = hrt_absolute_time() + 100000;
|
||||
|
||||
if (reset_mpu() != OK) {
|
||||
PX4_ERR("Exiting! Device failed to take initialization");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
/* Magnetometer setup */
|
||||
if (_whoami == MPU_WHOAMI_9250) {
|
||||
|
||||
#ifdef USE_I2C
|
||||
px4_usleep(100);
|
||||
|
||||
if (!_mag.is_passthrough() && _mag._interface->init() != PX4_OK) {
|
||||
PX4_ERR("failed to setup ak8963 interface");
|
||||
}
|
||||
|
||||
#endif /* USE_I2C */
|
||||
|
||||
ret = _mag.ak8963_reset();
|
||||
|
||||
if (ret != OK) {
|
||||
PX4_DEBUG("mag reset failed");
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
|
||||
start();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250::reset()
|
||||
{
|
||||
/* When the mpu9250 starts from 0V the internal power on circuit
|
||||
* per the data sheet will require:
|
||||
*
|
||||
* Start-up time for register read/write From power-up Typ:11 max:100 ms
|
||||
*
|
||||
*/
|
||||
px4_usleep(110000);
|
||||
|
||||
// Hold off sampling until done (100 MS will be shortened)
|
||||
_reset_wait = hrt_absolute_time() + 100000;
|
||||
|
||||
int ret = reset_mpu();
|
||||
|
||||
if (ret == OK && (_whoami == MPU_WHOAMI_9250)) {
|
||||
ret = _mag.ak8963_reset();
|
||||
}
|
||||
|
||||
_reset_wait = hrt_absolute_time() + 10;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250::reset_mpu()
|
||||
{
|
||||
switch (_whoami) {
|
||||
case MPU_WHOAMI_9250:
|
||||
case MPU_WHOAMI_6500:
|
||||
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
|
||||
write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO);
|
||||
write_checked_reg(MPUREG_PWR_MGMT_2, 0);
|
||||
px4_usleep(1000);
|
||||
break;
|
||||
}
|
||||
|
||||
// Enable I2C bus or Disable I2C bus (recommended on data sheet)
|
||||
const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C);
|
||||
write_checked_reg(MPUREG_USER_CTRL, is_i2c ? 0 : BIT_I2C_IF_DIS);
|
||||
|
||||
// SAMPLE RATE
|
||||
_set_sample_rate(_sample_rate);
|
||||
|
||||
_set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ);
|
||||
|
||||
// Gyro scale 2000 deg/s ()
|
||||
switch (_whoami) {
|
||||
case MPU_WHOAMI_9250:
|
||||
case MPU_WHOAMI_6500:
|
||||
write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
|
||||
break;
|
||||
}
|
||||
|
||||
// correct gyro scale factors
|
||||
// scale to rad/s in SI units
|
||||
// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
|
||||
// scaling factor:
|
||||
// 1/(2^15)*(2000/180)*PI
|
||||
_px4_gyro.set_scale(0.0174532 / 16.4); //1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
|
||||
|
||||
set_accel_range(ACCEL_RANGE_G);
|
||||
|
||||
// INT CFG => Interrupt on Data Ready
|
||||
write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready
|
||||
|
||||
#ifdef USE_I2C
|
||||
bool bypass = !_mag.is_passthrough();
|
||||
#else
|
||||
bool bypass = false;
|
||||
#endif
|
||||
|
||||
/* INT: Clear on any read.
|
||||
* If this instance is for a device is on I2C bus the Mag will have an i2c interface
|
||||
* that it will use to access the either: a) the internal mag device on the internal I2C bus
|
||||
* or b) it could be used to access a downstream I2C devices connected to the chip on
|
||||
* it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master
|
||||
* controller that chip provides as a SPI to I2C bridge.
|
||||
* so bypass is true if the mag has an i2c non null interfaces.
|
||||
*/
|
||||
|
||||
write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0));
|
||||
|
||||
write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ);
|
||||
|
||||
uint8_t retries = 3;
|
||||
bool all_ok = false;
|
||||
|
||||
while (!all_ok && retries--) {
|
||||
|
||||
// Assume all checked values are as expected
|
||||
all_ok = true;
|
||||
uint8_t reg = 0;
|
||||
|
||||
for (uint8_t i = 0; i < _num_checked_registers; i++) {
|
||||
if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) {
|
||||
|
||||
write_reg(_checked_registers[i], _checked_values[i]);
|
||||
PX4_ERR("Reg %d is:%d s/b:%d Tries:%d", _checked_registers[i], reg, _checked_values[i], retries);
|
||||
all_ok = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return all_ok ? OK : -EIO;
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250::probe()
|
||||
{
|
||||
int ret = PX4_ERROR;
|
||||
|
||||
// Try first for mpu9250/6500
|
||||
_whoami = read_reg(MPUREG_WHOAMI);
|
||||
|
||||
if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) {
|
||||
|
||||
_num_checked_registers = MPU9250_NUM_CHECKED_REGISTERS;
|
||||
_checked_registers = _mpu9250_checked_registers;
|
||||
memset(_checked_values, 0, MPU9250_NUM_CHECKED_REGISTERS);
|
||||
ret = PX4_OK;
|
||||
}
|
||||
|
||||
_checked_values[0] = _whoami;
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
PX4_DEBUG("unexpected whoami 0x%02x", _whoami);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/*
|
||||
set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
|
||||
*/
|
||||
void
|
||||
MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz)
|
||||
{
|
||||
uint8_t div = 1;
|
||||
|
||||
if (desired_sample_rate_hz == 0) {
|
||||
desired_sample_rate_hz = MPU9250_GYRO_DEFAULT_RATE;
|
||||
}
|
||||
|
||||
switch (_whoami) {
|
||||
case MPU_WHOAMI_9250:
|
||||
case MPU_WHOAMI_6500:
|
||||
div = 1000 / desired_sample_rate_hz;
|
||||
break;
|
||||
}
|
||||
|
||||
if (div > 200) { div = 200; }
|
||||
|
||||
if (div < 1) { div = 1; }
|
||||
|
||||
|
||||
switch (_whoami) {
|
||||
case MPU_WHOAMI_9250:
|
||||
case MPU_WHOAMI_6500:
|
||||
write_checked_reg(MPUREG_SMPLRT_DIV, div - 1);
|
||||
_sample_rate = 1000 / div;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
set the DLPF filter frequency. This affects both accel and gyro.
|
||||
*/
|
||||
void
|
||||
MPU9250::_set_dlpf_filter(uint16_t frequency_hz)
|
||||
{
|
||||
uint8_t filter;
|
||||
|
||||
switch (_whoami) {
|
||||
case MPU_WHOAMI_9250:
|
||||
case MPU_WHOAMI_6500:
|
||||
|
||||
/*
|
||||
choose next highest filter frequency available
|
||||
*/
|
||||
if (frequency_hz == 0) {
|
||||
_dlpf_freq = 0;
|
||||
filter = BITS_DLPF_CFG_3600HZ;
|
||||
|
||||
} else if (frequency_hz <= 5) {
|
||||
_dlpf_freq = 5;
|
||||
filter = BITS_DLPF_CFG_5HZ;
|
||||
|
||||
} else if (frequency_hz <= 10) {
|
||||
_dlpf_freq = 10;
|
||||
filter = BITS_DLPF_CFG_10HZ;
|
||||
|
||||
} else if (frequency_hz <= 20) {
|
||||
_dlpf_freq = 20;
|
||||
filter = BITS_DLPF_CFG_20HZ;
|
||||
|
||||
} else if (frequency_hz <= 41) {
|
||||
_dlpf_freq = 41;
|
||||
filter = BITS_DLPF_CFG_41HZ;
|
||||
|
||||
} else if (frequency_hz <= 92) {
|
||||
_dlpf_freq = 92;
|
||||
filter = BITS_DLPF_CFG_92HZ;
|
||||
|
||||
} else if (frequency_hz <= 184) {
|
||||
_dlpf_freq = 184;
|
||||
filter = BITS_DLPF_CFG_184HZ;
|
||||
|
||||
} else if (frequency_hz <= 250) {
|
||||
_dlpf_freq = 250;
|
||||
filter = BITS_DLPF_CFG_250HZ;
|
||||
|
||||
} else {
|
||||
_dlpf_freq = 0;
|
||||
filter = BITS_DLPF_CFG_3600HZ;
|
||||
}
|
||||
|
||||
write_checked_reg(MPUREG_CONFIG, filter);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t
|
||||
MPU9250::read_reg(unsigned reg, uint32_t speed)
|
||||
{
|
||||
uint8_t buf{};
|
||||
|
||||
_interface->read(MPU9250_SET_SPEED(reg, speed), &buf, 1);
|
||||
|
||||
return buf;
|
||||
}
|
||||
|
||||
uint8_t
|
||||
MPU9250::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count)
|
||||
{
|
||||
if (buf == NULL) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return _interface->read(MPU9250_SET_SPEED(start_reg, speed), buf, count);
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250::write_reg(unsigned reg, uint8_t value)
|
||||
{
|
||||
// general register transfer at low clock speed
|
||||
_interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
|
||||
{
|
||||
uint8_t val = read_reg(reg);
|
||||
val &= ~clearbits;
|
||||
val |= setbits;
|
||||
write_reg(reg, val);
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250::modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
|
||||
{
|
||||
uint8_t val = read_reg(reg);
|
||||
val &= ~clearbits;
|
||||
val |= setbits;
|
||||
write_checked_reg(reg, val);
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250::write_checked_reg(unsigned reg, uint8_t value)
|
||||
{
|
||||
write_reg(reg, value);
|
||||
|
||||
for (uint8_t i = 0; i < _num_checked_registers; i++) {
|
||||
if (reg == _checked_registers[i]) {
|
||||
_checked_values[i] = value;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250::set_accel_range(unsigned max_g_in)
|
||||
{
|
||||
uint8_t afs_sel;
|
||||
float lsb_per_g;
|
||||
//float max_accel_g;
|
||||
|
||||
if (max_g_in > 8) { // 16g - AFS_SEL = 3
|
||||
afs_sel = 3;
|
||||
lsb_per_g = 2048;
|
||||
//max_accel_g = 16;
|
||||
|
||||
} else if (max_g_in > 4) { // 8g - AFS_SEL = 2
|
||||
afs_sel = 2;
|
||||
lsb_per_g = 4096;
|
||||
//max_accel_g = 8;
|
||||
|
||||
} else if (max_g_in > 2) { // 4g - AFS_SEL = 1
|
||||
afs_sel = 1;
|
||||
lsb_per_g = 8192;
|
||||
//max_accel_g = 4;
|
||||
|
||||
} else { // 2g - AFS_SEL = 0
|
||||
afs_sel = 0;
|
||||
lsb_per_g = 16384;
|
||||
//max_accel_g = 2;
|
||||
}
|
||||
|
||||
switch (_whoami) {
|
||||
case MPU_WHOAMI_9250:
|
||||
case MPU_WHOAMI_6500:
|
||||
write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3);
|
||||
break;
|
||||
}
|
||||
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250::start()
|
||||
{
|
||||
ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 1000);
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250::RunImpl()
|
||||
{
|
||||
/* make another measurement */
|
||||
measure();
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250::check_registers()
|
||||
{
|
||||
/*
|
||||
we read the register at full speed, even though it isn't
|
||||
listed as a high speed register. The low speed requirement
|
||||
for some registers seems to be a propagation delay
|
||||
requirement for changing sensor configuration, which should
|
||||
not apply to reading a single register. It is also a better
|
||||
test of SPI bus health to read at the same speed as we read
|
||||
the data registers.
|
||||
*/
|
||||
uint8_t v = 0;
|
||||
|
||||
if ((v = read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) {
|
||||
|
||||
PX4_DEBUG("reg: %d = %d (should be %d) _reset_wait: %lu", _checked_registers[_checked_next], v,
|
||||
_checked_values[_checked_next], _reset_wait);
|
||||
|
||||
/*
|
||||
if we get the wrong value then we know the SPI bus
|
||||
or sensor is very sick. We set _register_wait to 20
|
||||
and wait until we have seen 20 good values in a row
|
||||
before we consider the sensor to be OK again.
|
||||
*/
|
||||
perf_count(_bad_registers);
|
||||
|
||||
/*
|
||||
try to fix the bad register value. We only try to
|
||||
fix one per loop to prevent a bad sensor hogging the
|
||||
bus.
|
||||
*/
|
||||
if (_register_wait == 0 || _checked_next == 0) {
|
||||
// if the product_id is wrong then reset the sensor completely
|
||||
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
|
||||
write_reg(MPUREG_PWR_MGMT_2, MPU_CLK_SEL_AUTO);
|
||||
|
||||
// after doing a reset we need to wait a long
|
||||
// time before we do any other register writes
|
||||
// or we will end up with the mpu9250 in a
|
||||
// bizarre state where it has all correct
|
||||
// register values but large offsets on the
|
||||
// accel axes
|
||||
_reset_wait = hrt_absolute_time() + 10000;
|
||||
_checked_next = 0;
|
||||
|
||||
} else {
|
||||
write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
|
||||
|
||||
// waiting 3ms between register writes seems
|
||||
// to raise the chance of the sensor
|
||||
// recovering considerably
|
||||
_reset_wait = hrt_absolute_time() + 3000;
|
||||
}
|
||||
|
||||
_register_wait = 20;
|
||||
}
|
||||
|
||||
_checked_next = (_checked_next + 1) % _num_checked_registers;
|
||||
}
|
||||
|
||||
bool
|
||||
MPU9250::check_duplicate(uint8_t *accel_data)
|
||||
{
|
||||
/*
|
||||
see if this is duplicate accelerometer data. Note that we
|
||||
can't use the data ready interrupt status bit in the status
|
||||
register as that also goes high on new gyro data, and when
|
||||
we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being
|
||||
sampled at 8kHz, so we would incorrectly think we have new
|
||||
data when we are in fact getting duplicate accelerometer data.
|
||||
*/
|
||||
if (!_got_duplicate && memcmp(accel_data, &_last_accel_data, sizeof(_last_accel_data)) == 0) {
|
||||
// it isn't new data - wait for next timer
|
||||
_got_duplicate = true;
|
||||
|
||||
} else {
|
||||
memcpy(&_last_accel_data, accel_data, sizeof(_last_accel_data));
|
||||
_got_duplicate = false;
|
||||
}
|
||||
|
||||
return _got_duplicate;
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250::measure()
|
||||
{
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
if (hrt_absolute_time() < _reset_wait) {
|
||||
// we're waiting for a reset to complete
|
||||
perf_end(_sample_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
MPUReport mpu_report{};
|
||||
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||||
|
||||
// Fetch the full set of measurements from the ICM20948 in one pass
|
||||
if (_mag.is_passthrough() && _register_wait == 0) {
|
||||
if (OK != read_reg_range(MPUREG_ACCEL_XOUT_H, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&mpu_report, sizeof(mpu_report))) {
|
||||
perf_end(_sample_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
check_registers();
|
||||
|
||||
if (check_duplicate(&mpu_report.ACCEL_XOUT_H)) {
|
||||
perf_end(_sample_perf);
|
||||
perf_count(_duplicates);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* In case of a mag passthrough read, hand the magnetometer data over to _mag. Else,
|
||||
* try to read a magnetometer report.
|
||||
*/
|
||||
|
||||
# ifdef USE_I2C
|
||||
|
||||
if (_mag.is_passthrough()) {
|
||||
# endif
|
||||
|
||||
if (_register_wait == 0) {
|
||||
_mag._measure(timestamp_sample, mpu_report.mag);
|
||||
}
|
||||
|
||||
# ifdef USE_I2C
|
||||
|
||||
} else {
|
||||
_mag.measure();
|
||||
}
|
||||
|
||||
# endif
|
||||
|
||||
if (_register_wait != 0) {
|
||||
// We are waiting for some good transfers before using the sensor again
|
||||
_register_wait--;
|
||||
|
||||
perf_end(_sample_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
// Convert from big to little endian
|
||||
int16_t accel_x = combine(mpu_report.ACCEL_XOUT_H, mpu_report.ACCEL_XOUT_L);
|
||||
int16_t accel_y = combine(mpu_report.ACCEL_YOUT_H, mpu_report.ACCEL_YOUT_L);
|
||||
int16_t accel_z = combine(mpu_report.ACCEL_ZOUT_H, mpu_report.ACCEL_ZOUT_L);
|
||||
int16_t temp = combine(mpu_report.TEMP_OUT_H, mpu_report.TEMP_OUT_L);
|
||||
int16_t gyro_x = combine(mpu_report.GYRO_XOUT_H, mpu_report.GYRO_XOUT_L);
|
||||
int16_t gyro_y = combine(mpu_report.GYRO_YOUT_H, mpu_report.GYRO_YOUT_L);
|
||||
int16_t gyro_z = combine(mpu_report.GYRO_ZOUT_H, mpu_report.GYRO_ZOUT_L);
|
||||
|
||||
// Get sensor temperature
|
||||
_last_temperature = temp / 333.87f + 21.0f;
|
||||
|
||||
_px4_accel.set_temperature(_last_temperature);
|
||||
_px4_gyro.set_temperature(_last_temperature);
|
||||
|
||||
// Swap axes and negate y
|
||||
int16_t accel_xt = accel_y;
|
||||
int16_t accel_yt = ((accel_x == -32768) ? 32767 : -accel_x);
|
||||
|
||||
int16_t gyro_xt = gyro_y;
|
||||
int16_t gyro_yt = ((gyro_x == -32768) ? 32767 : -gyro_x);
|
||||
|
||||
// report the error count as the sum of the number of bad
|
||||
// transfers and bad register reads. This allows the higher
|
||||
// level code to decide if it should use this sensor based on
|
||||
// whether it has had failures
|
||||
const uint64_t error_count = perf_event_count(_bad_registers);
|
||||
_px4_accel.set_error_count(error_count);
|
||||
_px4_gyro.set_error_count(error_count);
|
||||
|
||||
/* NOTE: Axes have been swapped to match the board a few lines above. */
|
||||
_px4_accel.update(timestamp_sample, accel_xt, accel_yt, accel_z);
|
||||
_px4_gyro.update(timestamp_sample, gyro_xt, gyro_yt, gyro_z);
|
||||
|
||||
/* stop measuring */
|
||||
perf_end(_sample_perf);
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_bad_registers);
|
||||
perf_print_counter(_duplicates);
|
||||
}
|
||||
@ -1,409 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
#include <lib/systemlib/conversions.h>
|
||||
#include <lib/systemlib/px4_macros.h>
|
||||
|
||||
#include "MPU9250_mag.h"
|
||||
|
||||
#if defined(PX4_I2C_OBDEV_MPU9250)
|
||||
# define USE_I2C
|
||||
#endif
|
||||
|
||||
|
||||
// MPU 9250 registers
|
||||
#define MPUREG_WHOAMI 0x75
|
||||
#define MPUREG_SMPLRT_DIV 0x19
|
||||
#define MPUREG_CONFIG 0x1A
|
||||
#define MPUREG_GYRO_CONFIG 0x1B
|
||||
#define MPUREG_ACCEL_CONFIG 0x1C
|
||||
#define MPUREG_ACCEL_CONFIG2 0x1D
|
||||
#define MPUREG_LPACCEL_ODR 0x1E
|
||||
#define MPUREG_WOM_THRESH 0x1F
|
||||
#define MPUREG_FIFO_EN 0x23
|
||||
#define MPUREG_I2C_MST_CTRL 0x24
|
||||
#define MPUREG_I2C_SLV0_ADDR 0x25
|
||||
#define MPUREG_I2C_SLV0_REG 0x26
|
||||
#define MPUREG_I2C_SLV0_CTRL 0x27
|
||||
#define MPUREG_I2C_SLV1_ADDR 0x28
|
||||
#define MPUREG_I2C_SLV1_REG 0x29
|
||||
#define MPUREG_I2C_SLV1_CTRL 0x2A
|
||||
#define MPUREG_I2C_SLV2_ADDR 0x2B
|
||||
#define MPUREG_I2C_SLV2_REG 0x2C
|
||||
#define MPUREG_I2C_SLV2_CTRL 0x2D
|
||||
#define MPUREG_I2C_SLV3_ADDR 0x2E
|
||||
#define MPUREG_I2C_SLV3_REG 0x2F
|
||||
#define MPUREG_I2C_SLV3_CTRL 0x30
|
||||
#define MPUREG_I2C_SLV4_ADDR 0x31
|
||||
#define MPUREG_I2C_SLV4_REG 0x32
|
||||
#define MPUREG_I2C_SLV4_DO 0x33
|
||||
#define MPUREG_I2C_SLV4_CTRL 0x34
|
||||
#define MPUREG_I2C_SLV4_DI 0x35
|
||||
#define MPUREG_I2C_MST_STATUS 0x36
|
||||
#define MPUREG_INT_PIN_CFG 0x37
|
||||
#define MPUREG_INT_ENABLE 0x38
|
||||
#define MPUREG_INT_STATUS 0x3A
|
||||
#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_EXT_SENS_DATA_00 0x49
|
||||
#define MPUREG_I2C_SLV0_D0 0x63
|
||||
#define MPUREG_I2C_SLV1_D0 0x64
|
||||
#define MPUREG_I2C_SLV2_D0 0x65
|
||||
#define MPUREG_I2C_SLV3_D0 0x66
|
||||
#define MPUREG_I2C_MST_DELAY_CTRL 0x67
|
||||
#define MPUREG_SIGNAL_PATH_RESET 0x68
|
||||
#define MPUREG_MOT_DETECT_CTRL 0x69
|
||||
#define MPUREG_USER_CTRL 0x6A
|
||||
#define MPUREG_PWR_MGMT_1 0x6B
|
||||
#define MPUREG_PWR_MGMT_2 0x6C
|
||||
#define MPUREG_FIFO_COUNTH 0x72
|
||||
#define MPUREG_FIFO_COUNTL 0x73
|
||||
#define MPUREG_FIFO_R_W 0x74
|
||||
|
||||
// Configuration bits MPU 9250
|
||||
#define BIT_SLEEP 0x40
|
||||
#define BIT_H_RESET 0x80
|
||||
#define MPU_CLK_SEL_AUTO 0x01
|
||||
|
||||
#define BITS_GYRO_ST_X 0x80
|
||||
#define BITS_GYRO_ST_Y 0x40
|
||||
#define BITS_GYRO_ST_Z 0x20
|
||||
#define BITS_FS_250DPS 0x00
|
||||
#define BITS_FS_500DPS 0x08
|
||||
#define BITS_FS_1000DPS 0x10
|
||||
#define BITS_FS_2000DPS 0x18
|
||||
#define BITS_FS_MASK 0x18
|
||||
|
||||
#define BITS_DLPF_CFG_250HZ 0x00
|
||||
#define BITS_DLPF_CFG_184HZ 0x01
|
||||
#define BITS_DLPF_CFG_92HZ 0x02
|
||||
#define BITS_DLPF_CFG_41HZ 0x03
|
||||
#define BITS_DLPF_CFG_20HZ 0x04
|
||||
#define BITS_DLPF_CFG_10HZ 0x05
|
||||
#define BITS_DLPF_CFG_5HZ 0x06
|
||||
#define BITS_DLPF_CFG_3600HZ 0x07
|
||||
#define BITS_DLPF_CFG_MASK 0x07
|
||||
|
||||
#define BITS_ACCEL_CONFIG2_41HZ 0x03
|
||||
|
||||
#define BIT_RAW_RDY_EN 0x01
|
||||
#define BIT_INT_ANYRD_2CLEAR 0x10
|
||||
#define BIT_INT_BYPASS_EN 0x02
|
||||
|
||||
#define BIT_I2C_READ_FLAG 0x80
|
||||
|
||||
#define BIT_I2C_SLV0_NACK 0x01
|
||||
#define BIT_I2C_FIFO_EN 0x40
|
||||
#define BIT_I2C_MST_EN 0x20
|
||||
#define BIT_I2C_IF_DIS 0x10
|
||||
#define BIT_FIFO_RST 0x04
|
||||
#define BIT_I2C_MST_RST 0x02
|
||||
#define BIT_SIG_COND_RST 0x01
|
||||
|
||||
#define BIT_I2C_SLV0_EN 0x80
|
||||
#define BIT_I2C_SLV0_BYTE_SW 0x40
|
||||
#define BIT_I2C_SLV0_REG_DIS 0x20
|
||||
#define BIT_I2C_SLV0_REG_GRP 0x10
|
||||
|
||||
#define BIT_I2C_MST_MULT_MST_EN 0x80
|
||||
#define BIT_I2C_MST_WAIT_FOR_ES 0x40
|
||||
#define BIT_I2C_MST_SLV_3_FIFO_EN 0x20
|
||||
#define BIT_I2C_MST_P_NSR 0x10
|
||||
#define BITS_I2C_MST_CLOCK_258HZ 0x08
|
||||
#define BITS_I2C_MST_CLOCK_400HZ 0x0D
|
||||
|
||||
#define BIT_I2C_SLV0_DLY_EN 0x01
|
||||
#define BIT_I2C_SLV1_DLY_EN 0x02
|
||||
#define BIT_I2C_SLV2_DLY_EN 0x04
|
||||
#define BIT_I2C_SLV3_DLY_EN 0x08
|
||||
|
||||
#define MPU_WHOAMI_9250 0x71
|
||||
#define MPU_WHOAMI_6500 0x70
|
||||
|
||||
#define MPU9250_ACCEL_DEFAULT_RATE 1000
|
||||
#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280
|
||||
#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
|
||||
#define MPU9250_GYRO_DEFAULT_RATE 1000
|
||||
/* rates need to be the same between accel and gyro */
|
||||
#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE
|
||||
#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
|
||||
|
||||
#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 92
|
||||
|
||||
#pragma pack(push, 1)
|
||||
/**
|
||||
* Report conversation within the mpu, including command byte and
|
||||
* interrupt status.
|
||||
*/
|
||||
struct MPUReport {
|
||||
uint8_t cmd;
|
||||
uint8_t ACCEL_XOUT_H;
|
||||
uint8_t ACCEL_XOUT_L;
|
||||
uint8_t ACCEL_YOUT_H;
|
||||
uint8_t ACCEL_YOUT_L;
|
||||
uint8_t ACCEL_ZOUT_H;
|
||||
uint8_t ACCEL_ZOUT_L;
|
||||
uint8_t TEMP_OUT_H;
|
||||
uint8_t TEMP_OUT_L;
|
||||
uint8_t GYRO_XOUT_H;
|
||||
uint8_t GYRO_XOUT_L;
|
||||
uint8_t GYRO_YOUT_H;
|
||||
uint8_t GYRO_YOUT_L;
|
||||
uint8_t GYRO_ZOUT_H;
|
||||
uint8_t GYRO_ZOUT_L;
|
||||
|
||||
struct ak8963_regs mag;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
/*
|
||||
The MPU9250 can only handle high bus speeds on the sensor and
|
||||
interrupt status registers. All other registers have a maximum 1MHz
|
||||
Communication with all registers of the device is performed using either
|
||||
I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications,
|
||||
the sensor and interrupt registers may be read using SPI at 20MHz
|
||||
*/
|
||||
#define MPU9250_LOW_BUS_SPEED 0
|
||||
#define MPU9250_HIGH_BUS_SPEED 0x8000
|
||||
#define MPU9250_REG_MASK 0x00FF
|
||||
# define MPU9250_IS_HIGH_SPEED(r) ((r) & MPU9250_HIGH_BUS_SPEED)
|
||||
# define MPU9250_REG(r) ((r) & MPU9250_REG_MASK)
|
||||
# define MPU9250_SET_SPEED(r, s) ((r)|(s))
|
||||
# define MPU9250_HIGH_SPEED_OP(r) MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED)
|
||||
# define MPU9250_LOW_SPEED_OP(r) ((r) &~MPU9250_HIGH_BUS_SPEED)
|
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; }
|
||||
|
||||
/* interface factories */
|
||||
extern device::Device *MPU9250_SPI_interface(int bus, uint32_t cs, int bus_frequency, spi_mode_e spi_mode);
|
||||
extern device::Device *MPU9250_I2C_interface(int bus, uint32_t address, int bus_frequency);
|
||||
|
||||
class MPU9250_mag;
|
||||
|
||||
class MPU9250 : public I2CSPIDriver<MPU9250>
|
||||
{
|
||||
public:
|
||||
MPU9250(device::Device *interface, device::Device *mag_interface, enum Rotation rotation, I2CSPIBusOption bus_option,
|
||||
int bus);
|
||||
virtual ~MPU9250();
|
||||
|
||||
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance);
|
||||
static void print_usage();
|
||||
|
||||
int init();
|
||||
uint8_t get_whoami() { return _whoami; }
|
||||
|
||||
/**
|
||||
* Diagnostics - print some basic information about the driver.
|
||||
*/
|
||||
void print_status() override;
|
||||
|
||||
void RunImpl();
|
||||
|
||||
protected:
|
||||
device::Device *_interface;
|
||||
uint8_t _whoami{0}; /** whoami result */
|
||||
|
||||
int probe();
|
||||
|
||||
friend class MPU9250_mag;
|
||||
|
||||
private:
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
MPU9250_mag _mag;
|
||||
|
||||
unsigned _call_interval{1000};
|
||||
|
||||
unsigned _dlpf_freq{0};
|
||||
|
||||
unsigned _sample_rate{1000};
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _bad_registers;
|
||||
perf_counter_t _duplicates;
|
||||
|
||||
uint8_t _register_wait{0};
|
||||
uint64_t _reset_wait{0};
|
||||
|
||||
// this is used to support runtime checking of key
|
||||
// configuration registers to detect SPI bus errors and sensor
|
||||
// reset
|
||||
|
||||
static constexpr int MPU9250_NUM_CHECKED_REGISTERS{11};
|
||||
static const uint16_t _mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS];
|
||||
|
||||
const uint16_t *_checked_registers{nullptr};
|
||||
|
||||
uint8_t _checked_values[MPU9250_NUM_CHECKED_REGISTERS] {};
|
||||
unsigned _checked_next{0};
|
||||
unsigned _num_checked_registers{0};
|
||||
|
||||
|
||||
// last temperature reading for print_info()
|
||||
float _last_temperature{0.0f};
|
||||
|
||||
bool check_duplicate(uint8_t *accel_data);
|
||||
|
||||
// keep last accel reading for duplicate detection
|
||||
uint8_t _last_accel_data[6] {};
|
||||
bool _got_duplicate{false};
|
||||
|
||||
void start();
|
||||
int reset();
|
||||
|
||||
/**
|
||||
* Resets the main chip (excluding the magnetometer if any).
|
||||
*/
|
||||
int reset_mpu();
|
||||
|
||||
/**
|
||||
* Fetch measurements from the sensor and update the report buffers.
|
||||
*/
|
||||
void measure();
|
||||
|
||||
/**
|
||||
* Read a register from the mpu
|
||||
*
|
||||
* @param The register to read.
|
||||
* @param The bus speed to read with.
|
||||
* @return The value that was read.
|
||||
*/
|
||||
uint8_t read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED);
|
||||
|
||||
/**
|
||||
* Read a register range from the mpu
|
||||
*
|
||||
* @param The start address to read from.
|
||||
* @param The bus speed to read with.
|
||||
* @param The address of the target data buffer.
|
||||
* @param The count of bytes to be read.
|
||||
* @return The value that was read.
|
||||
*/
|
||||
uint8_t read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count);
|
||||
|
||||
/**
|
||||
* Write a register in the mpu
|
||||
*
|
||||
* @param reg The register to write.
|
||||
* @param value The new value to write.
|
||||
*/
|
||||
void write_reg(unsigned reg, uint8_t value);
|
||||
|
||||
/**
|
||||
* Modify a register in the mpu
|
||||
*
|
||||
* Bits are cleared before bits are set.
|
||||
*
|
||||
* @param reg The register to modify.
|
||||
* @param clearbits Bits in the register to clear.
|
||||
* @param setbits Bits in the register to set.
|
||||
*/
|
||||
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
|
||||
|
||||
/**
|
||||
* Write a register in the mpu, updating _checked_values
|
||||
*
|
||||
* @param reg The register to write.
|
||||
* @param value The new value to write.
|
||||
*/
|
||||
void write_checked_reg(unsigned reg, uint8_t value);
|
||||
|
||||
/**
|
||||
* Modify a checked register in the mpu
|
||||
*
|
||||
* Bits are cleared before bits are set.
|
||||
*
|
||||
* @param reg The register to modify.
|
||||
* @param clearbits Bits in the register to clear.
|
||||
* @param setbits Bits in the register to set.
|
||||
*/
|
||||
void modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
|
||||
|
||||
/**
|
||||
* Set the mpu measurement range.
|
||||
*
|
||||
* @param max_g The maximum G value the range must support.
|
||||
* @return OK if the value can be supported, -ERANGE otherwise.
|
||||
*/
|
||||
int set_accel_range(unsigned max_g);
|
||||
|
||||
/**
|
||||
* Swap a 16-bit value read from the mpu to native byte order.
|
||||
*/
|
||||
uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
|
||||
|
||||
/**
|
||||
* Get the internal / external state
|
||||
*
|
||||
* @return true if the sensor is not on the main MCU board
|
||||
*/
|
||||
bool is_external() { return _interface->external(); }
|
||||
|
||||
/*
|
||||
set low pass filter frequency
|
||||
*/
|
||||
void _set_dlpf_filter(uint16_t frequency_hz);
|
||||
|
||||
/*
|
||||
set sample rate (approximate) - 1kHz to 5Hz
|
||||
*/
|
||||
void _set_sample_rate(unsigned desired_sample_rate_hz);
|
||||
|
||||
/*
|
||||
check that key registers still have the right value
|
||||
*/
|
||||
void check_registers();
|
||||
};
|
||||
@ -1,118 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mpu9250_i2c.cpp
|
||||
*
|
||||
* I2C interface for MPU9250
|
||||
*/
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
#include "mpu9250.h"
|
||||
|
||||
#ifdef USE_I2C
|
||||
|
||||
device::Device *MPU9250_I2C_interface(int bus, uint32_t address, int bus_frequency);
|
||||
|
||||
class MPU9250_I2C : public device::I2C
|
||||
{
|
||||
public:
|
||||
MPU9250_I2C(int bus, uint32_t address, int bus_frequency);
|
||||
~MPU9250_I2C() override = default;
|
||||
|
||||
int read(unsigned address, void *data, unsigned count) override;
|
||||
int write(unsigned address, void *data, unsigned count) override;
|
||||
|
||||
protected:
|
||||
virtual int probe() override;
|
||||
|
||||
private:
|
||||
|
||||
};
|
||||
|
||||
device::Device *
|
||||
MPU9250_I2C_interface(int bus, uint32_t address, int bus_frequency)
|
||||
{
|
||||
return new MPU9250_I2C(bus, address, bus_frequency);
|
||||
}
|
||||
|
||||
MPU9250_I2C::MPU9250_I2C(int bus, uint32_t address, int bus_frequency) :
|
||||
I2C(DRV_IMU_DEVTYPE_MPU9250, MODULE_NAME, bus, address, bus_frequency)
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250_I2C::write(unsigned reg_speed, void *data, unsigned count)
|
||||
{
|
||||
uint8_t cmd[2] {};
|
||||
|
||||
if (sizeof(cmd) < (count + 1)) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
cmd[0] = MPU9250_REG(reg_speed);
|
||||
cmd[1] = *(uint8_t *)data;
|
||||
return transfer(&cmd[0], count + 1, nullptr, 0);
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250_I2C::read(unsigned reg_speed, void *data, unsigned count)
|
||||
{
|
||||
/* We want to avoid copying the data of MPUReport: So if the caller
|
||||
* supplies a buffer not MPUReport in size, it is assume to be a reg or
|
||||
* reg 16 read
|
||||
* Since MPUReport has a cmd at front, we must return the data
|
||||
* after that. Foe anthing else we must return it
|
||||
*/
|
||||
uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, ACCEL_XOUT_H);
|
||||
uint8_t cmd = MPU9250_REG(reg_speed);
|
||||
return transfer(&cmd, 1, &((uint8_t *)data)[offset], count - offset);
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250_I2C::probe()
|
||||
{
|
||||
uint8_t whoami = 0;
|
||||
|
||||
// Try first for mpu9250/6500
|
||||
read(MPUREG_WHOAMI, &whoami, 1);
|
||||
|
||||
if (whoami == MPU_WHOAMI_9250 || whoami == MPU_WHOAMI_6500) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
#endif /* USE_I2C */
|
||||
@ -1,182 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mpu9250_spi.cpp
|
||||
*
|
||||
* Driver for the Invensense MPU9250 connected via SPI.
|
||||
*
|
||||
* @author Andrew Tridgell
|
||||
* @author Pat Hickey
|
||||
* @author David sidrane
|
||||
*/
|
||||
|
||||
#include <drivers/device/spi.h>
|
||||
#include "mpu9250.h"
|
||||
|
||||
#define DIR_READ 0x80
|
||||
#define DIR_WRITE 0x00
|
||||
|
||||
/*
|
||||
* The MPU9250 can only handle high SPI bus speeds of 20Mhz on the sensor and
|
||||
* interrupt status registers. All other registers have a maximum 1MHz
|
||||
* SPI speed
|
||||
*
|
||||
* The Actual Value will be rounded down by the spi driver.
|
||||
* for a 168Mhz CPU this will be 10.5 Mhz and for a 180 Mhz CPU
|
||||
* it will be 11.250 Mhz
|
||||
*/
|
||||
#define MPU9250_LOW_SPI_BUS_SPEED 1000*1000
|
||||
#define MPU9250_HIGH_SPI_BUS_SPEED 20*1000*1000
|
||||
|
||||
device::Device *MPU9250_SPI_interface(int bus, uint32_t cs, int bus_frequency, spi_mode_e spi_mode);
|
||||
|
||||
class MPU9250_SPI : public device::SPI
|
||||
{
|
||||
public:
|
||||
MPU9250_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
|
||||
~MPU9250_SPI() override = default;
|
||||
|
||||
int read(unsigned address, void *data, unsigned count) override;
|
||||
int write(unsigned address, void *data, unsigned count) override;
|
||||
|
||||
protected:
|
||||
int probe() override;
|
||||
|
||||
private:
|
||||
/* Helper to set the desired speed and isolate the register on return */
|
||||
void set_bus_frequency(unsigned ®_speed_reg_out);
|
||||
|
||||
const int _high_bus_speed;
|
||||
};
|
||||
|
||||
device::Device *
|
||||
MPU9250_SPI_interface(int bus, uint32_t cs, int bus_frequency, spi_mode_e spi_mode)
|
||||
{
|
||||
return new MPU9250_SPI(bus, cs, bus_frequency, spi_mode);
|
||||
}
|
||||
|
||||
MPU9250_SPI::MPU9250_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
|
||||
SPI(DRV_IMU_DEVTYPE_MPU9250, MODULE_NAME, bus, device, spi_mode, MPU9250_LOW_SPI_BUS_SPEED),
|
||||
_high_bus_speed(bus_frequency)
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250_SPI::set_bus_frequency(unsigned ®_speed)
|
||||
{
|
||||
/* Set the desired speed */
|
||||
set_frequency(MPU9250_IS_HIGH_SPEED(reg_speed) ? _high_bus_speed : MPU9250_LOW_SPI_BUS_SPEED);
|
||||
|
||||
/* Isolate the register on return */
|
||||
reg_speed = MPU9250_REG(reg_speed);
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250_SPI::write(unsigned reg_speed, void *data, unsigned count)
|
||||
{
|
||||
uint8_t cmd[2] {};
|
||||
|
||||
if (sizeof(cmd) < (count + 1)) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
/* Set the desired speed and isolate the register */
|
||||
set_bus_frequency(reg_speed);
|
||||
|
||||
cmd[0] = reg_speed | DIR_WRITE;
|
||||
cmd[1] = *(uint8_t *)data;
|
||||
|
||||
return transfer(&cmd[0], &cmd[0], count + 1);
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250_SPI::read(unsigned reg_speed, void *data, unsigned count)
|
||||
{
|
||||
/* We want to avoid copying the data of MPUReport: So if the caller
|
||||
* supplies a buffer not MPUReport in size, it is assume to be a reg or reg 16 read
|
||||
* and we need to provied the buffer large enough for the callers data
|
||||
* and our command.
|
||||
*/
|
||||
uint8_t cmd[3] {};
|
||||
|
||||
uint8_t *pbuff = count < sizeof(MPUReport) ? cmd : (uint8_t *) data ;
|
||||
|
||||
if (count < sizeof(MPUReport)) {
|
||||
/* add command */
|
||||
count++;
|
||||
}
|
||||
|
||||
set_bus_frequency(reg_speed);
|
||||
|
||||
/* Set command */
|
||||
pbuff[0] = reg_speed | DIR_READ ;
|
||||
|
||||
/* Transfer the command and get the data */
|
||||
int ret = transfer(pbuff, pbuff, count);
|
||||
|
||||
if (ret == OK && pbuff == &cmd[0]) {
|
||||
/* Adjust the count back */
|
||||
count--;
|
||||
|
||||
/* Return the data */
|
||||
memcpy(data, &cmd[1], count);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250_SPI::probe()
|
||||
{
|
||||
uint8_t whoami = 0;
|
||||
|
||||
int ret = read(MPUREG_WHOAMI, &whoami, 1);
|
||||
|
||||
if (ret != OK) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
switch (whoami) {
|
||||
case MPU_WHOAMI_9250:
|
||||
case MPU_WHOAMI_6500:
|
||||
ret = 0;
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_WARN("probe failed! %u", whoami);
|
||||
ret = -EIO;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user