uavcan-baro: use RingBuffer for read() support

2 files changed, 44 insertions, 22 deletions

diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 26a5fb27e..576e758df 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -36,13 +36,15 @@
  */
 
 #include "baro.hpp"
+#include <drivers/device/ringbuffer.h>
 #include <cmath>
 
 const char *const UavcanBarometerBridge::NAME = "baro";
 
 UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
 UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)),
-_sub_air_data(node)
+_sub_air_data(node),
+_reports(nullptr)
 {
 }
 
@@ -53,6 +55,11 @@ int UavcanBarometerBridge::init()
 		return res;
 	}
 
+	/* allocate basic report buffers */
+	_reports = new RingBuffer(2, sizeof(baro_report));
+	if (_reports == nullptr)
+		return -1;
+
 	res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb));
 	if (res < 0) {
 		log("failed to start uavcan sub: %d", res);
@@ -63,26 +70,23 @@ int UavcanBarometerBridge::init()
 
 ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t buflen)
 {
-	static uint64_t last_read = 0;
+	unsigned count = buflen / sizeof(struct baro_report);
 	struct baro_report *baro_buf = reinterpret_cast<struct baro_report *>(buffer);
+	int ret = 0;
 
 	/* buffer must be large enough */
-	unsigned count = buflen / sizeof(struct baro_report);
-	if (count < 1) {
+	if (count < 1)
 		return -ENOSPC;
-	}
 
-	if (last_read < _report.timestamp) {
-		/* copy report */
-		lock();
-		*baro_buf = _report;
-		last_read = _report.timestamp;
-		unlock();
-		return sizeof(struct baro_report);
-	} else {
-		/* no new data available, warn caller */
-		return -EAGAIN;
+	while (count--) {
+		if (_reports->get(baro_buf)) {
+			ret += sizeof(*baro_buf);
+			baro_buf++;
+		}
 	}
+
+	/* if there was no data, warn the caller */
+	return ret ? ret : -EAGAIN;
 }
 
 int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
@@ -105,7 +109,17 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 	}
 	case SENSORIOCSQUEUEDEPTH: {
-		// not supported yet, pretend that everything is 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;
 	}
 	default: {
@@ -116,9 +130,12 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg)
 {
-	_report.timestamp   = msg.getMonotonicTimestamp().toUSec();
-	_report.temperature = msg.static_temperature;
-	_report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
+	baro_report report;
+
+	report.timestamp   = msg.getMonotonicTimestamp().toUSec();
+	report.temperature = msg.static_temperature;
+	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
+	report.error_count = 0;
 
 	/*
 	 * Altitude computation
@@ -132,7 +149,10 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<
 	const double p1 = _msl_pressure / 1000.0;      // current pressure at MSL in kPa
 	const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa
 
-	_report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+	report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+
+	// add to the ring buffer
+	_reports->force(&report);
 
-	publish(msg.getSrcNodeID().get(), &_report);
+	publish(msg.getSrcNodeID().get(), &report);
 }
diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp
index 99e327061..6a39eebfb 100644
--- a/src/modules/uavcan/sensors/baro.hpp
+++ b/src/modules/uavcan/sensors/baro.hpp
@@ -42,6 +42,8 @@
 
 #include <uavcan/equipment/air_data/StaticAirData.hpp>
 
+class RingBuffer;
+
 class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
 {
 public:
@@ -66,5 +68,5 @@ private:
 
 	uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
 	unsigned _msl_pressure = 101325;
-	baro_report _report =  {};
+	RingBuffer	*_reports;
 };