uavcan-baro: add read()-style interface to baro device

2 files changed, 39 insertions, 7 deletions

diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 5348d4418..26a5fb27e 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -61,6 +61,30 @@ int UavcanBarometerBridge::init()
 	return 0;
 }
 
+ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t buflen)
+{
+	static uint64_t last_read = 0;
+	struct baro_report *baro_buf = reinterpret_cast<struct baro_report *>(buffer);
+
+	/* buffer must be large enough */
+	unsigned count = buflen / sizeof(struct baro_report);
+	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;
+	}
+}
+
 int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
@@ -76,6 +100,14 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case BAROIOCGMSLPRESSURE: {
 		return _msl_pressure;
 	}
+	case SENSORIOCSPOLLRATE: {
+		// not supported yet, pretend that everything is ok
+		return OK;
+	}
+	case SENSORIOCSQUEUEDEPTH: {
+		// not supported yet, pretend that everything is ok
+		return OK;
+	}
 	default: {
 		return CDev::ioctl(filp, cmd, arg);
 	}
@@ -84,11 +116,9 @@ 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)
 {
-	auto report = ::baro_report();
-
-	report.timestamp   = msg.getMonotonicTimestamp().toUSec();
-	report.temperature = msg.static_temperature;
-	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
+	_report.timestamp   = msg.getMonotonicTimestamp().toUSec();
+	_report.temperature = msg.static_temperature;
+	_report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
 
 	/*
 	 * Altitude computation
@@ -102,7 +132,7 @@ 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;
 
-	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 16cd476ea..99e327061 100644
--- a/src/modules/uavcan/sensors/baro.hpp
+++ b/src/modules/uavcan/sensors/baro.hpp
@@ -54,6 +54,7 @@ public:
 	int init() override;
 
 private:
+	ssize_t	read(struct file *filp, char *buffer, size_t buflen);
 	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
 
 	void air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg);
@@ -65,4 +66,5 @@ private:
 
 	uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
 	unsigned _msl_pressure = 101325;
+	baro_report _report =  {};
 };