aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/mpu6000/mpu6000.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/mpu6000/mpu6000.cpp')
-rw-r--r--src/drivers/mpu6000/mpu6000.cpp166
1 files changed, 115 insertions, 51 deletions
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index ac75682c4..1b3a96a0d 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -55,6 +55,7 @@
#include <stdio.h>
#include <math.h>
#include <unistd.h>
+#include <getopt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@@ -71,12 +72,15 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/conversion/rotation.h>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel"
#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro"
+#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu6000_accel_ext"
+#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu6000_gyro_ext"
// MPU 6000 registers
#define MPUREG_WHOAMI 0x75
@@ -177,7 +181,7 @@ class MPU6000_gyro;
class MPU6000 : public device::SPI
{
public:
- MPU6000(int bus, spi_dev_e device);
+ MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation);
virtual ~MPU6000();
virtual int init();
@@ -232,6 +236,8 @@ private:
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
+ enum Rotation _rotation;
+
/**
* Start automatic measurement.
*/
@@ -345,7 +351,7 @@ private:
class MPU6000_gyro : public device::CDev
{
public:
- MPU6000_gyro(MPU6000 *parent);
+ MPU6000_gyro(MPU6000 *parent, const char *path);
~MPU6000_gyro();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
@@ -368,9 +374,9 @@ private:
/** driver 'main' command */
extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
-MPU6000::MPU6000(int bus, spi_dev_e device) :
- SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
- _gyro(new MPU6000_gyro(this)),
+MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) :
+ SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
+ _gyro(new MPU6000_gyro(this, path_gyro)),
_product(0),
_call_interval(0),
_accel_reports(nullptr),
@@ -391,7 +397,8 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
- _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ)
+ _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+ _rotation(rotation)
{
// disable debug() calls
_debug_enabled = false;
@@ -544,7 +551,7 @@ void MPU6000::reset()
write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
irqrestore(state);
- up_udelay(1000);
+ usleep(1000);
// SAMPLE RATE
_set_sample_rate(_sample_rate);
@@ -666,7 +673,9 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
/*
choose next highest filter frequency available
*/
- if (frequency_hz <= 5) {
+ if (frequency_hz == 0) {
+ filter = BITS_DLPF_CFG_2100HZ_NOLPF;
+ } else if (frequency_hz <= 5) {
filter = BITS_DLPF_CFG_5HZ;
} else if (frequency_hz <= 10) {
filter = BITS_DLPF_CFG_10HZ;
@@ -922,10 +931,11 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
return _accel_filter_x.get_cutoff_freq();
case ACCELIOCSLOWPASS:
-
- // XXX decide on relationship of both filters
- // i.e. disable the on-chip filter
- //_set_dlpf_filter((uint16_t)arg);
+ if (arg == 0) {
+ // allow disabling of on-chip filter using
+ // zero as desired filter frequency
+ _set_dlpf_filter(0);
+ }
_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
@@ -1009,8 +1019,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
- // XXX check relation to the internal lowpass
- //_set_dlpf_filter((uint16_t)arg);
+ if (arg == 0) {
+ // allow disabling of on-chip filter using 0
+ // as desired frequency
+ _set_dlpf_filter(0);
+ }
return OK;
case GYROIOCSSCALE:
@@ -1295,6 +1308,9 @@ MPU6000::measure()
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
+ // apply user specified rotation
+ rotate_3f(_rotation, arb.x, arb.y, arb.z);
+
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
@@ -1313,6 +1329,9 @@ MPU6000::measure()
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
+ // apply user specified rotation
+ rotate_3f(_rotation, grb.x, grb.y, grb.z);
+
grb.scaling = _gyro_range_scale;
grb.range_rad_s = _gyro_range_rad_s;
@@ -1350,8 +1369,8 @@ MPU6000::print_info()
_gyro_reports->print_info("gyro queue");
}
-MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
- CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
+MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
+ CDev("MPU6000_gyro", path),
_parent(parent),
_gyro_topic(-1),
_gyro_class_instance(-1)
@@ -1380,7 +1399,6 @@ MPU6000_gyro::init()
_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
-out:
return ret;
}
@@ -1408,36 +1426,49 @@ MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
namespace mpu6000
{
-MPU6000 *g_dev;
+MPU6000 *g_dev_int; // on internal bus
+MPU6000 *g_dev_ext; // on external bus
-void start();
-void test();
-void reset();
-void info();
+void start(bool, enum Rotation);
+void test(bool);
+void reset(bool);
+void info(bool);
+void usage();
/**
* Start the driver.
*/
void
-start()
+start(bool external_bus, enum Rotation rotation)
{
int fd;
+ MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+ const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
+ const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO;
- if (g_dev != nullptr)
+ if (*g_dev_ptr != nullptr)
/* if already started, the still command succeeded */
errx(0, "already started");
/* create the driver */
- g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU);
+ if (external_bus) {
+#ifdef PX4_SPI_BUS_EXT
+ *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
+#else
+ errx(0, "External SPI not available");
+#endif
+ } else {
+ *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
+ }
- if (g_dev == nullptr)
+ if (*g_dev_ptr == nullptr)
goto fail;
- if (OK != g_dev->init())
+ if (OK != (*g_dev_ptr)->init())
goto fail;
/* set the poll rate to default, starts automatic data collection */
- fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
+ fd = open(path_accel, O_RDONLY);
if (fd < 0)
goto fail;
@@ -1450,9 +1481,9 @@ start()
exit(0);
fail:
- if (g_dev != nullptr) {
- delete g_dev;
- g_dev = nullptr;
+ if (*g_dev_ptr != nullptr) {
+ delete (*g_dev_ptr);
+ *g_dev_ptr = nullptr;
}
errx(1, "driver start failed");
@@ -1464,24 +1495,26 @@ fail:
* and automatic modes.
*/
void
-test()
+test(bool external_bus)
{
+ const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
+ const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO;
accel_report a_report;
gyro_report g_report;
ssize_t sz;
/* get the driver */
- int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
+ int fd = open(path_accel, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
- MPU_DEVICE_PATH_ACCEL);
+ path_accel);
/* get the driver */
- int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY);
+ int fd_gyro = open(path_gyro, O_RDONLY);
if (fd_gyro < 0)
- err(1, "%s open failed", MPU_DEVICE_PATH_GYRO);
+ err(1, "%s open failed", path_gyro);
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@@ -1529,7 +1562,7 @@ test()
/* XXX add poll-rate tests here too */
- reset();
+ reset(external_bus);
errx(0, "PASS");
}
@@ -1537,9 +1570,10 @@ test()
* Reset the driver.
*/
void
-reset()
+reset(bool external_bus)
{
- int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
+ const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
+ int fd = open(path_accel, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@@ -1559,47 +1593,77 @@ reset()
* Print a little info about the driver.
*/
void
-info()
+info(bool external_bus)
{
- if (g_dev == nullptr)
+ MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+ if (*g_dev_ptr == nullptr)
errx(1, "driver not running");
- printf("state @ %p\n", g_dev);
- g_dev->print_info();
+ printf("state @ %p\n", *g_dev_ptr);
+ (*g_dev_ptr)->print_info();
exit(0);
}
+void
+usage()
+{
+ warnx("missing command: try 'start', 'info', 'test', 'reset'");
+ warnx("options:");
+ warnx(" -X (external bus)");
+ warnx(" -R rotation");
+}
} // namespace
int
mpu6000_main(int argc, char *argv[])
{
+ bool external_bus = false;
+ int ch;
+ enum Rotation rotation = ROTATION_NONE;
+
+ /* jump over start/off/etc and look at options first */
+ while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+ switch (ch) {
+ case 'X':
+ external_bus = true;
+ break;
+ case 'R':
+ rotation = (enum Rotation)atoi(optarg);
+ break;
+ default:
+ mpu6000::usage();
+ exit(0);
+ }
+ }
+
+ const char *verb = argv[optind];
+
/*
* Start/load the driver.
*/
- if (!strcmp(argv[1], "start"))
- mpu6000::start();
+ if (!strcmp(verb, "start"))
+ mpu6000::start(external_bus, rotation);
/*
* Test the driver/device.
*/
- if (!strcmp(argv[1], "test"))
- mpu6000::test();
+ if (!strcmp(verb, "test"))
+ mpu6000::test(external_bus);
/*
* Reset the driver.
*/
- if (!strcmp(argv[1], "reset"))
- mpu6000::reset();
+ if (!strcmp(verb, "reset"))
+ mpu6000::reset(external_bus);
/*
* Print driver information.
*/
- if (!strcmp(argv[1], "info"))
- mpu6000::info();
+ if (!strcmp(verb, "info"))
+ mpu6000::info(external_bus);
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-06-18 20:41:22 +0000