diff options
author | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-08-23 23:14:59 +0400 |
---|---|---|
committer | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-08-23 23:14:59 +0400 |
commit | 4e0d7c6b0e52d3eecba65f4415d4c7372dfd8a49 (patch) | |
tree | aaa6c0ead7eb140d0943e3a5737ec24ee939c9f9 /src/modules/uavcan/sensors/mag.cpp | |
parent | 6a8971e28f492073a951d96065df30034853bea7 (diff) | |
download | px4-firmware-4e0d7c6b0e52d3eecba65f4415d4c7372dfd8a49.tar.gz px4-firmware-4e0d7c6b0e52d3eecba65f4415d4c7372dfd8a49.tar.bz2 px4-firmware-4e0d7c6b0e52d3eecba65f4415d4c7372dfd8a49.zip |
UAVCAN: redundant sensors support
Diffstat (limited to 'src/modules/uavcan/sensors/mag.cpp')
-rw-r--r-- | src/modules/uavcan/sensors/mag.cpp | 51 |
1 files changed, 8 insertions, 43 deletions
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 4f8a5e104..aaa3a4463 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,10 +37,16 @@ #include "mag.hpp" +static const orb_id_t MAG_TOPICS[3] = { + ORB_ID(sensor_mag0), + ORB_ID(sensor_mag1), + ORB_ID(sensor_mag2) +}; + const char *const UavcanMagnetometerBridge::NAME = "mag"; UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : -device::CDev("uavcan_mag", "/dev/uavcan/mag"), +UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), _sub_mag(node) { _scale.x_scale = 1.0F; @@ -48,45 +54,13 @@ _sub_mag(node) _scale.z_scale = 1.0F; } -UavcanMagnetometerBridge::~UavcanMagnetometerBridge() -{ - if (_class_instance > 0) { - (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); - } -} - int UavcanMagnetometerBridge::init() { - // Init the libuavcan subscription int res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); return res; } - - // Detect our device class - _class_instance = register_class_devname(MAG_DEVICE_PATH); - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: { - _orb_id = ORB_ID(sensor_mag0); - break; - } - case CLASS_DEVICE_SECONDARY: { - _orb_id = ORB_ID(sensor_mag1); - break; - } - case CLASS_DEVICE_TERTIARY: { - _orb_id = ORB_ID(sensor_mag2); - break; - } - default: { - log("invalid class instance: %d", _class_instance); - (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); - return -1; - } - } - - log("inited with class instance %d", _class_instance); return 0; } @@ -140,14 +114,5 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<ua report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; - if (_orb_advert >= 0) { - orb_publish(_orb_id, _orb_advert, &report); - } else { - _orb_advert = orb_advertise(_orb_id, &report); - if (_orb_advert < 0) { - log("ADVERT FAIL"); - } else { - log("advertised"); - } - } + publish(msg.getSrcNodeID().get(), &report); } |