From 4456ca8827c59d8711e76bd644336d5b3cd344dd Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 25 Aug 2012 00:12:11 -0700 Subject: Sensor IOCTL reorganization. Common sensor operations are now shared across sensor drivers. Revamp hmc5883, ms5611 and mpu6000 driver startup and test code. --- apps/drivers/drv_accel.h | 33 +++-- apps/drivers/drv_baro.h | 15 +- apps/drivers/drv_gyro.h | 34 +++-- apps/drivers/drv_mag.h | 25 +--- apps/drivers/drv_mixer.h | 2 +- apps/drivers/drv_orb_dev.h | 2 +- apps/drivers/drv_pwm_output.h | 2 +- apps/drivers/drv_sensor.h | 87 ++++++++++++ apps/drivers/hmc5883/hmc5883.cpp | 249 +++++++++++++++++---------------- apps/drivers/mpu6000/mpu6000.cpp | 288 ++++++++++++++++++++++++--------------- apps/drivers/ms5611/ms5611.cpp | 236 ++++++++++++++++++-------------- 11 files changed, 579 insertions(+), 394 deletions(-) create mode 100644 apps/drivers/drv_sensor.h diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h index 65ef8420f..47d366bc4 100644 --- a/apps/drivers/drv_accel.h +++ b/apps/drivers/drv_accel.h @@ -41,6 +41,7 @@ #include #include +#include "drv_sensor.h" #include "drv_orb_dev.h" #define ACCEL_DEVICE_PATH "/dev/accel" @@ -78,33 +79,37 @@ ORB_DECLARE(sensor_accel); /* * ioctl() definitions + * + * Accelerometer drivers also implement the generic sensor driver + * interfaces from drv_sensor.h */ -#define _ACCELIOCBASE (0x2000) +#define _ACCELIOCBASE (0x2100) #define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n)) -/** set the driver polling rate to (arg) Hz, or one of the ACC_POLLRATE constants */ -#define ACCELIOCSPOLLRATE _ACCELIOC(0) - -#define ACC_POLLRATE_MANUAL 1000000 /**< poll when read */ -#define ACC_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */ - -/** set the internal queue depth to (arg) entries, must be at least 1 */ -#define ACCELIOCSQUEUEDEPTH _ACCELIOC(1) /** set the accel internal sample rate to at least (arg) Hz */ -#define ACCELIOCSSAMPLERATE _ACCELIOC(2) +#define ACCELIOCSSAMPLERATE _ACCELIOC(0) + +/** return the accel internal sample rate in Hz */ +#define ACCELIOCGSAMPLERATE _ACCELIOC(1) /** set the accel internal lowpass filter to no lower than (arg) Hz */ -#define ACCELIOCSLOWPASS _ACCELIOC(3) +#define ACCELIOCSLOWPASS _ACCELIOC(2) -/** set the report format to (arg); zero is the standard, 1-10 are reserved, all others are driver-specific. */ -#define ACCELIOCSREPORTFORMAT _ACCELIOC(4) +/** return the accel internal lowpass filter in Hz */ +#define ACCELIOCGLOWPASS _ACCELIOC(3) /** set the accel scaling constants to the structure pointed to by (arg) */ #define ACCELIOCSSCALE _ACCELIOC(5) +/** get the accel scaling constants into the structure pointed to by (arg) */ +#define ACCELIOCGSCALE _ACCELIOC(6) + /** set the accel measurement range to handle at least (arg) g */ -#define ACCELIORANGE _ACCELIOC(6) +#define ACCELIOCSRANGE _ACCELIOC(7) + +/** get the current accel measurement range */ +#define ACCELIOCGRANGE _ACCELIOC(8) #endif /* _DRV_ACCEL_H */ diff --git a/apps/drivers/drv_baro.h b/apps/drivers/drv_baro.h index 323b25c83..b79d5b5b0 100644 --- a/apps/drivers/drv_baro.h +++ b/apps/drivers/drv_baro.h @@ -41,6 +41,7 @@ #include #include +#include "drv_sensor.h" #include "drv_orb_dev.h" #define BARO_DEVICE_PATH "/dev/baro" @@ -65,19 +66,9 @@ ORB_DECLARE(sensor_baro); * ioctl() definitions */ -#define _BAROIOCBASE (0x2100) +#define _BAROIOCBASE (0x2200) #define _BAROIOC(_n) (_IOC(_BAROIOCBASE, _n)) -/** set the driver polling rate to (arg) Hz, or one of the BARO_POLLRATE constants */ -#define BAROIOCSPOLLRATE _BAROIOC(0) - -#define BARO_POLLRATE_MANUAL 1000000 /**< poll when read */ -#define BARO_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */ - -/** set the internal queue depth to (arg) entries, must be at least 1 */ -#define BAROIOCSQUEUEDEPTH _BAROIOC(1) - -/** set the report format to (arg); zero is the standard, 1-10 are reserved, all others are driver-specific. */ -#define BAROIOCSREPORTFORMAT _BAROIOC(2) +/* currently no baro-specific ioctls */ #endif /* _DRV_BARO_H */ diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h index 2f1dab6ba..48c63d5f4 100644 --- a/apps/drivers/drv_gyro.h +++ b/apps/drivers/drv_gyro.h @@ -41,6 +41,7 @@ #include #include +#include "drv_sensor.h" #include "drv_orb_dev.h" #define GYRO_DEVICE_PATH "/dev/gyro" @@ -81,28 +82,31 @@ ORB_DECLARE(sensor_gyro); * ioctl() definitions */ -#define _GYROIOCBASE (0x2200) +#define _GYROIOCBASE (0x2300) #define _GYROIOC(_n) (_IOC(_GYROIOCBASE, _n)) -/** set the driver polling rate to (arg) Hz, or one of the GYRO_POLLRATE constants */ -#define GYROIOCSPOLLRATE _GYROIOC(0) - -#define GYRO_POLLRATE_MANUAL 1000000 /**< poll when read */ -#define GYRO_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */ - -/** set the internal queue depth to (arg) entries, must be at least 1 */ -#define GYROIOCSQUEUEDEPTH _GYROIOC(1) - /** set the gyro internal sample rate to at least (arg) Hz */ -#define GYROIOCSSAMPLERATE _GYROIOC(2) +#define GYROIOCSSAMPLERATE _GYROIOC(0) + +/** return the gyro internal sample rate in Hz */ +#define GYROIOCGSAMPLERATE _GYROIOC(1) /** set the gyro internal lowpass filter to no lower than (arg) Hz */ -#define GYROIOCSLOWPASS _GYROIOC(3) +#define GYROIOCSLOWPASS _GYROIOC(2) -/** set the report format to (arg); zero is the standard, 1-10 are reserved, all others are driver-specific. */ -#define GYROIOCSREPORTFORMAT _GYROIOC(4) +/** set the gyro internal lowpass filter to no lower than (arg) Hz */ +#define GYROIOCGLOWPASS _GYROIOC(3) /** set the gyro scaling constants to (arg) */ -#define GYROIOCSSCALE _GYROIOC(5) +#define GYROIOCSSCALE _GYROIOC(4) + +/** get the gyro scaling constants into (arg) */ +#define GYROIOCGSCALE _GYROIOC(5) + +/** set the gyro measurement range to handle at least (arg) g */ +#define GYROIOCSRANGE _GYROIOC(6) + +/** get the current gyro measurement range */ +#define GYROIOCGRANGE _GYROIOC(7) #endif /* _DRV_GYRO_H */ diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index f87cb7704..895d2d797 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -41,6 +41,7 @@ #include #include +#include "drv_sensor.h" #include "drv_orb_dev.h" #define MAG_DEVICE_PATH "/dev/mag" @@ -83,34 +84,22 @@ ORB_DECLARE(sensor_mag); * ioctl() definitions */ -#define _MAGIOCBASE (0x2300) +#define _MAGIOCBASE (0x2400) #define _MAGIOC(_n) (_IOC(_MAGIOCBASE, _n)) -/** set the driver polling rate to (arg) Hz, or one of the MAG_POLLRATE constants */ -#define MAGIOCSPOLLRATE _MAGIOC(0) - -#define MAG_POLLRATE_MANUAL 1000000 /**< poll when read */ -#define MAG_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */ - -/** set the internal queue depth to (arg) entries, must be at least 1 */ -#define MAGIOCSQUEUEDEPTH _MAGIOC(1) - /** set the mag internal sample rate to at least (arg) Hz */ -#define MAGIOCSSAMPLERATE _MAGIOC(2) +#define MAGIOCSSAMPLERATE _MAGIOC(0) /** set the mag internal lowpass filter to no lower than (arg) Hz */ -#define MAGIOCSLOWPASS _MAGIOC(3) - -/** set the report format to (arg); zero is the standard, 1-10 are reserved, all others are driver-specific. */ -#define MAGIOCSREPORTFORMAT _MAGIOC(4) +#define MAGIOCSLOWPASS _MAGIOC(1) /** set the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCSSCALE _MAGIOC(5) +#define MAGIOCSSCALE _MAGIOC(2) /** copy the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCGSCALE _MAGIOC(6) +#define MAGIOCGSCALE _MAGIOC(3) /** perform self-calibration, update scale factors to canonical units */ -#define MAGIOCCALIBRATE _MAGIOC(7) +#define MAGIOCCALIBRATE _MAGIOC(4) #endif /* _DRV_MAG_H */ diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h index fd54ed7fa..793e86b32 100644 --- a/apps/drivers/drv_mixer.h +++ b/apps/drivers/drv_mixer.h @@ -61,7 +61,7 @@ /* * ioctl() definitions */ -#define _MIXERIOCBASE (0x2400) +#define _MIXERIOCBASE (0x2500) #define _MIXERIOC(_n) (_IOC(_MIXERIOCBASE, _n)) /** get the number of mixable outputs */ diff --git a/apps/drivers/drv_orb_dev.h b/apps/drivers/drv_orb_dev.h index 50288f690..8495780d5 100644 --- a/apps/drivers/drv_orb_dev.h +++ b/apps/drivers/drv_orb_dev.h @@ -58,7 +58,7 @@ /** maximum ogbject name length */ #define ORB_MAXNAME 32 -#define _ORBIOCBASE (0x2500) +#define _ORBIOCBASE (0x2600) #define _ORBIOC(_n) (_IOC(_ORBIOCBASE, _n)) /* diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 73e3310ae..340175a4b 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -94,7 +94,7 @@ ORB_DECLARE(output_pwm); * Note that ioctls and ObjDev updates should not be mixed, as the * behaviour of the system in this case is not defined. */ -#define _PWM_SERVO_BASE 0x2600 +#define _PWM_SERVO_BASE 0x2700 /** arm all servo outputs handle by this driver */ #define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0) diff --git a/apps/drivers/drv_sensor.h b/apps/drivers/drv_sensor.h new file mode 100644 index 000000000..325bd42bf --- /dev/null +++ b/apps/drivers/drv_sensor.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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. + * + ****************************************************************************/ + +/** + * @file Common sensor API and ioctl definitions. + */ + +#ifndef _DRV_SENSOR_H +#define _DRV_SENSOR_H + +#include +#include + +/* + * ioctl() definitions + * + * Note that a driver may not implement all of these operations, but + * if the operation is implemented it should conform to this API. + */ + +#define _SENSORIOCBASE (0x2000) +#define _SENSORIOC(_n) (_IOC(_SENSORIOCBASE, _n)) + +/** + * Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE + * constants + */ +#define SENSORIOCSPOLLRATE _SENSORIOC(0) + +/** + * Return the driver's approximate polling rate in Hz, or one of the + * SENSOR_POLLRATE values. + */ +#define SENSORIOCGPOLLRATE _SENSORIOC(1) + +#define SENSOR_POLLRATE_MANUAL 1000000 /**< poll when read */ +#define SENSOR_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */ +#define SENSOR_POLLRATE_MAX 1000002 /**< poll at device maximum rate */ +#define SENSOR_POLLRATE_DEFAULT 1000003 /**< poll at driver normal rate */ + +/** + * Set the internal queue depth to (arg) entries, must be at least 1 + * + * This sets the upper bound on the number of readings that can be + * read from the driver. + */ +#define SENSORIOCSQUEUEDEPTH _SENSORIOC(2) + +/** return the internal queue depth */ +#define SENSORIOCGQUEUEDEPTH _SENSORIOC(3) + +/** + * Reset the sensor to its default configuration. + */ +#define SENSORIOCRESET _SENSORIOC(4) + +#endif /* _DRV_SENSOR_H */ \ No newline at end of file diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 0fa1f365d..a9212e7ab 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -61,6 +61,7 @@ #include #include +#include #include @@ -422,22 +423,38 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - case MAGIOCSPOLLRATE: { + case SENSORIOCSPOLLRATE: { switch (arg) { /* switching to manual polling */ - case MAG_POLLRATE_MANUAL: + case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; /* external signalling (DRDY) not supported */ - case MAG_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(HMC5883_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? */ @@ -462,7 +479,15 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) } } - case MAGIOCSQUEUEDEPTH: { + 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; @@ -483,6 +508,21 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; } + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case MAGIOCSSAMPLERATE: + /* not supported, always 1 sample per poll */ + return -EINVAL; + + case MAGIOCSLOWPASS: + /* not supported, no internal filtering */ + return -EINVAL; + case MAGIOCSSCALE: /* set new scale factors */ memcpy(&_scale, (mag_scale *)arg, sizeof(_scale)); @@ -497,18 +537,6 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX perform auto-calibration */ return -EINVAL; - case MAGIOCSSAMPLERATE: - /* not supported, always 1 sample per poll */ - return -EINVAL; - - case MAGIOCSLOWPASS: - /* not supported, no internal filtering */ - return -EINVAL; - - case MAGIOCSREPORTFORMAT: - /* not supported, no custom report format */ - return -EINVAL; - default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -747,7 +775,7 @@ HMC5883::print_info() /** * Local functions in support of the shell command. */ -namespace +namespace hmc5883 { /* oddly, ERROR is not defined for c++ */ @@ -758,72 +786,80 @@ const int ERROR = -1; HMC5883 *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 HMC5883(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(MAG_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 mag_report report; ssize_t sz; int ret; + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + if (fd < 0) + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_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("measurement: %.6f %.6f %.6f", report.x, report.y, report.z); - test_note("time: %lld", report.timestamp); - usleep(1000000); + warnx("single read"); + warnx("measurement: %.6f %.6f %.6f", report.x, report.y, report.z); + warnx("time: %lld", report.timestamp); /* set the queue depth to 10 */ - if (OK != ioctl(fd, MAGIOCSQUEUEDEPTH, 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, MAGIOCSPOLLRATE, 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++) { @@ -835,38 +871,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("measurement: %.6f %.6f %.6f", report.x, report.y, report.z); - test_note("time: %lld", report.timestamp); + warnx("periodic read %u", i); + warnx("measurement: %.6f %.6f %.6f", report.x, report.y, report.z); + warnx("time: %lld", report.timestamp); } - return test_note("PASS"); - return OK; + errx(0, "PASS"); } -int +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(MAG_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, "HMC5883: 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); } - } // namespace int @@ -874,58 +926,27 @@ hmc5883_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, "HMC5883: already loaded\n"); - return -EBUSY; - } - - /* create the driver */ - /* XXX HORRIBLE hack - the bus number should not come from here */ - g_dev = new HMC5883(2); - - if (g_dev == nullptr) { - fprintf(stderr, "HMC5883: driver alloc failed\n"); - return -ENOMEM; - } - - if (OK != g_dev->init()) { - fprintf(stderr, "HMC5883: driver init failed\n"); - usleep(100000); - delete g_dev; - g_dev = nullptr; - return -EIO; - } - - return OK; - } + if (!strcmp(argv[1], "start")) + hmc5883::start(); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) { - int fd, ret; + if (!strcmp(argv[1], "test")) + hmc5883::test(); - fd = open(MAG_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - return test_fail("driver open failed: %d", errno); - - ret = test(fd); - close(fd); - return ret; - } + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + hmc5883::reset(); /* * Print driver information. */ if (!strcmp(argv[1], "info")) - return info(); + hmc5883::info(); - fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n"); - return -EINVAL; + errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); } 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 #include #include +#include #include #include #include @@ -53,6 +54,7 @@ #include #include +#include #include #include @@ -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 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 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'"); } 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 #include #include +#include #include #include #include @@ -59,6 +60,7 @@ #include #include +#include #include @@ -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'"); } -- cgit v1.2.3