From 0800fa4715994921b6a0d15cd2c44b9e51417117 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Mon, 3 Nov 2014 18:47:07 +0100
Subject: [PATCH] UAVCAN: implemented motor testing

---
 src/modules/uavcan/actuators/esc.hpp |  1 -
 src/modules/uavcan/uavcan_main.cpp   | 28 ++++++++++++++++++++++++----
 src/modules/uavcan/uavcan_main.hpp   |  5 +++++
 3 files changed, 29 insertions(+), 5 deletions(-)

diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp
index ff3ecfb215..12c0355422 100644
--- a/src/modules/uavcan/actuators/esc.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -103,7 +103,6 @@ private:
 	 * ESC states
 	 */
 	uint32_t 			_armed_mask = 0;
-	uavcan::equipment::esc::Status	_states[MAX_ESCS];
 
 	/*
 	 * Perf counters
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 59f0f43004..653d4f98cf 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -279,6 +279,7 @@ int UavcanNode::run()
 	_output_count = 2;
 
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
 
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
@@ -344,7 +345,14 @@ int UavcanNode::run()
 			}
 
 			// can we mix?
-			if (controls_updated && (_mixers != nullptr)) {
+			if (_test_in_progress) {
+				float outputs[NUM_ACTUATOR_OUTPUTS] = {};
+				outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
+
+				// Output to the bus
+				_esc_controller.update_outputs(outputs, NUM_ACTUATOR_OUTPUTS);
+			}
+			else if (controls_updated && (_mixers != nullptr)) {
 
 				// XXX one output group has 8 outputs max,
 				// but this driver could well serve multiple groups.
@@ -384,15 +392,27 @@ int UavcanNode::run()
 			}
 		}
 
-		// Check arming state
+		// Check motor test state
 		bool updated = false;
+		orb_check(_test_motor_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor);
+
+			// Update the test status and check that we're not locked down
+			_test_in_progress = (_test_motor.value > 0);
+			_esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress);
+		}
+
+		// Check arming state
 		orb_check(_armed_sub, &updated);
 
 		if (updated) {
 			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 
-			// Update the armed status and check that we're not locked down
-			bool set_armed = _armed.armed && !_armed.lockdown;
+			// Update the armed status and check that we're not locked down and motor 
+			// test is not running
+			bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress;
 
 			arm_actuators(set_armed);
 		}
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index be7db97411..274321f0db 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -41,6 +41,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/test_motor.h>
 
 #include "actuators/esc.hpp"
 #include "sensors/sensor_bridge.hpp"
@@ -103,6 +104,10 @@ private:
 	actuator_armed_s	_armed;				///< the arming request of the system
 	bool			_is_armed = false;		///< the arming status of the actuators on the bus
 
+	int			_test_motor_sub = -1;   ///< uORB subscription of the test_motor status
+	test_motor_s		_test_motor;
+	bool			_test_in_progress = false;
+
 	unsigned		_output_count = 0;		///< number of actuators currently available
 
 	static UavcanNode	*_instance;			///< singleton pointer