From 58f36714f808c9d06527da1952d348201c4f7e72 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Thu, 14 Aug 2014 15:43:27 +0200
Subject: [PATCH] UAVCAN: allow to arm single ESCs

---
 src/modules/uavcan/actuators/esc.cpp | 26 +++++++++++++++++++++-----
 src/modules/uavcan/actuators/esc.hpp |  9 ++++++++-
 src/modules/uavcan/uavcan_main.cpp   |  2 +-
 3 files changed, 30 insertions(+), 7 deletions(-)

diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 1990651ef2..fbd4f0bcd4 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -40,6 +40,9 @@
 #include "esc.hpp"
 #include <systemlib/err.h>
 
+
+#define MOTOR_BIT(x) (1<<(x))
+
 UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_node(node),
 	_uavcan_pub_raw_cmd(node),
@@ -95,9 +98,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 
 	static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
 
-	if (_armed) {
-		for (unsigned i = 0; i < num_outputs; i++) {
-
+	for (unsigned i = 0; i < num_outputs; i++) {
+		if (_armed_mask & MOTOR_BIT(i)) {
 			float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
 			if (scaled < 1.0F) {
 				scaled = 1.0F;  // Since we're armed, we don't want to stop it completely
@@ -112,6 +114,9 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 
 			_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
 		}
+		else {
+			msg.cmd.push_back(static_cast<unsigned>(0));
+		}
 	}
 
 	/*
@@ -121,9 +126,20 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 	(void)_uavcan_pub_raw_cmd.broadcast(msg);
 }
 
-void UavcanEscController::arm_esc(bool arm)
+void UavcanEscController::arm_all_escs(bool arm)
 {
-	_armed = arm;
+	if (arm)
+		_armed_mask = -1;
+	else
+		_armed_mask = 0;
+}
+
+void UavcanEscController::arm_single_esc(int num, bool arm)
+{
+	if (arm)
+		_armed_mask = MOTOR_BIT(num);
+	else
+		_armed_mask = 0;
 }
 
 void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp
index f4a3877e62..ff3ecfb215 100644
--- a/src/modules/uavcan/actuators/esc.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -61,7 +61,8 @@ public:
 
 	void update_outputs(float *outputs, unsigned num_outputs);
 
-	void arm_esc(bool arm);
+	void arm_all_escs(bool arm);
+	void arm_single_esc(int num, bool arm);
 
 private:
 	/**
@@ -98,6 +99,12 @@ private:
 	uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder>	_uavcan_sub_status;
 	uavcan::TimerEventForwarder<TimerCbBinder>				_orb_timer;
 
+	/*
+	 * 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 a8485ee44e..59f0f43004 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -429,7 +429,7 @@ int
 UavcanNode::arm_actuators(bool arm)
 {
 	_is_armed = arm;
-	_esc_controller.arm_esc(arm);
+	_esc_controller.arm_all_escs(arm);
 	return OK;
 }