dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mpu6000 split into separate main, header, implementation

This commit is contained in:
Daniel Agar 2019-01-14 16:22:42 -05:00
parent d299d439c6
commit 52c848a556
6 changed files with 729 additions and 698 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/drivers/imu/mpu6000/CMakeLists.txt
View File

@ -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

692
src/drivers/imu/mpu6000/mpu6000.cpp → src/drivers/imu/mpu6000/MPU6000.cpp
View File

@ -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;
}

296
src/drivers/imu/mpu6000/mpu6000.h → src/drivers/imu/mpu6000/MPU6000.hpp
View File

@ -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 &);
};

2
src/drivers/imu/mpu6000/mpu6000_i2c.cpp → src/drivers/imu/mpu6000/MPU6000_I2C.cpp
View File

@ -42,7 +42,7 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include "mpu6000.h"
#include "MPU6000.hpp"
#ifdef USE_I2C

2
src/drivers/imu/mpu6000/mpu6000_spi.cpp → src/drivers/imu/mpu6000/MPU6000_SPI.cpp
View File

@ -46,7 +46,7 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include "mpu6000.h"
#include "MPU6000.hpp"
#include <string.h>

428
src/drivers/imu/mpu6000/mpu6000_main.cpp Normal file
View File

@ -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;
}