diff options
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/ets_airspeed/ets_airspeed.cpp | 5 | ||||
-rw-r--r-- | src/drivers/meas_airspeed/meas_airspeed.cpp | 9 | ||||
-rw-r--r-- | src/drivers/sf0x/sf0x.cpp | 131 | ||||
-rw-r--r-- | src/drivers/stm32/adc/adc.cpp | 2 |
4 files changed, 96 insertions, 51 deletions
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index d27ab9727..d873a1132 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -176,11 +176,14 @@ ETSAirspeed::collect() _max_differential_pressure_pa = diff_pres_pa; } - // XXX we may want to smooth out the readings to remove noise. differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.differential_pressure_pa = (float)diff_pres_pa; + + // XXX we may want to smooth out the readings to remove noise. + report.differential_pressure_filtered_pa = (float)diff_pres_pa; + report.temperature = -1000.0f; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index beca28e7a..2c3efdc35 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -79,6 +79,8 @@ #include <systemlib/param/param.h> #include <systemlib/perf_counter.h> +#include <mathlib/math/filter/LowPassFilter2p.hpp> + #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> @@ -99,6 +101,8 @@ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ /* Measurement rate is 100Hz */ +#define MEAS_RATE 100.0f +#define MEAS_DRIVER_FILTER_FREQ 3.0f #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ class MEASAirspeed : public Airspeed @@ -116,6 +120,7 @@ protected: virtual int measure(); virtual int collect(); + math::LowPassFilter2p _filter; }; /* @@ -124,7 +129,8 @@ protected: extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path) + CONVERSION_INTERVAL, path), + _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ) { } @@ -212,6 +218,7 @@ MEASAirspeed::collect() report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 70cd1ab1e..07163035b 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -276,34 +276,11 @@ SF0X::init() warnx("advert err"); } - /* attempt to get a measurement 5 times */ - while (ret != OK && i < 5) { - - if (measure()) { - ret = ERROR; - _sensor_ok = false; - } - - usleep(100000); - - if (collect()) { - ret = ERROR; - _sensor_ok = false; - - } else { - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; - } - - i++; - } - /* close the fd */ ::close(_fd); _fd = -1; out: - return ret; + return OK; } int @@ -376,6 +353,7 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { + /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -544,10 +522,16 @@ SF0X::collect() if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { _linebuf_index = 0; + } else if (_linebuf_index > 0) { + /* increment to next read position */ + _linebuf_index++; } + /* the buffer for read chars is buflen minus null termination */ + unsigned readlen = sizeof(_linebuf) - 1; + /* read from the sensor (uart buffer) */ - ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); + ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index); if (ret < 0) { _linebuf[sizeof(_linebuf) - 1] = '\0'; @@ -562,19 +546,30 @@ SF0X::collect() } else { return -EAGAIN; } + } else if (ret == 0) { + return -EAGAIN; } - _linebuf_index += ret; - - if (_linebuf_index >= sizeof(_linebuf)) { - _linebuf_index = 0; - } + /* we did increment the index to the next position already, so just add the additional fields */ + _linebuf_index += (ret - 1); _last_read = hrt_absolute_time(); - if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - /* incomplete read, reschedule ourselves */ + if (_linebuf_index < 1) { + /* we need at least the two end bytes to make sense of this string */ return -EAGAIN; + + } else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') { + + if (_linebuf_index >= readlen - 1) { + /* we have a full buffer, but no line ending - abort */ + _linebuf_index = 0; + perf_count(_comms_errors); + return -ENOMEM; + } else { + /* incomplete read, reschedule ourselves */ + return -EAGAIN; + } } char *end; @@ -582,22 +577,56 @@ SF0X::collect() bool valid; /* enforce line ending */ - _linebuf[sizeof(_linebuf) - 1] = '\0'; + unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1); + + _linebuf[lend] = '\0'; if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; } else { - si_units = strtod(_linebuf, &end); - valid = true; + + /* we need to find a dot in the string, as we're missing the meters part else */ + valid = false; + + /* wipe out partially read content from last cycle(s), check for dot */ + for (int i = 0; i < (lend - 2); i++) { + if (_linebuf[i] == '\n') { + char buf[sizeof(_linebuf)]; + memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); + memcpy(_linebuf, buf, (lend + 1) - (i + 1)); + } + + if (_linebuf[i] == '.') { + valid = true; + } + } + + if (valid) { + si_units = strtod(_linebuf, &end); + + /* we require at least 3 characters for a valid number */ + if (end > _linebuf + 3) { + valid = true; + } else { + si_units = -1.0f; + valid = false; + } + } } - debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf); + debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO")); - /* done with this chunk, resetting */ + /* done with this chunk, resetting - even if invalid */ _linebuf_index = 0; + /* if its invalid, there is no reason to forward the value */ + if (!valid) { + perf_count(_comms_errors); + return -EINVAL; + } + struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ @@ -689,7 +718,11 @@ SF0X::cycle() } if (OK != collect_ret) { - log("collection error"); + + /* we know the sensor needs about four seconds to initialize */ + if (hrt_absolute_time() > 5 * 1000 * 1000LL) { + log("collection error"); + } /* restart the measurement state machine */ start(); return; @@ -848,10 +881,10 @@ test() } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("val: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); - /* start the sensor polling at 2Hz */ + /* start the sensor polling at 2 Hz rate */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); } @@ -866,24 +899,26 @@ test() int ret = poll(&fds, 1, 2000); if (ret != 1) { - errx(1, "timed out waiting for sensor data"); + warnx("timed out"); + break; } /* now go get it */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - err(1, "periodic read failed"); + warnx("read failed: got %d vs exp. %d", sz, sizeof(report)); + break; } - warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("read #%u", i); + warnx("val: %0.3f m", (double)report.distance); + warnx("time: %lld", report.timestamp); } - /* reset the sensor polling to default rate */ + /* reset the sensor polling to the default rate */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - errx(1, "failed to set default poll rate"); + errx(1, "ERR: DEF RATE"); } errx(0, "PASS"); diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 00e46d6b8..0b8a275e6 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -341,7 +341,7 @@ test(void) err(1, "can't open ADC device"); for (unsigned i = 0; i < 50; i++) { - adc_msg_s data[10]; + adc_msg_s data[12]; ssize_t count = read(fd, data, sizeof(data)); if (count < 0) |