diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 7a6a58746c..b4570e4bbd 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -128,7 +128,7 @@ Airspeed::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(differential_pressure_s)); + _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); if (_reports == nullptr) goto out; diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 3fbf3f9ddf..fad04c9978 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -109,7 +109,7 @@ public: virtual void print_info(); private: - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; perf_counter_t _buffer_overflows; /* this class has pointer data members and should not be copied */ diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index a9e964ef94..c206a5037c 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -41,7 +41,8 @@ SRCS = \ cdev.cpp \ i2c_nuttx.cpp \ pio.cpp \ - spi.cpp + spi.cpp \ + ringbuffer.cpp else SRCS = \ device_posix.cpp \ @@ -50,5 +51,5 @@ SRCS = \ vdev_posix.cpp \ i2c_posix.cpp \ sim.cpp \ - ringbuffer.cpp + ringbuffer.cpp endif diff --git a/src/drivers/device/ringbuffer.cpp b/src/drivers/device/ringbuffer.cpp index 1790289338..c30e0a54df 100644 --- a/src/drivers/device/ringbuffer.cpp +++ b/src/drivers/device/ringbuffer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,6 +40,9 @@ #include "ringbuffer.h" #include +namespace ringbuffer +{ + RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : _num_items(num_items), _item_size(item_size), @@ -57,25 +60,25 @@ RingBuffer::~RingBuffer() unsigned RingBuffer::_next(unsigned index) { - return (0 == index) ? _num_items : (index - 1); + return (0 == index) ? _num_items : (index - 1); } bool RingBuffer::empty() { - return _tail == _head; + return _tail == _head; } bool RingBuffer::full() { - return _next(_head) == _tail; + return _next(_head) == _tail; } unsigned RingBuffer::size() { - return (_buf != nullptr) ? _num_items : 0; + return (_buf != nullptr) ? _num_items : 0; } void @@ -86,7 +89,7 @@ RingBuffer::flush() } bool -RingBuffer::put(const void *val, size_t val_size) +RingBuffer::put(const void *val, size_t val_size) { unsigned next = _next(_head); if (next != _tail) { @@ -250,7 +253,7 @@ static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned #define __PX4_SBCAP __sync_bool_compare_and_swap #endif bool -RingBuffer::get(void *val, size_t val_size) +RingBuffer::get(void *val, size_t val_size) { if (_tail != _head) { unsigned candidate; @@ -340,7 +343,7 @@ RingBuffer::get(double &val) } unsigned -RingBuffer::space(void) +RingBuffer::space(void) { unsigned tail, head; @@ -361,7 +364,7 @@ RingBuffer::space(void) } unsigned -RingBuffer::count(void) +RingBuffer::count(void) { /* * Note that due to the conservative nature of space(), this may @@ -371,7 +374,7 @@ RingBuffer::count(void) } bool -RingBuffer::resize(unsigned new_size) +RingBuffer::resize(unsigned new_size) { char *old_buffer; char *new_buffer = new char [(new_size+1) * _item_size]; @@ -388,13 +391,15 @@ RingBuffer::resize(unsigned new_size) } void -RingBuffer::print_info(const char *name) +RingBuffer::print_info(const char *name) { printf("%s %u/%lu (%u/%u @ %p)\n", - name, - _num_items, - (unsigned long)_num_items * _item_size, - _head, - _tail, + name, + _num_items, + (unsigned long)_num_items * _item_size, + _head, + _tail, _buf); } + +} // namespace ringbuffer diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index 1c65c02135..4fcdaf47fa 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,6 +43,9 @@ #include #include +namespace ringbuffer __EXPORT +{ + class RingBuffer { public: RingBuffer(unsigned ring_size, size_t entry_size); @@ -161,7 +164,7 @@ public: private: unsigned _num_items; const size_t _item_size; - char *_buf; + char *_buf; volatile unsigned _head; /**< insertion point in _item_size units */ volatile unsigned _tail; /**< removal point in _item_size units */ @@ -172,9 +175,4 @@ private: RingBuffer operator=(const RingBuffer&); }; -#ifdef __PX4_NUTTX -// Not sure why NuttX requires these to be defined in the header file -// but on other targets it causes a problem with multiple definitions -// at link time -#include "ringbuffer.cpp" -#endif +} // namespace ringbuffer diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 92ffa80665..2d3be15833 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -156,7 +156,7 @@ private: work_s _work; unsigned _measure_ticks; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; mag_scale _scale; float _range_scale; float _range_ga; @@ -423,7 +423,7 @@ HMC5883::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(mag_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); if (_reports == nullptr) goto out; @@ -921,10 +921,10 @@ HMC5883::collect() _temperature_counter = 0; - ret = _interface->read(ADDR_TEMP_OUT_MSB, + ret = _interface->read(ADDR_TEMP_OUT_MSB, raw_temperature, sizeof(raw_temperature)); if (ret == OK) { - int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + + int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + raw_temperature[1]; new_report.temperature = 25 + (temp16 / (16*8.0f)); _temperature_error_count = 0; @@ -1146,8 +1146,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = -EIO; goto out; } - float cal[3] = {fabsf(expected_cal[0] / report.x), - fabsf(expected_cal[1] / report.y), + float cal[3] = {fabsf(expected_cal[0] / report.x), + fabsf(expected_cal[1] / report.y), fabsf(expected_cal[2] / report.z)}; if (cal[0] > 0.7f && cal[0] < 1.35f && @@ -1381,7 +1381,7 @@ HMC5883::print_info() printf("poll interval: %u ticks\n", _measure_ticks); printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z); printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); - printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", + printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, (double)(1.0f/_range_scale), (double)_range_ga); printf("temperature %.2f\n", (double)_last_report.temperature); @@ -1451,7 +1451,7 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) bus.dev = NULL; return false; } - + int fd = open(bus.devpath, O_RDONLY); if (fd < 0) { return false; @@ -1505,7 +1505,7 @@ struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid) busid == bus_options[i].busid) && bus_options[i].dev != NULL) { return bus_options[i]; } - } + } errx(1, "bus %u not started", (unsigned)busid); } @@ -1750,7 +1750,7 @@ hmc5883_main(int argc, char *argv[]) } } - const char *verb = argv[optind]; + const char *verb = argv[optind]; /* * Start/load the driver. diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index f036987d7d..f276b67f00 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -229,7 +229,7 @@ private: struct hrt_call _call; unsigned _call_interval; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -472,7 +472,7 @@ L3GD20::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(gyro_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); if (_reports == nullptr) goto out; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index e0863f46cd..8a77eeb917 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -275,8 +275,8 @@ private: unsigned _call_accel_interval; unsigned _call_mag_interval; - RingBuffer *_accel_reports; - RingBuffer *_mag_reports; + ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_mag_reports; struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -588,7 +588,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _debug_enabled = true; _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D; - + /* Prime _mag with parents devid. */ _mag->_device_id.devid = _device_id.devid; _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; @@ -646,12 +646,12 @@ LSM303D::init() } /* allocate basic report buffers */ - _accel_reports = new RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; - _mag_reports = new RingBuffer(2, sizeof(mag_report)); + _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) goto out; @@ -1399,9 +1399,9 @@ LSM303D::start() _mag_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_accel_call, + hrt_call_every(&_accel_call, 1000, - _call_accel_interval - LSM303D_TIMER_REDUCTION, + _call_accel_interval - LSM303D_TIMER_REDUCTION, (hrt_callout)&LSM303D::measure_trampoline, this); hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); } diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 1a4ea17cf1..6a1740cb05 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -125,7 +125,7 @@ private: float _min_distance; float _max_distance; work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -255,7 +255,7 @@ MB12XX::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report)); _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 9dd7cc7048..7049bd0788 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -242,7 +242,7 @@ private: struct hrt_call _call; unsigned _call_interval; - RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_accel_reports; struct accel_scale _accel_scale; float _accel_range_scale; @@ -251,7 +251,7 @@ private: int _accel_orb_class_instance; int _accel_class_instance; - RingBuffer *_gyro_reports; + ringbuffer::RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -611,11 +611,11 @@ MPU6000::init() } /* allocate basic report buffers */ - _accel_reports = new RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; - _gyro_reports = new RingBuffer(2, sizeof(gyro_report)); + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); if (_gyro_reports == nullptr) goto out; diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp index 9d0e436eeb..cf96a7a95b 100644 --- a/src/drivers/ms5611/ms5611_nuttx.cpp +++ b/src/drivers/ms5611/ms5611_nuttx.cpp @@ -127,7 +127,7 @@ protected: struct work_s _work; unsigned _measure_ticks; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _collect_phase; unsigned _measure_phase; @@ -269,7 +269,7 @@ MS5611::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(baro_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { debug("can't get memory for reports"); @@ -896,7 +896,7 @@ start_bus(struct ms5611_bus_option &bus) bus.dev = NULL; return false; } - + int fd = open(bus.devpath, O_RDONLY); /* set the poll rate to default, starts automatic data collection */ @@ -955,7 +955,7 @@ struct ms5611_bus_option &find_bus(enum MS5611_BUS busid) busid == bus_options[i].busid) && bus_options[i].dev != NULL) { return bus_options[i]; } - } + } errx(1,"bus %u not started", (unsigned)busid); } diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index b92e950010..3918a6575e 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -121,7 +121,7 @@ private: work_s _work; ///< work queue for scheduling reads bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED uint8_t _num_healthy; ///< number of healthy LEDs - RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs + ringbuffer::RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs uint64_t _last_gencall; uint64_t _start_time; ///< system time we first attempt to communicate with battery }; @@ -176,7 +176,7 @@ OREOLED::init() } /* allocate command queue */ - _cmd_queue = new RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t)); + _cmd_queue = new ringbuffer::RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t)); if (_cmd_queue == nullptr) { return ENOTTY; diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index f35957caae..319c902f12 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -120,16 +120,16 @@ protected: private: work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; - bool _collect_phase; + bool _collect_phase; orb_advert_t _px4flow_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - + enum Rotation _sensor_rotation; /** @@ -217,7 +217,7 @@ PX4FLOW::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(optical_flow_s)); + _reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s)); if (_reports == nullptr) { goto out; @@ -486,10 +486,10 @@ PX4FLOW::collect() report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; - + /* rotate measurements according to parameter */ float zeroval = 0.0f; - rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index c45e73fd9a..8c11e4c14d 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -118,7 +118,7 @@ private: float _min_distance; float _max_distance; work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -270,7 +270,7 @@ SF0X::init() if (ret != OK) break; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) { warnx("mem err"); ret = -1; diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index a3c8633725..58799b46ac 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -82,7 +82,7 @@ #define TRONE_WHO_AM_I_REG 0x01 /* Who am I test register */ #define TRONE_WHO_AM_I_REG_VAL 0xA1 - + /* Device limits */ #define TRONE_MIN_DISTANCE (0.20f) #define TRONE_MAX_DISTANCE (14.00f) @@ -122,7 +122,7 @@ private: float _min_distance; float _max_distance; work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -281,7 +281,7 @@ TRONE::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) { goto out; diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 576e758df5..9c3a0aefb4 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -36,7 +36,6 @@ */ #include "baro.hpp" -#include #include const char *const UavcanBarometerBridge::NAME = "baro"; @@ -56,7 +55,7 @@ int UavcanBarometerBridge::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(baro_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) return -1; diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 6a39eebfb6..b5da6985a7 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -39,11 +39,10 @@ #include "sensor_bridge.hpp" #include +#include #include -class RingBuffer; - class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase { public: @@ -68,5 +67,5 @@ private: uavcan::Subscriber _sub_air_data; unsigned _msl_pressure = 101325; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; };