uavcan: added support for actuator_direct ORB topic

this watches the actuator_direct topic and uses it to allow for direct
output of actuator values, bypassing the mixer

2 files changed, 28 insertions, 0 deletions

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index ccc087920..cf40f3ea8 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -280,6 +280,7 @@ int UavcanNode::run()
 
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 	_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
+	_actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct));
 
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
@@ -310,6 +311,18 @@ int UavcanNode::run()
 	_poll_fds[_poll_fds_num].events = POLLIN;
 	_poll_fds_num += 1;
 
+	/*
+	 * setup poll to look for actuator direct input if we are
+	 * subscribed to the topic
+	 */
+	if (_actuator_direct_sub != -1) {
+		_poll_fds[_poll_fds_num] = ::pollfd();
+		_poll_fds[_poll_fds_num].fd = _actuator_direct_sub;
+		_poll_fds[_poll_fds_num].events = POLLIN;
+		_actuator_direct_poll_fd_num = _poll_fds_num;
+		_poll_fds_num += 1;            
+	}
+
 	while (!_task_should_exit) {
 		// update actuator controls subscriptions if needed
 		if (_groups_subscribed != _groups_required) {
@@ -342,6 +355,16 @@ int UavcanNode::run()
 				}
 			}
 
+			/*
+			  see if we have any direct actuator updates
+			 */
+			if (_actuator_direct_sub != -1 && 
+			    (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&
+			    orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK) {
+				// Output to the bus
+				_esc_controller.update_outputs(_actuator_direct.values, _actuator_direct.nvalues);
+			}
+
 			// can we mix?
 			if (_test_in_progress) {
 				float test_outputs[NUM_ACTUATOR_OUTPUTS] = {};
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index b791663d4..d7eb16764 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -42,6 +42,7 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/test_motor.h>
+#include <uORB/topics/actuator_direct.h>
 
 #include "actuators/esc.hpp"
 #include "sensors/sensor_bridge.hpp"
@@ -128,6 +129,10 @@ private:
 	pollfd			_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {};	///< +1 for /dev/uavcan/busevent
 	unsigned		_poll_fds_num = 0;
 
+	int			_actuator_direct_sub = -1;   ///< uORB subscription of the actuator_direct topic
+	uint8_t			_actuator_direct_poll_fd_num;
+	actuator_direct_s	_actuator_direct;
+
 	// index into _poll_fds for each _control_subs handle
 	uint8_t			_poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
 };