From 52c848a556fa7b82273dac7ff915893f43dd01f8 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 14 Jan 2019 16:22:42 -0500
Subject: [PATCH] mpu6000 split into separate main, header, implementation
---
src/drivers/imu/mpu6000/CMakeLists.txt | 7 +-
.../imu/mpu6000/{mpu6000.cpp => MPU6000.cpp} | 692 +-----------------
.../imu/mpu6000/{mpu6000.h => MPU6000.hpp} | 296 +++++++-
.../{mpu6000_i2c.cpp => MPU6000_I2C.cpp} | 2 +-
.../{mpu6000_spi.cpp => MPU6000_SPI.cpp} | 2 +-
src/drivers/imu/mpu6000/mpu6000_main.cpp | 428 +++++++++++
6 files changed, 729 insertions(+), 698 deletions(-)
rename src/drivers/imu/mpu6000/{mpu6000.cpp => MPU6000.cpp} (61%)
rename src/drivers/imu/mpu6000/{mpu6000.h => MPU6000.hpp} (53%)
rename src/drivers/imu/mpu6000/{mpu6000_i2c.cpp => MPU6000_I2C.cpp} (99%)
rename src/drivers/imu/mpu6000/{mpu6000_spi.cpp => MPU6000_SPI.cpp} (99%)
create mode 100644 src/drivers/imu/mpu6000/mpu6000_main.cpp
diff --git a/src/drivers/imu/mpu6000/CMakeLists.txt b/src/drivers/imu/mpu6000/CMakeLists.txt
index b904a0d51d..ec8c861537 100644
--- a/src/drivers/imu/mpu6000/CMakeLists.txt
+++ b/src/drivers/imu/mpu6000/CMakeLists.txt
@@ -35,9 +35,10 @@ px4_add_module(
MAIN mpu6000
STACK_MAIN 1500
SRCS
- mpu6000.cpp
- mpu6000_i2c.cpp
- mpu6000_spi.cpp
+ MPU6000.cpp
+ MPU6000_I2C.cpp
+ mpu6000_main.cpp
+ MPU6000_SPI.cpp
DEPENDS
drivers_accelerometer
drivers_gyroscope
diff --git a/src/drivers/imu/mpu6000/mpu6000.cpp b/src/drivers/imu/mpu6000/MPU6000.cpp
similarity index 61%
rename from src/drivers/imu/mpu6000/mpu6000.cpp
rename to src/drivers/imu/mpu6000/MPU6000.cpp
index 6d89fc0690..1cfd72b3c5 100644
--- a/src/drivers/imu/mpu6000/mpu6000.cpp
+++ b/src/drivers/imu/mpu6000/MPU6000.cpp
@@ -31,303 +31,7 @@
*
****************************************************************************/
-/**
- * @file mpu6000.cpp
- *
- * Driver for the Invensense MPU6000, MPU6050, ICM20608, and ICM20602 connected via
- * SPI or I2C.
- *
- * When the device is on the SPI bus the hrt is used to provide thread of
- * execution to the driver.
- *
- * When the device is on the I2C bus a work queue is used provide thread of
- * execution to the driver.
- *
- * The I2C code is only included in the build if USE_I2C is defined by the
- * existance of any of PX4_I2C_MPU6050_ADDR, PX4_I2C_MPU6000_ADDR
- * PX4_I2C_ICM_20608_G_ADDR in the board_config.h file.
- *
- * The command line option -T 6000|20608|20602 (default 6000) selects between
- * MPU60x0, ICM20608G, or ICM20602G;
- *
- * @author Andrew Tridgell
- * @author Pat Hickey
- * @author David Sidrane
- */
-
-#include <drivers/drv_hrt.h>
-#include <lib/cdev/CDev.hpp>
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/device/i2c.h>
-#include <lib/drivers/device/spi.h>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-#include <lib/ecl/geo/geo.h>
-#include <lib/perf/perf_counter.h>
-#include <px4_config.h>
-#include <px4_getopt.h>
-#include <px4_time.h>
-#include <px4_workqueue.h>
-#include <systemlib/conversions.h>
-#include <systemlib/px4_macros.h>
-
-#include "mpu6000.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
-
- I2C bus is running at 100 kHz Transaction time is 2.163ms
- I2C bus is running at 400 kHz (304 kHz actual) Transaction time
- is 583 us
-
- */
-#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 cdev::CDev
-{
-public:
- MPU6000(device::Device *interface, const char *path, enum Rotation rotation, int device_type);
-
- virtual ~MPU6000();
-
- virtual int init();
-
- /**
- * Diagnostics - print some basic information about the driver.
- */
- void print_info();
-
- void print_registers();
-
- /**
- * Test behaviour against factory offsets
- *
- * @return 0 on success, 1 on failure
- */
- int factory_self_test();
-
- // deliberately cause a sensor error
- void test_error();
-
- /**
- * Start automatic measurement.
- */
- void start();
-
- /**
- * Reset chip.
- *
- * Resets the chip and measurements ranges, but not scale and offset.
- */
- int reset();
-
-protected:
- device::Device *_interface;
-
- virtual int probe();
-
-private:
- int _device_type;
- uint8_t _product{0}; /** product code */
-
-#if defined(USE_I2C)
- /*
- * SPI bus based device use hrt
- * I2C bus needs to use work queue
- */
- work_s _work{};
-#endif
- bool _use_hrt;
-
- struct hrt_call _call {};
- unsigned _call_interval{1000};
-
- PX4Accelerometer _px4_accel;
- PX4Gyroscope _px4_gyro;
-
- unsigned _sample_rate{1000};
-
- perf_counter_t _sample_perf;
- perf_counter_t _measure_interval;
- perf_counter_t _bad_transfers;
- perf_counter_t _bad_registers;
- perf_counter_t _reset_retries;
- 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 MPU6000_CHECKED_PRODUCT_ID_INDEX = 0;
- static constexpr int MPU6000_NUM_CHECKED_REGISTERS = 10;
-
- static constexpr uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS] {
- MPUREG_PRODUCT_ID,
- MPUREG_PWR_MGMT_1,
- MPUREG_USER_CTRL,
- MPUREG_SMPLRT_DIV,
- MPUREG_CONFIG,
- MPUREG_GYRO_CONFIG,
- MPUREG_ACCEL_CONFIG,
- MPUREG_INT_ENABLE,
- MPUREG_INT_PIN_CFG,
- MPUREG_ICM_UNDOC1
- };
-
- uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS];
- uint8_t _checked_next{0};
-
- // use this to avoid processing measurements when in factory
- // self test
- volatile bool _in_factory_test{false};
-
- // keep last accel reading for duplicate detection
- uint16_t _last_accel[3] {};
- bool _got_duplicate{false};
-
- /**
- * Stop automatic measurement.
- */
- void stop();
-
- /**
- * is_icm_device
- */
- bool is_icm_device() { return !is_mpu_device(); }
- /**
- * is_mpu_device
- */
- bool is_mpu_device() { return _device_type == MPU_DEVICE_TYPE_MPU6000; }
-
-
-#if defined(USE_I2C)
- /**
- * When the I2C interfase is on
- * Perform a poll cycle; collect from the previous measurement
- * and start a new one.
- *
- * This is the heart of the measurement state machine. This function
- * alternately starts a measurement, or collects the data from the
- * previous measurement.
- *
- * When the interval between measurements is greater than the minimum
- * measurement interval, a gap is inserted between collection
- * and measurement to provide the most recent measurement possible
- * at the next interval.
- */
- void cycle();
-
- /**
- * Static trampoline from the workq context; because we don't have a
- * generic workq wrapper yet.
- *
- * @param arg Instance pointer for the driver that is polling.
- */
- static void cycle_trampoline(void *arg);
-
- void use_i2c(bool on_true) { _use_hrt = !on_true; }
-
-#endif
-
- bool is_i2c(void) { return !_use_hrt; }
-
-
- /**
- * Static trampoline from the hrt_call context; because we don't have a
- * generic hrt wrapper yet.
- *
- * Called by the HRT in interrupt context at the specified rate if
- * automatic polling is enabled.
- *
- * @param arg Instance pointer for the driver that is polling.
- */
- static void measure_trampoline(void *arg);
-
- /**
- * Fetch measurements from the sensor and update the report buffers.
- */
- int measure();
-
- /**
- * Read a register from the MPU6000
- *
- * @param The register to read.
- * @return The value that was read.
- */
- uint8_t read_reg(unsigned reg, uint32_t speed = MPU6000_LOW_BUS_SPEED);
- uint16_t read_reg16(unsigned reg);
-
-
- /**
- * Write a register in the MPU6000
- *
- * @param reg The register to write.
- * @param value The new value to write.
- */
- int write_reg(unsigned reg, uint8_t value);
-
- /**
- * Modify a register in the MPU6000
- *
- * 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 MPU6000, 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);
-
- /**
- * Set the MPU6000 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);
-
- /*
- set low pass filter frequency
- */
- void _set_dlpf_filter(uint16_t frequency_hz);
- void _set_icm_acc_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(void);
-
- /* do not allow to copy this class due to pointer data members */
- MPU6000(const MPU6000 &);
- MPU6000 operator=(const MPU6000 &);
-
-};
+#include "MPU6000.hpp"
/*
list of registers that will be checked in check_registers(). Note
@@ -335,9 +39,6 @@ private:
*/
constexpr uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS];
-/** driver 'main' command */
-extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
-
MPU6000::MPU6000(device::Device *interface, const char *path, enum Rotation rotation, int device_type) :
CDev(path),
_interface(interface),
@@ -1286,394 +987,3 @@ MPU6000::print_registers()
printf("\n");
}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace mpu6000
-{
-
-/*
- list of supported bus configurations
- */
-
-struct mpu6000_bus_option {
- enum MPU6000_BUS busid;
- MPU_DEVICE_TYPE device_type;
- const char *devpath;
- 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, MPU_DEVICE_PATH, &MPU6000_I2C_interface, PX4_I2C_BUS_ONBOARD, false, NULL },
-# endif
-# if defined(PX4_I2C_BUS_EXPANSION)
- { MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_EXT, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION, true, NULL },
-# endif
-# if defined(PX4_I2C_BUS_EXPANSION1)
- { MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_EXT1, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION1, true, NULL },
-# endif
-# if defined(PX4_I2C_BUS_EXPANSION2)
- { MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_EXT2, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION2, true, NULL },
-# endif
-#endif
-#ifdef PX4_SPIDEV_MPU
- { MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
-#endif
-#if defined(PX4_SPI_BUS_EXT)
- { MPU6000_BUS_SPI_EXTERNAL1, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_EXT, &MPU6000_SPI_interface, PX4_SPI_BUS_EXT, true, NULL },
-#endif
-#ifdef PX4_SPIDEV_ICM_20602
- { MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_ICM20602, ICM20602_DEVICE_PATH, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
-#endif
-#ifdef PX4_SPIDEV_ICM_20608
- { MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_ICM20608, ICM20608_DEVICE_PATH, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
-#endif
-#ifdef PX4_SPIDEV_ICM_20689
- { MPU6000_BUS_SPI_INTERNAL2, MPU_DEVICE_TYPE_ICM20689, ICM20689_DEVICE_PATH, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
-#endif
-#if defined(PX4_SPI_BUS_EXTERNAL)
- { MPU6000_BUS_SPI_EXTERNAL1, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_EXT, &MPU6000_SPI_interface, PX4_SPI_BUS_EXTERNAL, true, NULL },
- { MPU6000_BUS_SPI_EXTERNAL2, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_EXT1, &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);
-void factorytest(enum MPU6000_BUS busid);
-void usage();
-
-/**
- * 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);
-}
-
-/**
- * start driver for a specific bus option
- */
-bool
-start_bus(struct mpu6000_bus_option &bus, enum Rotation rotation, int device_type)
-{
- if (bus.dev != nullptr) {
- warnx("%s SPI not available", bus.external ? "External" : "Internal");
- return false;
- }
-
- 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;
- }
-
- if (interface->init() != OK) {
- delete interface;
- warnx("no device on bus #%u (SPI%u)", (unsigned)bus.busid, (unsigned)bus.busnum);
- return false;
- }
-
- bus.dev = new MPU6000(interface, bus.devpath, rotation, device_type);
-
- if (bus.dev == nullptr) {
- delete interface;
- return false;
- }
-
- if (OK != bus.dev->init()) {
- goto fail;
- }
-
- /* 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;
-}
-
-/**
- * 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);
-}
-
-/**
- * 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);
-}
-
-void
-usage()
-{
- warnx("missing command: try 'start', 'info', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'");
- 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
-
-int
-mpu6000_main(int argc, char *argv[])
-{
- int myoptind = 1;
- int ch;
- const char *myoptarg = nullptr;
-
- 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) {
- 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);
- break;
-
- case 'R':
- rotation = (enum Rotation)atoi(myoptarg);
- break;
-
- default:
- mpu6000::usage();
- return 0;
- }
- }
-
- if (myoptind >= argc) {
- mpu6000::usage();
- return -1;
- }
-
- const char *verb = argv[myoptind];
-
- /*
- * Start/load the driver.
- */
- if (!strcmp(verb, "start")) {
- mpu6000::start(busid, rotation, device_type);
- }
-
- if (!strcmp(verb, "stop")) {
- mpu6000::stop(busid);
- }
-
- /*
- * Reset the driver.
- */
- if (!strcmp(verb, "reset")) {
- mpu6000::reset(busid);
- }
-
- /*
- * Print driver information.
- */
- if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
- mpu6000::info(busid);
- }
-
- /*
- * Print register information.
- */
- if (!strcmp(verb, "regdump")) {
- mpu6000::regdump(busid);
- }
-
- if (!strcmp(verb, "factorytest")) {
- mpu6000::factorytest(busid);
- }
-
- if (!strcmp(verb, "testerror")) {
- mpu6000::testerror(busid);
- }
-
- mpu6000::usage();
- return -1;
-}
diff --git a/src/drivers/imu/mpu6000/mpu6000.h b/src/drivers/imu/mpu6000/MPU6000.hpp
similarity index 53%
rename from src/drivers/imu/mpu6000/mpu6000.h
rename to src/drivers/imu/mpu6000/MPU6000.hpp
index 7fc3a03bbf..5e7d37f929 100644
--- a/src/drivers/imu/mpu6000/mpu6000.h
+++ b/src/drivers/imu/mpu6000/MPU6000.hpp
@@ -32,9 +32,54 @@
****************************************************************************/
/**
- * @file .h
+ * @file mpu6000.cpp
*
- * Shared defines for the mpu6000 driver.
+ * Driver for the Invensense MPU6000, MPU6050, ICM20608, and ICM20602 connected via
+ * SPI or I2C.
+ *
+ * When the device is on the SPI bus the hrt is used to provide thread of
+ * execution to the driver.
+ *
+ * When the device is on the I2C bus a work queue is used provide thread of
+ * execution to the driver.
+ *
+ * The I2C code is only included in the build if USE_I2C is defined by the
+ * existance of any of PX4_I2C_MPU6050_ADDR, PX4_I2C_MPU6000_ADDR
+ * PX4_I2C_ICM_20608_G_ADDR in the board_config.h file.
+ *
+ * The command line option -T 6000|20608|20602 (default 6000) selects between
+ * MPU60x0, ICM20608G, or ICM20602G;
+ *
+ * @author Andrew Tridgell
+ * @author Pat Hickey
+ * @author David Sidrane
+ */
+
+#include <drivers/drv_hrt.h>
+#include <lib/cdev/CDev.hpp>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/device/i2c.h>
+#include <lib/drivers/device/spi.h>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <lib/ecl/geo/geo.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_config.h>
+#include <px4_getopt.h>
+#include <px4_time.h>
+#include <px4_workqueue.h>
+#include <systemlib/conversions.h>
+#include <systemlib/px4_macros.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
+
+ I2C bus is running at 100 kHz Transaction time is 2.163ms
+ I2C bus is running at 400 kHz (304 kHz actual) Transaction time
+ is 583 us
+
*/
#pragma once
@@ -243,3 +288,250 @@ extern device::Device *MPU6000_I2C_interface(int bus, int device_type, bool exte
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 cdev::CDev
+{
+public:
+ MPU6000(device::Device *interface, const char *path, enum Rotation rotation, int device_type);
+
+ virtual ~MPU6000();
+
+ virtual int init();
+
+ /**
+ * Diagnostics - print some basic information about the driver.
+ */
+ void print_info();
+
+ void print_registers();
+
+ /**
+ * Test behaviour against factory offsets
+ *
+ * @return 0 on success, 1 on failure
+ */
+ int factory_self_test();
+
+ // deliberately cause a sensor error
+ void test_error();
+
+ /**
+ * Start automatic measurement.
+ */
+ void start();
+
+ /**
+ * Reset chip.
+ *
+ * Resets the chip and measurements ranges, but not scale and offset.
+ */
+ int reset();
+
+protected:
+ device::Device *_interface;
+
+ virtual int probe();
+
+private:
+ int _device_type;
+ uint8_t _product{0}; /** product code */
+
+#if defined(USE_I2C)
+ /*
+ * SPI bus based device use hrt
+ * I2C bus needs to use work queue
+ */
+ work_s _work{};
+#endif
+ bool _use_hrt;
+
+ struct hrt_call _call {};
+ unsigned _call_interval{1000};
+
+ PX4Accelerometer _px4_accel;
+ PX4Gyroscope _px4_gyro;
+
+ unsigned _sample_rate{1000};
+
+ perf_counter_t _sample_perf;
+ perf_counter_t _measure_interval;
+ perf_counter_t _bad_transfers;
+ perf_counter_t _bad_registers;
+ perf_counter_t _reset_retries;
+ 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 MPU6000_CHECKED_PRODUCT_ID_INDEX = 0;
+ static constexpr int MPU6000_NUM_CHECKED_REGISTERS = 10;
+
+ static constexpr uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS] {
+ MPUREG_PRODUCT_ID,
+ MPUREG_PWR_MGMT_1,
+ MPUREG_USER_CTRL,
+ MPUREG_SMPLRT_DIV,
+ MPUREG_CONFIG,
+ MPUREG_GYRO_CONFIG,
+ MPUREG_ACCEL_CONFIG,
+ MPUREG_INT_ENABLE,
+ MPUREG_INT_PIN_CFG,
+ MPUREG_ICM_UNDOC1
+ };
+
+ uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS];
+ uint8_t _checked_next{0};
+
+ // use this to avoid processing measurements when in factory
+ // self test
+ volatile bool _in_factory_test{false};
+
+ // keep last accel reading for duplicate detection
+ uint16_t _last_accel[3] {};
+ bool _got_duplicate{false};
+
+ /**
+ * Stop automatic measurement.
+ */
+ void stop();
+
+ /**
+ * is_icm_device
+ */
+ bool is_icm_device() { return !is_mpu_device(); }
+ /**
+ * is_mpu_device
+ */
+ bool is_mpu_device() { return _device_type == MPU_DEVICE_TYPE_MPU6000; }
+
+
+#if defined(USE_I2C)
+ /**
+ * When the I2C interfase is on
+ * Perform a poll cycle; collect from the previous measurement
+ * and start a new one.
+ *
+ * This is the heart of the measurement state machine. This function
+ * alternately starts a measurement, or collects the data from the
+ * previous measurement.
+ *
+ * When the interval between measurements is greater than the minimum
+ * measurement interval, a gap is inserted between collection
+ * and measurement to provide the most recent measurement possible
+ * at the next interval.
+ */
+ void cycle();
+
+ /**
+ * Static trampoline from the workq context; because we don't have a
+ * generic workq wrapper yet.
+ *
+ * @param arg Instance pointer for the driver that is polling.
+ */
+ static void cycle_trampoline(void *arg);
+
+ void use_i2c(bool on_true) { _use_hrt = !on_true; }
+
+#endif
+
+ bool is_i2c(void) { return !_use_hrt; }
+
+
+ /**
+ * Static trampoline from the hrt_call context; because we don't have a
+ * generic hrt wrapper yet.
+ *
+ * Called by the HRT in interrupt context at the specified rate if
+ * automatic polling is enabled.
+ *
+ * @param arg Instance pointer for the driver that is polling.
+ */
+ static void measure_trampoline(void *arg);
+
+ /**
+ * Fetch measurements from the sensor and update the report buffers.
+ */
+ int measure();
+
+ /**
+ * Read a register from the MPU6000
+ *
+ * @param The register to read.
+ * @return The value that was read.
+ */
+ uint8_t read_reg(unsigned reg, uint32_t speed = MPU6000_LOW_BUS_SPEED);
+ uint16_t read_reg16(unsigned reg);
+
+
+ /**
+ * Write a register in the MPU6000
+ *
+ * @param reg The register to write.
+ * @param value The new value to write.
+ */
+ int write_reg(unsigned reg, uint8_t value);
+
+ /**
+ * Modify a register in the MPU6000
+ *
+ * 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 MPU6000, 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);
+
+ /**
+ * Set the MPU6000 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);
+
+ /*
+ set low pass filter frequency
+ */
+ void _set_dlpf_filter(uint16_t frequency_hz);
+ void _set_icm_acc_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(void);
+
+ /* do not allow to copy this class due to pointer data members */
+ MPU6000(const MPU6000 &);
+ MPU6000 operator=(const MPU6000 &);
+
+};
diff --git a/src/drivers/imu/mpu6000/mpu6000_i2c.cpp b/src/drivers/imu/mpu6000/MPU6000_I2C.cpp
similarity index 99%
rename from src/drivers/imu/mpu6000/mpu6000_i2c.cpp
rename to src/drivers/imu/mpu6000/MPU6000_I2C.cpp
index 18537e1e96..a7a29fee06 100644
--- a/src/drivers/imu/mpu6000/mpu6000_i2c.cpp
+++ b/src/drivers/imu/mpu6000/MPU6000_I2C.cpp
@@ -42,7 +42,7 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
-#include "mpu6000.h"
+#include "MPU6000.hpp"
#ifdef USE_I2C
diff --git a/src/drivers/imu/mpu6000/mpu6000_spi.cpp b/src/drivers/imu/mpu6000/MPU6000_SPI.cpp
similarity index 99%
rename from src/drivers/imu/mpu6000/mpu6000_spi.cpp
rename to src/drivers/imu/mpu6000/MPU6000_SPI.cpp
index 6872955163..da6fdb2ce3 100644
--- a/src/drivers/imu/mpu6000/mpu6000_spi.cpp
+++ b/src/drivers/imu/mpu6000/MPU6000_SPI.cpp
@@ -46,7 +46,7 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
-#include "mpu6000.h"
+#include "MPU6000.hpp"
#include <string.h>
diff --git a/src/drivers/imu/mpu6000/mpu6000_main.cpp b/src/drivers/imu/mpu6000/mpu6000_main.cpp
new file mode 100644
index 0000000000..43106953e1
--- /dev/null
+++ b/src/drivers/imu/mpu6000/mpu6000_main.cpp
@@ -0,0 +1,428 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2012-2015 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 "MPU6000.hpp"
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace mpu6000
+{
+
+/*
+ list of supported bus configurations
+ */
+
+struct mpu6000_bus_option {
+ enum MPU6000_BUS busid;
+ MPU_DEVICE_TYPE device_type;
+ const char *devpath;
+ 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, MPU_DEVICE_PATH, &MPU6000_I2C_interface, PX4_I2C_BUS_ONBOARD, false, NULL },
+# endif
+# if defined(PX4_I2C_BUS_EXPANSION)
+ { MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_EXT, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION, true, NULL },
+# endif
+# if defined(PX4_I2C_BUS_EXPANSION1)
+ { MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_EXT1, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION1, true, NULL },
+# endif
+# if defined(PX4_I2C_BUS_EXPANSION2)
+ { MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_EXT2, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION2, true, NULL },
+# endif
+#endif
+#ifdef PX4_SPIDEV_MPU
+ { MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
+#endif
+#if defined(PX4_SPI_BUS_EXT)
+ { MPU6000_BUS_SPI_EXTERNAL1, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_EXT, &MPU6000_SPI_interface, PX4_SPI_BUS_EXT, true, NULL },
+#endif
+#ifdef PX4_SPIDEV_ICM_20602
+ { MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_ICM20602, ICM20602_DEVICE_PATH, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
+#endif
+#ifdef PX4_SPIDEV_ICM_20608
+ { MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_ICM20608, ICM20608_DEVICE_PATH, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
+#endif
+#ifdef PX4_SPIDEV_ICM_20689
+ { MPU6000_BUS_SPI_INTERNAL2, MPU_DEVICE_TYPE_ICM20689, ICM20689_DEVICE_PATH, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
+#endif
+#if defined(PX4_SPI_BUS_EXTERNAL)
+ { MPU6000_BUS_SPI_EXTERNAL1, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_EXT, &MPU6000_SPI_interface, PX4_SPI_BUS_EXTERNAL, true, NULL },
+ { MPU6000_BUS_SPI_EXTERNAL2, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_EXT1, &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);
+void factorytest(enum MPU6000_BUS busid);
+void usage();
+
+/**
+ * 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);
+}
+
+/**
+ * start driver for a specific bus option
+ */
+bool
+start_bus(struct mpu6000_bus_option &bus, enum Rotation rotation, int device_type)
+{
+ if (bus.dev != nullptr) {
+ warnx("%s SPI not available", bus.external ? "External" : "Internal");
+ return false;
+ }
+
+ 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;
+ }
+
+ if (interface->init() != OK) {
+ delete interface;
+ warnx("no device on bus #%u (SPI%u)", (unsigned)bus.busid, (unsigned)bus.busnum);
+ return false;
+ }
+
+ bus.dev = new MPU6000(interface, bus.devpath, rotation, device_type);
+
+ if (bus.dev == nullptr) {
+ delete interface;
+ return false;
+ }
+
+ if (OK != bus.dev->init()) {
+ goto fail;
+ }
+
+ /* 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;
+}
+
+/**
+ * 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);
+}
+
+/**
+ * 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);
+}
+
+void
+usage()
+{
+ warnx("missing command: try 'start', 'info', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'");
+ 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;
+
+ 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) {
+ 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);
+ break;
+
+ case 'R':
+ rotation = (enum Rotation)atoi(myoptarg);
+ break;
+
+ default:
+ mpu6000::usage();
+ return 0;
+ }
+ }
+
+ if (myoptind >= argc) {
+ mpu6000::usage();
+ return -1;
+ }
+
+ const char *verb = argv[myoptind];
+
+ /*
+ * Start/load the driver.
+ */
+ if (!strcmp(verb, "start")) {
+ mpu6000::start(busid, rotation, device_type);
+ }
+
+ if (!strcmp(verb, "stop")) {
+ mpu6000::stop(busid);
+ }
+
+ /*
+ * Reset the driver.
+ */
+ if (!strcmp(verb, "reset")) {
+ mpu6000::reset(busid);
+ }
+
+ /*
+ * Print driver information.
+ */
+ if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
+ mpu6000::info(busid);
+ }
+
+ /*
+ * Print register information.
+ */
+ if (!strcmp(verb, "regdump")) {
+ mpu6000::regdump(busid);
+ }
+
+ if (!strcmp(verb, "factorytest")) {
+ mpu6000::factorytest(busid);
+ }
+
+ if (!strcmp(verb, "testerror")) {
+ mpu6000::testerror(busid);
+ }
+
+ mpu6000::usage();
+ return -1;
+}
--
GitLab