From 51ffb887c39ecfd0cb94879475f032ee65505321 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 11:13:08 +0100 Subject: UAVCAN: initialize device id for mag and baro to allow DEVIOCGDEVICEID ioctl to return useful data --- src/modules/uavcan/sensors/mag.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules/uavcan/sensors/mag.cpp') diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 8e6a9a22f..0d9ea08c5 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -49,6 +49,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), _sub_mag(node) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; + _scale.x_scale = 1.0F; _scale.y_scale = 1.0F; _scale.z_scale = 1.0F; -- cgit v1.2.3 From 4d489ef7f4c7626a86b700b6d7144e35000ce387 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 11:19:15 +0100 Subject: UAVCAN: improve mag compatibility --- src/modules/uavcan/sensors/mag.cpp | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src/modules/uavcan/sensors/mag.cpp') diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 0d9ea08c5..4f56038b5 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -74,6 +74,9 @@ int UavcanMagnetometerBridge::init() int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { + case SENSORIOCSQUEUEDEPTH: { + return OK; // Pretend that this stuff is supported to keep APM happy + } case MAGIOCSSCALE: { std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); return 0; -- cgit v1.2.3 From 0fa622f22b90048ad366ac304c2c9339a5c376d3 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 11:20:03 +0100 Subject: UAVCAN: declare mag external again to allow different rotation than the internal mag --- src/modules/uavcan/sensors/mag.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/uavcan/sensors/mag.cpp') diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 4f56038b5..4d473291c 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -89,7 +89,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar return 0; // Nothing to do } case MAGIOCGEXTERNAL: { - return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard + return 1; // We don't want anyone to transform the coordinate frame, so we declare it onboard } case MAGIOCSSAMPLERATE: { return 0; // Pretend that this stuff is supported to keep the sensor app happy -- cgit v1.2.3 From 16d74e3c31b9c2cf4f2de254956a58885b5c1fbf Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Wed, 12 Nov 2014 13:14:05 +0100 Subject: UACVAN: add read()-style interface to mag device --- src/modules/uavcan/sensors/mag.cpp | 48 +++++++++++++++++++++++++++++--------- src/modules/uavcan/sensors/mag.hpp | 2 ++ 2 files changed, 39 insertions(+), 11 deletions(-) (limited to 'src/modules/uavcan/sensors/mag.cpp') 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 + 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(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 &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 &msg); @@ -65,4 +66,5 @@ private: uavcan::Subscriber _sub_mag; mag_scale _scale = {}; + mag_report _report = {}; }; -- cgit v1.2.3 From e026324784409bd16a561544cb608e7089d500a0 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Thu, 13 Nov 2014 16:58:41 +0100 Subject: UAVCAN: fix mag report timestamp --- src/modules/uavcan/sensors/mag.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) (limited to 'src/modules/uavcan/sensors/mag.cpp') diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index a529e7e75..5cbb96433 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,6 +37,7 @@ #include "mag.hpp" +#include #include static const orb_id_t MAG_TOPICS[3] = { @@ -139,11 +140,7 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure Date: Tue, 18 Nov 2014 14:58:04 +0100 Subject: UAVCAN: preserve original UAVCAN message timestamps --- src/modules/uavcan/sensors/baro.cpp | 6 +----- src/modules/uavcan/sensors/gnss.cpp | 2 +- src/modules/uavcan/sensors/gnss.hpp | 2 -- src/modules/uavcan/sensors/mag.cpp | 3 +-- 4 files changed, 3 insertions(+), 10 deletions(-) (limited to 'src/modules/uavcan/sensors/mag.cpp') diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 80c5e3828..8741ae20d 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -91,11 +91,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure< { auto report = ::baro_report(); - report.timestamp = msg.getUtcTimestamp().toUSec(); - if (report.timestamp == 0) { - report.timestamp = msg.getMonotonicTimestamp().toUSec(); - } - + report.timestamp = msg.getMonotonicTimestamp().toUSec(); report.temperature = msg.static_temperature; report.pressure = msg.static_pressure / 100.0F; // Convert to millibar diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 24afe6aaf..a375db37f 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -92,7 +92,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure - #include #include diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 5cbb96433..35ebee542 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,7 +37,6 @@ #include "mag.hpp" -#include #include static const orb_id_t MAG_TOPICS[3] = { @@ -140,7 +139,7 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure