Rough in the new mixer path for PX4IO.

5 files changed, 132 insertions, 41 deletions

diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile
index 801695f84..d97f963ab 100644
--- a/apps/px4io/Makefile
+++ b/apps/px4io/Makefile
@@ -41,7 +41,9 @@
 #
 CSRCS		= $(wildcard *.c) \
 		  ../systemlib/hx_stream.c \
-		  ../systemlib/perf_counter.c
+		  ../systemlib/perf_counter.c \
+		  ../systemlib/up_cxxinitialize.c
+CXXSRCS		= $(wildcard *.cpp)
 
 INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
 
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 83a006d43..bc2142a4b 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -192,7 +192,6 @@ comms_handle_command(const void *buffer, size_t length)
 	irqrestore(flags);
 }
 
-
 static void
 comms_handle_frame(void *arg, const void *buffer, size_t length)
 {
@@ -208,6 +207,9 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
 		case F2I_CONFIG_MAGIC:
 			comms_handle_config(buffer, length);
 			break;
+		case F2I_MIXER_MAGIC:
+			mixer_handle_text(buffer, length);
+			break;
 		default:
 		    	frame_bad++;
 			break;
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.cpp
index f02e98ae4..6af1dd3dc 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file mixer.c
+ * @file mixer.cpp
  *
  * Control channel input/output mixer and failsafe.
  */
@@ -49,8 +49,11 @@
 #include <fcntl.h>
 
 #include <drivers/drv_pwm_output.h>
+#include <systemlib/mixer/mixer.h>
 
+extern "C" {
 #include "px4io.h"
+}
 
 /*
  * Count of periodic calls in which we have no FMU input.
@@ -58,28 +61,23 @@
 static unsigned fmu_input_drops;
 #define FMU_INPUT_DROP_LIMIT	20
 
-/*
- * Update a mixer based on the current control signals.
- */
-static void	mixer_update(int mixer, uint16_t *inputs, int input_count);
-
 /* current servo arm/disarm state */
 bool mixer_servos_armed = false;
 
-/*
- * Each mixer consumes a set of inputs and produces a single output.
- */
-struct mixer {
-	uint16_t current_value;
-	/* XXX more config here */
-} mixers[IO_SERVO_COUNT];
+/* selected control values and count for mixing */
+static uint16_t *control_values;
+static int control_count;
+
+static int	mixer_callback(uintptr_t handle,
+			uint8_t control_group,
+			uint8_t control_index,
+			float &control);
+
+static MixerGroup mixer_group(mixer_callback, 0);
 
 void
 mixer_tick(void)
 {
-	uint16_t *control_values;
-	int control_count;
-	int i;
 	bool should_arm;
 
 	/*
@@ -115,17 +113,30 @@ mixer_tick(void)
 	}
 
 	/*
-	 * Tickle each mixer, if we have control data.
+	 * Run the mixers if we have any control data at all.
 	 */
 	if (control_count > 0) {
-		for (i = 0; i < IO_SERVO_COUNT; i++) {
-			mixer_update(i, control_values, control_count);
+		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 < IO_SERVO_COUNT; i++) {
+			if (i < mixed) {
+				/* scale to servo output */
+				system_state.servos[i] = (outputs[i] * 1000.0f) + 1000;
+			} else {
+				/* set to zero to inhibit PWM pulse output */
+				system_state.servos[i] = 0;
+			}
 
 			/*
 			 * If we are armed, update the servo output.
 			 */
 			if (system_state.armed && system_state.arm_ok)
-				up_pwm_servo_set(i, mixers[i].current_value);
+				up_pwm_servo_set(i, system_state.servos[i]);
 		}
 	}
 
@@ -145,13 +156,57 @@ mixer_tick(void)
 	}
 }
 
-static void
-mixer_update(int mixer, uint16_t *inputs, int input_count)
+static int
+mixer_callback(uintptr_t handle,
+	       uint8_t control_group,
+	       uint8_t control_index,
+	       float &control)
 {
-	/* simple passthrough for now */
-	if (mixer < input_count) {
-		mixers[mixer].current_value = inputs[mixer];
-	} else {
-		mixers[mixer].current_value = 0;
-	}
+	/* if the control index refers to an input that's not valid, we can't return it */
+	if (control_index >= control_count)
+		return -1;
+
+	/* scale from current PWM units (1000-2000) to mixer input values */
+	/* XXX this presents some ugly problems w.r.t failsafe and R/C input scaling that have to be addressed */
+	control = ((float)control_values[control_index] - 1000.0f) / 1000.0f;
+
+	return 0;
 }
