yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

refactor mpu6000: use driver base class

Browse Source
This commit is contained in:
Beat Küng
2020-02-26 14:55:57 +01:00
committed by Daniel Agar
parent be0a205438
commit 969a77f889
14 changed files with 180 additions and 491 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
boards/holybro/durandal-v1/init/rc.board_sensors
View File

@@ -5,7 +5,7 @@
adc start
# Internal SPI bus ICM-20689
mpu6000 -R 8 -z -T 20689 start
mpu6000 -R 8 -s -T 20689 start
# Internal SPI bus BMI088 accel
bmi088 -A -R 10 start

2
boards/holybro/kakutef7/init/rc.board_sensors
View File

@@ -8,7 +8,7 @@ adc start
# The default IMU is an ICM20689, but there might also be an MPU6000
if ! mpu6000 -R 12 -s start
then
mpu6000 -R 12 -z -T 20689 start
mpu6000 -R 12 -s -T 20689 start
fi
# Internal Baro

4
boards/mro/x21-777/init/rc.board_sensors
View File

@@ -10,10 +10,10 @@ hmc5883 -C -T -X start
lis3mdl -X start
# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
mpu6000 -R 2 -T 20608 start
mpu6000 -s -R 2 -T 20608 start
# Internal SPI bus ICM-20602-G is rotated 90 deg yaw
mpu6000 -R 2 -T 20602 start
mpu6000 -s -R 2 -T 20602 start
# Internal SPI bus mpu9250 is rotated 90 deg yaw
mpu9250 -R 2 start

4
boards/mro/x21/init/rc.board_sensors
View File

@@ -12,10 +12,10 @@ ist8310 -X start
qmc5883 -X start
# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
mpu6000 -R 2 -T 20608 start
mpu6000 -s -R 2 -T 20608 start
# Internal SPI bus ICM-20602-G is rotated 90 deg yaw
mpu6000 -R 2 -T 20602 start
mpu6000 -s -R 2 -T 20602 start
# Internal SPI bus mpu9250 is rotated 90 deg yaw
mpu9250 -R 2 start

2
boards/nxp/fmurt1062-v1/init/rc.board_sensors
View File

@@ -21,7 +21,7 @@ adc start
mpu6000 -R 8 -s -T 20602 start
# Internal SPI bus ICM-20689
mpu6000 -R 8 -z -T 20689 start
mpu6000 -R 8 -s -T 20689 start
# Internal SPI bus BMI055 accel
bmi055 -A -R 10 start

14
boards/px4/fmu-v2/init/rc.board_sensors
View File

@@ -14,7 +14,7 @@ ist8310 -X start
hmc5883 -C -T -I -R 4 start
# Internal SPI bus ICM-20608-G
mpu6000 -T 20608 start
mpu6000 -s -T 20608 start
# External SPI
ms5611 -S start
@@ -28,8 +28,8 @@ set BOARD_FMUV3 0
if ver hwtypecmp V30
then
# Check for Pixhawk 2.0 cube
# external MPU6K is rotated 180 degrees yaw
if mpu6000 -S -R 4 start
# MPU6K is rotated 180 degrees yaw
if mpu6000 -s -R 4 start
then
set BOARD_FMUV3 20
else
@@ -48,8 +48,8 @@ then
# Pixhawk Mini doesn't have these sensors,
# so if they are found we know its a Pixhack
# external MPU6K is rotated 180 degrees yaw
if mpu6000 -S -R 4 start
# MPU6K is rotated 180 degrees yaw
if mpu6000 -s -R 4 start
then
set BOARD_FMUV3 20
else
@@ -79,7 +79,7 @@ then
if [ $BOARD_FMUV3 = 20 ]
then
# v2.0 internal MPU6000 is rotated 180 deg roll, 270 deg yaw
mpu6000 -R 14 start
mpu6000 -s -R 14 start
# v2.0 Has internal hmc5883 on SPI1
hmc5883 -C -T -S -R 8 start
@@ -94,7 +94,7 @@ then
else
# $BOARD_FMUV3 = 0 -> FMUv2
mpu6000 start
mpu6000 -s start
# As we will use the external mag and the ICM-20608-G
# V2 build hwtypecmp is always false

