Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

ets_airspeed posix port

Browse Source
This commit is contained in:
Daniel Agar
2017-08-24 21:36:58 -04:00
committed by Lorenz Meier
parent 15407afc6e
commit fd8a5644e8
3 changed files with 36 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cmake/configs/posix_rpi_common.cmake
+1
View File
@@ -18,6 +18,7 @@ set(config_module_list
# #
drivers/device drivers/device
drivers/airspeed drivers/airspeed
drivers/ets_airspeed
drivers/ms4525_airspeed drivers/ms4525_airspeed
drivers/ms5525_airspeed drivers/ms5525_airspeed
drivers/sdp3x_airspeed drivers/sdp3x_airspeed
cmake/configs/posix_sitl_default.cmake
+1
View File
@@ -11,6 +11,7 @@ set(config_module_list
drivers/vmount drivers/vmount
drivers/linux_gpio drivers/linux_gpio
drivers/airspeed drivers/airspeed
drivers/ets_airspeed
drivers/ms4525_airspeed drivers/ms4525_airspeed
drivers/ms5525_airspeed drivers/ms5525_airspeed
drivers/sdp3x_airspeed drivers/sdp3x_airspeed
src/drivers/ets_airspeed/ets_airspeed.cpp
+34 -59
View File
@@ -42,25 +42,6 @@
#include <drivers/device/i2c.h> #include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <board_config.h>
#include <systemlib/airspeed.h> #include <systemlib/airspeed.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
@@ -280,7 +261,7 @@ start(int i2c_bus)
int fd; int fd;
if (g_dev != nullptr) { if (g_dev != nullptr) {
errx(1, "already started"); PX4_ERR("already started");
} }
/* create the driver */ /* create the driver */
@@ -295,17 +276,17 @@ start(int i2c_bus)
} }
/* set the poll rate to default, starts automatic data collection */ /* set the poll rate to default, starts automatic data collection */
fd = open(AIRSPEED0_DEVICE_PATH, O_RDONLY); fd = px4_open(AIRSPEED0_DEVICE_PATH, O_RDONLY);
if (fd < 0) { if (fd < 0) {
goto fail; goto fail;
} }
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail; goto fail;
} }
exit(0); return;
fail: fail:
@@ -315,7 +296,6 @@ fail:
} }
PX4_WARN("no ETS airspeed sensor connected"); PX4_WARN("no ETS airspeed sensor connected");
exit(1);
} }
/** /**
@@ -329,10 +309,8 @@ stop()
g_dev = nullptr; g_dev = nullptr;
} else { } else {
errx(1, "driver not running"); PX4_ERR("driver not running");
} }
exit(0);
} }
/** /**
@@ -347,25 +325,25 @@ test()
ssize_t sz; ssize_t sz;
int ret; int ret;
int fd = open(ETS_PATH, O_RDONLY); int fd = px4_open(ETS_PATH, O_RDONLY);
if (fd < 0) { if (fd < 0) {
err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", ETS_PATH); PX4_ERR("%s open failed (try 'ets_airspeed start' if the driver is not running", ETS_PATH);
} }
/* do a simple demand read */ /* do a simple demand read */
sz = read(fd, &report, sizeof(report)); sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) { if (sz != sizeof(report)) {
err(1, "immediate read failed"); PX4_ERR("immediate read failed");
} }
warnx("single read"); PX4_INFO("single read");
warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); PX4_INFO("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
/* start the sensor polling at 2Hz */ /* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate"); PX4_ERR("failed to set 2Hz poll rate");
} }
/* read the sensor 5x and report each value */ /* read the sensor 5x and report each value */
@@ -378,23 +356,23 @@ test()
ret = poll(&fds, 1, 2000); ret = poll(&fds, 1, 2000);
if (ret != 1) { if (ret != 1) {
errx(1, "timed out waiting for sensor data"); PX4_ERR("timed out waiting for sensor data");
} }
/* now go get it */ /* now go get it */
sz = read(fd, &report, sizeof(report)); sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) { if (sz != sizeof(report)) {
err(1, "periodic read failed"); err(1, "periodic read failed");
} }
warnx("periodic read %u", i); PX4_INFO("periodic read %u", i);
warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); PX4_INFO("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
} }
/* reset the sensor polling to its default rate */ /* reset the sensor polling to its default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default rate"); PX4_ERR("failed to set default rate");
} }
errx(0, "PASS"); errx(0, "PASS");
@@ -406,21 +384,19 @@ test()
void void
reset() reset()
{ {
int fd = open(ETS_PATH, O_RDONLY); int fd = px4_open(ETS_PATH, O_RDONLY);
if (fd < 0) { if (fd < 0) {
err(1, "failed "); PX4_ERR("failed ");
} }
if (ioctl(fd, SENSORIOCRESET, 0) < 0) { if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed"); PX4_ERR("driver reset failed");
} }
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed"); PX4_ERR("driver poll restart failed");
} }
exit(0);
} }
/** /**
@@ -430,13 +406,11 @@ void
info() info()
{ {
if (g_dev == nullptr) { if (g_dev == nullptr) {
errx(1, "driver not running"); PX4_ERR("driver not running");
} }
printf("state @ %p\n", g_dev); PX4_INFO("state @ %p", g_dev);
g_dev->print_info(); g_dev->print_info();
exit(0);
} }
} // namespace } // namespace
@@ -445,11 +419,11 @@ info()
static void static void
ets_airspeed_usage() ets_airspeed_usage()
{ {
warnx("usage: ets_airspeed command [options]"); PX4_INFO("usage: ets_airspeed command [options]");
warnx("options:"); PX4_INFO("options:");
warnx("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT); PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
warnx("command:"); PX4_INFO("command:");
warnx("\tstart|stop|reset|test|info"); PX4_INFO("\tstart|stop|reset|test|info");
} }
int int
@@ -503,5 +477,6 @@ ets_airspeed_main(int argc, char *argv[])
} }
ets_airspeed_usage(); ets_airspeed_usage();
exit(0);
return PX4_OK;
} }