+
+void
+mixer_handle_text(const void *buffer, size_t length)
+{
+	static char mixer_text[256];
+	static unsigned mixer_text_length = 0;
+
+	px4io_mixdata	*msg = (px4io_mixdata *)buffer;
+	if (length < sizeof(px4io_mixdata))
+		return;
+
+	unsigned	text_length = length - sizeof(px4io_mixdata);
+
+	switch (msg->action) {
+	case F2I_MIXER_ACTION_RESET:
+		mixer_group.reset();
+		mixer_text_length = 0;
+		/* FALLTHROUGH */
+	case F2I_MIXER_ACTION_APPEND:
+		/* check for overflow - this is really fatal */
+		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
+			return;
+
+		/* append mixer text and nul-terminate */
+		memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
+		mixer_text_length += text_length;
+		mixer_text[mixer_text_length] = '\0';
+
+		/* process the text buffer, adding new mixers as their descriptions can be parsed */
+		char *end = &mixer_text[mixer_text_length];
+		mixer_group.load_from_buf(&mixer_text[0], mixer_text_length);
+
+		/* copy any leftover text to the base of the buffer for re-use */
+		if (mixer_text_length > 0)
+			memcpy(&mixer_text[0], end - mixer_text_length, mixer_text_length);
+		break;
+	}
+}
\ No newline at end of file
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index c704b1201..080d33001 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -47,25 +47,21 @@
 
 #pragma pack(push, 1)
 
-/* command from FMU to IO */
+/**
+ * Periodic command from FMU to IO.
+ */
 struct px4io_command {
 	uint16_t	f2i_magic;
-#define F2I_MAGIC	0x636d
+#define F2I_MAGIC		0x636d
 
 	uint16_t	servo_command[PX4IO_OUTPUT_CHANNELS];
 	bool		relay_state[PX4IO_RELAY_CHANNELS];
 	bool		arm_ok;
 };
 
-/* config message from FMU to IO */
-struct px4io_config {
-	uint16_t f2i_config_magic;
-#define F2I_CONFIG_MAGIC 0x6366
-
-	/* XXX currently nothing here */
-};
-
-/* report from IO to FMU */
+/**
+ * Periodic report from IO to FMU 
+ */
 struct px4io_report {
 	uint16_t	i2f_magic;
 #define I2F_MAGIC		0x7570
@@ -75,4 +71,34 @@ struct px4io_report {
 	uint8_t		channel_count;
 };
 
+/**
+ * As-needed config message from FMU to IO 
+ */
+struct px4io_config {
+	uint16_t	f2i_config_magic;
+#define F2I_CONFIG_MAGIC	 0x6366
+
+	/* XXX currently nothing here */
+};
+
+/**
+ * As-needed mixer data upload.
+ *
+ * This message adds text to the mixer text buffer; the text
+ * buffer is drained as the definitions are consumed.
+ */
+struct px4io_mixdata {
+	uint16_t	f2i_mixer_magic;
+#define F2I_MIXER_MAGIC		0x6d74
+
+	uint8_t		action;
+#define F2I_MIXER_ACTION_RESET		0
+#define F2I_MIXER_ACTION_APPEND		1
+
+	char		text[0];	/* actual text size may vary */
+};
+
+/* maximum size is limited by the HX frame size */
+#define F2I_MIXER_MAX_TEXT	(sizeof(struct px4io_mixdata) - HX_STREAM_MAX_FRAME)
+
 #pragma pack(pop)
\ No newline at end of file
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 45b7cf847..f127df4a3 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -85,6 +85,11 @@ struct sys_state_s
 	uint16_t	fmu_channel_data[PX4IO_OUTPUT_CHANNELS];
 
 	/*
+	 * Mixed servo outputs
+	 */
+	uint16_t	servos[IO_SERVO_COUNT];
+
+	/*
 	 * Relay controls
 	 */
 	bool		relays[PX4IO_RELAY_CHANNELS];
@@ -149,6 +154,7 @@ extern volatile int	timers[TIMER_NUM_TIMERS];
  * Mixer
  */
 extern void	mixer_tick(void);
+extern void	mixer_handle_text(const void *buffer, size_t length);
 
 /*
  * Safety switch/LED.