Merged IO mixing branch

4 files changed, 40 insertions, 21 deletions

diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index df07c34d8..c208db3ad 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -71,6 +71,8 @@ static struct px4io_report	report;
 
 static void			comms_handle_frame(void *arg, const void *buffer, size_t length);
 
+perf_counter_t			comms_rx_errors;
+
 static void
 comms_init(void)
 {
@@ -78,6 +80,9 @@ comms_init(void)
 	fmu_fd = open("/dev/ttyS1", O_RDWR);
 	stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
 
+	comms_rx_errors = perf_alloc(PC_COUNT, "rx_err");
+	hx_stream_set_counters(stream, 0, 0, comms_rx_errors);
+
 	/* default state in the report to FMU */
 	report.i2f_magic = I2F_MAGIC;
 
@@ -173,14 +178,12 @@ comms_handle_command(const void *buffer, size_t length)
 	irqstate_t flags = irqsave();
 
 	/* fetch new PWM output values */
-	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) {
-		system_state.fmu_channel_data[i] = cmd->servo_command[i];
-	}
+	for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++)
+		system_state.fmu_channel_data[i] = cmd->output_control[i];
 
-	/* if IO is armed and FMU gets disarmed, IO must also disarm */
-	if (system_state.arm_ok && !cmd->arm_ok) {
+	/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
+	if (system_state.arm_ok && !cmd->arm_ok)
 		system_state.armed = false;
-	}
 
 	system_state.arm_ok = cmd->arm_ok;
 	system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;
diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index b8ea308f3..2acd60ce3 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -56,6 +56,7 @@
 #include <systemlib/mixer/mixer.h>
 
 extern "C" {
+//#define DEBUG
 #include "px4io.h"
 }
 
@@ -94,15 +95,17 @@ mixer_tick(void)
 	/*
 	 * Decide which set of inputs we're using.
 	 */
+	 
 	/* this is for planes, where manual override makes sense */
 	if(system_state.manual_override_ok) {
 		/* if everything is ok */
 		if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
 			/* we have recent control data from the FMU */
-			control_count = PX4IO_OUTPUT_CHANNELS;
+			control_count = PX4IO_CONTROL_CHANNELS;
 			control_values = &system_state.fmu_channel_data[0];
-		/* when override is on or the fmu is not available */
+
 		} else if (system_state.rc_channels > 0) {
+			/* when override is on or the fmu is not available, but RC is present */
 			control_count = system_state.rc_channels;
 			control_values = &system_state.rc_channel_data[0];
 		} else {
@@ -116,7 +119,7 @@ mixer_tick(void)
 	} else {
 		/* if the fmu is available whe are good */
 		if(system_state.mixer_fmu_available) {
-			control_count = PX4IO_OUTPUT_CHANNELS;
+			control_count = PX4IO_CONTROL_CHANNELS;
 			control_values = &system_state.fmu_channel_data[0];
 		/* we better shut everything off */
 		} else {
@@ -183,20 +186,22 @@ mixer_callback(uintptr_t handle,
 		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] - 1500.0f) / 500.0f;
 
 	return 0;
 }
 
+static char mixer_text[256];
+static unsigned mixer_text_length = 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;
 
+	debug("mixer text %u", length);
+
 	if (length < sizeof(px4io_mixdata))
 		return;
 
@@ -204,11 +209,13 @@ mixer_handle_text(const void *buffer, size_t length)
 
 	switch (msg->action) {
 	case F2I_MIXER_ACTION_RESET:
+		debug("reset");
 		mixer_group.reset();
 		mixer_text_length = 0;
 
 		/* FALLTHROUGH */
 	case F2I_MIXER_ACTION_APPEND:
+		debug("append %d", length);
 
 		/* check for overflow - this is really fatal */
 		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
@@ -218,14 +225,22 @@ mixer_handle_text(const void *buffer, size_t length)
 		memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
 		mixer_text_length += text_length;
 		mixer_text[mixer_text_length] = '\0';
+		debug("buflen %u", mixer_text_length);
 
 		/* 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);
+		unsigned resid = mixer_text_length;
+		mixer_group.load_from_buf(&mixer_text[0], resid);
 
-		/* 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);
+		/* if anything was parsed */
+		if (resid != mixer_text_length) {
+			debug("used %u", mixer_text_length - resid);
+
+			/* copy any leftover text to the base of the buffer for re-use */
+			if (resid > 0)
+				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+
+			mixer_text_length = resid;
+		}
 
 		break;
 	}
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 5b485e2ff..c53b94eb0 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -41,7 +41,7 @@
 
 #pragma once
 
-#define PX4IO_OUTPUT_CHANNELS	8
+#define PX4IO_CONTROL_CHANNELS	8
 #define PX4IO_INPUT_CHANNELS	12
 #define PX4IO_RELAY_CHANNELS	4
 
@@ -55,7 +55,8 @@ struct px4io_command {
 #define F2I_MAGIC		0x636d
 
 	uint16_t	servo_command[PX4IO_OUTPUT_CHANNELS];	/**< servo output channels */
-	uint16_t	servo_rate;				/**< PWM output rate in Hz */
+	uint16_t	servo_rate;
+	uint16_t	output_control[PX4IO_CONTROL_CHANNELS]; /**< PWM output rate in Hz */
 	bool		relay_state[PX4IO_RELAY_CHANNELS];	/**< relay states as requested by FMU */
 	bool		arm_ok;					/**< FMU allows full arming */
 	bool		vector_flight_mode_ok;			/**< FMU aquired a valid position lock, ready for pos control */
@@ -102,6 +103,6 @@ struct px4io_mixdata {
 };
 
 /* maximum size is limited by the HX frame size */
-#define F2I_MIXER_MAX_TEXT	(sizeof(struct px4io_mixdata) - HX_STREAM_MAX_FRAME)
+#define F2I_MIXER_MAX_TEXT	(HX_STREAM_MAX_FRAME - sizeof(struct px4io_mixdata))
 
 #pragma pack(pop)
\ No newline at end of file
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 1c9d11e99..fef0a9ba4 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -82,7 +82,7 @@ struct sys_state_s {
 	/**
 	 * Control signals from FMU.
 	 */
-	uint16_t	fmu_channel_data[PX4IO_OUTPUT_CHANNELS];
+	uint16_t	fmu_channel_data[PX4IO_CONTROL_CHANNELS];
 
 	/**
 	 * Mixed servo outputs