Fix mixer limiter to never output min for an input of max + 1 quantum

1 files changed, 13 insertions, 3 deletions

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 5bc437670..27e77e9c5 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -301,9 +301,7 @@ int UavcanNode::run()
 				// iterate actuators
 				for (unsigned i = 0; i < outputs.noutputs; i++) {
 					// last resort: catch NaN, INF and out-of-band errors
-					if (!isfinite(outputs.output[i]) ||
-						outputs.output[i] < -1.0f ||
-						outputs.output[i] > 1.0f) {
+					if (!isfinite(outputs.output[i])) {
 						/*
 						 * Value is NaN, INF or out of band - set to the minimum value.
 						 * This will be clearly visible on the servo status and will limit the risk of accidentally
@@ -311,6 +309,18 @@ int UavcanNode::run()
 						 */
 						outputs.output[i] = -1.0f;
 					}
+
+					// limit outputs to valid range
+
+					// never go below min
+					if (outputs.output[i] < -1.0f) {
+						outputs.output[i] = -1.0f;
+					}
+
+					// never go below max
+					if (outputs.output[i] > 1.0f) {
+						outputs.output[i] = 1.0f;
+					}
 				}
 
 				// Output to the bus