Implement settable failsafe values for PWM outputs.

By default in failsafe mode, PWM output pulses are not generated.

5 files changed, 44 insertions, 10 deletions

diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c
index dca5ceb1d..bbbbb6e46 100644
--- a/apps/px4io/i2c.c
+++ b/apps/px4io/i2c.c
@@ -82,7 +82,7 @@ static unsigned		rx_len;
 
 static const uint8_t	junk_buf[] = { 0xff, 0xff, 0xff, 0xff };
 
-static uint8_t		*tx_buf = junk_buf;
+static const uint8_t	*tx_buf = junk_buf;
 static unsigned		tx_len = sizeof(junk_buf);
 
 static uint8_t		selected_page;
diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index 1c7a05343..91a914575 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -92,7 +92,7 @@ mixer_tick(void)
 
 		/* too long without FMU input, time to go to failsafe */
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
-		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM);
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
 		debug("AP RX timeout");
 	}
@@ -102,7 +102,7 @@ mixer_tick(void)
 	/*
 	 * Decide which set of controls we're using.
 	 */
-	if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) {
+	if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) {
 
 		/* don't actually mix anything - we already have raw PWM values */
 		source = MIX_NONE;
@@ -127,7 +127,13 @@ mixer_tick(void)
 	/*
 	 * Run the mixers.
 	 */
-	if (source != MIX_NONE) {
+	if (source == MIX_FAILSAFE) {
+
+		/* copy failsafe values to the servo outputs */
+		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+			r_page_servos[i] = r_page_servo_failsafe[i];
+
+	} else if (source != MIX_NONE) {
 
 		float	outputs[IO_SERVO_COUNT];
 		unsigned mixed;
@@ -225,9 +231,12 @@ mixer_tick(void)
 	 *
 	 * We must be armed, and we must have a PWM source; either raw from
 	 * FMU or from the mixer.
+	 *
+	 * XXX correct behaviour for failsafe may require an additional case
+	 * here.
 	 */
 	bool should_arm = ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
-		(r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PPM | PX4IO_P_STATUS_FLAGS_MIXER_OK)));
+		(r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)));
 
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 39ee4c0b1..6dbe16229 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -100,7 +100,7 @@
 #define PX4IO_P_STATUS_FLAGS_RC_DSM		(1 << 4) /* DSM input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_SBUS		(1 << 5) /* SBUS input is valid */
 #define PX4IO_P_STATUS_FLAGS_FMU_OK		(1 << 6) /* controls from FMU are valid */
-#define PX4IO_P_STATUS_FLAGS_RAW_PPM		(1 << 7) /* raw PPM from FMU is bypassing the mixer */
+#define PX4IO_P_STATUS_FLAGS_RAW_PWM		(1 << 7) /* raw PWM from FMU is bypassing the mixer */
 #define PX4IO_P_STATUS_FLAGS_MIXER_OK		(1 << 8) /* mixer is OK */
 
 #define PX4IO_P_STATUS_ALARMS			3	/* alarm flags - alarms latch, write 1 to a bit to clear it */
@@ -172,6 +172,9 @@
 /* PWM output - overrides mixer */
 #define PX4IO_PAGE_DIRECT_PWM		104		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
+/* PWM failsafe values - zero disables the output */
+#define PX4IO_PAGE_FAILSAFE_PWM		105		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
 /**
  * As-needed mixer data upload.
  *
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index d8cdafb4b..c90db5d78 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -76,6 +76,7 @@ extern uint16_t			r_page_adc[];		/* PX4IO_PAGE_RAW_ADC_INPUT */
 extern volatile uint16_t	r_page_setup[];		/* PX4IO_PAGE_SETUP */
 extern volatile uint16_t	r_page_controls[];	/* PX4IO_PAGE_CONTROLS */
 extern uint16_t			r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
+extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
 
 /*
  * Register aliases.
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index a4d81fd6e..22ed1de70 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -119,6 +119,11 @@ uint16_t 		r_page_actuators[IO_SERVO_COUNT];
 uint16_t		r_page_servos[IO_SERVO_COUNT];
 
 /**
+ * Servo PWM values
+ */
+uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT];
+
+/**
  * Scaled/routed RC input
  */
 uint16_t		r_page_rc_input[] = {
@@ -166,7 +171,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		/* XXX we should cause a mixer tick ASAP */
 		system_state.fmu_data_received_time = hrt_absolute_time();
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PPM;
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
 		break;
 
 		/* handle raw PWM input */
@@ -183,9 +188,24 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 			values++;
 		}
 
-		/* XXX need to force these values to the servos */
+		/* XXX we should cause a mixer tick ASAP */
 		system_state.fmu_data_received_time = hrt_absolute_time();
-		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM;
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM;
+		break;
+
+		/* handle setup for servo failsafe values */
+	case PX4IO_PAGE_FAILSAFE_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+
+			/* XXX range-check value? */
+			r_page_servo_failsafe[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
 		break;
 
 		/* handle text going to the mixer parser */
@@ -353,7 +373,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 int
 registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
 {
-#define SELECT_PAGE(_page_name)	{ *values = _page_name; *num_values = sizeof(_page_name) / sizeof(_page_name[0]); }
+#define SELECT_PAGE(_page_name)	{ *values = (uint16_t *)_page_name; *num_values = sizeof(_page_name) / sizeof(_page_name[0]); }
 
 	switch (page) {
 
@@ -441,6 +461,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 	COPY_PAGE(PX4IO_PAGE_SETUP,		r_page_setup);
 	COPY_PAGE(PX4IO_PAGE_CONTROLS,		r_page_controls);
 	COPY_PAGE(PX4IO_PAGE_RC_CONFIG,		r_page_rc_input_config);
+	COPY_PAGE(PX4IO_PAGE_FAILSAFE_PWM,	r_page_servo_failsafe);
 
 	default:
 		return -1;