From 04f8e338b682d0f72f00ad12f22b5071a8f6bd18 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Sep 2013 21:42:00 +1000 Subject: [PATCH 01/14] hmc5883: add perf count, and removed unnecessary checks for -32768 we've already checked that the absolute value is <= 2048 --- src/drivers/hmc5883/hmc5883.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 0de82c3042..378f433cd1 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -842,8 +842,10 @@ HMC5883::collect() */ if ((abs(report.x) > 2048) || (abs(report.y) > 2048) || - (abs(report.z) > 2048)) + (abs(report.z) > 2048)) { + perf_count(_comms_errors); goto out; + } /* * RAW outputs @@ -852,7 +854,7 @@ HMC5883::collect() * and y needs to be negated */ _reports[_next_report].x_raw = report.y; - _reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x); + _reports[_next_report].y_raw = -report.x; /* z remains z */ _reports[_next_report].z_raw = report.z; @@ -878,14 +880,14 @@ HMC5883::collect() /* to align the sensor axes with the board, x and y need to be flipped */ _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; + _reports[_next_report].y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; } else { #endif /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, * therefore switch x and y and invert y */ - _reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale; + _reports[_next_report].x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ From 1828b57c581dda473d03c9c00cdbf354c7927f23 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 9 Sep 2013 17:36:07 +1000 Subject: [PATCH 02/14] ringbuffer: added force() and use lockless methods this adds force() which can be used for drivers wanting consumers to get the latest data when the buffer overflows --- src/drivers/device/ringbuffer.h | 136 ++++++++++++++++++++++++++++---- 1 file changed, 120 insertions(+), 16 deletions(-) diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index dc0c84052b..c859be647a 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -55,13 +55,29 @@ public: bool put(T &val); /** - * Put an item into the buffer. + * Put an item into the buffer if there is space. * * @param val Item to put * @return true if the item was put, false if the buffer is full */ bool put(const T &val); + /** + * Force an item into the buffer, discarding an older item if there is not space. + * + * @param val Item to put + * @return true if an item was discarded to make space + */ + bool force(T &val); + + /** + * Force an item into the buffer, discarding an older item if there is not space. + * + * @param val Item to put + * @return true if an item was discarded to make space + */ + bool force(const T &val); + /** * Get an item from the buffer. * @@ -73,8 +89,8 @@ public: /** * Get an item from the buffer (scalars only). * - * @return The value that was fetched, or zero if the buffer was - * empty. + * @return The value that was fetched. If the buffer is empty, + * returns zero. */ T get(void); @@ -97,23 +113,23 @@ public: /* * Returns true if the buffer is empty. */ - bool empty() { return _tail == _head; } + bool empty(); /* * Returns true if the buffer is full. */ - bool full() { return _next(_head) == _tail; } + bool full(); /* * Returns the capacity of the buffer, or zero if the buffer could * not be allocated. */ - unsigned size() { return (_buf != nullptr) ? _size : 0; } + unsigned size(); /* * Empties the buffer. */ - void flush() { _head = _tail = _size; } + void flush(); private: T *const _buf; @@ -139,6 +155,38 @@ RingBuffer::~RingBuffer() delete[] _buf; } +template +bool RingBuffer::empty() +{ + return _tail == _head; +} + +template +bool RingBuffer::full() +{ + return _next(_head) == _tail; +} + +template +unsigned RingBuffer::size() +{ + return (_buf != nullptr) ? _size : 0; +} + +template +void RingBuffer::flush() +{ + T junk; + while (!empty()) + get(junk); +} + +template +unsigned RingBuffer::_next(unsigned index) +{ + return (0 == index) ? _size : (index - 1); +} + template bool RingBuffer::put(T &val) { @@ -165,12 +213,55 @@ bool RingBuffer::put(const T &val) } } +template +bool RingBuffer::force(T &val) +{ + bool overwrote = false; + + for (;;) { + if (put(val)) + break; + T junk; + get(junk); + overwrote = true; + } + return overwrote; +} + +template +bool RingBuffer::force(const T &val) +{ + bool overwrote = false; + + for (;;) { + if (put(val)) + break; + T junk; + get(junk); + overwrote = true; + } + return overwrote; +} + template bool RingBuffer::get(T &val) { if (_tail != _head) { - val = _buf[_tail]; - _tail = _next(_tail); + unsigned candidate; + unsigned next; + do { + /* decide which element we think we're going to read */ + candidate = _tail; + + /* and what the corresponding next index will be */ + next = _next(candidate); + + /* go ahead and read from this index */ + val = _buf[candidate]; + + /* if the tail pointer didn't change, we got our item */ + } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); + return true; } else { return false; @@ -187,17 +278,30 @@ T RingBuffer::get(void) template unsigned RingBuffer::space(void) { - return (_tail >= _head) ? (_size - (_tail - _head)) : (_head - _tail - 1); + unsigned tail, head; + + /* + * Make a copy of the head/tail pointers in a fashion that + * may err on the side of under-estimating the free space + * in the buffer in the case that the buffer is being updated + * asynchronously with our check. + * If the head pointer changes (reducing space) while copying, + * re-try the copy. + */ + do { + head = _head; + tail = _tail; + } while (head != _head); + + return (tail >= head) ? (_size - (tail - head)) : (head - tail - 1); } template unsigned RingBuffer::count(void) { + /* + * Note that due to the conservative nature of space(), this may + * over-estimate the number of items in the buffer. + */ return _size - space(); } - -template -unsigned RingBuffer::_next(unsigned index) -{ - return (0 == index) ? _size : (index - 1); -} From 3329e3c38c6c566fd8833d862ecf06c07ce4279e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:37:29 +1000 Subject: [PATCH 03/14] ringbuffer: added resize() and print_info() methods this simplifies the drivers --- src/drivers/device/ringbuffer.h | 44 +++++++++++++++++++++++++++++++-- 1 file changed, 42 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index c859be647a..e3c5a20bdd 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -131,9 +131,25 @@ public: */ void flush(); + /* + * resize the buffer. This is unsafe to be called while + * a producer or consuming is running. Caller is responsible + * for any locking needed + * + * @param new_size new size for buffer + * @return true if the resize succeeds, false if + * not (allocation error) + */ + bool resize(unsigned new_size); + + /* + * printf() some info on the buffer + */ + void print_info(const char *name); + private: - T *const _buf; - const unsigned _size; + T *_buf; + unsigned _size; volatile unsigned _head; /**< insertion point */ volatile unsigned _tail; /**< removal point */ @@ -305,3 +321,27 @@ unsigned RingBuffer::count(void) */ return _size - space(); } + +template +bool RingBuffer::resize(unsigned new_size) +{ + T *old_buffer; + T *new_buffer = new T[new_size + 1]; + if (new_buffer == nullptr) { + return false; + } + old_buffer = _buf; + _buf = new_buffer; + _size = new_size; + _head = new_size; + _tail = new_size; + delete[] old_buffer; + return true; +} + +template +void RingBuffer::print_info(const char *name) +{ + printf("%s %u (%u/%u @ %p)\n", + name, _size, _head, _tail, _buf); +} From 3c4526111731ad6701e054f586ae585c9c81f106 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Sep 2013 21:43:24 +1000 Subject: [PATCH 04/14] hmc5883: use a RingBuffer to hold report queue this simplifies the queue handling, and avoids the need for a start()/stop() on queue resize --- src/drivers/hmc5883/hmc5883.cpp | 107 ++++++++++++-------------------- 1 file changed, 40 insertions(+), 67 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 378f433cd1..b838bf16b8 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -65,6 +65,7 @@ #include #include +#include #include #include @@ -148,10 +149,7 @@ private: work_s _work; unsigned _measure_ticks; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - mag_report *_reports; + RingBuffer *_reports; mag_scale _scale; float _range_scale; float _range_ga; @@ -310,9 +308,6 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - /* * Driver 'main' command. */ @@ -322,9 +317,6 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus) : I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), @@ -356,9 +348,8 @@ HMC5883::~HMC5883() /* make sure we are truly inactive */ stop(); - /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; // free perf counters perf_free(_sample_perf); @@ -375,21 +366,18 @@ HMC5883::init() if (I2C::init() != OK) goto out; - /* reset the device configuration */ - reset(); - /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct mag_report[_num_reports]; - + _reports = new RingBuffer(2); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; + /* reset the device configuration */ + reset(); /* get a publish handle on the mag topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]); + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); if (_mag_topic < 0) debug("failed to create sensor_mag object"); @@ -493,6 +481,7 @@ ssize_t HMC5883::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); + struct mag_report *mag_buf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -501,17 +490,15 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is enabled */ if (_measure_ticks > 0) { - /* * While there is space in the caller's buffer, and reports, copy them. * Note that we may be pre-empted by the workq thread while we are doing this; * we are careful to avoid racing with them. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(*mag_buf)) { + ret += sizeof(struct mag_report); + mag_buf++; } } @@ -522,7 +509,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) /* manual measurement - run one conversion */ /* XXX really it'd be nice to lock against other readers here */ do { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -539,10 +526,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) break; } - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - + if (_reports->get(*mag_buf)) { + ret = sizeof(struct mag_report); + } } while (0); return ret; @@ -615,31 +601,22 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000/TICK2USEC(_measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the 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 mag_report *buf = new struct mag_report[arg]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: return reset(); @@ -701,7 +678,7 @@ HMC5883::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1); @@ -810,9 +787,10 @@ HMC5883::collect() perf_begin(_sample_perf); + struct mag_report new_report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); + new_report.timestamp = hrt_absolute_time(); /* * @note We could read the status register here, which could tell us that @@ -853,10 +831,10 @@ HMC5883::collect() * to align the sensor axes with the board, x and y need to be flipped * and y needs to be negated */ - _reports[_next_report].x_raw = report.y; - _reports[_next_report].y_raw = -report.x; + new_report.x_raw = report.y; + new_report.y_raw = -report.x; /* z remains z */ - _reports[_next_report].z_raw = report.z; + new_report.z_raw = report.z; /* scale values for output */ @@ -878,34 +856,30 @@ HMC5883::collect() #ifdef PX4_I2C_BUS_ONBOARD if (_bus == PX4_I2C_BUS_ONBOARD) { /* to align the sensor axes with the board, x and y need to be flipped */ - _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ - _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; } else { #endif /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, * therefore switch x and y and invert y */ - _reports[_next_report].x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ - _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD } #endif /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { + /* post a report to the ring */ + if (_reports->force(new_report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -1224,8 +1198,7 @@ HMC5883::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); } /** From 37d09f09448a274d5f9c55674fd6734709f8a383 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 10:55:22 +1000 Subject: [PATCH 05/14] mpu6000: use a wrapper struct to avoid a linker error the linker doesn't cope with us having multiple modules implementing RingBuffer this also switches to use force() instead of put(), so we discard old entries when the buffer overflows --- src/drivers/mpu6000/mpu6000.cpp | 133 +++++++++++++++----------------- 1 file changed, 64 insertions(+), 69 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 14f8f44b82..81612aee79 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -194,7 +194,14 @@ private: struct hrt_call _call; unsigned _call_interval; - typedef RingBuffer AccelReportBuffer; + /* + these wrapper types are needed to avoid a linker error for + RingBuffer instances which appear in two drivers. + */ + struct _accel_report { + struct accel_report r; + }; + typedef RingBuffer<_accel_report> AccelReportBuffer; AccelReportBuffer *_accel_reports; struct accel_scale _accel_scale; @@ -202,7 +209,10 @@ private: float _accel_range_m_s2; orb_advert_t _accel_topic; - typedef RingBuffer GyroReportBuffer; + struct _gyro_report { + struct gyro_report r; + }; + typedef RingBuffer<_gyro_report> GyroReportBuffer; GyroReportBuffer *_gyro_reports; struct gyro_scale _gyro_scale; @@ -465,16 +475,16 @@ MPU6000::init() if (gyro_ret != OK) { _gyro_topic = -1; } else { - gyro_report gr; + _gyro_report gr; _gyro_reports->get(gr); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr.r); } /* advertise accel topic */ - accel_report ar; + _accel_report ar; _accel_reports->get(ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar.r); out: return ret; @@ -655,7 +665,7 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - accel_report *arp = reinterpret_cast(buffer); + _accel_report *arp = reinterpret_cast<_accel_report *>(buffer); int transferred = 0; while (count--) { if (!_accel_reports->get(*arp++)) @@ -748,7 +758,7 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - gyro_report *arp = reinterpret_cast(buffer); + _gyro_report *arp = reinterpret_cast<_gyro_report *>(buffer); int transferred = 0; while (count--) { if (!_gyro_reports->get(*arp++)) @@ -837,28 +847,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - 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; - _accel_reports = buf; - start(); - - return OK; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -935,21 +936,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - GyroReportBuffer *buf = new GyroReportBuffer(arg); - - if (nullptr == buf) - return -ENOMEM; - if (buf->size() == 0) { - delete buf; + irqstate_t flags = irqsave(); + if (!_gyro_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; } - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete _gyro_reports; - _gyro_reports = buf; - start(); + irqrestore(flags); return OK; } @@ -1197,13 +1189,13 @@ MPU6000::measure() /* * Report buffers. */ - accel_report arb; - gyro_report grb; + _accel_report arb; + _gyro_report grb; /* * Adjust and scale results to m/s^2. */ - grb.timestamp = arb.timestamp = hrt_absolute_time(); + grb.r.timestamp = arb.r.timestamp = hrt_absolute_time(); /* @@ -1224,53 +1216,53 @@ MPU6000::measure() /* NOTE: Axes have been swapped to match the board a few lines above. */ - arb.x_raw = report.accel_x; - arb.y_raw = report.accel_y; - arb.z_raw = report.accel_z; + arb.r.x_raw = report.accel_x; + arb.r.y_raw = report.accel_y; + arb.r.z_raw = report.accel_z; float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - arb.x = _accel_filter_x.apply(x_in_new); - arb.y = _accel_filter_y.apply(y_in_new); - arb.z = _accel_filter_z.apply(z_in_new); + arb.r.x = _accel_filter_x.apply(x_in_new); + arb.r.y = _accel_filter_y.apply(y_in_new); + arb.r.z = _accel_filter_z.apply(z_in_new); - arb.scaling = _accel_range_scale; - arb.range_m_s2 = _accel_range_m_s2; + arb.r.scaling = _accel_range_scale; + arb.r.range_m_s2 = _accel_range_m_s2; - arb.temperature_raw = report.temp; - arb.temperature = (report.temp) / 361.0f + 35.0f; + arb.r.temperature_raw = report.temp; + arb.r.temperature = (report.temp) / 361.0f + 35.0f; - grb.x_raw = report.gyro_x; - grb.y_raw = report.gyro_y; - grb.z_raw = report.gyro_z; + grb.r.x_raw = report.gyro_x; + grb.r.y_raw = report.gyro_y; + grb.r.z_raw = report.gyro_z; float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - grb.x = _gyro_filter_x.apply(x_gyro_in_new); - grb.y = _gyro_filter_y.apply(y_gyro_in_new); - grb.z = _gyro_filter_z.apply(z_gyro_in_new); + grb.r.x = _gyro_filter_x.apply(x_gyro_in_new); + grb.r.y = _gyro_filter_y.apply(y_gyro_in_new); + grb.r.z = _gyro_filter_z.apply(z_gyro_in_new); - grb.scaling = _gyro_range_scale; - grb.range_rad_s = _gyro_range_rad_s; + grb.r.scaling = _gyro_range_scale; + grb.r.range_rad_s = _gyro_range_rad_s; - grb.temperature_raw = report.temp; - grb.temperature = (report.temp) / 361.0f + 35.0f; + grb.r.temperature_raw = report.temp; + grb.r.temperature = (report.temp) / 361.0f + 35.0f; - _accel_reports->put(arb); - _gyro_reports->put(grb); + _accel_reports->force(arb); + _gyro_reports->force(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, &arb); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb.r); if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb.r); } /* stop measuring */ @@ -1280,7 +1272,10 @@ MPU6000::measure() void MPU6000::print_info() { + perf_print_counter(_sample_perf); printf("reads: %u\n", _reads); + _accel_reports->print_info("accel queue"); + _gyro_reports->print_info("gyro queue"); } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : From 815ccee0e7f642b2471084296c893a4dd49e5dfb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:16:12 +1000 Subject: [PATCH 06/14] mpu6000: fixed race condition in buffer increment --- src/drivers/mpu6000/mpu6000.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 81612aee79..66d36826a8 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -668,9 +668,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) _accel_report *arp = reinterpret_cast<_accel_report *>(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(*arp++)) + if (!_accel_reports->get(*arp)) break; transferred++; + arp++; } /* return the number of bytes transferred */ @@ -758,12 +759,13 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - _gyro_report *arp = reinterpret_cast<_gyro_report *>(buffer); + _gyro_report *grp = reinterpret_cast<_gyro_report *>(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(*arp++)) + if (!_gyro_reports->get(*grp)) break; transferred++; + grp++; } /* return the number of bytes transferred */ From 36b7b7bc5f078f373f67cdce3f5c7855c0dcd58b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 16:35:20 +1000 Subject: [PATCH 07/14] airspeed: convert to using RingBuffer class --- src/drivers/airspeed/airspeed.cpp | 74 +++++++++------------ src/drivers/airspeed/airspeed.h | 19 ++++-- src/drivers/ets_airspeed/ets_airspeed.cpp | 28 ++++---- src/drivers/meas_airspeed/meas_airspeed.cpp | 26 +++----- 4 files changed, 67 insertions(+), 80 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 1ec61eb60b..2a6b190de3 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -68,6 +68,7 @@ #include #include +#include #include #include @@ -77,10 +78,8 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), + _max_differential_pressure_pa(0), _sensor_ok(false), _measure_ticks(0), _collect_phase(false), @@ -105,7 +104,7 @@ Airspeed::~Airspeed() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; // free perf counters perf_free(_sample_perf); @@ -123,20 +122,14 @@ Airspeed::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct differential_pressure_s[_num_reports]; - - for (unsigned i = 0; i < _num_reports; i++) - _reports[i].max_differential_pressure_pa = 0; - + _reports = new RingBuffer(2); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; - /* get a publish handle on the airspeed topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]); + differential_pressure_s zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report); if (_airspeed_pub < 0) warnx("failed to create airspeed sensor object. Did you start uOrb?"); @@ -229,31 +222,22 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the 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 differential_pressure_s *buf = new struct differential_pressure_s[arg]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -281,7 +265,8 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t Airspeed::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct differential_pressure_s); + unsigned count = buflen / sizeof(differential_pressure_s); + differential_pressure_s *abuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -297,10 +282,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(*abuf)) { + ret += sizeof(*abuf); + abuf++; } } @@ -309,9 +293,8 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ do { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -329,8 +312,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*abuf)) { + ret = sizeof(*abuf); + } } while (0); @@ -342,7 +326,7 @@ Airspeed::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); @@ -385,6 +369,12 @@ Airspeed::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); warnx("poll interval: %u ticks", _measure_ticks); - warnx("report queue: %u (%u/%u @ %p)", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); +} + +void +Airspeed::new_report(const differential_pressure_s &report) +{ + if (!_reports->force(report)) + perf_count(_buffer_overflows); } diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index b87494b40f..7850ccc7e9 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -68,6 +68,7 @@ #include #include +#include #include #include @@ -102,6 +103,10 @@ public: */ virtual void print_info(); +private: + RingBuffer *_reports; + perf_counter_t _buffer_overflows; + protected: virtual int probe(); @@ -114,10 +119,7 @@ protected: virtual int collect() = 0; work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - differential_pressure_s *_reports; + uint16_t _max_differential_pressure_pa; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -129,7 +131,6 @@ protected: perf_counter_t _sample_perf; perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; /** @@ -162,8 +163,12 @@ protected: */ static void cycle_trampoline(void *arg); + /** + * add a new report to the reports queue + * + * @param report differential_pressure_s report + */ + void new_report(const differential_pressure_s &report); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 257b41935c..dd8436b10e 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -68,6 +68,7 @@ #include #include +#include #include #include @@ -173,27 +174,22 @@ ETSAirspeed::collect() diff_pres_pa -= _diff_pres_offset; } - // XXX we may want to smooth out the readings to remove noise. - - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].differential_pressure_pa = diff_pres_pa; - // Track maximum differential pressure measured (so we can work out top speed). - if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) { - _reports[_next_report].max_differential_pressure_pa = diff_pres_pa; + if (diff_pres_pa > _max_differential_pressure_pa) { + _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.differential_pressure_pa = diff_pres_pa; + report.voltage = 0; + report.max_differential_pressure_pa = _max_differential_pressure_pa; + /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { - perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); - } + new_report(report); /* notify anyone waiting for data */ poll_notify(POLLIN); diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index b1cb2b3d84..03d7bbfb91 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -199,27 +199,23 @@ MEASAirspeed::collect() // Calculate differential pressure. As its centered around 8000 // and can go positive or negative, enforce absolute value uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); - - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].temperature = temperature; - _reports[_next_report].differential_pressure_pa = diff_press_pa; + struct differential_pressure_s report; // Track maximum differential pressure measured (so we can work out top speed). - if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) { - _reports[_next_report].max_differential_pressure_pa = diff_press_pa; + if (diff_press_pa > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_press_pa; } + report.timestamp = hrt_absolute_time(); + report.temperature = temperature; + report.differential_pressure_pa = diff_press_pa; + report.voltage = 0; + report.max_differential_pressure_pa = _max_differential_pressure_pa; + /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { - perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); - } + new_report(report); /* notify anyone waiting for data */ poll_notify(POLLIN); From 63fb702d7fe46619e4315ec5edacb82119ee8c8a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 16:36:07 +1000 Subject: [PATCH 08/14] l3gd20: convert to using RingBuffer class --- src/drivers/l3gd20/l3gd20.cpp | 126 ++++++++++++++-------------------- 1 file changed, 51 insertions(+), 75 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 970e8cf4b4..7cebebeb41 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include @@ -183,11 +184,8 @@ private: struct hrt_call _call; unsigned _call_interval; - - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct gyro_report *_reports; + + RingBuffer *_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -299,16 +297,9 @@ private: int self_test(); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - - L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000), _call_interval(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), @@ -340,7 +331,7 @@ L3GD20::~L3GD20() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; /* delete the perf counter */ perf_free(_sample_perf); @@ -356,16 +347,15 @@ L3GD20::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct gyro_report[_num_reports]; + _reports = new RingBuffer(2); if (_reports == nullptr) goto out; /* advertise sensor topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); + struct gyro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); reset(); @@ -415,6 +405,7 @@ ssize_t L3GD20::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct gyro_report); + struct gyro_report *gbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -430,10 +421,9 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(*gbuf)) { + ret += sizeof(*gbuf); + gbuf++; } } @@ -442,12 +432,13 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_report = _next_report = 0; + _reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*gbuf)) { + ret = sizeof(*gbuf); + } return ret; } @@ -515,31 +506,22 @@ L3GD20::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 < 1) || (arg > 100)) + return -EINVAL; - /* 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[] _reports; - _num_reports = arg; - _reports = buf; - start(); - - return OK; + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: reset(); @@ -708,7 +690,7 @@ L3GD20::start() stop(); /* reset the report ring */ - _oldest_report = _next_report = 0; + _reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this); @@ -768,7 +750,7 @@ L3GD20::measure() } raw_report; #pragma pack(pop) - gyro_report *report = &_reports[_next_report]; + gyro_report report; /* start the performance counter */ perf_begin(_sample_perf); @@ -791,61 +773,56 @@ L3GD20::measure() * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ - report->timestamp = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); switch (_orientation) { case SENSOR_BOARD_ROTATION_000_DEG: /* keep axes in place */ - report->x_raw = raw_report.x; - report->y_raw = raw_report.y; + report.x_raw = raw_report.x; + report.y_raw = raw_report.y; break; case SENSOR_BOARD_ROTATION_090_DEG: /* swap x and y */ - report->x_raw = raw_report.y; - report->y_raw = raw_report.x; + report.x_raw = raw_report.y; + report.y_raw = raw_report.x; break; case SENSOR_BOARD_ROTATION_180_DEG: /* swap x and y and negate both */ - report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); - report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); + report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); break; case SENSOR_BOARD_ROTATION_270_DEG: /* swap x and y and negate y */ - report->x_raw = raw_report.y; - report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + report.x_raw = raw_report.y; + report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); break; } - report->z_raw = raw_report.z; + report.z_raw = raw_report.z; - report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - report->x = _gyro_filter_x.apply(report->x); - report->y = _gyro_filter_y.apply(report->y); - report->z = _gyro_filter_z.apply(report->z); + report.x = _gyro_filter_x.apply(report.x); + report.y = _gyro_filter_y.apply(report.y); + report.z = _gyro_filter_z.apply(report.z); - report->scaling = _gyro_range_scale; - report->range_rad_s = _gyro_range_rad_s; + report.scaling = _gyro_range_scale; + report.range_rad_s = _gyro_range_rad_s; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, fix it */ - if (_next_report == _oldest_report) - INCREMENT(_oldest_report, _num_reports); + _reports->force(report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ if (_gyro_topic > 0) - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); _read++; @@ -858,8 +835,7 @@ L3GD20::print_info() { printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); } int From b8ffb574ca3d2a3bc9b85144bdaa778a47fea809 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 16:36:22 +1000 Subject: [PATCH 09/14] mb12xx: convert to using RingBuffer class --- src/drivers/mb12xx/mb12xx.cpp | 102 +++++++++++++--------------------- 1 file changed, 39 insertions(+), 63 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index f834169935..fabe10b872 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -64,6 +64,7 @@ #include #include +#include #include #include @@ -119,10 +120,7 @@ private: float _min_distance; float _max_distance; work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - range_finder_report *_reports; + RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -183,9 +181,6 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - /* * Driver 'main' command. */ @@ -195,9 +190,6 @@ MB12XX::MB12XX(int bus, int address) : I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), _min_distance(MB12XX_MIN_DISTANCE), _max_distance(MB12XX_MAX_DISTANCE), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), @@ -221,7 +213,7 @@ MB12XX::~MB12XX() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; } int @@ -234,17 +226,15 @@ MB12XX::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct range_finder_report[_num_reports]; + _reports = new RingBuffer(2); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; - /* get a publish handle on the range finder topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]); + struct range_finder_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); if (_range_finder_topic < 0) debug("failed to create sensor_range_finder object. Did you start uOrb?"); @@ -354,31 +344,22 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the 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 range_finder_report *buf = new struct range_finder_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); - - return OK; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -406,6 +387,7 @@ ssize_t MB12XX::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -421,10 +403,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(*rbuf)) { + ret += sizeof(*rbuf); + rbuf++; } } @@ -433,9 +414,8 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ do { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -453,8 +433,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*rbuf)) { + ret = sizeof(*rbuf); + } } while (0); @@ -498,26 +479,25 @@ MB12XX::collect() if (ret < 0) { log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } uint16_t distance = val[0] << 8 | val[1]; float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + struct range_finder_report report; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].distance = si_units; - _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.timestamp = hrt_absolute_time(); + report.distance = si_units; + report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { + if (_reports->force(report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -525,11 +505,8 @@ MB12XX::collect() ret = OK; -out: perf_end(_sample_perf); return ret; - - return ret; } void @@ -537,7 +514,7 @@ MB12XX::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); @@ -626,8 +603,7 @@ MB12XX::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); } /** From 274e3aa2ca073b7fdfd30f270a043fd79954e1d4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:15:19 +1000 Subject: [PATCH 10/14] bma180: convert to using RingBuffer --- src/drivers/bma180/bma180.cpp | 128 +++++++++++++++------------------- 1 file changed, 55 insertions(+), 73 deletions(-) diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index 079b5d21cb..f14c008cea 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -63,6 +63,7 @@ #include #include +#include /* oddly, ERROR is not defined for c++ */ @@ -146,10 +147,14 @@ private: struct hrt_call _call; unsigned _call_interval; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct accel_report *_reports; + /* + this wrapper type is needed to avoid a linker error for + RingBuffer instances which appear in two drivers. + */ + struct _accel_report { + accel_report r; + }; + RingBuffer *_reports; struct accel_scale _accel_scale; float _accel_range_scale; @@ -233,16 +238,9 @@ private: int set_lowpass(unsigned frequency); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - - BMA180::BMA180(int bus, spi_dev_e device) : SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000), _call_interval(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), @@ -270,7 +268,7 @@ BMA180::~BMA180() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; /* delete the perf counter */ perf_free(_sample_perf); @@ -286,16 +284,15 @@ BMA180::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct accel_report[_num_reports]; + _reports = new RingBuffer<_accel_report>(2); if (_reports == nullptr) goto out; /* advertise sensor topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]); + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); /* perform soft reset (p48) */ write_reg(ADDR_RESET, SOFT_RESET); @@ -352,6 +349,7 @@ ssize_t BMA180::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); + struct _accel_report *arp = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -367,10 +365,9 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(*arp)) { + ret += sizeof(*arp); + arp++; } } @@ -379,12 +376,12 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_report = _next_report = 0; + _reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*arp)) + ret = sizeof(*arp); return ret; } @@ -449,31 +446,22 @@ BMA180::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)) - 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[] _reports; - _num_reports = arg; - _reports = buf; - start(); - - return OK; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement */ @@ -654,7 +642,7 @@ BMA180::start() stop(); /* reset the report ring */ - _oldest_report = _next_report = 0; + _reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this); @@ -688,7 +676,7 @@ BMA180::measure() // } raw_report; // #pragma pack(pop) - accel_report *report = &_reports[_next_report]; + struct _accel_report report; /* start the performance counter */ perf_begin(_sample_perf); @@ -708,45 +696,40 @@ BMA180::measure() * them before. There is no good way to synchronise with the internal * measurement flow without using the external interrupt. */ - _reports[_next_report].timestamp = hrt_absolute_time(); + report.r.timestamp = hrt_absolute_time(); /* * y of board is x of sensor and x of board is -y of sensor * perform only the axis assignment here. * Two non-value bits are discarded directly */ - report->y_raw = read_reg(ADDR_ACC_X_LSB + 0); - report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; - report->x_raw = read_reg(ADDR_ACC_X_LSB + 2); - report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; - report->z_raw = read_reg(ADDR_ACC_X_LSB + 4); - report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; + report.r.y_raw = read_reg(ADDR_ACC_X_LSB + 0); + report.r.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; + report.r.x_raw = read_reg(ADDR_ACC_X_LSB + 2); + report.r.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; + report.r.z_raw = read_reg(ADDR_ACC_X_LSB + 4); + report.r.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; /* discard two non-value bits in the 16 bit measurement */ - report->x_raw = (report->x_raw / 4); - report->y_raw = (report->y_raw / 4); - report->z_raw = (report->z_raw / 4); + report.r.x_raw = (report.r.x_raw / 4); + report.r.y_raw = (report.r.y_raw / 4); + report.r.z_raw = (report.r.z_raw / 4); /* invert y axis, due to 14 bit data no overflow can occur in the negation */ - report->y_raw = -report->y_raw; + report.r.y_raw = -report.r.y_raw; - report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - report->scaling = _accel_range_scale; - report->range_m_s2 = _accel_range_m_s2; + report.r.x = ((report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + report.r.y = ((report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + report.r.z = ((report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + report.r.scaling = _accel_range_scale; + report.r.range_m_s2 = _accel_range_m_s2; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, fix it */ - if (_next_report == _oldest_report) - INCREMENT(_oldest_report, _num_reports); + _reports->force(report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &report.r); /* stop the perf counter */ perf_end(_sample_perf); @@ -756,8 +739,7 @@ void BMA180::print_info() { perf_print_counter(_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); } /** From 4b4f4fee5b901da0c93b7fe981426c469840ee64 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:15:39 +1000 Subject: [PATCH 11/14] lsm303d: convert to using RingBuffer --- src/drivers/lsm303d/lsm303d.cpp | 216 +++++++++++++------------------- 1 file changed, 86 insertions(+), 130 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 35904cc4d6..997b80fab2 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -62,6 +62,7 @@ #include #include #include +#include #include #include @@ -218,15 +219,19 @@ private: unsigned _call_accel_interval; unsigned _call_mag_interval; - unsigned _num_accel_reports; - volatile unsigned _next_accel_report; - volatile unsigned _oldest_accel_report; - struct accel_report *_accel_reports; + /* + these wrapper types are needed to avoid a linker error for + RingBuffer instances which appear in two drivers. + */ + struct _accel_report { + struct accel_report r; + }; + RingBuffer *_accel_reports; - unsigned _num_mag_reports; - volatile unsigned _next_mag_report; - volatile unsigned _oldest_mag_report; - struct mag_report *_mag_reports; + struct _mag_report { + struct mag_report r; + }; + RingBuffer *_mag_reports; struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -420,22 +425,12 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - - LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), _mag(new LSM303D_mag(this)), _call_accel_interval(0), _call_mag_interval(0), - _num_accel_reports(0), - _next_accel_report(0), - _oldest_accel_report(0), _accel_reports(nullptr), - _num_mag_reports(0), - _next_mag_report(0), - _oldest_mag_report(0), _mag_reports(nullptr), _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), @@ -480,9 +475,9 @@ LSM303D::~LSM303D() /* free any existing reports */ if (_accel_reports != nullptr) - delete[] _accel_reports; + delete _accel_reports; if (_mag_reports != nullptr) - delete[] _mag_reports; + delete _mag_reports; delete _mag; @@ -504,20 +499,17 @@ LSM303D::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 RingBuffer(2); if (_accel_reports == nullptr) goto out; /* advertise accel topic */ - memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - _num_mag_reports = 2; - _oldest_mag_report = _next_mag_report = 0; - _mag_reports = new struct mag_report[_num_mag_reports]; + _mag_reports = new RingBuffer(2); if (_mag_reports == nullptr) goto out; @@ -525,8 +517,9 @@ LSM303D::init() reset(); /* advertise mag topic */ - memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); + struct mag_report zero_mag_report; + memset(&zero_mag_report, 0, sizeof(zero_mag_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report); /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); @@ -586,6 +579,7 @@ ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); + struct _accel_report *arb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -594,17 +588,13 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is enabled */ if (_call_accel_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 (_accel_reports->get(*arb)) { + ret += sizeof(*arb); + arb++; } } @@ -613,12 +603,11 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) } /* 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); + if (_accel_reports->get(*arb)) + ret = sizeof(*arb); return ret; } @@ -627,6 +616,7 @@ ssize_t LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); + struct _mag_report *mrb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -638,14 +628,11 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) /* * 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_mag_report != _next_mag_report) { - memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports)); - ret += sizeof(_mag_reports[0]); - INCREMENT(_oldest_mag_report, _num_mag_reports); + if (_mag_reports->get(*mrb)) { + ret += sizeof(*mrb); + mrb++; } } @@ -654,12 +641,12 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_mag_report = _next_mag_report = 0; + _mag_reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _mag_reports, sizeof(*_mag_reports)); - ret = sizeof(*_mag_reports); + if (_mag_reports->get(*mrb)) + ret = sizeof(*mrb); return ret; } @@ -727,31 +714,22 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_accel_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]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_accel_reports->resize(arg)) { + irqrestore(flags); 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(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_accel_reports - 1; + return _accel_reports->size(); case SENSORIOCRESET: reset(); @@ -863,31 +841,22 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_mag_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct mag_report *buf = new struct mag_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _mag_reports; - _num_mag_reports = arg; - _mag_reports = buf; - start(); - - return OK; + irqstate_t flags = irqsave(); + if (!_mag_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_mag_reports - 1; + return _mag_reports->size(); case SENSORIOCRESET: reset(); @@ -1220,8 +1189,8 @@ LSM303D::start() stop(); /* reset the report ring */ - _oldest_accel_report = _next_accel_report = 0; - _oldest_mag_report = _next_mag_report = 0; + _accel_reports->flush(); + _mag_reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); @@ -1268,7 +1237,7 @@ LSM303D::measure() } raw_accel_report; #pragma pack(pop) - accel_report *accel_report = &_accel_reports[_next_accel_report]; + struct _accel_report accel_report; /* start the performance counter */ perf_begin(_accel_sample_perf); @@ -1293,35 +1262,30 @@ LSM303D::measure() */ - accel_report->timestamp = hrt_absolute_time(); + accel_report.r.timestamp = hrt_absolute_time(); - accel_report->x_raw = raw_accel_report.x; - accel_report->y_raw = raw_accel_report.y; - accel_report->z_raw = raw_accel_report.z; + accel_report.r.x_raw = raw_accel_report.x; + accel_report.r.y_raw = raw_accel_report.y; + accel_report.r.z_raw = raw_accel_report.z; - float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float x_in_new = ((accel_report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((accel_report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((accel_report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - accel_report->x = _accel_filter_x.apply(x_in_new); - accel_report->y = _accel_filter_y.apply(y_in_new); - accel_report->z = _accel_filter_z.apply(z_in_new); + accel_report.r.x = _accel_filter_x.apply(x_in_new); + accel_report.r.y = _accel_filter_y.apply(y_in_new); + accel_report.r.z = _accel_filter_z.apply(z_in_new); - accel_report->scaling = _accel_range_scale; - accel_report->range_m_s2 = _accel_range_m_s2; + accel_report.r.scaling = _accel_range_scale; + accel_report.r.range_m_s2 = _accel_range_m_s2; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_accel_report, _num_accel_reports); - - /* 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); + _accel_reports->force(accel_report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report.r); _accel_read++; @@ -1343,7 +1307,7 @@ LSM303D::mag_measure() } raw_mag_report; #pragma pack(pop) - mag_report *mag_report = &_mag_reports[_next_mag_report]; + struct _mag_report mag_report; /* start the performance counter */ perf_begin(_mag_sample_perf); @@ -1368,30 +1332,25 @@ LSM303D::mag_measure() */ - mag_report->timestamp = hrt_absolute_time(); + mag_report.r.timestamp = hrt_absolute_time(); - mag_report->x_raw = raw_mag_report.x; - mag_report->y_raw = raw_mag_report.y; - mag_report->z_raw = raw_mag_report.z; - mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; - mag_report->scaling = _mag_range_scale; - mag_report->range_ga = (float)_mag_range_ga; + mag_report.r.x_raw = raw_mag_report.x; + mag_report.r.y_raw = raw_mag_report.y; + mag_report.r.z_raw = raw_mag_report.z; + mag_report.r.x = ((mag_report.r.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.r.y = ((mag_report.r.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.r.z = ((mag_report.r.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + mag_report.r.scaling = _mag_range_scale; + mag_report.r.range_ga = (float)_mag_range_ga; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_mag_report, _num_mag_reports); - - /* if we are running up against the oldest report, fix it */ - if (_next_mag_report == _oldest_mag_report) - INCREMENT(_oldest_mag_report, _num_mag_reports); + _mag_reports->force(mag_report); /* XXX please check this poll_notify, is it the right one? */ /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report.r); _mag_read++; @@ -1405,11 +1364,8 @@ LSM303D::print_info() printf("accel reads: %u\n", _accel_read); printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); - perf_print_counter(_mag_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports); + _accel_reports->print_info("accel reports"); + _mag_reports->print_info("mag reports"); } LSM303D_mag::LSM303D_mag(LSM303D *parent) : From a5821d29281243385363745d1725a6b3210f7f96 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 17:16:38 +1000 Subject: [PATCH 12/14] ms5611: converted to using RingBuffer --- src/drivers/ms5611/ms5611.cpp | 91 +++++++++++++---------------------- 1 file changed, 34 insertions(+), 57 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 4e43f19c5d..3f57cd68f3 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -60,6 +60,7 @@ #include #include #include +#include #include #include @@ -114,10 +115,7 @@ protected: struct work_s _work; unsigned _measure_ticks; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct baro_report *_reports; + RingBuffer *_reports; bool _collect_phase; unsigned _measure_phase; @@ -196,9 +194,6 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _interface(interface), _prom(prom_buf.s), _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _collect_phase(false), _measure_phase(0), @@ -223,7 +218,7 @@ MS5611::~MS5611() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; // free perf counters perf_free(_sample_perf); @@ -246,8 +241,7 @@ MS5611::init() } /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct baro_report[_num_reports]; + _reports = new RingBuffer(2); if (_reports == nullptr) { debug("can't get memory for reports"); @@ -255,11 +249,10 @@ MS5611::init() goto out; } - _oldest_report = _next_report = 0; - /* get a publish handle on the baro topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); + struct baro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report); if (_baro_topic < 0) { debug("failed to create sensor_baro object"); @@ -276,6 +269,7 @@ ssize_t MS5611::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); + struct baro_report *brp = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -291,10 +285,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(*brp)) { + ret += sizeof(*brp); + brp++; } } @@ -303,10 +296,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ do { _measure_phase = 0; - _oldest_report = _next_report = 0; + _reports->flush(); /* do temperature first */ if (OK != measure()) { @@ -335,8 +327,8 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(*brp)) + ret = sizeof(*brp); } while (0); @@ -411,31 +403,21 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the sentinel in the ring */ - arg++; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct baro_report *buf = new struct baro_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start_cycle(); - - return OK; + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -469,7 +451,7 @@ MS5611::start_cycle() /* reset the report ring and state machine */ _collect_phase = false; _measure_phase = 0; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); @@ -588,8 +570,9 @@ MS5611::collect() perf_begin(_sample_perf); + struct baro_report report; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); /* read the most recent measurement - read offset/size are hardcoded in the interface */ ret = _interface->read(0, (void *)&raw, 0); @@ -638,8 +621,8 @@ MS5611::collect() int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; /* generate a new report */ - _reports[_next_report].temperature = _TEMP / 100.0f; - _reports[_next_report].pressure = P / 100.0f; /* convert to millibar */ + report.temperature = _TEMP / 100.0f; + report.pressure = P / 100.0f; /* convert to millibar */ /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ @@ -676,18 +659,13 @@ MS5611::collect() * h = ------------------------------- + h1 * a */ - _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { + if (_reports->force(report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -709,8 +687,7 @@ MS5611::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); printf("TEMP: %d\n", _TEMP); printf("SENS: %lld\n", _SENS); printf("OFF: %lld\n", _OFF); From cefc7ac00e55ade983562a081c3ccda8030e95ce Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 9 Sep 2013 22:23:48 -0700 Subject: [PATCH 13/14] Rework the ringbuffer class so that it's not templated, and refactor its clients so they aren't dancing around the linker anymore. --- src/drivers/airspeed/airspeed.cpp | 12 +- src/drivers/airspeed/airspeed.h | 2 +- src/drivers/bma180/bma180.cpp | 55 ++--- src/drivers/device/ringbuffer.h | 374 +++++++++++++++++++++--------- src/drivers/hmc5883/hmc5883.cpp | 10 +- src/drivers/l3gd20/l3gd20.cpp | 10 +- src/drivers/lsm303d/lsm303d.cpp | 85 +++---- src/drivers/mb12xx/mb12xx.cpp | 10 +- src/drivers/mpu6000/mpu6000.cpp | 94 ++++---- src/drivers/ms5611/ms5611.cpp | 10 +- 10 files changed, 396 insertions(+), 266 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 2a6b190de3..5e45cc936c 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -79,6 +79,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), _reports(nullptr), + _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), _max_differential_pressure_pa(0), _sensor_ok(false), _measure_ticks(0), @@ -87,8 +88,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _airspeed_pub(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), - _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")) + _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // enable debug() calls _debug_enabled = true; @@ -122,7 +122,7 @@ Airspeed::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2); + _reports = new RingBuffer(2, sizeof(differential_pressure_s)); if (_reports == nullptr) goto out; @@ -282,7 +282,7 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_reports->get(*abuf)) { + if (_reports->get(abuf)) { ret += sizeof(*abuf); abuf++; } @@ -312,7 +312,7 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(*abuf)) { + if (_reports->get(abuf)) { ret = sizeof(*abuf); } @@ -375,6 +375,6 @@ Airspeed::print_info() void Airspeed::new_report(const differential_pressure_s &report) { - if (!_reports->force(report)) + if (!_reports->force(&report)) perf_count(_buffer_overflows); } diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 7850ccc7e9..0487848133 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -104,7 +104,7 @@ public: virtual void print_info(); private: - RingBuffer *_reports; + RingBuffer *_reports; perf_counter_t _buffer_overflows; protected: diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index f14c008cea..f0044d36f1 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -147,14 +147,7 @@ private: struct hrt_call _call; unsigned _call_interval; - /* - this wrapper type is needed to avoid a linker error for - RingBuffer instances which appear in two drivers. - */ - struct _accel_report { - accel_report r; - }; - RingBuffer *_reports; + RingBuffer *_reports; struct accel_scale _accel_scale; float _accel_range_scale; @@ -284,7 +277,7 @@ BMA180::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer<_accel_report>(2); + _reports = new RingBuffer(2, sizeof(accel_report)); if (_reports == nullptr) goto out; @@ -349,7 +342,7 @@ ssize_t BMA180::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); - struct _accel_report *arp = reinterpret_cast(buffer); + struct accel_report *arp = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -365,7 +358,7 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_reports->get(*arp)) { + if (_reports->get(arp)) { ret += sizeof(*arp); arp++; } @@ -380,7 +373,7 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_reports->get(*arp)) + if (_reports->get(arp)) ret = sizeof(*arp); return ret; @@ -676,7 +669,7 @@ BMA180::measure() // } raw_report; // #pragma pack(pop) - struct _accel_report report; + struct accel_report report; /* start the performance counter */ perf_begin(_sample_perf); @@ -696,40 +689,40 @@ BMA180::measure() * them before. There is no good way to synchronise with the internal * measurement flow without using the external interrupt. */ - report.r.timestamp = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); /* * y of board is x of sensor and x of board is -y of sensor * perform only the axis assignment here. * Two non-value bits are discarded directly */ - report.r.y_raw = read_reg(ADDR_ACC_X_LSB + 0); - report.r.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; - report.r.x_raw = read_reg(ADDR_ACC_X_LSB + 2); - report.r.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; - report.r.z_raw = read_reg(ADDR_ACC_X_LSB + 4); - report.r.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; + report.y_raw = read_reg(ADDR_ACC_X_LSB + 0); + report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; + report.x_raw = read_reg(ADDR_ACC_X_LSB + 2); + report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; + report.z_raw = read_reg(ADDR_ACC_X_LSB + 4); + report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; /* discard two non-value bits in the 16 bit measurement */ - report.r.x_raw = (report.r.x_raw / 4); - report.r.y_raw = (report.r.y_raw / 4); - report.r.z_raw = (report.r.z_raw / 4); + report.x_raw = (report.x_raw / 4); + report.y_raw = (report.y_raw / 4); + report.z_raw = (report.z_raw / 4); /* invert y axis, due to 14 bit data no overflow can occur in the negation */ - report.r.y_raw = -report.r.y_raw; + report.y_raw = -report.y_raw; - report.r.x = ((report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - report.r.y = ((report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - report.r.z = ((report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - report.r.scaling = _accel_range_scale; - report.r.range_m_s2 = _accel_range_m_s2; + report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + report.scaling = _accel_range_scale; + report.range_m_s2 = _accel_range_m_s2; - _reports->force(report); + _reports->force(&report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &report.r); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); /* stop the perf counter */ perf_end(_sample_perf); diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index e3c5a20bdd..24893b977c 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,15 +34,14 @@ /** * @file ringbuffer.h * - * A simple ringbuffer template. + * A flexible ringbuffer class. */ #pragma once -template class RingBuffer { public: - RingBuffer(unsigned size); + RingBuffer(unsigned ring_size, size_t entry_size); virtual ~RingBuffer(); /** @@ -52,15 +50,18 @@ public: * @param val Item to put * @return true if the item was put, false if the buffer is full */ - bool put(T &val); + bool put(const void *val, size_t val_size = 0); - /** - * Put an item into the buffer if there is space. - * - * @param val Item to put - * @return true if the item was put, false if the buffer is full - */ - bool put(const T &val); + bool put(int8_t val); + bool put(uint8_t val); + bool put(int16_t val); + bool put(uint16_t val); + bool put(int32_t val); + bool put(uint32_t val); + bool put(int64_t val); + bool put(uint64_t val); + bool put(float val); + bool put(double val); /** * Force an item into the buffer, discarding an older item if there is not space. @@ -68,15 +69,18 @@ public: * @param val Item to put * @return true if an item was discarded to make space */ - bool force(T &val); + bool force(const void *val, size_t val_size = 0); - /** - * Force an item into the buffer, discarding an older item if there is not space. - * - * @param val Item to put - * @return true if an item was discarded to make space - */ - bool force(const T &val); + bool force(int8_t val); + bool force(uint8_t val); + bool force(int16_t val); + bool force(uint16_t val); + bool force(int32_t val); + bool force(uint32_t val); + bool force(int64_t val); + bool force(uint64_t val); + bool force(float val); + bool force(double val); /** * Get an item from the buffer. @@ -84,15 +88,18 @@ public: * @param val Item that was gotten * @return true if an item was got, false if the buffer was empty. */ - bool get(T &val); + bool get(void *val, size_t val_size = 0); - /** - * Get an item from the buffer (scalars only). - * - * @return The value that was fetched. If the buffer is empty, - * returns zero. - */ - T get(void); + bool get(int8_t &val); + bool get(uint8_t &val); + bool get(int16_t &val); + bool get(uint16_t &val); + bool get(int32_t &val); + bool get(uint32_t &val); + bool get(int64_t &val); + bool get(uint64_t &val); + bool get(float &val); + bool get(double &val); /* * Get the number of slots free in the buffer. @@ -148,67 +155,68 @@ public: void print_info(const char *name); private: - T *_buf; - unsigned _size; + unsigned _ring_size; + const size_t _item_size; + char *_buf; volatile unsigned _head; /**< insertion point */ volatile unsigned _tail; /**< removal point */ unsigned _next(unsigned index); }; -template -RingBuffer::RingBuffer(unsigned with_size) : - _buf(new T[with_size + 1]), - _size(with_size), - _head(with_size), - _tail(with_size) +RingBuffer::RingBuffer(unsigned ring_size, size_t item_size) : + _ring_size((ring_size + 1) * item_size), + _item_size(item_size), + _buf(new char[_ring_size]), + _head(ring_size), + _tail(ring_size) {} -template -RingBuffer::~RingBuffer() +RingBuffer::~RingBuffer() { if (_buf != nullptr) delete[] _buf; } -template -bool RingBuffer::empty() +unsigned +RingBuffer::_next(unsigned index) +{ + return (0 == index) ? _ring_size : (index - _item_size); +} + +bool +RingBuffer::empty() { return _tail == _head; } -template -bool RingBuffer::full() +bool +RingBuffer::full() { return _next(_head) == _tail; } -template -unsigned RingBuffer::size() +unsigned +RingBuffer::size() { - return (_buf != nullptr) ? _size : 0; + return (_buf != nullptr) ? _ring_size : 0; } -template -void RingBuffer::flush() +void +RingBuffer::flush() { - T junk; while (!empty()) - get(junk); + get(NULL); } -template -unsigned RingBuffer::_next(unsigned index) -{ - return (0 == index) ? _size : (index - 1); -} - -template -bool RingBuffer::put(T &val) +bool +RingBuffer::put(const void *val, size_t val_size) { unsigned next = _next(_head); if (next != _tail) { - _buf[_head] = val; + if ((val_size == 0) || (val_size > _item_size)) + val_size = _item_size; + memcpy(&_buf[_head], val, val_size); _head = next; return true; } else { @@ -216,55 +224,150 @@ bool RingBuffer::put(T &val) } } -template -bool RingBuffer::put(const T &val) +bool +RingBuffer::put(int8_t val) { - unsigned next = _next(_head); - if (next != _tail) { - _buf[_head] = val; - _head = next; - return true; - } else { - return false; - } + return put(&val, sizeof(val)); } -template -bool RingBuffer::force(T &val) +bool +RingBuffer::put(uint8_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(float val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(double val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::force(const void *val, size_t val_size) { bool overwrote = false; for (;;) { - if (put(val)) + if (put(val, val_size)) break; - T junk; - get(junk); + get(NULL); overwrote = true; } return overwrote; } -template -bool RingBuffer::force(const T &val) +bool +RingBuffer::force(int8_t val) { - bool overwrote = false; - - for (;;) { - if (put(val)) - break; - T junk; - get(junk); - overwrote = true; - } - return overwrote; + return force(&val, sizeof(val)); } -template -bool RingBuffer::get(T &val) +bool +RingBuffer::force(uint8_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int32_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint32_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(float val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(double val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::get(void *val, size_t val_size) { if (_tail != _head) { unsigned candidate; unsigned next; + + if ((val_size == 0) || (val_size > _item_size)) + val_size = _item_size; + do { /* decide which element we think we're going to read */ candidate = _tail; @@ -273,7 +376,8 @@ bool RingBuffer::get(T &val) next = _next(candidate); /* go ahead and read from this index */ - val = _buf[candidate]; + if (val != NULL) + memcpy(val, &_buf[candidate], val_size); /* if the tail pointer didn't change, we got our item */ } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); @@ -284,15 +388,68 @@ bool RingBuffer::get(T &val) } } -template -T RingBuffer::get(void) +bool +RingBuffer::get(int8_t &val) { - T val; - return get(val) ? val : 0; + return get(&val, sizeof(val)); } -template -unsigned RingBuffer::space(void) +bool +RingBuffer::get(uint8_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int16_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint16_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(float &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(double &val) +{ + return get(&val, sizeof(val)); +} + +unsigned +RingBuffer::space(void) { unsigned tail, head; @@ -309,39 +466,42 @@ unsigned RingBuffer::space(void) tail = _tail; } while (head != _head); - return (tail >= head) ? (_size - (tail - head)) : (head - tail - 1); + unsigned reported_space = (tail >= head) ? (_ring_size - (tail - head)) : (head - tail - 1); + + return reported_space / _item_size; } -template -unsigned RingBuffer::count(void) +unsigned +RingBuffer::count(void) { /* * Note that due to the conservative nature of space(), this may * over-estimate the number of items in the buffer. */ - return _size - space(); + return (_ring_size / _item_size) - space(); } -template -bool RingBuffer::resize(unsigned new_size) +bool +RingBuffer::resize(unsigned new_size) { - T *old_buffer; - T *new_buffer = new T[new_size + 1]; + unsigned new_ring_size = (new_size + 1) * _item_size; + char *old_buffer; + char *new_buffer = new char [new_ring_size]; if (new_buffer == nullptr) { return false; } old_buffer = _buf; _buf = new_buffer; - _size = new_size; + _ring_size = new_ring_size; _head = new_size; _tail = new_size; delete[] old_buffer; return true; } -template -void RingBuffer::print_info(const char *name) +void +RingBuffer::print_info(const char *name) { - printf("%s %u (%u/%u @ %p)\n", - name, _size, _head, _tail, _buf); + printf("%s %u/%u (%u/%u @ %p)\n", + name, _ring_size/_item_size, _head, _tail, _buf); } diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index b838bf16b8..58a1593eda 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -149,7 +149,7 @@ private: work_s _work; unsigned _measure_ticks; - RingBuffer *_reports; + RingBuffer *_reports; mag_scale _scale; float _range_scale; float _range_ga; @@ -367,7 +367,7 @@ HMC5883::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2); + _reports = new RingBuffer(2, sizeof(mag_report)); if (_reports == nullptr) goto out; @@ -496,7 +496,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_reports->get(*mag_buf)) { + if (_reports->get(mag_buf)) { ret += sizeof(struct mag_report); mag_buf++; } @@ -526,7 +526,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) break; } - if (_reports->get(*mag_buf)) { + if (_reports->get(mag_buf)) { ret = sizeof(struct mag_report); } } while (0); @@ -878,7 +878,7 @@ HMC5883::collect() orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); /* post a report to the ring */ - if (_reports->force(new_report)) { + if (_reports->force(&new_report)) { perf_count(_buffer_overflows); } diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 7cebebeb41..4c3b0ce514 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -185,7 +185,7 @@ private: struct hrt_call _call; unsigned _call_interval; - RingBuffer *_reports; + RingBuffer *_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -347,7 +347,7 @@ L3GD20::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2); + _reports = new RingBuffer(2, sizeof(gyro_report)); if (_reports == nullptr) goto out; @@ -421,7 +421,7 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_reports->get(*gbuf)) { + if (_reports->get(gbuf)) { ret += sizeof(*gbuf); gbuf++; } @@ -436,7 +436,7 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_reports->get(*gbuf)) { + if (_reports->get(gbuf)) { ret = sizeof(*gbuf); } @@ -815,7 +815,7 @@ L3GD20::measure() report.scaling = _gyro_range_scale; report.range_rad_s = _gyro_range_rad_s; - _reports->force(report); + _reports->force(&report); /* notify anyone waiting for data */ poll_notify(POLLIN); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 997b80fab2..a90cd0a3d1 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -219,19 +219,8 @@ private: unsigned _call_accel_interval; unsigned _call_mag_interval; - /* - these wrapper types are needed to avoid a linker error for - RingBuffer instances which appear in two drivers. - */ - struct _accel_report { - struct accel_report r; - }; - RingBuffer *_accel_reports; - - struct _mag_report { - struct mag_report r; - }; - RingBuffer *_mag_reports; + RingBuffer *_accel_reports; + RingBuffer *_mag_reports; struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -499,7 +488,7 @@ LSM303D::init() } /* allocate basic report buffers */ - _accel_reports = new RingBuffer(2); + _accel_reports = new RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; @@ -509,7 +498,7 @@ LSM303D::init() memset(&zero_report, 0, sizeof(zero_report)); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - _mag_reports = new RingBuffer(2); + _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) goto out; @@ -579,7 +568,7 @@ ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); - struct _accel_report *arb = reinterpret_cast(buffer); + accel_report *arb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -592,7 +581,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) * While there is space in the caller's buffer, and reports, copy them. */ while (count--) { - if (_accel_reports->get(*arb)) { + if (_accel_reports->get(arb)) { ret += sizeof(*arb); arb++; } @@ -606,7 +595,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(*arb)) + if (_accel_reports->get(arb)) ret = sizeof(*arb); return ret; @@ -616,7 +605,7 @@ ssize_t LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); - struct _mag_report *mrb = reinterpret_cast(buffer); + mag_report *mrb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -630,7 +619,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) * While there is space in the caller's buffer, and reports, copy them. */ while (count--) { - if (_mag_reports->get(*mrb)) { + if (_mag_reports->get(mrb)) { ret += sizeof(*mrb); mrb++; } @@ -645,7 +634,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_mag_reports->get(*mrb)) + if (_mag_reports->get(mrb)) ret = sizeof(*mrb); return ret; @@ -1237,7 +1226,7 @@ LSM303D::measure() } raw_accel_report; #pragma pack(pop) - struct _accel_report accel_report; + accel_report accel_report; /* start the performance counter */ perf_begin(_accel_sample_perf); @@ -1262,30 +1251,30 @@ LSM303D::measure() */ - accel_report.r.timestamp = hrt_absolute_time(); + accel_report.timestamp = hrt_absolute_time(); - accel_report.r.x_raw = raw_accel_report.x; - accel_report.r.y_raw = raw_accel_report.y; - accel_report.r.z_raw = raw_accel_report.z; + accel_report.x_raw = raw_accel_report.x; + accel_report.y_raw = raw_accel_report.y; + accel_report.z_raw = raw_accel_report.z; - float x_in_new = ((accel_report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((accel_report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((accel_report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - accel_report.r.x = _accel_filter_x.apply(x_in_new); - accel_report.r.y = _accel_filter_y.apply(y_in_new); - accel_report.r.z = _accel_filter_z.apply(z_in_new); + accel_report.x = _accel_filter_x.apply(x_in_new); + accel_report.y = _accel_filter_y.apply(y_in_new); + accel_report.z = _accel_filter_z.apply(z_in_new); - accel_report.r.scaling = _accel_range_scale; - accel_report.r.range_m_s2 = _accel_range_m_s2; + accel_report.scaling = _accel_range_scale; + accel_report.range_m_s2 = _accel_range_m_s2; - _accel_reports->force(accel_report); + _accel_reports->force(&accel_report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report.r); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); _accel_read++; @@ -1307,7 +1296,7 @@ LSM303D::mag_measure() } raw_mag_report; #pragma pack(pop) - struct _mag_report mag_report; + mag_report mag_report; /* start the performance counter */ perf_begin(_mag_sample_perf); @@ -1332,25 +1321,25 @@ LSM303D::mag_measure() */ - mag_report.r.timestamp = hrt_absolute_time(); + mag_report.timestamp = hrt_absolute_time(); - mag_report.r.x_raw = raw_mag_report.x; - mag_report.r.y_raw = raw_mag_report.y; - mag_report.r.z_raw = raw_mag_report.z; - mag_report.r.x = ((mag_report.r.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.r.y = ((mag_report.r.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.r.z = ((mag_report.r.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; - mag_report.r.scaling = _mag_range_scale; - mag_report.r.range_ga = (float)_mag_range_ga; + mag_report.x_raw = raw_mag_report.x; + mag_report.y_raw = raw_mag_report.y; + mag_report.z_raw = raw_mag_report.z; + mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + mag_report.scaling = _mag_range_scale; + mag_report.range_ga = (float)_mag_range_ga; - _mag_reports->force(mag_report); + _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report.r); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); _mag_read++; diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index fabe10b872..ccc5bc15e7 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -120,7 +120,7 @@ private: float _min_distance; float _max_distance; work_s _work; - RingBuffer *_reports; + RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -226,7 +226,7 @@ MB12XX::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2); + _reports = new RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) goto out; @@ -403,7 +403,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_reports->get(*rbuf)) { + if (_reports->get(rbuf)) { ret += sizeof(*rbuf); rbuf++; } @@ -433,7 +433,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(*rbuf)) { + if (_reports->get(rbuf)) { ret = sizeof(*rbuf); } @@ -496,7 +496,7 @@ MB12XX::collect() /* publish it */ orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); - if (_reports->force(report)) { + if (_reports->force(&report)) { perf_count(_buffer_overflows); } diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 66d36826a8..14a3571dec 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -194,26 +194,14 @@ private: struct hrt_call _call; unsigned _call_interval; - /* - these wrapper types are needed to avoid a linker error for - RingBuffer instances which appear in two drivers. - */ - struct _accel_report { - struct accel_report r; - }; - typedef RingBuffer<_accel_report> AccelReportBuffer; - AccelReportBuffer *_accel_reports; + RingBuffer *_accel_reports; struct accel_scale _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - struct _gyro_report { - struct gyro_report r; - }; - typedef RingBuffer<_gyro_report> GyroReportBuffer; - GyroReportBuffer *_gyro_reports; + RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -441,11 +429,11 @@ MPU6000::init() } /* allocate basic report buffers */ - _accel_reports = new AccelReportBuffer(2); + _accel_reports = new RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; - _gyro_reports = new GyroReportBuffer(2); + _gyro_reports = new RingBuffer(2, sizeof(gyro_report)); if (_gyro_reports == nullptr) goto out; @@ -475,16 +463,16 @@ MPU6000::init() if (gyro_ret != OK) { _gyro_topic = -1; } else { - _gyro_report gr; - _gyro_reports->get(gr); + gyro_report gr; + _gyro_reports->get(&gr); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr.r); + _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.r); + accel_report ar; + _accel_reports->get(&ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); out: return ret; @@ -665,10 +653,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - _accel_report *arp = reinterpret_cast<_accel_report *>(buffer); + accel_report *arp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(*arp)) + if (!_accel_reports->get(arp)) break; transferred++; arp++; @@ -759,10 +747,10 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - _gyro_report *grp = reinterpret_cast<_gyro_report *>(buffer); + gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(*grp)) + if (!_gyro_reports->get(grp)) break; transferred++; grp++; @@ -1191,13 +1179,13 @@ MPU6000::measure() /* * Report buffers. */ - _accel_report arb; - _gyro_report grb; + accel_report arb; + gyro_report grb; /* * Adjust and scale results to m/s^2. */ - grb.r.timestamp = arb.r.timestamp = hrt_absolute_time(); + grb.timestamp = arb.timestamp = hrt_absolute_time(); /* @@ -1218,53 +1206,53 @@ MPU6000::measure() /* NOTE: Axes have been swapped to match the board a few lines above. */ - arb.r.x_raw = report.accel_x; - arb.r.y_raw = report.accel_y; - arb.r.z_raw = report.accel_z; + arb.x_raw = report.accel_x; + arb.y_raw = report.accel_y; + arb.z_raw = report.accel_z; float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - arb.r.x = _accel_filter_x.apply(x_in_new); - arb.r.y = _accel_filter_y.apply(y_in_new); - arb.r.z = _accel_filter_z.apply(z_in_new); + arb.x = _accel_filter_x.apply(x_in_new); + arb.y = _accel_filter_y.apply(y_in_new); + arb.z = _accel_filter_z.apply(z_in_new); - arb.r.scaling = _accel_range_scale; - arb.r.range_m_s2 = _accel_range_m_s2; + arb.scaling = _accel_range_scale; + arb.range_m_s2 = _accel_range_m_s2; - arb.r.temperature_raw = report.temp; - arb.r.temperature = (report.temp) / 361.0f + 35.0f; + arb.temperature_raw = report.temp; + arb.temperature = (report.temp) / 361.0f + 35.0f; - grb.r.x_raw = report.gyro_x; - grb.r.y_raw = report.gyro_y; - grb.r.z_raw = report.gyro_z; + grb.x_raw = report.gyro_x; + grb.y_raw = report.gyro_y; + grb.z_raw = report.gyro_z; float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - grb.r.x = _gyro_filter_x.apply(x_gyro_in_new); - grb.r.y = _gyro_filter_y.apply(y_gyro_in_new); - grb.r.z = _gyro_filter_z.apply(z_gyro_in_new); + grb.x = _gyro_filter_x.apply(x_gyro_in_new); + grb.y = _gyro_filter_y.apply(y_gyro_in_new); + grb.z = _gyro_filter_z.apply(z_gyro_in_new); - grb.r.scaling = _gyro_range_scale; - grb.r.range_rad_s = _gyro_range_rad_s; + grb.scaling = _gyro_range_scale; + grb.range_rad_s = _gyro_range_rad_s; - grb.r.temperature_raw = report.temp; - grb.r.temperature = (report.temp) / 361.0f + 35.0f; + grb.temperature_raw = report.temp; + grb.temperature = (report.temp) / 361.0f + 35.0f; - _accel_reports->force(arb); - _gyro_reports->force(grb); + _accel_reports->force(&arb); + _gyro_reports->force(&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, &arb.r); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb.r); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); } /* stop measuring */ diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 3f57cd68f3..1c8a4d776a 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -115,7 +115,7 @@ protected: struct work_s _work; unsigned _measure_ticks; - RingBuffer *_reports; + RingBuffer *_reports; bool _collect_phase; unsigned _measure_phase; @@ -241,7 +241,7 @@ MS5611::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2); + _reports = new RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { debug("can't get memory for reports"); @@ -285,7 +285,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_reports->get(*brp)) { + if (_reports->get(brp)) { ret += sizeof(*brp); brp++; } @@ -327,7 +327,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(*brp)) + if (_reports->get(brp)) ret = sizeof(*brp); } while (0); @@ -664,7 +664,7 @@ MS5611::collect() /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - if (_reports->force(report)) { + if (_reports->force(&report)) { perf_count(_buffer_overflows); } From 760b3ab2e7050154a8ea09fe9358adfed85480ca Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 11 Sep 2013 14:36:44 +1000 Subject: [PATCH 14/14] ringbuffer: converted to item_size units this fixes a number of indexing bugs --- src/drivers/device/ringbuffer.h | 42 +++++++++++++++++---------------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index 24893b977c..a9e22eaa6b 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -155,21 +155,21 @@ public: void print_info(const char *name); private: - unsigned _ring_size; + unsigned _num_items; const size_t _item_size; char *_buf; - volatile unsigned _head; /**< insertion point */ - volatile unsigned _tail; /**< removal point */ + volatile unsigned _head; /**< insertion point in _item_size units */ + volatile unsigned _tail; /**< removal point in _item_size units */ unsigned _next(unsigned index); }; -RingBuffer::RingBuffer(unsigned ring_size, size_t item_size) : - _ring_size((ring_size + 1) * item_size), +RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : + _num_items(num_items), _item_size(item_size), - _buf(new char[_ring_size]), - _head(ring_size), - _tail(ring_size) + _buf(new char[(_num_items+1) * item_size]), + _head(_num_items), + _tail(_num_items) {} RingBuffer::~RingBuffer() @@ -181,7 +181,7 @@ RingBuffer::~RingBuffer() unsigned RingBuffer::_next(unsigned index) { - return (0 == index) ? _ring_size : (index - _item_size); + return (0 == index) ? _num_items : (index - 1); } bool @@ -199,7 +199,7 @@ RingBuffer::full() unsigned RingBuffer::size() { - return (_buf != nullptr) ? _ring_size : 0; + return (_buf != nullptr) ? _num_items : 0; } void @@ -216,7 +216,7 @@ RingBuffer::put(const void *val, size_t val_size) if (next != _tail) { if ((val_size == 0) || (val_size > _item_size)) val_size = _item_size; - memcpy(&_buf[_head], val, val_size); + memcpy(&_buf[_head * _item_size], val, val_size); _head = next; return true; } else { @@ -377,7 +377,7 @@ RingBuffer::get(void *val, size_t val_size) /* go ahead and read from this index */ if (val != NULL) - memcpy(val, &_buf[candidate], val_size); + memcpy(val, &_buf[candidate * _item_size], val_size); /* if the tail pointer didn't change, we got our item */ } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); @@ -466,9 +466,7 @@ RingBuffer::space(void) tail = _tail; } while (head != _head); - unsigned reported_space = (tail >= head) ? (_ring_size - (tail - head)) : (head - tail - 1); - - return reported_space / _item_size; + return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1); } unsigned @@ -478,21 +476,20 @@ RingBuffer::count(void) * Note that due to the conservative nature of space(), this may * over-estimate the number of items in the buffer. */ - return (_ring_size / _item_size) - space(); + return _num_items - space(); } bool RingBuffer::resize(unsigned new_size) { - unsigned new_ring_size = (new_size + 1) * _item_size; char *old_buffer; - char *new_buffer = new char [new_ring_size]; + char *new_buffer = new char [(new_size+1) * _item_size]; if (new_buffer == nullptr) { return false; } old_buffer = _buf; _buf = new_buffer; - _ring_size = new_ring_size; + _num_items = new_size; _head = new_size; _tail = new_size; delete[] old_buffer; @@ -503,5 +500,10 @@ void RingBuffer::print_info(const char *name) { printf("%s %u/%u (%u/%u @ %p)\n", - name, _ring_size/_item_size, _head, _tail, _buf); + name, + _num_items, + _num_items * _item_size, + _head, + _tail, + _buf); }