diff options
author | Holger Steinhaus <holger@steinhaus-home.de> | 2014-11-12 13:14:05 +0100 |
---|---|---|
committer | Holger Steinhaus <holger@steinhaus-home.de> | 2014-11-13 16:17:35 +0100 |
commit | 16d74e3c31b9c2cf4f2de254956a58885b5c1fbf (patch) | |
tree | 9cdf9ebc24d8c77366e337cff45e177ac4ff459c | |
parent | 0fa622f22b90048ad366ac304c2c9339a5c376d3 (diff) | |
download | px4-firmware-16d74e3c31b9c2cf4f2de254956a58885b5c1fbf.tar.gz px4-firmware-16d74e3c31b9c2cf4f2de254956a58885b5c1fbf.tar.bz2 px4-firmware-16d74e3c31b9c2cf4f2de254956a58885b5c1fbf.zip |
UACVAN: add read()-style interface to mag device
-rw-r--r-- | src/modules/uavcan/sensors/mag.cpp | 48 | ||||
-rw-r--r-- | src/modules/uavcan/sensors/mag.hpp | 2 |
2 files changed, 39 insertions, 11 deletions
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 4d473291c..a529e7e75 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,6 +37,8 @@ #include "mag.hpp" +#include <systemlib/err.h> + static const orb_id_t MAG_TOPICS[3] = { ORB_ID(sensor_mag0), ORB_ID(sensor_mag1), @@ -71,6 +73,30 @@ int UavcanMagnetometerBridge::init() return 0; } +ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen) +{ + static uint64_t last_read = 0; + struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer); + + /* buffer must be large enough */ + unsigned count = buflen / sizeof(struct mag_report); + if (count < 1) { + return -ENOSPC; + } + + if (last_read < _report.timestamp) { + /* copy report */ + lock(); + *mag_buf = _report; + last_read = _report.timestamp; + unlock(); + return sizeof(struct mag_report); + } else { + /* no new data available, warn caller */ + return -EAGAIN; + } +} + int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -89,7 +115,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar return 0; // Nothing to do } case MAGIOCGEXTERNAL: { - return 1; // We don't want anyone to transform the coordinate frame, so we declare it onboard + return 1; // declare it external rise it's priority and to allow for correct orientation compensation } case MAGIOCSSAMPLERATE: { return 0; // Pretend that this stuff is supported to keep the sensor app happy @@ -111,18 +137,18 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg) { - auto report = ::mag_report(); - - report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything + lock(); + _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything - report.timestamp = msg.getUtcTimestamp().toUSec(); - if (report.timestamp == 0) { - report.timestamp = msg.getMonotonicTimestamp().toUSec(); + _report.timestamp = msg.getUtcTimestamp().toUSec(); + if (_report.timestamp == 0) { + _report.timestamp = msg.getMonotonicTimestamp().toUSec(); } - report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; - report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; - report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + _report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; + _report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; + _report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + unlock(); - publish(msg.getSrcNodeID().get(), &report); + publish(msg.getSrcNodeID().get(), &_report); } diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 6d413a8f7..74077d883 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.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 mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg); @@ -65,4 +66,5 @@ private: uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag; mag_scale _scale = {}; + mag_report _report = {}; }; |