diff options
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/airspeed/airspeed.cpp | 12 | ||||
-rw-r--r-- | src/drivers/airspeed/airspeed.h | 2 | ||||
-rw-r--r-- | src/drivers/bma180/bma180.cpp | 55 | ||||
-rw-r--r-- | src/drivers/device/ringbuffer.h | 378 | ||||
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 10 | ||||
-rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 10 | ||||
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 85 | ||||
-rw-r--r-- | src/drivers/mb12xx/mb12xx.cpp | 10 | ||||
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 94 | ||||
-rw-r--r-- | src/drivers/ms5611/ms5611.cpp | 10 |
10 files changed, 398 insertions, 268 deletions
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 2a6b190de..5e45cc936 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<differential_pressure_s>(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 7850ccc7e..048784813 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -104,7 +104,7 @@ public: virtual void print_info(); private: - RingBuffer<differential_pressure_s> *_reports; + RingBuffer *_reports; perf_counter_t _buffer_overflows; protected: diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index f14c008ce..f0044d36f 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<struct _accel_report> *_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<struct _accel_report *>(buffer); + struct accel_report *arp = reinterpret_cast<struct accel_report *>(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 e3c5a20bd..24893b977 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 <lm@inf.ethz.ch> * * 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<typename T> 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); - - /** - * 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(const void *val, size_t val_size = 0); + + 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); - - /** - * 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(const void *val, size_t val_size = 0); + + 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); - - /** - * 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(void *val, size_t val_size = 0); + + 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 <typename T> -RingBuffer<T>::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 <typename T> -RingBuffer<T>::~RingBuffer() +RingBuffer::~RingBuffer() { if (_buf != nullptr) delete[] _buf; } -template <typename T> -bool RingBuffer<T>::empty() +unsigned +RingBuffer::_next(unsigned index) { - return _tail == _head; + return (0 == index) ? _ring_size : (index - _item_size); } -template <typename T> -bool RingBuffer<T>::full() +bool +RingBuffer::empty() { - return _next(_head) == _tail; + return _tail == _head; } -template <typename T> -unsigned RingBuffer<T>::size() +bool +RingBuffer::full() { - return (_buf != nullptr) ? _size : 0; + return _next(_head) == _tail; } -template <typename T> -void RingBuffer<T>::flush() +unsigned +RingBuffer::size() { - T junk; - while (!empty()) - get(junk); + return (_buf != nullptr) ? _ring_size : 0; } -template <typename T> -unsigned RingBuffer<T>::_next(unsigned index) +void +RingBuffer::flush() { - return (0 == index) ? _size : (index - 1); + while (!empty()) + get(NULL); } -template <typename T> -bool RingBuffer<T>::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<T>::put(T &val) } } -template <typename T> -bool RingBuffer<T>::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 <typename T> -bool RingBuffer<T>::force(T &val) +bool +RingBuffer::put(uint8_t val) { - bool overwrote = false; + return put(&val, sizeof(val)); +} - for (;;) { - if (put(val)) - break; - T junk; - get(junk); - overwrote = true; - } - return overwrote; +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)); } -template <typename T> -bool RingBuffer<T>::force(const T &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 <typename T> -bool RingBuffer<T>::get(T &val) +bool +RingBuffer::force(int8_t val) +{ + return force(&val, sizeof(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<T>::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<T>::get(T &val) } } -template <typename T> -T RingBuffer<T>::get(void) +bool +RingBuffer::get(int8_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint8_t &val) { - T val; - return get(val) ? val : 0; + return get(&val, sizeof(val)); } -template <typename T> -unsigned RingBuffer<T>::space(void) +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<T>::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 <typename T> -unsigned RingBuffer<T>::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 <typename T> -bool RingBuffer<T>::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 <typename T> -void RingBuffer<T>::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 b838bf16b..58a1593ed 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<struct mag_report> *_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<struct mag_report>(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 7cebebeb4..4c3b0ce51 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<gyro_report> *_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<struct gyro_report>(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 997b80fab..a90cd0a3d 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<struct _accel_report> *_accel_reports; - - struct _mag_report { - struct mag_report r; - }; - RingBuffer<struct _mag_report> *_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<struct _accel_report>(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<struct _mag_report>(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<struct _accel_report *>(buffer); + accel_report *arb = reinterpret_cast<accel_report *>(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<struct _mag_report *>(buffer); + mag_report *mrb = reinterpret_cast<mag_report *>(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 fabe10b87..ccc5bc15e 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<struct range_finder_report> *_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<struct range_finder_report>(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 66d36826a..14a3571de 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<accel_report *>(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<gyro_report *>(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 3f57cd68f..1c8a4d776 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<struct baro_report> *_reports; + RingBuffer *_reports; bool _collect_phase; unsigned _measure_phase; @@ -241,7 +241,7 @@ MS5611::init() } /* allocate basic report buffers */ - _reports = new RingBuffer<struct baro_report>(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); } |