Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 18:06:39 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

mpu9250: create dedicated i2c version and delete legacy driver

Browse Source 
- 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
2020-07-26 21:38:35 -04:00
parent c9a7894230
commit 3abe2e82d1
21 changed files with 830 additions and 2197 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/beaglebone/blue/default.cmake
+1 -1
View File
@@ -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
boards/bitcraze/crazyflie/default.cmake
+4 -3
View File
@@ -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
boards/bitcraze/crazyflie/init/rc.board_sensors
+5 -4
View File
@@ -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
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
+5 -5
View File
@@ -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};
posix-configs/bbblue/px4.config
+1 -1
View File
@@ -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
posix-configs/bbblue/px4_fw.config
+1 -1
View File
@@ -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
posix-configs/eagle/flight/px4.config
+1 -1
View File
@@ -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
src/drivers/imu/invensense/mpu9250/CMakeLists.txt
+16 -1
View File
@@ -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
)
src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp
+11 -3
View File
@@ -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
src/drivers/imu/invensense/mpu9250/MPU9250.hpp
+1 -1
View File
@@ -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 },
src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp
+591
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp
+172
View 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 &reg_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 &timestamp_sample, uint8_t samples);
void FIFOReset();
bool ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_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 },
};
};
src/drivers/imu/mpu9250/mpu9250_main.cpp → src/drivers/imu/invensense/mpu9250/mpu9250_i2c_main.cpp
+21 -49
View File
@@ -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) {
src/drivers/imu/mpu9250/AK8963_I2C.cpp
-113
View File
@@ -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
src/drivers/imu/mpu9250/CMakeLists.txt
-49
View File
@@ -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
)
src/drivers/imu/mpu9250/MPU9250_mag.cpp
-428
View File
@@ -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 &timestamp_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);
}
}
src/drivers/imu/mpu9250/MPU9250_mag.h
-156
View File
@@ -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 &timestamp_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;
};
src/drivers/imu/mpu9250/mpu9250.cpp
-672
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/mpu9250/mpu9250.h
-409
View File
@@ -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();
};
src/drivers/imu/mpu9250/mpu9250_i2c.cpp
-118
View File
@@ -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 */
src/drivers/imu/mpu9250/mpu9250_spi.cpp
-182
View File
@@ -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 &reg_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 &reg_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;
}