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/ms5611 | |
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/ms5611')
-rw-r--r-- | apps/drivers/ms5611/ms5611.cpp | 236 |
1 files changed, 130 insertions, 106 deletions
diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 8f8f96217..c85e5b3a6 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -43,6 +43,7 @@ #include <sys/types.h> #include <stdint.h> #include <stdbool.h> +#include <stdlib.h> #include <semaphore.h> #include <string.h> #include <fcntl.h> @@ -59,6 +60,7 @@ #include <arch/board/up_hrt.h> #include <systemlib/perf_counter.h> +#include <systemlib/err.h> #include <drivers/drv_baro.h> @@ -425,22 +427,38 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - case BAROIOCSPOLLRATE: { + case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ - case BARO_POLLRATE_MANUAL: + case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; /* external signalling not supported */ - case BARO_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: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ @@ -465,7 +483,15 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } } - case BAROIOCSQUEUEDEPTH: { + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 2) || (arg > 100)) return -EINVAL; @@ -486,7 +512,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; } - case BAROIOCSREPORTFORMAT: + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ return -EINVAL; default: @@ -773,7 +803,7 @@ MS5611::print_info() /** * Local functions in support of the shell command. */ -namespace +namespace ms5611 { /* oddly, ERROR is not defined for c++ */ @@ -784,74 +814,82 @@ const int ERROR = -1; MS5611 *g_dev; -/* - * XXX this should just be part of the generic sensors test... +void start(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. */ +void +start() +{ + int fd; + if (g_dev != nullptr) + errx(1, "already started"); -int -test_fail(const char *fmt, ...) -{ - va_list ap; - - fprintf(stderr, "FAIL: "); - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - return ERROR; -} + /* create the driver */ + /* XXX HORRIBLE hack - the bus number should not come from here */ + g_dev = new MS5611(2); -int -test_note(const char *fmt, ...) -{ - va_list ap; - - fprintf(stderr, "note: "); - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - return OK; + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) + goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + exit(0); + +fail: + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + errx(1, "driver start failed"); } /** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. - * - * @param fd An open file descriptor on the driver. */ -int -test(int fd) +void +test() { struct baro_report report; ssize_t sz; int ret; + int fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) + err(1, "%s open failed (try 'ms5611 start' if the driver is not running", BARO_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) - return test_fail("immediate read failed: %d", errno); + err(1, "immediate read failed"); - test_note("single read"); - test_note("pressure: %u", (unsigned)report.pressure); - test_note("altitude: %u", (unsigned)report.altitude); - test_note("temperature: %u", (unsigned)report.temperature); - test_note("time: %lld", report.timestamp); - usleep(1000000); + warnx("single read"); + warnx("pressure: %u", (unsigned)report.pressure); + warnx("altitude: %u", (unsigned)report.altitude); + warnx("temperature: %u", (unsigned)report.temperature); + warnx("time: %lld", report.timestamp); /* set the queue depth to 10 */ - if (OK != ioctl(fd, BAROIOCSQUEUEDEPTH, 10)) - return test_fail("failed to set queue depth"); + if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) + errx(1, "failed to set queue depth"); /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, BAROIOCSPOLLRATE, 2)) - return test_fail("failed to set 2Hz poll rate"); + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -863,37 +901,54 @@ test(int fd) ret = poll(&fds, 1, 2000); if (ret != 1) - return test_fail("timed out waiting for sensor data"); + errx(1, "timed out waiting for sensor data"); /* now go get it */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) - return test_fail("periodic read failed: %d", errno); + err(1, "periodic read failed"); - test_note("periodic read %u", i); - test_note("pressure: %u", (unsigned)report.pressure); - test_note("altitude: %u", (unsigned)report.altitude); - test_note("temperature: %u", (unsigned)report.temperature); - test_note("time: %lld", report.timestamp); + warnx("periodic read %u", i); + warnx("pressure: %u", (unsigned)report.pressure); + warnx("altitude: %u", (unsigned)report.altitude); + warnx("temperature: %u", (unsigned)report.temperature); + warnx("time: %lld", report.timestamp); } - return test_note("PASS"); - return OK; + errx(0, "PASS"); } -int +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(BARO_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); +} + +/** + * Print a little info about the driver. + */ +void info() { - if (g_dev == nullptr) { - fprintf(stderr, "MS5611: 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); } @@ -904,58 +959,27 @@ ms5611_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, "MS5611: already loaded\n"); - return -EBUSY; - } - - /* create the driver */ - /* XXX HORRIBLE hack - the bus number should not come from here */ - g_dev = new MS5611(2); - - if (g_dev == nullptr) { - fprintf(stderr, "MS5611: driver alloc failed\n"); - return -ENOMEM; - } - - if (OK != g_dev->init()) { - fprintf(stderr, "MS5611: driver init failed\n"); - usleep(100000); - delete g_dev; - g_dev = nullptr; - return -EIO; - } - - return OK; - } + if (!strcmp(argv[1], "start")) + ms5611::start(); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) { - int fd, ret; - - fd = open(BARO_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - return test_fail("driver open failed: %d", errno); + if (!strcmp(argv[1], "test")) + ms5611::test(); - ret = test(fd); - close(fd); - return ret; - } + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + ms5611::reset(); /* * Print driver information. */ if (!strcmp(argv[1], "info")) - return info(); + ms5611::info(); - fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n"); - return -EINVAL; + errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); } |