Add support for raw PWM passthrough from FMU via IO.

4 files changed, 71 insertions, 26 deletions

diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index f394233f4..178b0bb43 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -81,6 +81,7 @@ static int	mixer_callback(uintptr_t handle,
 			       uint8_t control_index,
 			       float &control);
 
+static void mix();
 static MixerGroup mixer_group(mixer_callback, 0);
 
 void
@@ -91,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;
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM);
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
 		debug("AP RX timeout");
 	}
@@ -100,13 +101,26 @@ mixer_tick(void)
 	 * Decide which set of controls we're using.
 	 */
 	if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && 
-		(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE)) {
+		(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
 	 	/* this is for planes, where manual override makes sense */
 		source = MIX_OVERRIDE;
+
+		/* mix from the override controls */
+		mix();
+
 	} else if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
-		source = MIX_FMU;
+
+		if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) {
+			/* FMU has already provided PWM values */
+		} else {
+			/* mix from FMU controls */
+			source = MIX_FMU;
+			mix();
+		}
 	} else {
 		source = MIX_FAILSAFE;
+		/* XXX actually, have no idea what to do here... load hardcoded failsafe controls? */
 	}
 
 #if 0
@@ -173,27 +187,6 @@ mixer_tick(void)
 		}
 	}
 #endif
-	/*
-	 * Run the mixers.
-	 */
-	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++) {
-
-		/* save actuator values for FMU readback */
-		r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
-
-		/* scale to servo output */
-		r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
-
-	}
-	for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
-		r_page_servos[i] = 0;
 
 	/*
 	 * Update the servo outputs.
@@ -219,6 +212,33 @@ mixer_tick(void)
 	}
 }
 
+static void
+mix()
+{
+	/*
+	 * Run the mixers.
+	 */
+	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++) {
+
+		/* save actuator values for FMU readback */
+		r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
+
+		/* scale to servo output */
+		r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
+
+	}
+	for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+		r_page_servos[i] = 0;
+
+}
+
 static int
 mixer_callback(uintptr_t handle,
 	       uint8_t control_group,
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 501691cd2..a2be4bfc4 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -100,6 +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_ALARMS			3	/* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
@@ -149,7 +150,7 @@
 #define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
 
 /* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS		101		/* 0..STATUS_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS		101		/* 0..CONFIG_CONTROL_COUNT */
 
 /* raw text load to the mixer parser - ignores offset */
 #define PX4IO_PAGE_MIXERLOAD		102
@@ -166,6 +167,9 @@
 #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE	(1 << 1)
 #define PX4IO_P_RC_CONFIG_STRIDE		6	/* spacing between channel config data */
 
+/* PWM output - overrides mixer */
+#define PX4IO_PAGE_DIRECT_PWM		104		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
 /**
  * As-needed mixer data upload.
  *
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 13f46939a..c3dacb2a6 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -115,6 +115,7 @@ int user_start(int argc, char *argv[])
 	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
 
 	/* run the mixer at 100Hz (for now...) */
+	/* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */
 	for (;;) {
 		poll(NULL, 0, 10);
 		perf_begin(mixer_perf);
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index 0dd8fe28d..2f411eebf 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -152,7 +152,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		/* copy channel data */
 		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
 
-			/* XXX scaling - should be -10000..10000 */
+			/* XXX range-check value? */
 			r_page_controls[offset] = *values;
 
 			offset++;
@@ -163,6 +163,26 @@ 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;
+		break;
+
+		/* handle raw PWM input */
+	case PX4IO_PAGE_DIRECT_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+
+			/* XXX range-check value? */
+			r_page_servos[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+
+		/* XXX need to force these values to the servos */
+		system_state.fmu_data_received_time = hrt_absolute_time();
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM;
 		break;
 
 		/* handle text going to the mixer parser */