1 files changed, 40 insertions, 13 deletions

diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 8e6a9a22f..35ebee542 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),
@@ -49,6 +51,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;
@@ -69,9 +73,36 @@ 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) {
+	case SENSORIOCSQUEUEDEPTH: {
+		return OK;			// Pretend that this stuff is supported to keep APM happy
+	}
 	case MAGIOCSSCALE: {
 		std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
 		return 0;
@@ -84,7 +115,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;           // 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
@@ -106,18 +137,14 @@ 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
-
-	report.timestamp = msg.getUtcTimestamp().toUSec();
-	if (report.timestamp == 0) {
-		report.timestamp = msg.getMonotonicTimestamp().toUSec();
-	}
+	lock();
+	_report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
+	_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);
 }