Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

Sensor IOCTL reorganization. Common sensor operations are now shared across sensor drivers.

Browse Source 
Revamp hmc5883, ms5611 and mpu6000 driver startup and test code.
This commit is contained in:
px4dev
2012-08-25 00:12:11 -07:00
parent f901a35bd4
commit 4456ca8827
11 changed files with 572 additions and 387 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/drv_accel.h
+19 -14
View File
@@ -41,6 +41,7 @@
#include <stdint.h>
#include <sys/ioctl.h>
#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 */
apps/drivers/drv_baro.h
+3 -12
View File
@@ -41,6 +41,7 @@
#include <stdint.h>
#include <sys/ioctl.h>
#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 */
apps/drivers/drv_gyro.h
+19 -15
View File
@@ -41,6 +41,7 @@
#include <stdint.h>
#include <sys/ioctl.h>
#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 */
apps/drivers/drv_mag.h
+7 -18
View File
@@ -41,6 +41,7 @@
#include <stdint.h>
#include <sys/ioctl.h>
#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 */
apps/drivers/drv_mixer.h
+1 -1
View File
@@ -61,7 +61,7 @@
/*
* ioctl() definitions
*/
#define _MIXERIOCBASE (0x2400)
#define _MIXERIOCBASE (0x2500)
#define _MIXERIOC(_n) (_IOC(_MIXERIOCBASE, _n))
/** get the number of mixable outputs */
apps/drivers/drv_orb_dev.h
+1 -1
View File
@@ -58,7 +58,7 @@
/** maximum ogbject name length */
#define ORB_MAXNAME 32
#define _ORBIOCBASE (0x2500)
#define _ORBIOCBASE (0x2600)
#define _ORBIOC(_n) (_IOC(_ORBIOCBASE, _n))
/*
apps/drivers/drv_pwm_output.h
+1 -1
View File
@@ -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)
apps/drivers/drv_sensor.h
+87
View File
@@ -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 <stdint.h>
#include <sys/ioctl.h>
/*
* 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 */
apps/drivers/hmc5883/hmc5883.cpp
+134 -113
View File
@@ -61,6 +61,7 @@
#include <arch/board/up_hrt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_mag.h>
@@ -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.
*/
int
test_fail(const char *fmt, ...)
void
start()
{
va_list ap;
int fd;
fprintf(stderr, "FAIL: ");
va_start(ap, fmt);
vfprintf(stderr, fmt, ap);
va_end(ap);
fprintf(stderr, "\n");
fflush(stderr);
return ERROR;
}
if (g_dev != nullptr)
errx(1, "already started");
int
test_note(const char *fmt, ...)
{
va_list ap;
/* create the driver */
/* XXX HORRIBLE hack - the bus number should not come from here */
g_dev = new HMC5883(2);
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'");
}
apps/drivers/mpu6000/mpu6000.cpp
+171 -107
View File
@@ -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,16 +881,58 @@ 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
/**
* Start the driver.
*/
void
start()
{
int fd;
if (g_dev != nullptr)
errx(1, "already started");
/* create the driver */
g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU);
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(ACCEL_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.
*/
void
test()
{
int fd = -1;
@@ -864,75 +940,82 @@ test()
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);
/* 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);
if (fd < 0) {
reason = "can't open accel driver, use <mpu6000 start> first";
break;
}
/* get the driver */
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
if (fd_gyro < 0)
err(1, "%s open failed", GYRO_DEVICE_PATH);
/* get the driver */
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
/* 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");
if (fd_gyro < 0) {
reason = "can't open gyro driver, use <mpu6000 start> first";
break;
}
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, &report, sizeof(report));
/* 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");
if (sz != sizeof(report)) {
reason = "immediate acc read failed";
break;
}
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));
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));
/* XXX add poll-rate tests here too */
/* 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;
}
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));
} while (0);
printf("MPU6000: %s\n", reason);
return OK;
reset();
errx(0, "PASS");
}
int
/**
* 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);
}
/**
* 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'");
}
apps/drivers/ms5611/ms5611.cpp
+129 -105
View File
@@ -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.
*/
int
test_fail(const char *fmt, ...)
void
start()
{
va_list ap;
int fd;
fprintf(stderr, "FAIL: ");
va_start(ap, fmt);
vfprintf(stderr, fmt, ap);
va_end(ap);
fprintf(stderr, "\n");
fflush(stderr);
return ERROR;
}
if (g_dev != nullptr)
errx(1, "already started");
int
test_note(const char *fmt, ...)
{
va_list ap;
/* create the driver */
/* XXX HORRIBLE hack - the bus number should not come from here */
g_dev = new MS5611(2);
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;
if (!strcmp(argv[1], "test"))
ms5611::test();
fd = open(BARO_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"))
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'");
}