From e27d3f4e1302c2d5ff593aac31e2471f8367103b Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Sat, 6 Aug 2016 14:47:50 +0300
Subject: [PATCH] Added new configuration parameter UAVCAN_ESC_IDLT. This
 parameter, when enabled, enforces that the UAVCAN ESC driver never outputs
 zero throttle while the system is armed. This feature is disabled by default,
 so the change will not break the experience of current users.

---
 src/modules/uavcan/actuators/esc.cpp | 13 ++++---------
 src/modules/uavcan/actuators/esc.hpp |  3 +++
 src/modules/uavcan/uavcan_main.cpp   |  6 ++++++
 src/modules/uavcan/uavcan_params.c   |  9 +++++++++
 4 files changed, 22 insertions(+), 9 deletions(-)

diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index b8856fb357..c2d455050c 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -39,6 +39,7 @@
 
 #include "esc.hpp"
 #include <systemlib/err.h>
+#include <algorithm>
 
 
 #define MOTOR_BIT(x) (1<<(x))
@@ -110,20 +111,14 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 	uavcan::equipment::esc::RawCommand msg;
 
 	static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
+	const float cmd_min = _run_at_idle_throttle_when_armed ? 1.0F : 0.0F;
 
 	for (unsigned i = 0; i < num_outputs; i++) {
 		if (_armed_mask & MOTOR_BIT(i)) {
 			float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
 
-			// 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;
-			}
+			// trim negative values back to minimum
+			scaled = std::max(cmd_min, scaled);
 
 			if (scaled > cmd_max) {
 				scaled = cmd_max;
diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp
index 2cbc23ec9d..40b151e308 100644
--- a/src/modules/uavcan/actuators/esc.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -64,6 +64,8 @@ public:
 	void arm_all_escs(bool arm);
 	void arm_single_esc(int num, bool arm);
 
+	void enable_idle_throttle_when_armed(bool value) { _run_at_idle_throttle_when_armed = value; }
+
 private:
 	/**
 	 * ESC status message reception will be reported via this callback.
@@ -88,6 +90,7 @@ private:
 	TimerCbBinder;
 
 	bool		_armed = false;
+	bool		_run_at_idle_throttle_when_armed = false;
 	esc_status_s	_esc_status = {};
 	orb_advert_t	_esc_status_pub = nullptr;
 
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index d7c3455157..d245c2fb3a 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -660,6 +660,12 @@ int UavcanNode::init(uavcan::NodeID node_id)
 		return ret;
 	}
 
+	{
+		std::int32_t idle_throttle_when_armed = 0;
+		(void) param_get(param_find("UAVCAN_ESC_IDLT"), &idle_throttle_when_armed);
+		_esc_controller.enable_idle_throttle_when_armed(idle_throttle_when_armed > 0);
+	}
+
 	ret = _hardpoint_controller.init();
 
 	if (ret < 0) {
diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c
index 2196a5fb30..e24a5a1f6b 100644
--- a/src/modules/uavcan/uavcan_params.c
+++ b/src/modules/uavcan/uavcan_params.c
@@ -76,3 +76,12 @@ PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
  * @group UAVCAN
  */
 PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);
+
+/**
+ * UAVCAN ESC will spin at idle throttle when armed, even if the mixer outputs zero setpoints.
+ *
+ * @min 0
+ * @max 1
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_ESC_IDLT, 0);