|
|
|
@ -94,7 +94,7 @@
@@ -94,7 +94,7 @@
|
|
|
|
|
class ETSAirspeed : public Airspeed |
|
|
|
|
{ |
|
|
|
|
public: |
|
|
|
|
ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH); |
|
|
|
|
ETSAirspeed(int bus, int address = I2C_ADDRESS, const char *path = ETS_PATH); |
|
|
|
|
|
|
|
|
|
protected: |
|
|
|
|
|
|
|
|
@ -113,8 +113,8 @@ protected:
@@ -113,8 +113,8 @@ protected:
|
|
|
|
|
*/ |
|
|
|
|
extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); |
|
|
|
|
|
|
|
|
|
ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, |
|
|
|
|
CONVERSION_INTERVAL, path) |
|
|
|
|
ETSAirspeed::ETSAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, |
|
|
|
|
CONVERSION_INTERVAL, path) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
} |
|
|
|
@ -155,15 +155,16 @@ ETSAirspeed::collect()
@@ -155,15 +155,16 @@ ETSAirspeed::collect()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
uint16_t diff_pres_pa_raw = val[1] << 8 | val[0]; |
|
|
|
|
if (diff_pres_pa_raw == 0) { |
|
|
|
|
|
|
|
|
|
if (diff_pres_pa_raw == 0) { |
|
|
|
|
// a zero value means the pressure sensor cannot give us a
|
|
|
|
|
// value. We need to return, and not report a value or the
|
|
|
|
|
// caller could end up using this value as part of an
|
|
|
|
|
// average
|
|
|
|
|
perf_count(_comms_errors); |
|
|
|
|
log("zero value from sensor");
|
|
|
|
|
log("zero value from sensor"); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// The raw value still should be compensated for the known offset
|
|
|
|
|
diff_pres_pa_raw -= _diff_pres_offset; |
|
|
|
@ -175,7 +176,7 @@ ETSAirspeed::collect()
@@ -175,7 +176,7 @@ ETSAirspeed::collect()
|
|
|
|
|
|
|
|
|
|
differential_pressure_s report; |
|
|
|
|
report.timestamp = hrt_absolute_time(); |
|
|
|
|
report.error_count = perf_event_count(_comms_errors); |
|
|
|
|
report.error_count = perf_event_count(_comms_errors); |
|
|
|
|
|
|
|
|
|
// XXX we may want to smooth out the readings to remove noise.
|
|
|
|
|
report.differential_pressure_filtered_pa = diff_pres_pa_raw; |
|
|
|
@ -210,6 +211,7 @@ ETSAirspeed::cycle()
@@ -210,6 +211,7 @@ ETSAirspeed::cycle()
|
|
|
|
|
|
|
|
|
|
/* perform collection */ |
|
|
|
|
ret = collect(); |
|
|
|
|
|
|
|
|
|
if (OK != ret) { |
|
|
|
|
perf_count(_comms_errors); |
|
|
|
|
/* restart the measurement state machine */ |
|
|
|
@ -239,6 +241,7 @@ ETSAirspeed::cycle()
@@ -239,6 +241,7 @@ ETSAirspeed::cycle()
|
|
|
|
|
|
|
|
|
|
/* measurement phase */ |
|
|
|
|
ret = measure(); |
|
|
|
|
|
|
|
|
|
if (OK != ret) { |
|
|
|
|
debug("measure error"); |
|
|
|
|
} |
|
|
|
@ -287,26 +290,31 @@ start(int i2c_bus)
@@ -287,26 +290,31 @@ start(int i2c_bus)
|
|
|
|
|
{ |
|
|
|
|
int fd; |
|
|
|
|
|
|
|
|
|
if (g_dev != nullptr) |
|
|
|
|
if (g_dev != nullptr) { |
|
|
|
|
errx(1, "already started"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* create the driver */ |
|
|
|
|
g_dev = new ETSAirspeed(i2c_bus); |
|
|
|
|
|
|
|
|
|
if (g_dev == nullptr) |
|
|
|
|
if (g_dev == nullptr) { |
|
|
|
|
goto fail; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (OK != g_dev->Airspeed::init()) |
|
|
|
|
if (OK != g_dev->Airspeed::init()) { |
|
|
|
|
goto fail; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* set the poll rate to default, starts automatic data collection */ |
|
|
|
|
fd = open(AIRSPEED0_DEVICE_PATH, O_RDONLY); |
|
|
|
|
|
|
|
|
|
if (fd < 0) |
|
|
|
|
if (fd < 0) { |
|
|
|
|
goto fail; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) |
|
|
|
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { |
|
|
|
|
goto fail; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
exit(0); |
|
|
|
|
|
|
|
|
@ -351,21 +359,24 @@ test()
@@ -351,21 +359,24 @@ test()
|
|
|
|
|
|
|
|
|
|
int fd = 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); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* do a simple demand read */ |
|
|
|
|
sz = read(fd, &report, sizeof(report)); |
|
|
|
|
|
|
|
|
|
if (sz != sizeof(report)) |
|
|
|
|
if (sz != sizeof(report)) { |
|
|
|
|
err(1, "immediate read failed"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
warnx("single read"); |
|
|
|
|
warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); |
|
|
|
|
|
|
|
|
|
/* start the sensor polling at 2Hz */ |
|
|
|
|
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) |
|
|
|
|
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++) { |
|
|
|
@ -376,22 +387,25 @@ test()
@@ -376,22 +387,25 @@ test()
|
|
|
|
|
fds.events = POLLIN; |
|
|
|
|
ret = poll(&fds, 1, 2000); |
|
|
|
|
|
|
|
|
|
if (ret != 1) |
|
|
|
|
if (ret != 1) { |
|
|
|
|
errx(1, "timed out waiting for sensor data"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* now go get it */ |
|
|
|
|
sz = read(fd, &report, sizeof(report)); |
|
|
|
|
|
|
|
|
|
if (sz != 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); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* reset the sensor polling to its default rate */ |
|
|
|
|
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) |
|
|
|
|
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { |
|
|
|
|
errx(1, "failed to set default rate"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
errx(0, "PASS"); |
|
|
|
|
} |
|
|
|
@ -404,14 +418,17 @@ reset()
@@ -404,14 +418,17 @@ reset()
|
|
|
|
|
{ |
|
|
|
|
int fd = open(ETS_PATH, O_RDONLY); |
|
|
|
|
|
|
|
|
|
if (fd < 0) |
|
|
|
|
if (fd < 0) { |
|
|
|
|
err(1, "failed "); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (ioctl(fd, SENSORIOCRESET, 0) < 0) |
|
|
|
|
if (ioctl(fd, SENSORIOCRESET, 0) < 0) { |
|
|
|
|
err(1, "driver reset failed"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) |
|
|
|
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { |
|
|
|
|
err(1, "driver poll restart failed"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
exit(0); |
|
|
|
|
} |
|
|
|
@ -422,8 +439,9 @@ reset()
@@ -422,8 +439,9 @@ reset()
|
|
|
|
|
void |
|
|
|
|
info() |
|
|
|
|
{ |
|
|
|
|
if (g_dev == nullptr) |
|
|
|
|
if (g_dev == nullptr) { |
|
|
|
|
errx(1, "driver not running"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
printf("state @ %p\n", g_dev); |
|
|
|
|
g_dev->print_info(); |
|
|
|
@ -462,32 +480,37 @@ ets_airspeed_main(int argc, char *argv[])
@@ -462,32 +480,37 @@ ets_airspeed_main(int argc, char *argv[])
|
|
|
|
|
/*
|
|
|
|
|
* Start/load the driver. |
|
|
|
|
*/ |
|
|
|
|
if (!strcmp(argv[1], "start")) |
|
|
|
|
if (!strcmp(argv[1], "start")) { |
|
|
|
|
ets_airspeed::start(i2c_bus); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Stop the driver |
|
|
|
|
*/ |
|
|
|
|
if (!strcmp(argv[1], "stop")) |
|
|
|
|
if (!strcmp(argv[1], "stop")) { |
|
|
|
|
ets_airspeed::stop(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Test the driver/device. |
|
|
|
|
*/ |
|
|
|
|
if (!strcmp(argv[1], "test")) |
|
|
|
|
if (!strcmp(argv[1], "test")) { |
|
|
|
|
ets_airspeed::test(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Reset the driver. |
|
|
|
|
*/ |
|
|
|
|
if (!strcmp(argv[1], "reset")) |
|
|
|
|
if (!strcmp(argv[1], "reset")) { |
|
|
|
|
ets_airspeed::reset(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Print driver information. |
|
|
|
|
*/ |
|
|
|
|
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) |
|
|
|
|
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { |
|
|
|
|
ets_airspeed::info(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
ets_airspeed_usage(); |
|
|
|
|
exit(0); |
|
|
|
|