From 9a97001cc849038223aa5eda6a76ac0fc6f1094f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 01:50:58 +0200 Subject: Added queue to mpu6k driver --- src/drivers/mpu6000/mpu6000.cpp | 254 +++++++++++++++++++++++++++++++--------- 1 file changed, 201 insertions(+), 53 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 1fd6cb17e..d37d39a7a 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -181,13 +181,21 @@ private: struct hrt_call _call; unsigned _call_interval; - struct accel_report _accel_report; + unsigned _num_accel_reports; + volatile unsigned _next_accel_report; + volatile unsigned _oldest_accel_report; + struct accel_report *_accel_reports; + struct accel_scale _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - struct gyro_report _gyro_report; + unsigned _num_gyro_reports; + volatile unsigned _next_gyro_report; + volatile unsigned _oldest_gyro_report; + struct gyro_report *_gyro_reports; + struct gyro_scale _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; @@ -306,14 +314,25 @@ private: /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + MPU6000::MPU6000(int bus, spi_dev_e device) : SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), + _num_accel_reports(0), + _next_accel_report(0), + _oldest_accel_report(0), + _accel_reports(nullptr), _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _num_gyro_reports(0), + _next_gyro_report(0), + _oldest_gyro_report(0), + _gyro_reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), @@ -340,8 +359,6 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; - memset(&_accel_report, 0, sizeof(_accel_report)); - memset(&_gyro_report, 0, sizeof(_gyro_report)); memset(&_call, 0, sizeof(_call)); } @@ -353,6 +370,12 @@ MPU6000::~MPU6000() /* delete the gyro subdriver */ delete _gyro; + /* free any existing reports */ + if (_accel_reports != nullptr) + delete[] _accel_reports; + if (_gyro_reports != nullptr) + delete[] _gyro_reports; + /* delete the perf counter */ perf_free(_sample_perf); } @@ -361,6 +384,7 @@ int MPU6000::init() { int ret; + int gyro_ret; /* do SPI init (and probe) first */ ret = SPI::init(); @@ -371,6 +395,21 @@ MPU6000::init() return ret; } + /* allocate basic report buffers */ + _num_accel_reports = 2; + _oldest_accel_report = _next_accel_report = 0; + _accel_reports = new struct accel_report[_num_accel_reports]; + + if (_accel_reports == nullptr) + goto out; + + _num_gyro_reports = 2; + _oldest_gyro_report = _next_gyro_report = 0; + _gyro_reports = new struct gyro_report[_num_gyro_reports]; + + if (_gyro_reports == nullptr) + goto out; + // Chip reset write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); @@ -462,7 +501,10 @@ MPU6000::init() usleep(1000); /* do CDev init for the gyro device node, keep it optional */ - int gyro_ret = _gyro->init(); + gyro_ret = _gyro->init(); + + memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); + memset(&_gyro_reports[0], 0, sizeof(_gyro_reports[0])); /* ensure we got real values to share */ measure(); @@ -470,12 +512,13 @@ MPU6000::init() if (gyro_ret != OK) { _gyro_topic = -1; } else { - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_reports[0]); } - /* advertise sensor topics */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report); + /* advertise accel topic */ + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); +out: return ret; } @@ -555,19 +598,40 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) ssize_t MPU6000::read(struct file *filp, char *buffer, size_t buflen) { + unsigned count = buflen / sizeof(struct accel_report); int ret = 0; /* buffer must be large enough */ - if (buflen < sizeof(_accel_report)) + if (count < 1) return -ENOSPC; - /* if automatic measurement is not enabled */ - if (_call_interval == 0) - measure(); + /* if automatic measurement is enabled */ + if (_call_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_accel_report != _next_accel_report) { + memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports)); + ret += sizeof(_accel_reports[0]); + INCREMENT(_oldest_accel_report, _num_accel_reports); + } + } - /* copy out the latest reports */ - memcpy(buffer, &_accel_report, sizeof(_accel_report)); - ret = sizeof(_accel_report); + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_accel_report = _next_accel_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); + ret = sizeof(*_accel_reports); return ret; } @@ -586,19 +650,40 @@ MPU6000::self_test() ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { + unsigned count = buflen / sizeof(struct gyro_report); int ret = 0; /* buffer must be large enough */ - if (buflen < sizeof(_gyro_report)) + if (count < 1) return -ENOSPC; - /* if automatic measurement is not enabled */ - if (_call_interval == 0) - measure(); + /* if automatic measurement is enabled */ + if (_call_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_gyro_report != _next_gyro_report) { + memcpy(buffer, _gyro_reports + _oldest_gyro_report, sizeof(*_gyro_reports)); + ret += sizeof(_gyro_reports[0]); + INCREMENT(_oldest_gyro_report, _num_gyro_reports); + } + } - /* copy out the latest report */ - memcpy(buffer, &_gyro_report, sizeof(_gyro_report)); - ret = sizeof(_gyro_report); + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_gyro_report = _next_gyro_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _gyro_reports, sizeof(*_gyro_reports)); + ret = sizeof(*_gyro_reports); return ret; } @@ -661,14 +746,32 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; - case SENSORIOCSQUEUEDEPTH: - /* XXX not implemented */ - return -EINVAL; + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; - case SENSORIOCGQUEUEDEPTH: - /* XXX not implemented */ - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct accel_report *buf = new struct accel_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _accel_reports; + _num_accel_reports = arg; + _accel_reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_accel_reports - 1; case ACCELIOCGSAMPLERATE: return _sample_rate; @@ -726,11 +829,36 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: - case SENSORIOCSQUEUEDEPTH: - case SENSORIOCGQUEUEDEPTH: case SENSORIOCRESET: return ioctl(filp, cmd, arg); + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct gyro_report *buf = new struct gyro_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _gyro_reports; + _num_gyro_reports = arg; + _gyro_reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_gyro_reports - 1; + case GYROIOCGSAMPLERATE: return _sample_rate; @@ -959,10 +1087,16 @@ MPU6000::measure() report.gyro_x = gyro_xt; report.gyro_y = gyro_yt; + /* + * Get references to the current reports + */ + accel_report *accel_report = &_accel_reports[_next_accel_report]; + gyro_report *gyro_report = &_gyro_reports[_next_gyro_report]; + /* * Adjust and scale results to m/s^2. */ - _gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time(); + gyro_report->timestamp = accel_report->timestamp = hrt_absolute_time(); /* @@ -983,40 +1117,54 @@ MPU6000::measure() /* NOTE: Axes have been swapped to match the board a few lines above. */ - _accel_report.x_raw = report.accel_x; - _accel_report.y_raw = report.accel_y; - _accel_report.z_raw = report.accel_z; + accel_report->x_raw = report.accel_x; + accel_report->y_raw = report.accel_y; + accel_report->z_raw = report.accel_z; + + accel_report->x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + accel_report->y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + accel_report->z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + accel_report->scaling = _accel_range_scale; + accel_report->range_m_s2 = _accel_range_m_s2; + + accel_report->temperature_raw = report.temp; + accel_report->temperature = (report.temp) / 361.0f + 35.0f; + + gyro_report->x_raw = report.gyro_x; + gyro_report->y_raw = report.gyro_y; + gyro_report->z_raw = report.gyro_z; + + gyro_report->x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + gyro_report->y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + gyro_report->z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + gyro_report->scaling = _gyro_range_scale; + gyro_report->range_rad_s = _gyro_range_rad_s; - _accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - _accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - _accel_report.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - _accel_report.scaling = _accel_range_scale; - _accel_report.range_m_s2 = _accel_range_m_s2; + gyro_report->temperature_raw = report.temp; + gyro_report->temperature = (report.temp) / 361.0f + 35.0f; - _accel_report.temperature_raw = report.temp; - _accel_report.temperature = (report.temp) / 361.0f + 35.0f; + /* ACCEL: post a report to the ring - note, not locked */ + INCREMENT(_next_accel_report, _num_accel_reports); - _gyro_report.x_raw = report.gyro_x; - _gyro_report.y_raw = report.gyro_y; - _gyro_report.z_raw = report.gyro_z; + /* ACCEL: if we are running up against the oldest report, fix it */ + if (_next_accel_report == _oldest_accel_report) + INCREMENT(_oldest_accel_report, _num_accel_reports); - _gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - _gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - _gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - _gyro_report.scaling = _gyro_range_scale; - _gyro_report.range_rad_s = _gyro_range_rad_s; + /* GYRO: post a report to the ring - note, not locked */ + INCREMENT(_next_gyro_report, _num_gyro_reports); - _gyro_report.temperature_raw = report.temp; - _gyro_report.temperature = (report.temp) / 361.0f + 35.0f; + /* GYRO: if we are running up against the oldest report, fix it */ + if (_next_gyro_report == _oldest_gyro_report) + INCREMENT(_oldest_gyro_report, _num_gyro_reports); /* notify anyone waiting for data */ poll_notify(POLLIN); _gyro->parent_poll_notify(); /* and publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, gyro_report); } /* stop measuring */ -- cgit v1.2.3 From a3ab88872cbca30be62b04461c8294399923ec89 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 3 Aug 2013 19:02:54 -0700 Subject: Add some more useful methods to the ringbuffer class. --- src/drivers/device/ringbuffer.h | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index 37686fdbe..dc0c84052 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -104,6 +104,17 @@ public: */ bool full() { return _next(_head) == _tail; } + /* + * Returns the capacity of the buffer, or zero if the buffer could + * not be allocated. + */ + unsigned size() { return (_buf != nullptr) ? _size : 0; } + + /* + * Empties the buffer. + */ + void flush() { _head = _tail = _size; } + private: T *const _buf; const unsigned _size; @@ -114,11 +125,11 @@ private: }; template -RingBuffer::RingBuffer(unsigned size) : - _buf(new T[size + 1]), - _size(size), - _head(size), - _tail(size) +RingBuffer::RingBuffer(unsigned with_size) : + _buf(new T[with_size + 1]), + _size(with_size), + _head(with_size), + _tail(with_size) {} template -- cgit v1.2.3 From 02f5b79948742d6b7121399e39444286cc01f161 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 3 Aug 2013 19:03:24 -0700 Subject: Try to save our sanity a bit and use the generic ringbuffer class rather than re-implementing the wheel. --- src/drivers/mpu6000/mpu6000.cpp | 255 ++++++++++++++++------------------------ 1 file changed, 104 insertions(+), 151 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index d37d39a7a..f848cca6b 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -67,6 +67,7 @@ #include #include +#include #include #include @@ -181,20 +182,16 @@ private: struct hrt_call _call; unsigned _call_interval; - unsigned _num_accel_reports; - volatile unsigned _next_accel_report; - volatile unsigned _oldest_accel_report; - struct accel_report *_accel_reports; + typedef RingBuffer AccelReportBuffer; + AccelReportBuffer *_accel_reports; struct accel_scale _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - unsigned _num_gyro_reports; - volatile unsigned _next_gyro_report; - volatile unsigned _oldest_gyro_report; - struct gyro_report *_gyro_reports; + typedef RingBuffer GyroReportBuffer; + GyroReportBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -227,7 +224,7 @@ private: static void measure_trampoline(void *arg); /** - * Fetch measurements from the sensor and update the report ring. + * Fetch measurements from the sensor and update the report buffers. */ void measure(); @@ -314,24 +311,15 @@ private: /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - MPU6000::MPU6000(int bus, spi_dev_e device) : SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), - _num_accel_reports(0), - _next_accel_report(0), - _oldest_accel_report(0), _accel_reports(nullptr), _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), - _num_gyro_reports(0), - _next_gyro_report(0), - _oldest_gyro_report(0), _gyro_reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), @@ -372,9 +360,9 @@ MPU6000::~MPU6000() /* free any existing reports */ if (_accel_reports != nullptr) - delete[] _accel_reports; + delete _accel_reports; if (_gyro_reports != nullptr) - delete[] _gyro_reports; + delete _gyro_reports; /* delete the perf counter */ perf_free(_sample_perf); @@ -396,17 +384,11 @@ MPU6000::init() } /* allocate basic report buffers */ - _num_accel_reports = 2; - _oldest_accel_report = _next_accel_report = 0; - _accel_reports = new struct accel_report[_num_accel_reports]; - + _accel_reports = new AccelReportBuffer(2); if (_accel_reports == nullptr) goto out; - _num_gyro_reports = 2; - _oldest_gyro_report = _next_gyro_report = 0; - _gyro_reports = new struct gyro_report[_num_gyro_reports]; - + _gyro_reports = new GyroReportBuffer(2); if (_gyro_reports == nullptr) goto out; @@ -503,20 +485,22 @@ MPU6000::init() /* do CDev init for the gyro device node, keep it optional */ gyro_ret = _gyro->init(); - memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); - memset(&_gyro_reports[0], 0, sizeof(_gyro_reports[0])); - - /* ensure we got real values to share */ + /* fetch an initial set of measurements for advertisement */ measure(); if (gyro_ret != OK) { _gyro_topic = -1; } else { - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_reports[0]); + gyro_report gr; + _gyro_reports->get(gr); + + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); } /* advertise accel topic */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); + accel_report ar; + _accel_reports->get(ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); out: return ret; @@ -598,42 +582,31 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) ssize_t MPU6000::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct accel_report); - int ret = 0; + unsigned count = buflen / sizeof(accel_report); /* buffer must be large enough */ if (count < 1) return -ENOSPC; - /* if automatic measurement is enabled */ - if (_call_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the measurement code while we are doing this; - * we are careful to avoid racing with it. - */ - while (count--) { - if (_oldest_accel_report != _next_accel_report) { - memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports)); - ret += sizeof(_accel_reports[0]); - INCREMENT(_oldest_accel_report, _num_accel_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; + /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ + if (_call_interval == 0) { + _accel_reports->flush(); + measure(); } - /* manual measurement */ - _oldest_accel_report = _next_accel_report = 0; - measure(); + /* if no data, error (we could block here) */ + if (_accel_reports->empty()) + return -EAGAIN; - /* measurement will have generated a report, copy it out */ - memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); - ret = sizeof(*_accel_reports); + /* copy reports out of our buffer to the caller */ + accel_report *arp = reinterpret_cast(buffer); + while (count--) { + if (!_accel_reports->get(*arp++)) + break; + } - return ret; + /* return the number of bytes transferred */ + return (buflen - (count * sizeof(accel_report))); } int @@ -651,41 +624,30 @@ ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct gyro_report); - int ret = 0; /* buffer must be large enough */ if (count < 1) return -ENOSPC; - /* if automatic measurement is enabled */ - if (_call_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the measurement code while we are doing this; - * we are careful to avoid racing with it. - */ - while (count--) { - if (_oldest_gyro_report != _next_gyro_report) { - memcpy(buffer, _gyro_reports + _oldest_gyro_report, sizeof(*_gyro_reports)); - ret += sizeof(_gyro_reports[0]); - INCREMENT(_oldest_gyro_report, _num_gyro_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; + /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ + if (_call_interval == 0) { + _gyro_reports->flush(); + measure(); } - /* manual measurement */ - _oldest_gyro_report = _next_gyro_report = 0; - measure(); + /* if no data, error (we could block here) */ + if (_gyro_reports->empty()) + return -EAGAIN; - /* measurement will have generated a report, copy it out */ - memcpy(buffer, _gyro_reports, sizeof(*_gyro_reports)); - ret = sizeof(*_gyro_reports); + /* copy reports out of our buffer to the caller */ + gyro_report *arp = reinterpret_cast(buffer); + while (count--) { + if (!_gyro_reports->get(*arp++)) + break; + } - return ret; + /* return the number of bytes transferred */ + return (buflen - (count * sizeof(gyro_report))); } int @@ -747,23 +709,23 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) + if ((arg < 1) || (arg > 100)) return -EINVAL; /* allocate new buffer */ - struct accel_report *buf = new struct accel_report[arg]; + AccelReportBuffer *buf = new AccelReportBuffer(arg); if (nullptr == buf) return -ENOMEM; + if (buf->size() == 0) { + delete buf; + return -ENOMEM; + } /* reset the measurement state machine with the new buffer, free the old */ stop(); - delete[] _accel_reports; - _num_accel_reports = arg; + delete _accel_reports; _accel_reports = buf; start(); @@ -771,14 +733,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGQUEUEDEPTH: - return _num_accel_reports - 1; + return _accel_reports->size(); case ACCELIOCGSAMPLERATE: return _sample_rate; case ACCELIOCSSAMPLERATE: - _set_sample_rate(arg); - return OK; + _set_sample_rate(arg); + return OK; case ACCELIOCSLOWPASS: case ACCELIOCGLOWPASS: @@ -833,23 +795,23 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) + if ((arg < 1) || (arg > 100)) return -EINVAL; /* allocate new buffer */ - struct gyro_report *buf = new struct gyro_report[arg]; + GyroReportBuffer *buf = new GyroReportBuffer(arg); if (nullptr == buf) return -ENOMEM; + if (buf->size() == 0) { + delete buf; + return -ENOMEM; + } /* reset the measurement state machine with the new buffer, free the old */ stop(); - delete[] _gyro_reports; - _num_gyro_reports = arg; + delete _gyro_reports; _gyro_reports = buf; start(); @@ -857,7 +819,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGQUEUEDEPTH: - return _num_gyro_reports - 1; + return _gyro_reports->size(); case GYROIOCGSAMPLERATE: return _sample_rate; @@ -993,6 +955,10 @@ MPU6000::start() /* make sure we are stopped first */ stop(); + /* discard any stale data in the buffers */ + _accel_reports->flush(); + _gyro_reports->flush(); + /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this); } @@ -1006,7 +972,7 @@ MPU6000::stop() void MPU6000::measure_trampoline(void *arg) { - MPU6000 *dev = (MPU6000 *)arg; + MPU6000 *dev = reinterpret_cast(arg); /* make another measurement */ dev->measure(); @@ -1088,15 +1054,15 @@ MPU6000::measure() report.gyro_y = gyro_yt; /* - * Get references to the current reports + * Report buffers. */ - accel_report *accel_report = &_accel_reports[_next_accel_report]; - gyro_report *gyro_report = &_gyro_reports[_next_gyro_report]; + accel_report arb; + gyro_report grb; /* * Adjust and scale results to m/s^2. */ - gyro_report->timestamp = accel_report->timestamp = hrt_absolute_time(); + grb.timestamp = arb.timestamp = hrt_absolute_time(); /* @@ -1117,54 +1083,43 @@ MPU6000::measure() /* NOTE: Axes have been swapped to match the board a few lines above. */ - accel_report->x_raw = report.accel_x; - accel_report->y_raw = report.accel_y; - accel_report->z_raw = report.accel_z; - - accel_report->x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - accel_report->y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - accel_report->z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - accel_report->scaling = _accel_range_scale; - accel_report->range_m_s2 = _accel_range_m_s2; + arb.x_raw = report.accel_x; + arb.y_raw = report.accel_y; + arb.z_raw = report.accel_z; - accel_report->temperature_raw = report.temp; - accel_report->temperature = (report.temp) / 361.0f + 35.0f; + arb.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + arb.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + arb.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + arb.scaling = _accel_range_scale; + arb.range_m_s2 = _accel_range_m_s2; - gyro_report->x_raw = report.gyro_x; - gyro_report->y_raw = report.gyro_y; - gyro_report->z_raw = report.gyro_z; + arb.temperature_raw = report.temp; + arb.temperature = (report.temp) / 361.0f + 35.0f; - gyro_report->x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - gyro_report->y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - gyro_report->z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - gyro_report->scaling = _gyro_range_scale; - gyro_report->range_rad_s = _gyro_range_rad_s; + grb.x_raw = report.gyro_x; + grb.y_raw = report.gyro_y; + grb.z_raw = report.gyro_z; - gyro_report->temperature_raw = report.temp; - gyro_report->temperature = (report.temp) / 361.0f + 35.0f; + grb.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + grb.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + grb.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + grb.scaling = _gyro_range_scale; + grb.range_rad_s = _gyro_range_rad_s; - /* ACCEL: post a report to the ring - note, not locked */ - INCREMENT(_next_accel_report, _num_accel_reports); + grb.temperature_raw = report.temp; + grb.temperature = (report.temp) / 361.0f + 35.0f; - /* ACCEL: if we are running up against the oldest report, fix it */ - if (_next_accel_report == _oldest_accel_report) - INCREMENT(_oldest_accel_report, _num_accel_reports); - - /* GYRO: post a report to the ring - note, not locked */ - INCREMENT(_next_gyro_report, _num_gyro_reports); - - /* GYRO: if we are running up against the oldest report, fix it */ - if (_next_gyro_report == _oldest_gyro_report) - INCREMENT(_oldest_gyro_report, _num_gyro_reports); + _accel_reports->put(arb); + _gyro_reports->put(grb); /* notify anyone waiting for data */ poll_notify(POLLIN); _gyro->parent_poll_notify(); /* and publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, gyro_report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); } /* stop measuring */ @@ -1267,21 +1222,19 @@ fail: void test() { - int fd = -1; - int fd_gyro = -1; - struct accel_report a_report; - struct gyro_report g_report; + accel_report a_report; + gyro_report g_report; ssize_t sz; /* get the driver */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", ACCEL_DEVICE_PATH); /* get the driver */ - fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); if (fd_gyro < 0) err(1, "%s open failed", GYRO_DEVICE_PATH); -- cgit v1.2.3 From f45e74265e3952f350970d665adccdf539e136f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 15:15:41 +0200 Subject: Fixed driver test / direct read, looks good --- src/drivers/mpu6000/mpu6000.cpp | 111 +++++++++++++++++++++++++--------------- 1 file changed, 70 insertions(+), 41 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index f848cca6b..9dcb4be9e 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -212,6 +212,13 @@ private: */ void stop(); + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + void reset(); + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -392,6 +399,50 @@ MPU6000::init() if (_gyro_reports == nullptr) goto out; + reset(); + + /* Initialize offsets and scales */ + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; + + /* do CDev init for the gyro device node, keep it optional */ + gyro_ret = _gyro->init(); + + /* fetch an initial set of measurements for advertisement */ + measure(); + + if (gyro_ret != OK) { + _gyro_topic = -1; + } else { + gyro_report gr; + _gyro_reports->get(gr); + + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); + } + + /* advertise accel topic */ + accel_report ar; + _accel_reports->get(ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + +out: + return ret; +} + +void MPU6000::reset() +{ + // Chip reset write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); @@ -423,12 +474,6 @@ MPU6000::init() // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s // scaling factor: // 1/(2^15)*(2000/180)*PI - _gyro_scale.x_offset = 0; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0; - _gyro_scale.z_scale = 1.0f; _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; @@ -461,12 +506,6 @@ MPU6000::init() // Correct accel scale factors of 4096 LSB/g // scale to m/s^2 ( 1g = 9.81 m/s^2) - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; _accel_range_scale = (9.81f / 4096.0f); _accel_range_m_s2 = 8.0f * 9.81f; @@ -482,28 +521,6 @@ MPU6000::init() // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); - /* do CDev init for the gyro device node, keep it optional */ - gyro_ret = _gyro->init(); - - /* fetch an initial set of measurements for advertisement */ - measure(); - - if (gyro_ret != OK) { - _gyro_topic = -1; - } else { - gyro_report gr; - _gyro_reports->get(gr); - - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); - } - - /* advertise accel topic */ - accel_report ar; - _accel_reports->get(ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); - -out: - return ret; } int @@ -600,13 +617,15 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); + int transferred = 0; while (count--) { if (!_accel_reports->get(*arp++)) break; + transferred++; } /* return the number of bytes transferred */ - return (buflen - (count * sizeof(accel_report))); + return (transferred * sizeof(accel_report)); } int @@ -623,7 +642,7 @@ MPU6000::self_test() ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct gyro_report); + unsigned count = buflen / sizeof(gyro_report); /* buffer must be large enough */ if (count < 1) @@ -641,13 +660,15 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) /* copy reports out of our buffer to the caller */ gyro_report *arp = reinterpret_cast(buffer); + int transferred = 0; while (count--) { if (!_gyro_reports->get(*arp++)) break; + transferred++; } /* return the number of bytes transferred */ - return (buflen - (count * sizeof(gyro_report))); + return (transferred * sizeof(gyro_report)); } int @@ -655,6 +676,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { + case SENSORIOCRESET: + reset(); + return OK; + case SENSORIOCSPOLLRATE: { switch (arg) { @@ -674,8 +699,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: - /* XXX 500Hz is just a wild guess */ - return ioctl(filp, SENSORIOCSPOLLRATE, 500); + /* set to same as sample rate per default */ + return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate); /* adjust to a legal polling interval in Hz */ default: { @@ -1246,8 +1271,10 @@ test() /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); - if (sz != sizeof(a_report)) + if (sz != sizeof(a_report)) { + warnx("ret: %d, expected: %d", sz, sizeof(a_report)); err(1, "immediate acc read failed"); + } warnx("single read"); warnx("time: %lld", a_report.timestamp); @@ -1263,8 +1290,10 @@ test() /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); - if (sz != sizeof(g_report)) + if (sz != sizeof(g_report)) { + warnx("ret: %d, expected: %d", sz, sizeof(g_report)); err(1, "immediate gyro read failed"); + } warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); -- cgit v1.2.3 From 36cca7a31b428408eb686df592f092ba5fc24006 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 19:10:56 +0200 Subject: Added rate limit in sensors app. Just pending accel filters now --- src/modules/sensors/sensors.cpp | 66 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 63 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7ff9e19b4..0f1782fca 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -92,8 +92,35 @@ #define BARO_HEALTH_COUNTER_LIMIT_OK 5 #define ADC_HEALTH_COUNTER_LIMIT_OK 5 -#define ADC_BATTERY_VOLTAGE_CHANNEL 10 -#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +/** + * Analog layout: + * FMU: + * IN2 - battery voltage + * IN3 - battery current + * IN4 - 5V sense + * IN10 - spare (we could actually trim these from the set) + * IN11 - spare (we could actually trim these from the set) + * IN12 - spare (we could actually trim these from the set) + * IN13 - aux1 + * IN14 - aux2 + * IN15 - pressure sensor + * + * IO: + * IN4 - servo supply rail + * IN5 - analog RSSI + */ + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + #define ADC_BATTERY_VOLTAGE_CHANNEL 10 + #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + #define ADC_BATTERY_VOLTAGE_CHANNEL 2 + #define ADC_BATTERY_CURRENT_CHANNEL 3 + #define ADC_5V_RAIL_SENSE 4 + #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 +#endif #define BAT_VOL_INITIAL 0.f #define BAT_VOL_LOWPASS_1 0.99f @@ -731,12 +758,27 @@ Sensors::accel_init() errx(1, "FATAL: no accelerometer found"); } else { + + // XXX do the check more elegantly + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* set the accel internal sampling rate up to at leat 500Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 500); /* set the driver to poll at 500Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 500); + #else + + /* set the accel internal sampling rate up to at leat 800Hz */ + ioctl(fd, ACCELIOCSSAMPLERATE, 800); + + /* set the driver to poll at 800Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 800); + + #endif + warnx("using system accel"); close(fd); } @@ -754,12 +796,27 @@ Sensors::gyro_init() errx(1, "FATAL: no gyro found"); } else { + + // XXX do the check more elegantly + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* set the gyro internal sampling rate up to at leat 500Hz */ ioctl(fd, GYROIOCSSAMPLERATE, 500); /* set the driver to poll at 500Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 500); + #else + + /* set the gyro internal sampling rate up to at leat 800Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 800); + + /* set the driver to poll at 800Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 800); + + #endif + warnx("using system gyro"); close(fd); } @@ -1360,6 +1417,9 @@ Sensors::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); + /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ + orb_set_interval(_gyro_sub, 4); + /* * do advertisements */ @@ -1395,7 +1455,7 @@ Sensors::task_main() while (!_task_should_exit) { - /* wait for up to 500ms for data */ + /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ -- cgit v1.2.3 From ac89322d74ac1f5a29a6665feb4220aba3025f82 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Aug 2013 15:09:16 +1000 Subject: mathlib: added LowPassFilter2p object used in gyro and accel drivers for better filtering --- .../mathlib/math/filter/LowPassFilter2p.cpp | 77 +++++++++++++++++++++ .../mathlib/math/filter/LowPassFilter2p.hpp | 78 ++++++++++++++++++++++ src/modules/mathlib/math/filter/module.mk | 44 ++++++++++++ 3 files changed, 199 insertions(+) create mode 100644 src/modules/mathlib/math/filter/LowPassFilter2p.cpp create mode 100644 src/modules/mathlib/math/filter/LowPassFilter2p.hpp create mode 100644 src/modules/mathlib/math/filter/module.mk diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp new file mode 100644 index 000000000..efb17225d --- /dev/null +++ b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp @@ -0,0 +1,77 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/// @file LowPassFilter.cpp +/// @brief A class to implement a second order low pass filter +/// Author: Leonard Hall + +#include "LowPassFilter2p.hpp" +#include "math.h" + +namespace math +{ + +void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) +{ + _cutoff_freq = cutoff_freq; + float fr = sample_freq/_cutoff_freq; + float ohm = tanf(M_PI_F/fr); + float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm; + _b0 = ohm*ohm/c; + _b1 = 2.0f*_b0; + _b2 = _b0; + _a1 = 2.0f*(ohm*ohm-1.0f)/c; + _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c; +} + +float LowPassFilter2p::apply(float sample) +{ + // do the filtering + float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; + if (isnan(delay_element_0) || isinf(delay_element_0)) { + // don't allow bad values to propogate via the filter + delay_element_0 = sample; + } + float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; + + _delay_element_2 = _delay_element_1; + _delay_element_1 = delay_element_0; + + // return the value. Should be no need to check limits + return output; +} + +} // namespace math + diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp new file mode 100644 index 000000000..208ec98d4 --- /dev/null +++ b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp @@ -0,0 +1,78 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/// @file LowPassFilter.h +/// @brief A class to implement a second order low pass filter +/// Author: Leonard Hall +/// Adapted for PX4 by Andrew Tridgell + +#pragma once + +namespace math +{ +class __EXPORT LowPassFilter2p +{ +public: + // constructor + LowPassFilter2p(float sample_freq, float cutoff_freq) { + // set initial parameters + set_cutoff_frequency(sample_freq, cutoff_freq); + _delay_element_1 = _delay_element_2 = 0; + } + + // change parameters + void set_cutoff_frequency(float sample_freq, float cutoff_freq); + + // apply - Add a new raw value to the filter + // and retrieve the filtered result + float apply(float sample); + + // return the cutoff frequency + float get_cutoff_freq(void) const { + return _cutoff_freq; + } + +private: + float _cutoff_freq; + float _a1; + float _a2; + float _b0; + float _b1; + float _b2; + float _delay_element_1; // buffered sample -1 + float _delay_element_2; // buffered sample -2 +}; + +} // namespace math diff --git a/src/modules/mathlib/math/filter/module.mk b/src/modules/mathlib/math/filter/module.mk new file mode 100644 index 000000000..fe92c8c70 --- /dev/null +++ b/src/modules/mathlib/math/filter/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# filter library +# +SRCS = LowPassFilter2p.cpp + +# +# In order to include .config we first have to save off the +# current makefile name, since app.mk needs it. +# +APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) +-include $(TOPDIR)/.config -- cgit v1.2.3 From 7c3ee6714c0e184c297ffced880488a41e7d255d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 19:14:25 +0200 Subject: Enabled mathlib --- makefiles/config_px4fmu-v1_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index e6ec840f9..38cf437e0 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -94,6 +94,7 @@ MODULES += modules/sdlog2 MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/mathlib +MODULES += modules/mathlib/math/filter MODULES += modules/controllib MODULES += modules/uORB -- cgit v1.2.3