Commit 473d321a authored by Pavel Kirienko's avatar Pavel Kirienko Committed by Lorenz Meier
Browse files

UAVCAN Magnetometer driver update - no logical changes

parent 4ec16efe
......@@ -45,7 +45,7 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
_sub_mag(node)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // <-- Why?
_scale.x_scale = 1.0F;
_scale.y_scale = 1.0F;
......@@ -141,16 +141,16 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
}
}
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer>
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
&msg)
{
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_ga[0] - _scale.x_offset) * _scale.x_scale;
_report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale;
_report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale;
unlock();
publish(msg.getSrcNodeID().get(), &_report);
......
......@@ -40,7 +40,7 @@
#include "sensor_bridge.hpp"
#include <drivers/drv_mag.h>
#include <uavcan/equipment/ahrs/Magnetometer.hpp>
#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
{
......@@ -57,14 +57,14 @@ 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);
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &msg);
typedef uavcan::MethodBinder < UavcanMagnetometerBridge *,
void (UavcanMagnetometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &) >
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &) >
MagCbBinder;
uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength, MagCbBinder> _sub_mag;
mag_scale _scale = {};
mag_report _report = {};
};
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment