Merge remote-tracking branch 'upstream/master' into ros_messagelayer_merge2_attctrl_posctrl

Conflicts:
	src/drivers/px4fmu/fmu.cpp

9 files changed, 22 insertions, 35 deletions

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..de130b078 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
@@ -75,8 +75,7 @@ public:
 
 	/**
 	 * Sensor bridge factory.
-	 * Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
-	 * @return nullptr if such bridge can't be created.
+	 * Creates all known sensor bridges and puts them in the linked list.
 	 */
 	static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
 };
@@ -90,28 +89,29 @@ 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>
+	static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody
+
 	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,
+	                           const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
 	device::CDev(name, devname),
-	_max_channels(MaxChannels),
+	_max_channels(max_channels),
 	_class_devname(class_devname),
-	_orb_topics(new orb_id_t[MaxChannels]),
-	_channels(new Channel[MaxChannels])
+	_orb_topic(orb_topic_sensor),
+	_channels(new Channel[max_channels])
 	{
-		memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
 		_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
 		_device_id.devid_s.bus = 0;
 	}