From afc850326031f5a3ec170b5bfb4e2b464c7a9d24 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sun, 29 Oct 2017 16:14:32 +0100
Subject: [PATCH] PX4IO Firmware: Fix usage of new operator in IRQ

The mixer was creating a new class in interrupt context, which is not valid use in NuttX. This change copies the mixer into the buffer and runs the mixer management section of the code outside of interrupt context in the normal task.
---
 src/modules/px4iofirmware/mixer.cpp | 71 +++++++++++++++++++++--------
 src/modules/px4iofirmware/px4io.h   |  1 +
 2 files changed, 54 insertions(+), 18 deletions(-)

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index f299bbad5f..014df97141 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -114,6 +114,8 @@ int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits)
 void
 mixer_tick(void)
 {
+	/* check if the mixer got modified */
+	mixer_handle_text_create_mixer();
 
 	/* check that we are receiving fresh data from the FMU */
 	if ((system_state.fmu_data_received_time == 0) ||
@@ -487,6 +489,50 @@ mixer_callback(uintptr_t handle,
 
 static char mixer_text[PX4IO_MAX_MIXER_LENGHT];		/* large enough for one mixer */
 static unsigned mixer_text_length = 0;
+static bool mixer_update_pending = false;
+
+int
+mixer_handle_text_create_mixer()
+{
+	/* only run on update */
+	if (!mixer_update_pending) {
+		return 0;
+	}
+
+	/* do not allow a mixer change while safety off and FMU armed */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
+	    (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+		return 1;
+	}
+
+	/* abort if we're in the mixer - it will be tried again in the next iteration */
+	if (in_mixer) {
+		return 1;
+	}
+
+	/* process the text buffer, adding new mixers as their descriptions can be parsed */
+	unsigned resid = mixer_text_length;
+	mixer_group.load_from_buf(&mixer_text[0], resid);
+
+	/* if anything was parsed */
+	if (resid != mixer_text_length) {
+
+		isr_debug(2, "used %u", mixer_text_length - resid);
+
+		/* copy any leftover text to the base of the buffer for re-use */
+		if (resid > 0) {
+			memmove(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+			/* enforce null termination */
+			mixer_text[resid] = '\0';
+		}
+
+		mixer_text_length = resid;
+	}
+
+	mixer_update_pending = false;
+
+	return 0;
+}
 
 int
 mixer_handle_text(const void *buffer, size_t length)
@@ -533,30 +579,19 @@ mixer_handle_text(const void *buffer, size_t length)
 			return 0;
 		}
 
+		/* check if the last item has been processed - bail out if not */
+		if (mixer_update_pending) {
+			return 1;
+		}
+
 		/* append mixer text and nul-terminate, guard against overflow */
 		memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
 		mixer_text_length += text_length;
 		mixer_text[mixer_text_length] = '\0';
 		isr_debug(2, "buflen %u", mixer_text_length);
 
-		/* process the text buffer, adding new mixers as their descriptions can be parsed */
-		unsigned resid = mixer_text_length;
-		mixer_group.load_from_buf(&mixer_text[0], resid);
-
-		/* if anything was parsed */
-		if (resid != mixer_text_length) {
-
-			isr_debug(2, "used %u", mixer_text_length - resid);
-
-			/* copy any leftover text to the base of the buffer for re-use */
-			if (resid > 0) {
-				memmove(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
-				/* enforce null termination */
-				mixer_text[resid] = '\0';
-			}
-
-			mixer_text_length = resid;
-		}
+		/* flag the buffer as ready */
+		mixer_update_pending = true;
 
 		break;
 	}
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index cb6390cea6..b358a09a64 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -191,6 +191,7 @@ extern pwm_limit_t pwm_limit;
  * Mixer
  */
 extern void	mixer_tick(void);
+extern int	mixer_handle_text_create_mixer(void);
 extern int	mixer_handle_text(const void *buffer, size_t length);
 /* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
 extern void	mixer_set_failsafe(void);