diff options
author | px4dev <px4@purgatory.org> | 2012-08-25 00:12:11 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-08-25 00:12:11 -0700 |
commit | 4456ca8827c59d8711e76bd644336d5b3cd344dd (patch) | |
tree | bc1f646e83d3649459540b75a752d0a7c3b164d9 /apps/drivers/mpu6000/mpu6000.cpp | |
parent | f901a35bd4393523c48a73a805b0f5d451cec35d (diff) | |
download | px4-firmware-4456ca8827c59d8711e76bd644336d5b3cd344dd.tar.gz px4-firmware-4456ca8827c59d8711e76bd644336d5b3cd344dd.tar.bz2 px4-firmware-4456ca8827c59d8711e76bd644336d5b3cd344dd.zip |
Sensor IOCTL reorganization. Common sensor operations are now shared across sensor drivers.
Revamp hmc5883, ms5611 and mpu6000 driver startup and test code.
Diffstat (limited to 'apps/drivers/mpu6000/mpu6000.cpp')
-rw-r--r-- | apps/drivers/mpu6000/mpu6000.cpp | 288 |
1 files changed, 176 insertions, 112 deletions
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 017a4dd9e..0a1876426 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -43,6 +43,7 @@ #include <stdint.h> #include <stdbool.h> #include <stddef.h> +#include <stdlib.h> #include <semaphore.h> #include <string.h> #include <fcntl.h> @@ -53,6 +54,7 @@ #include <unistd.h> #include <systemlib/perf_counter.h> +#include <systemlib/err.h> #include <nuttx/arch.h> #include <nuttx/clock.h> @@ -536,22 +538,28 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - case ACCELIOCSPOLLRATE: { + case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ - case ACC_POLLRATE_MANUAL: + case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; /* external signalling not supported */ - case ACC_POLLRATE_EXTERNAL: + case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ case 0: return -EINVAL; + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* XXX 500Hz is just a wild guess */ + return ioctl(filp, SENSORIOCSPOLLRATE, 500); + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ @@ -566,7 +574,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval; + _call.period = _call_interval = ticks; /* if we need to start the poll state machine, do it */ if (want_start) @@ -577,20 +585,38 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) } } - case ACCELIOCSQUEUEDEPTH: + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) + return SENSOR_POLLRATE_MANUAL; + return 1000000 / _call_interval; + + case SENSORIOCSQUEUEDEPTH: + /* XXX not implemented */ + return -EINVAL; + + case SENSORIOCGQUEUEDEPTH: + /* XXX not implemented */ + return -EINVAL; + + + case ACCELIOCSSAMPLERATE: + case ACCELIOCGSAMPLERATE: /* XXX not implemented */ return -EINVAL; case ACCELIOCSLOWPASS: + case ACCELIOCGLOWPASS: /* XXX not implemented */ return -EINVAL; - case ACCELIORANGE: - /* XXX not implemented really */ - return set_range(arg); + case ACCELIOCSSCALE: + case ACCELIOCGSCALE: + /* XXX not implemented */ + return -EINVAL; - case ACCELIOCSSAMPLERATE: /* sensor sample rate is not (really) adjustable */ - case ACCELIOCSREPORTFORMAT: /* no alternate report formats */ + case ACCELIOCSRANGE: + case ACCELIOCGRANGE: + /* XXX not implemented */ return -EINVAL; default: @@ -604,24 +630,32 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - case GYROIOCSPOLLRATE: - /* gyro and accel poll rates are shared */ - return ioctl(filp, ACCELIOCSPOLLRATE, arg); + /* these are shared with the accel side */ + case SENSORIOCSPOLLRATE: + case SENSORIOCGPOLLRATE: + case SENSORIOCSQUEUEDEPTH: + case SENSORIOCGQUEUEDEPTH: + case SENSORIOCRESET: + return ioctl(filp, cmd, arg); - case GYROIOCSQUEUEDEPTH: + case GYROIOCSSAMPLERATE: + case GYROIOCGSAMPLERATE: /* XXX not implemented */ return -EINVAL; case GYROIOCSLOWPASS: + case GYROIOCGLOWPASS: /* XXX not implemented */ return -EINVAL; case GYROIOCSSCALE: - /* XXX not implemented really */ - return set_range(arg); + case GYROIOCGSCALE: + /* XXX not implemented */ + return -EINVAL; - case GYROIOCSSAMPLERATE: /* sensor sample rate is not (really) adjustable */ - case GYROIOCSREPORTFORMAT: /* no alternate report formats */ + case GYROIOCSRANGE: + case GYROIOCGRANGE: + /* XXX not implemented */ return -EINVAL; default: @@ -847,92 +881,141 @@ MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) /** * Local functions in support of the shell command. */ -namespace +namespace mpu6000 { MPU6000 *g_dev; -/* - * XXX this should just be part of the generic sensors test... - */ +void start(); +void test(); +void reset(); +void info(); -int -test() +/** + * Start the driver. + */ +void +start() { - int fd = -1; - int fd_gyro = -1; - struct accel_report report; - struct gyro_report g_report; - ssize_t sz; - const char *reason = "test OK"; - - do { - /* get the driver */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd; - if (fd < 0) { - reason = "can't open accel driver, use <mpu6000 start> first"; - break; - } + if (g_dev != nullptr) + errx(1, "already started"); - /* get the driver */ - fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + /* create the driver */ + g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU); - if (fd_gyro < 0) { - reason = "can't open gyro driver, use <mpu6000 start> first"; - break; - } + if (g_dev == nullptr) + goto fail; - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); + if (OK != g_dev->init()) + goto fail; - if (sz != sizeof(report)) { - reason = "immediate acc read failed"; - break; - } - - printf("single read\n"); - fflush(stdout); - printf("time: %lld\n", report.timestamp); - printf("acc x: \t% 5.2f\tm/s^2\n", (double)report.x); - printf("acc y: \t% 5.2f\tm/s^2\n", (double)report.y); - printf("acc z: \t% 5.2f\tm/s^2\n", (double)report.z); - printf("acc range: %6.2f m/s^2 (%6.2f g)\n", (double)report.range_m_s2, - (double)(report.range_m_s2 / 9.81f)); - - /* do a simple demand read */ - sz = read(fd_gyro, &g_report, sizeof(g_report)); - - if (sz != sizeof(g_report)) { - reason = "immediate gyro read failed"; - break; - } + /* set the poll rate to default, starts automatic data collection */ + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) + goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + exit(0); - printf("gyro x: \t% 5.2f\trad/s\n", (double)g_report.x); - printf("gyro y: \t% 5.2f\trad/s\n", (double)g_report.y); - printf("gyro z: \t% 5.2f\trad/s\n", (double)g_report.z); - printf("gyro range: %6.3f rad/s (%8.1f deg/s)\n", (double)g_report.range_rad_s, - (double)((g_report.range_rad_s / M_PI_F) * 180.0f)); +fail: + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + errx(1, "driver start failed"); +} - } while (0); +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + int fd = -1; + int fd_gyro = -1; + struct accel_report report; + struct gyro_report g_report; + ssize_t sz; - printf("MPU6000: %s\n", reason); + /* get the driver */ + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) + err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", + ACCEL_DEVICE_PATH); + + /* get the driver */ + fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + if (fd_gyro < 0) + err(1, "%s open failed", GYRO_DEVICE_PATH); + + /* reset to manual polling */ + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + err(1, "reset to manual polling"); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + if (sz != sizeof(report)) + err(1, "immediate acc read failed"); + + warnx("single read"); + warnx("time: %lld", report.timestamp); + warnx("acc x: \t% 5.2f\tm/s^2", (double)report.x); + warnx("acc y: \t% 5.2f\tm/s^2", (double)report.y); + warnx("acc z: \t% 5.2f\tm/s^2", (double)report.z); + warnx("acc range: %6.2f m/s^2 (%6.2f g)", (double)report.range_m_s2, + (double)(report.range_m_s2 / 9.81f)); + + /* do a simple demand read */ + sz = read(fd_gyro, &g_report, sizeof(g_report)); + if (sz != sizeof(g_report)) + err(1, "immediate gyro read failed"); + + warnx("gyro x: \t% 5.2f\trad/s", (double)g_report.x); + warnx("gyro y: \t% 5.2f\trad/s", (double)g_report.y); + warnx("gyro z: \t% 5.2f\trad/s", (double)g_report.z); + warnx("gyro range: %6.3f rad/s (%8.1f deg/s)", (double)g_report.range_rad_s, + (double)((g_report.range_rad_s / M_PI_F) * 180.0f)); + + /* XXX add poll-rate tests here too */ + + reset(); + errx(0, "PASS"); +} - return OK; +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + if (fd < 0) + err(1, "failed "); + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); } -int +/** + * Print a little info about the driver. + */ +void info() { - if (g_dev == nullptr) { - fprintf(stderr, "MPU6000: driver not running\n"); - return -ENOENT; - } + if (g_dev == nullptr) + errx(1, "driver not running"); printf("state @ %p\n", g_dev); g_dev->print_info(); - return OK; + exit(0); } @@ -943,47 +1026,28 @@ mpu6000_main(int argc, char *argv[]) { /* * Start/load the driver. - * - * XXX it would be nice to have a wrapper for this... - */ - if (!strcmp(argv[1], "start")) { - - if (g_dev != nullptr) { - fprintf(stderr, "MPU6000: already loaded\n"); - return -EBUSY; - } - - /* create the driver */ - g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU); - - if (g_dev == nullptr) { - fprintf(stderr, "MPU6000: driver alloc failed\n"); - return -ENOMEM; - } - if (OK != g_dev->init()) { - fprintf(stderr, "MPU6000: driver init failed\n"); - usleep(100000); - delete g_dev; - g_dev = nullptr; - return -EIO; - } - - return OK; - } + */ + if (!strcmp(argv[1], "start")) + mpu6000::start(); /* * Test the driver/device. */ if (!strcmp(argv[1], "test")) - return test(); + mpu6000::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + mpu6000::reset(); /* * Print driver information. */ if (!strcmp(argv[1], "info")) - return info(); + mpu6000::info(); - fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n"); - return -EINVAL; + errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); } |