1 files changed, 34 insertions, 10 deletions

diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 1990651ef..9f682c7e1 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),
@@ -73,7 +76,9 @@ int UavcanEscController::init()
 
 void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 {
-	if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) {
+	if ((outputs == nullptr) || 
+            (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
+            (num_outputs > CONNECTED_ESC_MAX)) {
 		perf_count(_perfcnt_invalid_input);
 		return;
 	}
@@ -95,14 +100,18 @@ 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
-			}
-
+                        // trim negative values back to 0. Previously
+                        // we set this to 0.1, which meant motors kept
+                        // spinning when armed, but that should be a
+                        // policy decision for a specific vehicle
+                        // type, as it is not appropriate for all
+                        // types of vehicles (eg. fixed wing).
+			if (scaled < 0.0F) {
+				scaled = 0.0F;
+                        }
 			if (scaled > cmd_max) {
 				scaled = cmd_max;
 				perf_count(_perfcnt_scaling_error);
@@ -111,6 +120,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 			msg.cmd.push_back(static_cast<int>(scaled));
 
 			_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
+		} else {
+			msg.cmd.push_back(static_cast<unsigned>(0));
 		}
 	}
 
@@ -121,9 +132,22 @@ 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)
+{
+	if (arm) {
+		_armed_mask = -1;
+	} else {
+		_armed_mask = 0;
+	}
+}
+
+void UavcanEscController::arm_single_esc(int num, bool arm)
 {
-	_armed = 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)