diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/uavcan/sensors/baro.cpp | 62 | ||||
-rw-r--r-- | src/modules/uavcan/sensors/baro.hpp | 4 |
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; }; |