UAVCAN: Receive commands from the GCS

msg/vehicle_command.msg

@@ -53,6 +53,7 @@ uint32 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003            # Enable or disable on-b
 uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000    # Command VTOL transition
 uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001	# Prepare a payload deployment in the flight plan
 uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed payload deployment
+uint32 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started 
 
 uint32 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
 uint32 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |

src/modules/commander/commander.cpp

@@ -802,6 +802,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE:
+	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:

src/modules/uavcan/uavcan_servers.cpp

@@ -53,9 +53,11 @@
 #include "uavcan_servers.hpp"
 #include "uavcan_virtual_can_driver.hpp"
 
-# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
-# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
-# include <uavcan_posix/firmware_version_checker.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
+#include <uavcan_posix/firmware_version_checker.hpp>
+
+#include <uORB/topics/vehicle_command.h>
 
 //todo:The Inclusion of file_server_backend is killing
 // #include <sys/types.h> and leaving OK undefined
@@ -277,6 +279,10 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 
 	Lock lock(_subnode_mutex);
 
+	/* the subscribe call needs to happen in the same thread,
+	 * so not in the constructor */
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+
 	while (1) {
 
 		if (_check_fw == true) {
@@ -286,6 +292,23 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 
 		const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100));
 
+		/* check if new commands are pending */
+		bool cmd_ready;
+		orb_check(cmd_sub, &cmd_ready);
+
+		if (cmd_ready) {
+			struct vehicle_command_s cmd;
+			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+			if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
+				warnx("received UAVCAN CONFIG command");
+
+				if (static_cast<int>(cmd.param1 + 0.5f) == 1) {
+					warnx("UAVCAN CONFIG: Actuator assignment requested");
+				}
+			}
+		}
+
 		if (spin_res < 0) {
 			warnx("node spin error %i", spin_res);
 		}