diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-10 12:17:28 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-10 12:17:28 +0200 |
commit | b32f79fc99cd81ed301f9c30e60af1fde05377df (patch) | |
tree | 07e14b5e4ea1975297ba18fbcea54c680bdf07c2 /src | |
parent | 12e4e393bdedc4a8f929bff8bff73b00e35752dd (diff) | |
parent | ba712e1a22b4b47b925471d9350ebfdb4e6b8c69 (diff) | |
download | px4-firmware-b32f79fc99cd81ed301f9c30e60af1fde05377df.tar.gz px4-firmware-b32f79fc99cd81ed301f9c30e60af1fde05377df.tar.bz2 px4-firmware-b32f79fc99cd81ed301f9c30e60af1fde05377df.zip |
Merge branch 'ringbuffer' of github.com:PX4/Firmware into fixedwing_l1
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/airspeed/airspeed.cpp | 74 | ||||
-rw-r--r-- | src/drivers/airspeed/airspeed.h | 19 | ||||
-rw-r--r-- | src/drivers/bma180/bma180.cpp | 128 | ||||
-rw-r--r-- | src/drivers/device/ringbuffer.h | 172 | ||||
-rw-r--r-- | src/drivers/ets_airspeed/ets_airspeed.cpp | 28 | ||||
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 111 | ||||
-rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 128 | ||||
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 216 | ||||
-rw-r--r-- | src/drivers/mb12xx/mb12xx.cpp | 102 | ||||
-rw-r--r-- | src/drivers/meas_airspeed/meas_airspeed.cpp | 26 | ||||
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 139 | ||||
-rw-r--r-- | src/drivers/ms5611/ms5611.cpp | 91 | ||||
-rw-r--r-- | src/drivers/rgbled/rgbled.cpp | 6 |
13 files changed, 605 insertions, 635 deletions
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 1ec61eb60..2a6b190de 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -68,6 +68,7 @@ #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/differential_pressure.h> @@ -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<differential_pressure_s>(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<differential_pressure_s *>(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 b87494b40..7850ccc7e 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -68,6 +68,7 @@ #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/differential_pressure.h> @@ -102,6 +103,10 @@ public: */ virtual void print_info(); +private: + RingBuffer<differential_pressure_s> *_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/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index 079b5d21c..f14c008ce 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -63,6 +63,7 @@ #include <drivers/device/spi.h> #include <drivers/drv_accel.h> +#include <drivers/device/ringbuffer.h> /* 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<struct _accel_report> *_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<struct _accel_report *>(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->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.y_raw = -report.r.y_raw; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + 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; - /* 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"); } /** diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index dc0c84052..e3c5a20bd 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -55,7 +55,7 @@ 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 @@ -63,6 +63,22 @@ public: 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. * * @param val Item that was gotten @@ -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,27 +113,43 @@ 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(); + + /* + * 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 */ @@ -140,6 +172,38 @@ RingBuffer<T>::~RingBuffer() } template <typename T> +bool RingBuffer<T>::empty() +{ + return _tail == _head; +} + +template <typename T> +bool RingBuffer<T>::full() +{ + return _next(_head) == _tail; +} + +template <typename T> +unsigned RingBuffer<T>::size() +{ + return (_buf != nullptr) ? _size : 0; +} + +template <typename T> +void RingBuffer<T>::flush() +{ + T junk; + while (!empty()) + get(junk); +} + +template <typename T> +unsigned RingBuffer<T>::_next(unsigned index) +{ + return (0 == index) ? _size : (index - 1); +} + +template <typename T> bool RingBuffer<T>::put(T &val) { unsigned next = _next(_head); @@ -166,11 +230,54 @@ bool RingBuffer<T>::put(const T &val) } template <typename T> +bool RingBuffer<T>::force(T &val) +{ + bool overwrote = false; + + for (;;) { + if (put(val)) + break; + T junk; + get(junk); + overwrote = true; + } + return overwrote; +} + +template <typename T> +bool RingBuffer<T>::force(const T &val) +{ + bool overwrote = false; + + for (;;) { + if (put(val)) + break; + T junk; + get(junk); + overwrote = true; + } + return overwrote; +} + +template <typename T> bool RingBuffer<T>::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 +294,54 @@ T RingBuffer<T>::get(void) template <typename T> unsigned RingBuffer<T>::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 <typename T> unsigned RingBuffer<T>::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 <typename T> -unsigned RingBuffer<T>::_next(unsigned index) +bool RingBuffer<T>::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 <typename T> +void RingBuffer<T>::print_info(const char *name) { - return (0 == index) ? _size : (index - 1); + printf("%s %u (%u/%u @ %p)\n", + name, _size, _head, _tail, _buf); } diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 257b41935..dd8436b10 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -68,6 +68,7 @@ #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/differential_pressure.h> @@ -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; } - /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + // 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; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + /* announce the airspeed if needed, just publish else */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); - /* 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/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 0de82c304..b838bf16b 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -65,6 +65,7 @@ #include <drivers/drv_mag.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/subsystem_info.h> @@ -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<struct mag_report> *_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<struct mag_report>(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<struct mag_report *>(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 @@ -842,8 +820,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 @@ -851,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 == -32768) ? 32767 : -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 */ @@ -876,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 == -32768) ? 32767 : -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 == -32768) ? 32767 : -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]); - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); - /* 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 */ @@ -1222,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"); } /** diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index e6d765e13..7fb1cbbbc 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -61,6 +61,7 @@ #include <drivers/drv_hrt.h> #include <drivers/device/spi.h> #include <drivers/drv_gyro.h> +#include <drivers/device/ringbuffer.h> #include <board_config.h> #include <mathlib/math/filter/LowPassFilter2p.hpp> @@ -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<gyro_report> *_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<struct gyro_report>(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(); @@ -406,6 +396,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<struct gyro_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -421,10 +412,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++; } } @@ -433,12 +423,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; } @@ -506,31 +497,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 < 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; + /* 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: reset(); @@ -699,7 +681,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); @@ -759,7 +741,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); @@ -782,61 +764,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->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.z_raw = raw_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.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->scaling = _gyro_range_scale; - report->range_rad_s = _gyro_range_rad_s; + report.x = _gyro_filter_x.apply(report.x); + report.y = _gyro_filter_y.apply(report.y); + report.z = _gyro_filter_z.apply(report.z); - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + report.scaling = _gyro_range_scale; + report.range_rad_s = _gyro_range_rad_s; - /* 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++; @@ -849,8 +826,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 diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 05d6f1881..947e17712 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -62,6 +62,7 @@ #include <drivers/device/spi.h> #include <drivers/drv_accel.h> #include <drivers/drv_mag.h> +#include <drivers/device/ringbuffer.h> #include <board_config.h> #include <mathlib/math/filter/LowPassFilter2p.hpp> @@ -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<struct _accel_report> *_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<struct _mag_report> *_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; @@ -502,20 +497,17 @@ LSM303D::init() goto out; /* 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<struct _accel_report>(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<struct _mag_report>(2); if (_mag_reports == nullptr) goto out; @@ -523,8 +515,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(); @@ -577,6 +570,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); int ret = 0; /* buffer must be large enough */ @@ -585,17 +579,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++; } } @@ -604,12 +594,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; } @@ -618,6 +607,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); int ret = 0; /* buffer must be large enough */ @@ -629,14 +619,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++; } } @@ -645,12 +632,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; } @@ -718,31 +705,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(); @@ -854,31 +832,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 < 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(); + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; - 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(); @@ -1211,8 +1180,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); @@ -1259,7 +1228,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); @@ -1284,35 +1253,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++; @@ -1334,7 +1298,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); @@ -1359,30 +1323,25 @@ LSM303D::mag_measure() */ - mag_report->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.timestamp = hrt_absolute_time(); - /* post a report to the ring - note, not locked */ - INCREMENT(_next_mag_report, _num_mag_reports); + 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; - /* 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++; @@ -1396,11 +1355,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) : diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index f83416993..fabe10b87 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -64,6 +64,7 @@ #include <drivers/drv_hrt.h> #include <drivers/drv_range_finder.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/subsystem_info.h> @@ -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<struct range_finder_report> *_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<struct range_finder_report>(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<struct range_finder_report *>(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]); - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); - /* 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"); } /** diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index b1cb2b3d8..03d7bbfb9 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; } - /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + 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; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + /* announce the airspeed if needed, just publish else */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); - /* 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/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 14f8f44b8..66d36826a 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<accel_report> 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<gyro_report> 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,12 +665,13 @@ 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++; } /* return the number of bytes transferred */ @@ -748,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 */ @@ -837,28 +849,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 +938,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 +1191,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 +1218,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 +1274,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) : diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 4e43f19c5..3f57cd68f 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -60,6 +60,7 @@ #include <drivers/device/device.h> #include <drivers/drv_baro.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <systemlib/perf_counter.h> #include <systemlib/err.h> @@ -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<struct baro_report> *_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<struct baro_report>(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<struct baro_report *>(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 < 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(); + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; - 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]); - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - /* 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); diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index feb8f1c6c..ee1d472a2 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -590,9 +590,9 @@ rgbled_main(int argc, char *argv[]) errx(1, "Usage: rgbled rgb <red> <green> <blue>"); } rgbled_rgbset_t v; - v.red = strtol(argv[1], NULL, 0); - v.green = strtol(argv[2], NULL, 0); - v.blue = strtol(argv[3], NULL, 0); + v.red = strtol(argv[2], NULL, 0); + v.green = strtol(argv[3], NULL, 0); + v.blue = strtol(argv[4], NULL, 0); ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); close(fd); exit(ret); |