14
boards/px4/fmu-v3/init/rc.board_sensors
View File

@@ -15,7 +15,7 @@ qmc5883 -X start
hmc5883 -C -T -I -R 4 start
# Internal SPI bus ICM-20608-G
mpu6000 -T 20608 start
mpu6000 -s -T 20608 start
# External SPI
ms5611 -S start
@@ -29,8 +29,8 @@ set BOARD_FMUV3 0
if ver hwtypecmp V30
then
# Check for Pixhawk 2.0 cube
# external MPU6K is rotated 180 degrees yaw
if mpu6000 -S -R 4 start
# MPU6K is rotated 180 degrees yaw
if mpu6000 -s -R 4 start
then
set BOARD_FMUV3 20
else
@@ -49,8 +49,8 @@ then
# Pixhawk Mini doesn't have these sensors,
# so if they are found we know its a Pixhack
# external MPU6K is rotated 180 degrees yaw
if mpu6000 -S -R 4 start
# MPU6K is rotated 180 degrees yaw
if mpu6000 -s -R 4 start
then
set BOARD_FMUV3 20
else
@@ -87,7 +87,7 @@ then
if [ $BOARD_FMUV3 = 20 ]
then
# v2.0 internal MPU6000 is rotated 180 deg roll, 270 deg yaw
mpu6000 -R 14 start
mpu6000 -s -R 14 start
# v2.0 Has internal hmc5883 on SPI1
hmc5883 -C -T -S -R 8 start
@@ -102,7 +102,7 @@ then
else
# $BOARD_FMUV3 = 0 -> FMUv2
mpu6000 start
mpu6000 -s start
# As we will use the external mag and the ICM-20608-G
# V2 build hwtypecmp is always false

2
boards/px4/fmu-v5/init/rc.board_sensors
View File

@@ -10,7 +10,7 @@ mpu6000 -R 8 -s -T 20602 start
#icm20602 -R 6 start
# Internal SPI bus ICM-20689
mpu6000 -R 8 -z -T 20689 start
mpu6000 -R 8 -s -T 20689 start
#icm20689 -R 6 start
# new sensor drivers (in testing)

2
boards/uvify/core/init/rc.board_sensors
View File

@@ -14,7 +14,7 @@ then
# GPS LED
rgbled_ncp5623c start -a 0x38
#mpu6000 -R 4 -T 20608 start
#mpu6000 -s -R 4 -T 20608 start
mpu9250 -R 4 start
# Default GNSS with LIS3MDL magnetometer with external i2c.

18
src/drivers/imu/mpu6000/MPU6000.cpp
View File

@@ -39,8 +39,9 @@
*/
constexpr uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS];
MPU6000::MPU6000(device::Device *interface, enum Rotation rotation, int device_type) :
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
MPU6000::MPU6000(device::Device *interface, enum Rotation rotation, int device_type, I2CSPIBusOption bus_option,
int bus) :
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, 0, device_type),
_interface(interface),
_device_type(device_type),
_px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation),
@@ -77,10 +78,6 @@ MPU6000::MPU6000(device::Device *interface, enum Rotation rotation, int device_t
MPU6000::~MPU6000()
{
/* make sure we are truly inactive */
stop();
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_bad_transfers);
perf_free(_bad_registers);
@@ -379,7 +376,7 @@ MPU6000::_set_icm_acc_dlpf_filter(uint16_t frequency_hz)
about 200ms and will return OK if the current values are within 14%
of the expected values (as per datasheet)
*/
int
void
MPU6000::factory_self_test()
{
_in_factory_test = true;
@@ -515,7 +512,6 @@ MPU6000::factory_self_test()
::printf("PASSED\n");
}
return ret;
}
#endif
@@ -710,7 +706,7 @@ MPU6000::check_registers(void)
_checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS;
}
void MPU6000::Run()
void MPU6000::RunImpl()
{
if (_in_factory_test) {
// don't publish any data while in factory test mode
@@ -873,8 +869,10 @@ void MPU6000::Run()
}
void
MPU6000::print_info()
MPU6000::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("Device type: %i", _device_type);
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);

47
src/drivers/imu/mpu6000/MPU6000.hpp
View File

