Upgrade UAVCAN to multi pub/sub A API

10 files changed, 17 insertions, 31 deletions

diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 68a65efed..3abebd82f 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -70,7 +70,7 @@ MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
-#MODULES		+= modules/uavcan
+MODULES		+= modules/uavcan
 MODULES 	+= modules/land_detector
 
 #
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index 600cb47f3..4f63629a0 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
 #   Author: Pavel Kirienko <pavel.kirienko@gmail.com>
 #
 # Redistribution and use in source and binary forms, with or without
diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 8741ae20d..ad09dfcac 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,15 +38,10 @@
 #include "baro.hpp"
 #include <cmath>
 
-static const orb_id_t BARO_TOPICS[2] = {
-	ORB_ID(sensor_baro0),
-	ORB_ID(sensor_baro1)
-};
-
 const char *const UavcanBarometerBridge::NAME = "baro";
 
 UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
-UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS),
+UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, ORB_ID(sensor_baro)),
 _sub_air_data(node)
 {
 }
diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp
index 9d470219e..c7bbc5af8 100644
--- a/src/modules/uavcan/sensors/baro.hpp
+++ b/src/modules/uavcan/sensors/baro.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 571a6f1cd..3ae07367f 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp
index 2111ff80b..96ff9404f 100644
--- a/src/modules/uavcan/sensors/gnss.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 35ebee542..ee278aaf5 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,16 +39,10 @@
 
 #include <systemlib/err.h>
 
-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) :
-UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
+UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, ORB_ID(sensor_mag)),
 _sub_mag(node)
 {
 	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp
index 74077d883..db38aee1d 100644
--- a/src/modules/uavcan/sensors/mag.hpp
+++ b/src/modules/uavcan/sensors/mag.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
index 0999938fc..b37076444 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.cpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -62,7 +62,6 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
 			(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
 		}
 	}
-	delete [] _orb_topics;
 	delete [] _channels;
 }
 
@@ -116,11 +115,10 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 		}
 
 		// Publish to the appropriate topic, abort on failure
-		channel->orb_id         = _orb_topics[class_instance];
 		channel->node_id        = node_id;
 		channel->class_instance = class_instance;
 
-		channel->orb_advert = orb_advertise(channel->orb_id, report);
+		channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH);
 		if (channel->orb_advert < 0) {
 			log("ADVERTISE FAILED");
 			(void)unregister_class_devname(_class_devname, class_instance);
@@ -132,7 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 	}
 	assert(channel != nullptr);
 
-	(void)orb_publish(channel->orb_id, channel->orb_advert, report);
+	(void)orb_publish(_orb_topic, channel->orb_advert, report);
 }
 
 unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
index e31960537..94e52dbe5 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.hpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -90,28 +90,27 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD
 	struct Channel
 	{
 		int node_id              = -1;
-		orb_id_t orb_id          = nullptr;
 		orb_advert_t orb_advert  = -1;
 		int class_instance       = -1;
+		int orb_instance	 = -1;
 	};
 
 	const unsigned _max_channels;
 	const char *const _class_devname;
-	orb_id_t *const _orb_topics;
+	const orb_id_t _orb_topic;
 	Channel *const _channels;
 	bool _out_of_channels = false;
 
 protected:
 	template <unsigned MaxChannels>
 	UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
-	                           const orb_id_t (&orb_topics)[MaxChannels]) :
+	                           const orb_id_t orb_topic_sensor) :
 	device::CDev(name, devname),
 	_max_channels(MaxChannels),
 	_class_devname(class_devname),
-	_orb_topics(new orb_id_t[MaxChannels]),
+	_orb_topic(orb_topic_sensor),
 	_channels(new Channel[MaxChannels])
 	{
-		memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
 		_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
 		_device_id.devid_s.bus = 0;
 	}