From fd8a5644e8af36aa0a6eb0982944959376b3399f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 24 Aug 2017 21:36:58 -0400 Subject: [PATCH] ets_airspeed posix port --- cmake/configs/posix_rpi_common.cmake | 1 + cmake/configs/posix_sitl_default.cmake | 1 + src/drivers/ets_airspeed/ets_airspeed.cpp | 93 +++++++++-------------- 3 files changed, 36 insertions(+), 59 deletions(-) diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake index c633f7511f..e54331d0a2 100644 --- a/cmake/configs/posix_rpi_common.cmake +++ b/cmake/configs/posix_rpi_common.cmake @@ -18,6 +18,7 @@ set(config_module_list # drivers/device drivers/airspeed + drivers/ets_airspeed drivers/ms4525_airspeed drivers/ms5525_airspeed drivers/sdp3x_airspeed diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 0aa3eecd1a..276b1f20c1 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -11,6 +11,7 @@ set(config_module_list drivers/vmount drivers/linux_gpio drivers/airspeed + drivers/ets_airspeed drivers/ms4525_airspeed drivers/ms5525_airspeed drivers/sdp3x_airspeed diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 72c455f7b5..a0da42cd55 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -42,25 +42,6 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - #include #include #include @@ -280,7 +261,7 @@ start(int i2c_bus) int fd; if (g_dev != nullptr) { - errx(1, "already started"); + PX4_ERR("already started"); } /* create the driver */ @@ -295,17 +276,17 @@ start(int i2c_bus) } /* 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) { goto fail; } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; } - exit(0); + return; fail: @@ -315,7 +296,6 @@ fail: } PX4_WARN("no ETS airspeed sensor connected"); - exit(1); } /** @@ -329,10 +309,8 @@ stop() g_dev = nullptr; } else { - errx(1, "driver not running"); + PX4_ERR("driver not running"); } - - exit(0); } /** @@ -347,25 +325,25 @@ test() ssize_t sz; int ret; - int fd = open(ETS_PATH, O_RDONLY); + int fd = px4_open(ETS_PATH, O_RDONLY); 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 */ - sz = read(fd, &report, sizeof(report)); + sz = px4_read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - err(1, "immediate read failed"); + PX4_ERR("immediate read failed"); } - warnx("single read"); - warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); + PX4_INFO("single read"); + PX4_INFO("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - errx(1, "failed to set 2Hz poll rate"); + if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + PX4_ERR("failed to set 2Hz poll rate"); } /* read the sensor 5x and report each value */ @@ -378,23 +356,23 @@ test() ret = poll(&fds, 1, 2000); if (ret != 1) { - errx(1, "timed out waiting for sensor data"); + PX4_ERR("timed out waiting for sensor data"); } /* now go get it */ - sz = read(fd, &report, sizeof(report)); + sz = px4_read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { err(1, "periodic read failed"); } - warnx("periodic read %u", i); - warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); + PX4_INFO("periodic read %u", i); + PX4_INFO("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); } /* reset the sensor polling to its default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - errx(1, "failed to set default rate"); + if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + PX4_ERR("failed to set default rate"); } errx(0, "PASS"); @@ -406,21 +384,19 @@ test() void reset() { - int fd = open(ETS_PATH, O_RDONLY); + int fd = px4_open(ETS_PATH, O_RDONLY); if (fd < 0) { - err(1, "failed "); + PX4_ERR("failed "); } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - err(1, "driver reset failed"); + if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + PX4_ERR("driver reset failed"); } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - err(1, "driver poll restart failed"); + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("driver poll restart failed"); } - - exit(0); } /** @@ -430,13 +406,11 @@ void info() { 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(); - - exit(0); } } // namespace @@ -445,11 +419,11 @@ info() static void ets_airspeed_usage() { - warnx("usage: ets_airspeed command [options]"); - warnx("options:"); - warnx("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT); - warnx("command:"); - warnx("\tstart|stop|reset|test|info"); + PX4_INFO("usage: ets_airspeed command [options]"); + PX4_INFO("options:"); + PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT); + PX4_INFO("command:"); + PX4_INFO("\tstart|stop|reset|test|info"); } int @@ -503,5 +477,6 @@ ets_airspeed_main(int argc, char *argv[]) } ets_airspeed_usage(); - exit(0); + + return PX4_OK; }