@@ -63,6 +63,7 @@
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <systemlib/conversions.h>
#include <systemlib/px4_macros.h>
@@ -263,48 +264,36 @@ struct MPUReport {
# define MPU6000_LOW_SPEED_OP(r) MPU6000_REG((r))
/* interface factories */
extern device::Device *MPU6000_SPI_interface(int bus, int device_type, bool external_bus);
extern device::Device *MPU6000_I2C_interface(int bus, int device_type, bool external_bus);
extern device::Device *MPU6000_SPI_interface(int bus, uint32_t devid, int device_type, bool external_bus,
int bus_frequency, spi_mode_e spi_mode);
extern device::Device *MPU6000_I2C_interface(int bus, uint32_t devid, int device_type, bool external_bus,
int bus_frequency);
extern int MPU6000_probe(device::Device *dev, int device_type);
typedef device::Device *(*MPU6000_constructor)(int, int, bool);
#define MPU6000_TIMER_REDUCTION 200
enum MPU6000_BUS {
MPU6000_BUS_ALL = 0,
MPU6000_BUS_I2C_INTERNAL,
MPU6000_BUS_I2C_EXTERNAL,
MPU6000_BUS_SPI_INTERNAL1,
MPU6000_BUS_SPI_INTERNAL2,
MPU6000_BUS_SPI_EXTERNAL1,
MPU6000_BUS_SPI_EXTERNAL2
};
class MPU6000 : public px4::ScheduledWorkItem
class MPU6000 : public I2CSPIDriver<MPU6000>
{
public:
MPU6000(device::Device *interface, enum Rotation rotation, int device_type);
MPU6000(device::Device *interface, enum Rotation rotation, int device_type, I2CSPIBusOption bus_option, int bus);
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
virtual ~MPU6000();
virtual int init();
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
int init();
void print_registers();
#ifndef CONSTRAINED_FLASH
/**
* Test behaviour against factory offsets
*
* @return 0 on success, 1 on failure
*/
int factory_self_test();
void factory_self_test();
#endif
// deliberately cause a sensor error
@@ -322,15 +311,19 @@ public:
*/
int reset();
void RunImpl();
protected:
device::Device *_interface;
virtual int probe();
int probe();
void print_status() override;
void custom_method(const BusCLIArguments &cli) override;
private:
void Run() override;
int _device_type;
uint8_t _product{0}; /** product code */

22
src/drivers/imu/mpu6000/MPU6000_I2C.cpp
View File

@@ -41,14 +41,14 @@
#include "MPU6000.hpp"
#ifdef USE_I2C
#ifdef PX4_I2C_MPU6050_ADDR
device::Device *MPU6000_I2C_interface(int bus, int device_type, bool external_bus);
device::Device *MPU6000_I2C_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency);
class MPU6000_I2C : public device::I2C
{
public:
MPU6000_I2C(int bus, int device_type);
MPU6000_I2C(int bus, int device_type, int bus_frequency);
~MPU6000_I2C() override = default;
int read(unsigned address, void *data, unsigned count) override;
@@ -64,13 +64,13 @@ private:
device::Device *
MPU6000_I2C_interface(int bus, int device_type, bool external_bus)
MPU6000_I2C_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency)
{
return new MPU6000_I2C(bus, device_type);
return new MPU6000_I2C(bus, device_type, bus_frequency);
}
MPU6000_I2C::MPU6000_I2C(int bus, int device_type) :
I2C("MPU6000_I2C", nullptr, bus, PX4_I2C_MPU6050_ADDR, 400000),
MPU6000_I2C::MPU6000_I2C(int bus, int device_type, int bus_frequency) :
I2C("MPU6000_I2C", nullptr, bus, PX4_I2C_MPU6050_ADDR, bus_frequency),
_device_type(device_type)
{
}
@@ -112,4 +112,12 @@ MPU6000_I2C::probe()
return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
}
#else
device::Device *
MPU6000_I2C_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency)
{
return nullptr;
}
#endif /* USE_I2C */

93
src/drivers/imu/mpu6000/MPU6000_SPI.cpp
View File

