Robustified actuator output topic, added number of mixed outputs

1 files changed, 19 insertions, 8 deletions

diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp
index cb4a48fab..276a642fd 100644
--- a/apps/drivers/hil/hil.cpp
+++ b/apps/drivers/hil/hil.cpp
@@ -68,11 +68,11 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
-// #include <drivers/boards/HIL/HIL_internal.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mixer.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/mixer/mixer.h>
-#include <drivers/drv_mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
@@ -395,16 +395,27 @@ HIL::task_main()
 			if (_mixers != nullptr) {
 
 				/* do mixing */
-				_mixers->mix(&outputs.output[0], num_outputs);
+				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+				outputs.timestamp = hrt_absolute_time();
 
 				/* iterate actuators */
 				for (unsigned i = 0; i < num_outputs; i++) {
 
-					/* scale for PWM output 900 - 2100us */
-					outputs.output[i] = 1500 + (600 * outputs.output[i]);
-
-					/* output to the servo */
-					// up_pwm_servo_set(i, outputs.output[i]);
+					/* last resort: catch NaN, INF and out-of-band errors */
+					if (i < (unsigned)outputs.noutputs &&
+						isfinite(outputs.output[i]) &&
+						outputs.output[i] >= -1.0f &&
+						outputs.output[i] <= 1.0f) {
+						/* scale for PWM output 900 - 2100us */
+						outputs.output[i] = 1500 + (600 * outputs.output[i]);
+					} else {
+						/*
+						 * 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
+						 * spinning motors. It would be deadly in flight.
+						 */
+						outputs.output[i] = 900;
+					}
 				}
 
 				/* and publish for anyone that cares to see */