Robustified actuator output topic, added number of mixed outputs

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 */

apps/drivers/px4fmu/fmu.cpp

@@ -58,6 +58,7 @@
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
 #include <drivers/boards/px4fmu/px4fmu_internal.h>
+#include <drivers/drv_hrt.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
@@ -382,7 +383,8 @@ PX4FMU::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();
 
 				// XXX output actual limited values
 				memcpy(&controls_effective, &_controls, sizeof(controls_effective));
@@ -392,8 +394,21 @@ PX4FMU::task_main()
 				/* iterate actuators */
 				for (unsigned i = 0; i < num_outputs; i++) {
 
-					/* scale for PWM output 900 - 2100us */
-					outputs.output[i] = 1500 + (600 * outputs.output[i]);
+					/* last resort: catch NaN, INF and out-of-band errors */
+					if (i < 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;
+					}
 
 					/* output to the servo */
 					up_pwm_servo_set(i, outputs.output[i]);

apps/drivers/px4io/px4io.cpp

@@ -402,8 +402,8 @@ PX4IO::task_main()
 
 				/* mix */
 				if (_mixers != nullptr) {
-					/* XXX is this the right count? */
-					_mixers->mix(&_outputs.output[0], _max_actuators);
+					_outputs.timestamp = hrt_absolute_time();
+					_outputs.noutputs = _mixers->mix(&_outputs.output[0], _max_actuators);
 
 					// XXX output actual limited values
 					memcpy(&_controls_effective, &_controls, sizeof(_controls_effective));
@@ -413,7 +413,11 @@ PX4IO::task_main()
 					/* convert to PWM values */
 					for (unsigned i = 0; i < _max_actuators; 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 (i < _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 {
 							/*

apps/mavlink/orb_listener.c

@@ -507,12 +507,26 @@ l_actuator_outputs(struct listener *l)
 					0);
 			} else {
 
+				/*
+				 * Catch the case where no rudder is in use and throttle is not
+				 * on output four
+				 */
+				float rudder, throttle;
+
+				if (act_outputs.noutputs < 4) {
+					rudder = 0.0f;
+					throttle = (act_outputs.output[2] - 900.0f) / 1200.0f;
+				} else {
+					rudder = (act_outputs.output[2] - 1500.0f) / 600.0f;
+					throttle = (act_outputs.output[3] - 900.0f) / 1200.0f;
+				}
+
 				mavlink_msg_hil_controls_send(chan,
 					hrt_absolute_time(),
 					(act_outputs.output[0] - 1500.0f) / 600.0f,
 					(act_outputs.output[1] - 1500.0f) / 600.0f,
-					0.0f/*(act_outputs.output[2] - 1500.0f) / 600.0f*/,
-					(act_outputs.output[2] - 900.0f) / 1200.0f,
+					rudder,
+					throttle,
 					(act_outputs.output[4] - 1500.0f) / 600.0f,
 					(act_outputs.output[5] - 1500.0f) / 600.0f,
 					(act_outputs.output[6] - 1500.0f) / 600.0f,

apps/uORB/topics/actuator_outputs.h

@@ -53,8 +53,9 @@
 #define NUM_ACTUATOR_OUTPUT_GROUPS	4	/**< for sanity checking */
 
 struct actuator_outputs_s {
-	uint64_t timestamp;
-	float	output[NUM_ACTUATOR_OUTPUTS];
+	uint64_t timestamp;				/**< output timestamp in us since system boot */
+	float	output[NUM_ACTUATOR_OUTPUTS];		/**< output data, in natural output units */
+	int noutputs;					/**< valid outputs */
 };
 
 /* actuator output sets; this list can be expanded as more drivers emerge */