Make the buffer ring work.

Avoid reading from the misaligned structure more than once.
Discard some redundant whitespace / prototype.

1 files changed, 10 insertions, 12 deletions

diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp
index 812cf418f..bfdabe273 100644
--- a/apps/drivers/l3gd20/l3gd20.cpp
+++ b/apps/drivers/l3gd20/l3gd20.cpp
@@ -146,9 +146,6 @@ static const int ERROR = -1;
 #define FIFO_CTRL_STREAM_TO_FIFO_MODE		(3<<5)
 #define FIFO_CTRL_BYPASS_TO_STREAM_MODE		(1<<7)
 
-
-
-
 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
 
 class L3GD20 : public device::SPI
@@ -268,12 +265,6 @@ private:
 #define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
 
 
-/*
- * Driver 'main' command.
- */
-extern "C" { int l3gd20_main(int argc, char *argv[]); }
-
-
 L3GD20::L3GD20(int bus, spi_dev_e device) :
 	SPI("L3GD20", GYRO_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
 	_call_interval(0),
@@ -689,12 +680,19 @@ L3GD20::measure()
 	report->y_raw = raw_report.y;
 	report->z_raw = raw_report.z;
 
-	report->x = ((raw_report.x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-	report->y = ((raw_report.y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-	report->z = ((raw_report.z * _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->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);
+
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);