From 41ff46b5577d6706ff1251ad9dd46a274d885df8 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Thu, 19 Jan 2017 18:25:47 +0100
Subject: [PATCH] tap_esc: fixed offset to really keep the range 1200-1900 even
 if the input to the driver is too low or too high. This prevents motor
 stalling. NaN or Inf still stop the motor.

---
 src/drivers/tap_esc/tap_esc.cpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index 20ef177026..6937ee73e7 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -40,6 +40,7 @@
 #include <termios.h>
 #include <cmath>	// NAN
 
+#include <lib/mathlib/mathlib.h>
 #include <systemlib/px4_macros.h>
 #include <drivers/device/device.h>
 #include <uORB/uORB.h>
@@ -664,15 +665,14 @@ TAP_ESC::cycle()
 			for (unsigned i = 0; i < num_outputs; i++) {
 				/* last resort: catch NaN, INF and out-of-band errors */
 				if (i < _outputs.noutputs &&
-				    PX4_ISFINITE(_outputs.output[i]) &&
-				    _outputs.output[i] >= -1.0f &&
-				    _outputs.output[i] <= 1.0f) {
-					/* scale for PWM output 1000 - 2000us */
-					_outputs.output[i] = 1600 + (350 * _outputs.output[i]);
+				    PX4_ISFINITE(_outputs.output[i])) {
+					/* scale for PWM output 1200 - 1900us */
+					_outputs.output[i] = (RPMMAX + RPMMIN) / 2 + ((RPMMAX - RPMMIN) / 2) * _outputs.output[i]);
+					math::constrain(_outputs.output[i], (float)RPMMIN, (float)RPMMAX);
 
 				} else {
 					/*
-					 * Value is NaN, INF or out of band - set to the minimum value.
+					 * Value is NaN, INF - stop the motor.
 					 * This will be clearly visible on the servo status and will limit the risk of accidentally
 					 * spinning motors. It would be deadly in flight.
 					 */