fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 08:53:33 +08:00
Code Issues 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/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'");
}