Set default failsafe value to 0 of mixer

3 files changed, 62 insertions, 9 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index f25d471aa..873d7eb82 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -704,12 +704,8 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
 
 	unsigned max = (len < _max_actuators) ? len : _max_actuators;
 
-	/* set failsafe values */
-	for (unsigned i = 0; i < max; i++)
-		regs[i] = FLOAT_TO_REG(vals[i]);
-
 	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, regs, max);
+	return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, max);
 }
 
 int
@@ -1744,11 +1740,28 @@ px4io_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "failsafe")) {
 
-		/* XXX parse arguments here */
+		if (argc < 3) {
+			errx(1, "failsafe command needs at least one channel value (ppm)");
+		}
 
 		if (g_dev != nullptr) {
-			/* XXX testing values */
-			uint16_t failsafe[4] = {1500, 1500, 1200, 1200};
+
+			/* set values for first 8 channels, fill unassigned channels with 1500. */
+			uint16_t failsafe[8];
+
+			for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++)
+			{
+				/* set channel to commanline argument or to 900 for non-provided channels */
+				if (argc > i + 2) {
+					failsafe[i] = atoi(argv[i+2]);
+					if (failsafe[i] < 800 || failsafe[i] > 2200) {
+						errx(1, "value out of range of 800 < value < 2200. Aborting.");
+					}
+				} else {
+					failsafe[i] = 1500;
+				}
+			}
+
 			g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
 		} else {
 			errx(1, "not loaded");
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 448d2355f..0b8ed6dc5 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -85,6 +85,9 @@ static int	mixer_callback(uintptr_t handle,
 
 static MixerGroup mixer_group(mixer_callback, 0);
 
+/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
+static void mixer_set_failsafe();
+
 void
 mixer_tick(void)
 {
@@ -243,6 +246,7 @@ mixer_callback(uintptr_t handle,
 
 	case MIX_FAILSAFE:
 	case MIX_NONE:
+		control = 0.0f;
 		return -1;
 	}
 
@@ -320,8 +324,41 @@ mixer_handle_text(const void *buffer, size_t length)
 				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
 
 			mixer_text_length = resid;
+
+			/* update failsafe values */
+			mixer_set_failsafe();
 		}
 
 		break;
 	}
 }
+
+static void
+mixer_set_failsafe()
+{
+	/* 
+	 * Check if a custom failsafe value has been written,
+	 * else use the opportunity to set decent defaults.
+	 */
+	if (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+		return;
+
+	float	outputs[IO_SERVO_COUNT];
+	unsigned mixed;
+
+	/* mix */
+	mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+
+	/* scale to PWM and update the servo outputs as required */
+	for (unsigned i = 0; i < mixed; i++) {
+
+		/* scale to servo output */
+		r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;
+
+	}
+
+	/* disable the rest of the outputs */
+	for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+		r_page_servo_failsafe[i] = 0;
+
+}
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 3abf56a18..49a723352 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -231,11 +231,14 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 	case PX4IO_PAGE_FAILSAFE_PWM:
 
 		/* copy channel data */
-		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+		while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
 
 			/* XXX range-check value? */
 			r_page_servo_failsafe[offset] = *values;
 
+			/* flag the failsafe values as custom */
+			r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
+
 			offset++;
 			num_values--;
 			values++;