UAVCAN: initializing all bridges by default

5 files changed, 29 insertions, 126 deletions

diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index b3a9cb99b..8548660fe 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -56,17 +56,12 @@ _report_pub(-1)
 
 int UavcanGnssBridge::init()
 {
-	int res = -1;
-
-	// GNSS fix subscription
-	res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
+	int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
 	if (res < 0)
 	{
 		warnx("GNSS fix sub failed %i", res);
 		return res;
 	}
-
-	warnx("gnss sensor bridge init ok");
 	return res;
 }
 
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
index 05d589b58..a69514d77 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.cpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -45,27 +45,11 @@
 /*
  * IUavcanSensorBridge
  */
-IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name)
+void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list)
 {
-	/*
-	 * TODO: make a linked list of known implementations at startup?
-	 */
-	if (!std::strncmp(UavcanGnssBridge::NAME, bridge_name, MAX_NAME_LEN)) {
-		return new UavcanGnssBridge(node);
-	} else if (!std::strncmp(UavcanMagnetometerBridge::NAME, bridge_name, MAX_NAME_LEN)) {
-		return new UavcanMagnetometerBridge(node);
-	} else if (!std::strncmp(UavcanBarometerBridge::NAME, bridge_name, MAX_NAME_LEN)) {
-		return new UavcanBarometerBridge(node);
-	} else {
-		return nullptr;
-	}
-}
-
-void IUavcanSensorBridge::print_known_names(const char *prefix)
-{
-	printf("%s%s\n", prefix, UavcanGnssBridge::NAME);
-	printf("%s%s\n", prefix, UavcanMagnetometerBridge::NAME);
-	printf("%s%s\n", prefix, UavcanBarometerBridge::NAME);
+	list.add(new UavcanBarometerBridge(node));
+	list.add(new UavcanMagnetometerBridge(node));
+	list.add(new UavcanGnssBridge(node));
 }
 
 /*
diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
index d27ccb8a0..a13d0ab35 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.hpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -73,12 +73,7 @@ public:
 	 * Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
 	 * @return nullptr if such bridge can't be created.
 	 */
-	static IUavcanSensorBridge* make(uavcan::INode &node, const char *bridge_name);
-
-	/**
-	 * Prints all valid bridge names into stdout via printf(), one name per line with prefix.
-	 */
-	static void print_known_names(const char *prefix);
+	static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
 };
 
 /**
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index a15f83696..19b54b9cf 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -217,11 +217,12 @@ int UavcanNode::init(uavcan::NodeID node_id)
 {
 	int ret = -1;
 
-	/* do regular cdev init */
+	// Do regular cdev init
 	ret = CDev::init();
 
-	if (ret != OK)
+	if (ret != OK) {
 		return ret;
+	}
 
 	_node.setName("org.pixhawk.pixhawk");
 
@@ -229,10 +230,24 @@ int UavcanNode::init(uavcan::NodeID node_id)
 
 	fill_node_info();
 
-	/* initializing the bridges UAVCAN <--> uORB */
+	// Actuators
 	ret = _esc_controller.init();
-	if (ret < 0)
+	if (ret < 0) {
 		return ret;
+	}
+
+	// Sensor bridges
+	IUavcanSensorBridge::make_all(_node, _sensor_bridges);
+	auto br = _sensor_bridges.getHead();
+	while (br != nullptr) {
+		ret = br->init();
+		if (ret < 0) {
+			warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret);
+			return ret;
+		}
+		warnx("sensor bridge '%s' init ok", br->get_name());
+		br = br->getSibling();
+	}
 
 	return _node.start();
 }
@@ -522,62 +537,10 @@ UavcanNode::print_info()
 		warnx("not running, start first");
 	}
 
-	warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
-	warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
-}
-
-int UavcanNode::sensor_enable(const char *bridge_name)
-{
-	int retval = -1;
-
-	(void)pthread_mutex_lock(&_node_mutex);
-
-	// Checking if such bridge already exists
-	{
-		auto pos = _sensor_bridges.getHead();
-		while (pos != nullptr) {
-			if (!std::strncmp(pos->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN)) {
-				warnx("sensor bridge '%s' already exists", bridge_name);
-				retval = -1;
-				goto leave;
-			}
-			pos = pos->getSibling();
-		}
-	}
-
-	// Creating and initing
-	{
-		auto bridge = IUavcanSensorBridge::make(get_node(), bridge_name);
-		if (bridge == nullptr) {
-			warnx("cannot create sensor bridge '%s'", bridge_name);
-			retval = -1;
-			goto leave;
-		}
-
-		assert(!std::strncmp(bridge->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN));
-
-		retval = bridge->init();
-		if (retval >= 0) {
-			_sensor_bridges.add(bridge);
-		} else {
-			delete bridge;
-		}
-	}
-
-leave:
-	(void)pthread_mutex_unlock(&_node_mutex);
-	return retval;
-}
-
-void UavcanNode::sensor_print_enabled()
-{
 	(void)pthread_mutex_lock(&_node_mutex);
 
-	auto pos = _sensor_bridges.getHead();
-	while (pos != nullptr) {
-		warnx("%s", pos->get_name());
-		pos = pos->getSibling();
-	}
+	warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+	warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
 
 	(void)pthread_mutex_unlock(&_node_mutex);
 }
@@ -588,12 +551,7 @@ void UavcanNode::sensor_print_enabled()
 static void print_usage()
 {
 	warnx("usage: \n"
-	      "\tuavcan start\n"
-	      "\tuavcan sensor enable <sensor name>\n"
-	      "\tuavcan sensor list");
-
-	warnx("known sensor bridges:");
-	IUavcanSensorBridge::print_known_names("\t");
+	      "\tuavcan {start|status|stop}");
 }
 
 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -645,31 +603,6 @@ int uavcan_main(int argc, char *argv[])
 		::exit(0);
 	}
 
-	if (!std::strcmp(argv[1], "sensor")) {
-		if (argc < 3) {
-			print_usage();
-			::exit(1);
-		}
-
-		if (!std::strcmp(argv[2], "list")) {
-			inst->sensor_print_enabled();
-			::exit(0);
-		}
-
-		if (argc < 4) {
-			print_usage();
-			::exit(1);
-		}
-
-		if (!std::strcmp(argv[2], "enable")) {
-			const int res = inst->sensor_enable(argv[3]);
-			if (res < 0) {
-				warnx("failed to enable sensor '%s': error %d", argv[3], res);
-			}
-			::exit(0);
-		}
-	}
-
 	print_usage();
 	::exit(1);
 }
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index bca1aa530..be7db9741 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -89,10 +89,6 @@ public:
 
 	void		print_info();
 
-	int		sensor_enable(const char *bridge_name);
-
-	void		sensor_print_enabled();
-
 	static UavcanNode* instance() { return _instance; }
 
 private:
@@ -115,7 +111,7 @@ private:
 
 	UavcanEscController	_esc_controller;
 
-	List<IUavcanSensorBridge*> _sensor_bridges;		///< Append-only list of active sensor bridges
+	List<IUavcanSensorBridge*> _sensor_bridges;		///< List of active sensor bridges
 
 	MixerGroup		*_mixers = nullptr;