@@ -48,7 +48,6 @@
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#if defined(PX4_SPIDEV_MPU) || defined(PX4_SPIDEV_ICM_20602) || defined(PX4_SPIDEV_ICM_20689)
/* The MPU6000 can only handle high SPI bus speeds of 20Mhz on the sensor and
* interrupt status registers. All other registers have a maximum 1MHz
@@ -73,13 +72,14 @@
#define UNKNOWN_HIGH_SPI_BUS_SPEED 8*1000*1000 // Use the minimum
device::Device *MPU6000_SPI_interface(int bus, int device_type, bool external_bus);
device::Device *MPU6000_SPI_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency,
spi_mode_e spi_mode);
class MPU6000_SPI : public device::SPI
{
public:
MPU6000_SPI(int bus, uint32_t device, int device_type);
MPU6000_SPI(int bus, uint32_t device, int device_type, int bus_frequency, spi_mode_e spi_mode);
~MPU6000_SPI() override = default;
int read(unsigned address, void *data, unsigned count) override;
@@ -98,90 +98,14 @@ private:
};
device::Device *
MPU6000_SPI_interface(int bus, int device_type, bool external_bus)
MPU6000_SPI_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency,
spi_mode_e spi_mode)
{
int cs = SPIDEV_NONE(0);
device::Device *interface = nullptr;
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) || defined(PX4_SPI_BUS_EXTERNAL)
switch (device_type) {
case MPU_DEVICE_TYPE_MPU6000:
# if defined(PX4_SPIDEV_EXT_MPU)
cs = PX4_SPIDEV_EXT_MPU;
# endif
break;
case MPU_DEVICE_TYPE_ICM20602:
# if defined(PX4_SPIDEV_ICM_20602_EXT)
cs = PX4_SPIDEV_ICM_20602_EXT;
# endif
break;
case MPU_DEVICE_TYPE_ICM20608:
# if defined(PX4_SPIDEV_EXT_ICM)
cs = PX4_SPIDEV_EXT_ICM;
# elif defined(PX4_SPIDEV_ICM_20608_EXT)
cs = PX4_SPIDEV_ICM_20608_EXT;
# endif
break;
case MPU_DEVICE_TYPE_ICM20689:
# if defined(PX4_SPIDEV_ICM_20689_EXT)
cs = PX4_SPIDEV_ICM_20689_EXT;
# endif
break;
default:
break;
}
#endif
} else {
switch (device_type) {
case MPU_DEVICE_TYPE_MPU6000:
#if defined(PX4_SPIDEV_MPU)
cs = PX4_SPIDEV_MPU;
#endif
break;
case MPU_DEVICE_TYPE_ICM20602:
#if defined(PX4_SPIDEV_ICM_20602)
cs = PX4_SPIDEV_ICM_20602;
#endif
break;
case MPU_DEVICE_TYPE_ICM20608:
#if defined(PX4_SPIDEV_ICM_20608)
cs = PX4_SPIDEV_ICM_20608;
#endif
break;
case MPU_DEVICE_TYPE_ICM20689:
# if defined(PX4_SPIDEV_ICM_20689)
cs = PX4_SPIDEV_ICM_20689;
# endif
default:
break;
}
}
if (cs != SPIDEV_NONE(0)) {
interface = new MPU6000_SPI(bus, cs, device_type);
}
return interface;
return new MPU6000_SPI(bus, devid, device_type, bus_frequency, spi_mode);
}
MPU6000_SPI::MPU6000_SPI(int bus, uint32_t device, int device_type) :
SPI("MPU6000", nullptr, bus, device, SPIDEV_MODE3, MPU6000_LOW_SPI_BUS_SPEED),
MPU6000_SPI::MPU6000_SPI(int bus, uint32_t device, int device_type, int bus_frequency, spi_mode_e spi_mode) :
SPI("MPU6000", nullptr, bus, device, spi_mode, bus_frequency),
_device_type(device_type)
{
}
@@ -285,4 +209,3 @@ MPU6000_SPI::probe()
return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
}
#endif // PX4_SPIDEV_MPU

445
src/drivers/imu/mpu6000/mpu6000_main.cpp
View File

@@ -32,414 +32,181 @@
****************************************************************************/
#include "MPU6000.hpp"
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
/**
* Local functions in support of the shell command.
*/
namespace mpu6000
void
MPU6000::print_usage()
{
PRINT_MODULE_USAGE_NAME("mpu6000", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
/*
list of supported bus configurations
*/
PRINT_MODULE_USAGE_PARAM_STRING('T', "6000", "6000|20608|20602|20689", "Device type", true);
struct mpu6000_bus_option {
enum MPU6000_BUS busid;
MPU_DEVICE_TYPE device_type;
MPU6000_constructor interface_constructor;
uint8_t busnum;
bool external;
MPU6000 *dev;
} bus_options[] = {
#if defined (USE_I2C)
# if defined(PX4_I2C_BUS_ONBOARD)
{ MPU6000_BUS_I2C_INTERNAL, MPU_DEVICE_TYPE_MPU6000, &MPU6000_I2C_interface, PX4_I2C_BUS_ONBOARD, false, NULL },
# endif
# if defined(PX4_I2C_BUS_EXPANSION)
{ MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_TYPE_MPU6000, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION, true, NULL },
# endif
# if defined(PX4_I2C_BUS_EXPANSION1)
{ MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_TYPE_MPU6000, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION1, true, NULL },
# endif
# if defined(PX4_I2C_BUS_EXPANSION2)
{ MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_TYPE_MPU6000, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION2, true, NULL },
# endif
#endif
#ifdef PX4_SPIDEV_MPU
{ MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_MPU6000, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
#endif
#if defined(PX4_SPI_BUS_EXT)
{ MPU6000_BUS_SPI_EXTERNAL1, MPU_DEVICE_TYPE_MPU6000, &MPU6000_SPI_interface, PX4_SPI_BUS_EXT, true, NULL },
#endif
#if defined(PX4_SPIDEV_ICM_20602) && defined(PX4_SPI_BUS_SENSORS)
{ MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_ICM20602, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
#endif
#if defined(PX4_SPIDEV_ICM_20602) && defined(PX4_SPI_BUS_SENSORS1)
{ MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_ICM20602, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS1, false, NULL },
#endif
#if defined(PX4_SPIDEV_ICM_20602) && defined(PX4_SPI_BUS_1)
{ MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_ICM20602, &MPU6000_SPI_interface, PX4_SPI_BUS_1, false, NULL },
#endif
#ifdef PX4_SPIDEV_ICM_20608
{ MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_ICM20608, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
#endif
#ifdef PX4_SPIDEV_ICM_20689
{ MPU6000_BUS_SPI_INTERNAL2, MPU_DEVICE_TYPE_ICM20689, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
#endif
#if defined(PX4_SPI_BUS_EXTERNAL)
{ MPU6000_BUS_SPI_EXTERNAL1, MPU_DEVICE_TYPE_MPU6000, &MPU6000_SPI_interface, PX4_SPI_BUS_EXTERNAL, true, NULL },
{ MPU6000_BUS_SPI_EXTERNAL2, MPU_DEVICE_TYPE_MPU6000, &MPU6000_SPI_interface, PX4_SPI_BUS_EXTERNAL, true, NULL },
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
void start(enum MPU6000_BUS busid, enum Rotation rotation, int device_type);
bool start_bus(struct mpu6000_bus_option &bus, enum Rotation rotation, int device_type);
void stop(enum MPU6000_BUS busid);
static struct mpu6000_bus_option &find_bus(enum MPU6000_BUS busid);
void reset(enum MPU6000_BUS busid);
void info(enum MPU6000_BUS busid);
void regdump(enum MPU6000_BUS busid);
void testerror(enum MPU6000_BUS busid);
PRINT_MODULE_USAGE_COMMAND("reset");
PRINT_MODULE_USAGE_COMMAND("regdump");
#ifndef CONSTRAINED_FLASH
void factorytest(enum MPU6000_BUS busid);
PRINT_MODULE_USAGE_COMMAND("factorytest");
#endif
void usage();
PRINT_MODULE_USAGE_COMMAND("testerror");
/**
* find a bus structure for a busid
*/
struct mpu6000_bus_option &find_bus(enum MPU6000_BUS busid)
{
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
if ((busid == MPU6000_BUS_ALL ||
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
return bus_options[i];
}
}
errx(1, "bus %u not started", (unsigned)busid);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
/**
* start driver for a specific bus option
*/
bool
start_bus(struct mpu6000_bus_option &bus, enum Rotation rotation, int device_type)
void
MPU6000::custom_method(const BusCLIArguments &cli)
{
if (bus.dev != nullptr) {
warnx("%s SPI not available", bus.external ? "External" : "Internal");
return false;
switch (cli.custom1) {
case 0: reset();
break;
case 1: print_registers();
break;
#ifndef CONSTRAINED_FLASH
case 2: factory_self_test();
break;
#endif
case 3: test_error();
break;
}
}
I2CSPIDriverBase *MPU6000::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
device::Device *interface = nullptr;
int device_type = cli.type;
if (iterator.busType() == BOARD_I2C_BUS) {
interface = MPU6000_I2C_interface(iterator.bus(), iterator.devid(), device_type, iterator.external(),
cli.bus_frequency);
} else if (iterator.busType() == BOARD_SPI_BUS) {
interface = MPU6000_SPI_interface(iterator.bus(), iterator.devid(), device_type, iterator.external(),
cli.bus_frequency, cli.spi_mode);
}
device::Device *interface = bus.interface_constructor(bus.busnum, device_type, bus.external);
if (interface == nullptr) {
warnx("failed creating interface for bus #%u (SPI%u)", (unsigned)bus.busid, (unsigned)bus.busnum);
return false;
PX4_ERR("alloc failed");
return nullptr;
}
if (interface->init() != OK) {
delete interface;
warnx("no device on bus #%u (SPI%u)", (unsigned)bus.busid, (unsigned)bus.busnum);
return false;
PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
return nullptr;
}
bus.dev = new MPU6000(interface, rotation, device_type);
MPU6000 *dev = new MPU6000(interface, cli.rotation, device_type, iterator.configuredBusOption(), iterator.bus());
if (bus.dev == nullptr) {
if (dev == nullptr) {
delete interface;
return false;
return nullptr;
}
if (OK != bus.dev->init()) {
goto fail;
if (OK != dev->init()) {
delete dev;
return nullptr;
}
/* set the poll rate to default, starts automatic data collection */
bus.dev->start();
return true;
fail:
if (bus.dev != nullptr) {
delete bus.dev;
bus.dev = nullptr;
}
return false;
dev->start();
return dev;
}
/**
* Start the driver.
*
* This function only returns if the driver is up and running
* or failed to detect the sensor.
*/
void
start(enum MPU6000_BUS busid, enum Rotation rotation, int device_type)
{
bool started = false;
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (busid == MPU6000_BUS_ALL && bus_options[i].dev != NULL) {
// this device is already started
continue;
}
if (busid != MPU6000_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for
continue;
}
if (bus_options[i].device_type != device_type) {
// not the one that is asked for
continue;
}
started |= start_bus(bus_options[i], rotation, device_type);
}
exit(started ? 0 : 1);
}
void
stop(enum MPU6000_BUS busid)
{
struct mpu6000_bus_option &bus = find_bus(busid);
if (bus.dev != nullptr) {
delete bus.dev;
bus.dev = nullptr;
} else {
/* warn, but not an error */
warnx("already stopped.");
}
exit(0);
}
/**
* Reset the driver.
*/
void
reset(enum MPU6000_BUS busid)
{
struct mpu6000_bus_option &bus = find_bus(busid);
if (bus.dev == nullptr) {
errx(1, "driver not running");
}
bus.dev->reset();
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info(enum MPU6000_BUS busid)
{
struct mpu6000_bus_option &bus = find_bus(busid);
if (bus.dev == nullptr) {
errx(1, "driver not running");
}
bus.dev->print_info();
exit(0);
}
/**
* Dump the register information
*/
void
regdump(enum MPU6000_BUS busid)
{
struct mpu6000_bus_option &bus = find_bus(busid);
if (bus.dev == nullptr) {
errx(1, "driver not running");
}
printf("regdump @ %p\n", bus.dev);
bus.dev->print_registers();
exit(0);
}
/**
* deliberately produce an error to test recovery
*/
void
testerror(enum MPU6000_BUS busid)
{
struct mpu6000_bus_option &bus = find_bus(busid);
if (bus.dev == nullptr) {
errx(1, "driver not running");
}
bus.dev->test_error();
exit(0);
}
#ifndef CONSTRAINED_FLASH
/**
* Dump the register information
*/
void
factorytest(enum MPU6000_BUS busid)
{
struct mpu6000_bus_option &bus = find_bus(busid);
if (bus.dev == nullptr) {
errx(1, "driver not running");
}
bus.dev->factory_self_test();
exit(0);
}
#endif
void
usage()
{
warnx("missing command: try 'start', 'info', 'stop',\n'reset', 'regdump', 'testerror'"
#ifndef CONSTRAINED_FLASH
", 'factorytest'"
#endif
);
warnx("options:");
warnx(" -X external I2C bus");
warnx(" -I internal I2C bus");
warnx(" -S external SPI bus");
warnx(" -s internal SPI bus");
warnx(" -Z external1 SPI bus");
warnx(" -z internal2 SPI bus");
warnx(" -T 6000|20608|20602 (default 6000)");
warnx(" -R rotation");
}
} // namespace
/** driver 'main' command */
extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
int
mpu6000_main(int argc, char *argv[])
{
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
using ThisDriver = MPU6000;
BusCLIArguments cli{true, true};
cli.type = MPU_DEVICE_TYPE_MPU6000;
cli.default_spi_frequency = 1000 * 1000; // low speed bus frequency
enum MPU6000_BUS busid = MPU6000_BUS_ALL;
int device_type = MPU_DEVICE_TYPE_MPU6000;
enum Rotation rotation = ROTATION_NONE;
while ((ch = px4_getopt(argc, argv, "T:XISsZzR:a:", &myoptind, &myoptarg)) != EOF) {
while ((ch = cli.getopt(argc, argv, "T:R:")) != EOF) {
switch (ch) {
case 'X':
busid = MPU6000_BUS_I2C_EXTERNAL;
break;
case 'I':
busid = MPU6000_BUS_I2C_INTERNAL;
break;
case 'S':
busid = MPU6000_BUS_SPI_EXTERNAL1;
break;
case 's':
busid = MPU6000_BUS_SPI_INTERNAL1;
break;
case 'Z':
busid = MPU6000_BUS_SPI_EXTERNAL2;
break;
case 'z':
busid = MPU6000_BUS_SPI_INTERNAL2;
break;
case 'T':
device_type = atoi(myoptarg);
cli.type = atoi(cli.optarg());
break;
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
default:
mpu6000::usage();
return 0;
}
}
if (myoptind >= argc) {
mpu6000::usage();
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
const char *verb = argv[myoptind];
uint16_t dev_type_driver = 0;
switch (cli.type) {
case MPU_DEVICE_TYPE_MPU6000:
dev_type_driver = DRV_IMU_DEVTYPE_MPU6000;
break;
case MPU_DEVICE_TYPE_ICM20602:
dev_type_driver = DRV_IMU_DEVTYPE_ICM20602;
break;
case MPU_DEVICE_TYPE_ICM20608:
dev_type_driver = DRV_IMU_DEVTYPE_ICM20608;
break;
case MPU_DEVICE_TYPE_ICM20689:
dev_type_driver = DRV_IMU_DEVTYPE_ICM20689;
break;
}
BusInstanceIterator iterator(MODULE_NAME, cli, dev_type_driver);
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
mpu6000::start(busid, rotation, device_type);
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
mpu6000::stop(busid);
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
mpu6000::reset(busid);
cli.custom1 = 0;
return ThisDriver::module_custom_method(cli, iterator);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
mpu6000::info(busid);
}
/*
* Print register information.
*/
if (!strcmp(verb, "regdump")) {
mpu6000::regdump(busid);
cli.custom1 = 1;
return ThisDriver::module_custom_method(cli, iterator);
}
#ifndef CONSTRAINED_FLASH
if (!strcmp(verb, "factorytest")) {
mpu6000::factorytest(busid);
cli.custom1 = 2;
return ThisDriver::module_custom_method(cli, iterator);
}
#endif
if (!strcmp(verb, "testerror")) {
mpu6000::testerror(busid);
cli.custom1 = 3;
return ThisDriver::module_custom_method(cli, iterator);
}
mpu6000::usage();
ThisDriver::print_usage();
return -1;
}