Working on restart resilience, hunting down multi-load mixer issue (still present)

5 files changed, 49 insertions, 14 deletions

diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 8fb53295f..882ca9fed 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -1556,7 +1556,7 @@ px4io_main(int argc, char *argv[])
 			errx(1, "already loaded");
 
 		/* create the driver - it will set g_dev */
-		(void)new PX4IO;
+		(void)new PX4IO();
 
 		if (g_dev == nullptr)
 			errx(1, "driver alloc failed");
@@ -1567,7 +1567,7 @@ px4io_main(int argc, char *argv[])
 		}
 
 		/* look for the optional pwm update rate for the supported modes */
-		if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) {
+		if (argc > 2 && strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) {
 			if (argc > 2 + 1) {
 #warning implement this 
 			} else {
@@ -1579,16 +1579,31 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "recovery")) {
+
+		if (g_dev != nullptr) {
+			/*
+			 * Enable in-air restart support.
+			 * We can cheat and call the driver directly, as it
+		 	 * doesn't reference filp in ioctl()
+			 */
+			g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0);
+		} else {
+			errx(1, "not loaded");
+		}
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "stop")) {
 
-			if (g_dev != nullptr) {
-				/* stop the driver */
-				delete g_dev;
-			} else {
-				errx(1, "not loaded");
-			}
-			exit(0);
+		if (g_dev != nullptr) {
+			/* stop the driver */
+			delete g_dev;
+		} else {
+			errx(1, "not loaded");
 		}
+		exit(0);
+	}
 
 
 	if (!strcmp(argv[1], "status")) {
@@ -1613,8 +1628,9 @@ px4io_main(int argc, char *argv[])
 			exit(1);
 		}
 		uint8_t level = atoi(argv[2]);
-		// we can cheat and call the driver directly, as it
-		// doesn't reference filp in ioctl()
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
 		int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level);
 		if (ret != 0) {
 			printf("SET_DEBUG failed - %d\n", ret);
diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index 0fba2cbe5..54584e685 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -108,9 +108,11 @@ mixer_tick(void)
 	/*
 	 * Decide which set of controls we're using.
 	 */
-	if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) {
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ||
+		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
-		/* don't actually mix anything - we already have raw PWM values */
+		/* don't actually mix anything - we already have raw PWM values or
+		 not a valid mixer. */
 		source = MIX_NONE;
 
 	} else {
@@ -239,6 +241,11 @@ static unsigned mixer_text_length = 0;
 void
 mixer_handle_text(const void *buffer, size_t length)
 {
+	/* do not allow a mixer change while fully armed */
+	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+	    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+		return;
+	}
 
 	px4io_mixdata	*msg = (px4io_mixdata *)buffer;
 
@@ -252,9 +259,12 @@ mixer_handle_text(const void *buffer, size_t length)
 	switch (msg->action) {
 	case F2I_MIXER_ACTION_RESET:
 		isr_debug(2, "reset");
+
+		/* FIRST mark the mixer as invalid */
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
+		/* THEN actually delete it */
 		mixer_group.reset();
 		mixer_text_length = 0;
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
 
 		/* FALLTHROUGH */
 	case F2I_MIXER_ACTION_APPEND:
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index d97fd8d86..511a47f8d 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -361,6 +361,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		break;
 
 	case PX4IO_PAGE_RC_CONFIG: {
+
+		/* do not allow a RC config change while fully armed */
+		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+			break;
+		}
+
 		unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
 		unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
 		uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp
index 72f765d90..df0dfe838 100644
--- a/apps/systemlib/mixer/mixer.cpp
+++ b/apps/systemlib/mixer/mixer.cpp
@@ -54,6 +54,7 @@
 #include "mixer.h"
 
 Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) :
+	_next(nullptr),
 	_control_cb(control_cb),
 	_cb_handle(cb_handle)
 {
diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp
index 60eeff225..1dbc512cd 100644
--- a/apps/systemlib/mixer/mixer_group.cpp
+++ b/apps/systemlib/mixer/mixer_group.cpp
@@ -93,6 +93,7 @@ MixerGroup::reset()
 		mixer = _first;
 		_first = mixer->_next;
 		delete mixer;
+		mixer = nullptr;
 	}
 }