diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index e6d765e13a..7fb1cbbbc8 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include @@ -183,11 +184,8 @@ private: struct hrt_call _call; unsigned _call_interval; - - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct gyro_report *_reports; + + RingBuffer *_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -299,16 +297,9 @@ private: int self_test(); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - - L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000), _call_interval(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), @@ -340,7 +331,7 @@ L3GD20::~L3GD20() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; /* delete the perf counter */ perf_free(_sample_perf); @@ -356,16 +347,15 @@ L3GD20::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct gyro_report[_num_reports]; + _reports = new RingBuffer(2); if (_reports == nullptr) goto out; /* advertise sensor topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); + struct gyro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); reset(); @@ -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(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 < 1) || (arg > 100)) + return -EINVAL; - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct gyro_report *buf = new struct gyro_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); - - return OK; + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: reset(); @@ -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.z_raw = raw_report.z; - report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - report->x = _gyro_filter_x.apply(report->x); - report->y = _gyro_filter_y.apply(report->y); - report->z = _gyro_filter_z.apply(report->z); + report.x = _gyro_filter_x.apply(report.x); + report.y = _gyro_filter_y.apply(report.y); + report.z = _gyro_filter_z.apply(report.z); - report->scaling = _gyro_range_scale; - report->range_rad_s = _gyro_range_rad_s; + report.scaling = _gyro_range_scale; + report.range_rad_s = _gyro_range_rad_s; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, fix it */ - if (_next_report == _oldest_report) - INCREMENT(_oldest_report, _num_reports); + _reports->force(report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ if (_gyro_topic > 0) - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); _read++; @@ -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