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

Conflicts:
	src/modules/mavlink/mavlink_main.cpp
	src/modules/mavlink/mavlink_main.h
	src/modules/mavlink/mavlink_receiver.cpp

3 files changed, 64 insertions, 40 deletions

diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index 1ef6f0cfa..3865f2468 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -65,10 +65,6 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM
 
 #
 # Invoke DSDL compiler
-# TODO: Add make target for this, or invoke dsdlc manually.
-#       The second option assumes that the generated headers shall be saved
-#       under the version control, which may be undesirable.
-#       The first option requires any Python and the Python Mako library for the sources to be built.
 #
 $(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR)))
 INCLUDE_DIRS += dsdlc_generated
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 27e77e9c5..4535b6d6a 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -39,6 +39,8 @@
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/mixer/mixer.h>
+#include <systemlib/board_serial.h>
+#include <version/version.h>
 #include <arch/board/board.h>
 #include <arch/chip/chip.h>
 
@@ -173,6 +175,41 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 	return OK;
 }
 
+void UavcanNode::fill_node_info()
+{
+	/* software version */
+	uavcan::protocol::SoftwareVersion swver;
+
+	// Extracting the first 8 hex digits of FW_GIT and converting them to int
+	char fw_git_short[9] = {};
+	std::memmove(fw_git_short, FW_GIT, 8);
+	assert(fw_git_short[8] == '\0');
+	char *end = nullptr;
+	swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
+	swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT;
+
+	warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
+
+	_node.setSoftwareVersion(swver);
+
+	/* hardware version */
+	uavcan::protocol::HardwareVersion hwver;
+
+	if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
+		hwver.major = 1;
+	} else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
+		hwver.major = 2;
+	} else {
+		; // All other values of HW_ARCH resolve to zero
+	}
+
+	uint8_t udid[12] = {};  // Someone seems to love magic numbers
+	get_board_serial(udid);
+	uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin());
+
+	_node.setHardwareVersion(hwver);
+}
+
 int UavcanNode::init(uavcan::NodeID node_id)
 {
 	int ret = -1;
@@ -183,6 +220,13 @@ int UavcanNode::init(uavcan::NodeID node_id)
 	if (ret != OK)
 		return ret;
 
+	_node.setName("org.pixhawk.pixhawk");
+
+	_node.setNodeID(node_id);
+
+	fill_node_info();
+
+	/* initializing the bridges UAVCAN <--> uORB */
 	ret = _esc_controller.init();
 	if (ret < 0)
 		return ret;
@@ -191,20 +235,6 @@ int UavcanNode::init(uavcan::NodeID node_id)
 	if (ret < 0)
 		return ret;
 
-	uavcan::protocol::SoftwareVersion swver;
-	swver.major = 12;                        // TODO fill version info
-	swver.minor = 34;
-	_node.setSoftwareVersion(swver);
-
-	uavcan::protocol::HardwareVersion hwver;
-	hwver.major = 42;                        // TODO fill version info
-	hwver.minor = 42;
-	_node.setHardwareVersion(hwver);
-
-	_node.setName("org.pixhawk"); // Huh?
-
-	_node.setNodeID(node_id);
-
 	return _node.start();
 }
 
@@ -223,7 +253,6 @@ int UavcanNode::run()
 	// XXX figure out the output count
 	_output_count = 2;
 
-
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
 	actuator_outputs_s outputs;
@@ -243,21 +272,23 @@ int UavcanNode::run()
 
 	_node.setStatusOk();
 
-	while (!_task_should_exit) {
+	/*
+	 * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
+	 * Please note that with such multiplexing it is no longer possible to rely only on
+	 * the value returned from poll() to detect whether actuator control has timed out or not.
+	 * Instead, all ORB events need to be checked individually (see below).
+	 */
+	_poll_fds_num = 0;
+	_poll_fds[_poll_fds_num] = ::pollfd();
+	_poll_fds[_poll_fds_num].fd = busevent_fd;
+	_poll_fds[_poll_fds_num].events = POLLIN;
+	_poll_fds_num += 1;
 
+	while (!_task_should_exit) {
+		// update actuator controls subscriptions if needed
 		if (_groups_subscribed != _groups_required) {
 			subscribe();
 			_groups_subscribed = _groups_required;
-			/*
-			 * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
-			 * Please note that with such multiplexing it is no longer possible to rely only on
-			 * the value returned from poll() to detect whether actuator control has timed out or not.
-			 * Instead, all ORB events need to be checked individually (see below).
-			 */
-			_poll_fds[_poll_fds_num] = ::pollfd();
-			_poll_fds[_poll_fds_num].fd = busevent_fd;
-			_poll_fds[_poll_fds_num].events = POLLIN;
-			_poll_fds_num += 1;
 		}
 
 		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
@@ -271,7 +302,7 @@ int UavcanNode::run()
 		} else {
 			// get controls for required topics
 			bool controls_updated = false;
-			unsigned poll_id = 0;
+			unsigned poll_id = 1;
 			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					if (_poll_fds[poll_id].revents & POLLIN) {
@@ -282,12 +313,7 @@ int UavcanNode::run()
 				}
 			}
 
-			if (!controls_updated) {
-				// timeout: no control data, switch to failsafe values
-				// XXX trigger failsafe
-			}
-
-			//can we mix?
+			// can we mix?
 			if (controls_updated && (_mixers != nullptr)) {
 
 				// XXX one output group has 8 outputs max,
@@ -387,7 +413,8 @@ UavcanNode::subscribe()
 	// Subscribe/unsubscribe to required actuator control groups
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
-	_poll_fds_num = 0;
+	// the first fd used by CAN
+	_poll_fds_num = 1;
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
@@ -493,8 +520,8 @@ UavcanNode::print_info()
 		warnx("not running, start first");
 	}
 
-	warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
-	warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK");
+	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");
 }
 
 /*
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 443525379..05b66fd7b 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -94,6 +94,7 @@ public:
 	static UavcanNode* instance() { return _instance; }
 
 private:
+	void		fill_node_info();
 	int		init(uavcan::NodeID node_id);
 	void		node_spin_once();
 	int		run();