From bb9086b87f640f0490d01f52d1b58306a7dbc999 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <michael@swiftradioplanes.com>
Date: Sun, 5 Jun 2016 15:47:55 -0700
Subject: [PATCH] Plane: setup mixer in the one second loop when disarmed

---
 ArduPlane/ArduPlane.cpp     |  7 +++++++
 ArduPlane/control_modes.cpp | 11 ++++-------
 ArduPlane/px4_mixer.cpp     | 21 ++++++++++-----------
 3 files changed, 21 insertions(+), 18 deletions(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index e9fcf52218..b04978bf2b 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -311,6 +311,13 @@ void Plane::one_second_loop()
     // make it possible to change control channel ordering at runtime
     set_control_channels();
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    if (!hal.util->get_soft_armed() && (last_mixer_crc == -1)) {
+        // if disarmed try to configure the mixer
+        setup_failsafe_mixing();
+    }
+#endif // CONFIG_HAL_BOARD
+
     // make it possible to change orientation at runtime
     ahrs.set_orientation();
 
diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp
index 7b7886cb5e..54c9749760 100644
--- a/ArduPlane/control_modes.cpp
+++ b/ArduPlane/control_modes.cpp
@@ -73,17 +73,14 @@ void Plane::read_control_switch()
         // if the user has configured an override channel then check it
         bool override_requested = (hal.rcin->read(g.override_channel-1) >= PX4IO_OVERRIDE_PWM);
         if (override_requested && !px4io_override_enabled) {
-            // we only update the mixer if we are not armed. This is
-            // important as otherwise we will need to temporarily
-            // disarm to change the mixer
-            if (hal.util->get_soft_armed() || setup_failsafe_mixing()) {
+            if (hal.util->get_soft_armed() || (last_mixer_crc != -1)) {
                 px4io_override_enabled = true;
                 // disable output channels to force PX4IO override
                 gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override enabled");
             } else {
-                // we'll try again next loop. The PX4IO code sometimes
-                // rejects a mixer, probably due to it being busy in
-                // some way?
+                // we'll let the one second loop reconfigure the mixer. The
+                // PX4IO code sometimes rejects a mixer, probably due to it
+                // being busy in some way?
                 gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override enable failed");
             }
         } else if (!override_requested && px4io_override_enabled) {
diff --git a/ArduPlane/px4_mixer.cpp b/ArduPlane/px4_mixer.cpp
index 0dca5a19a9..0d3bd05a4f 100644
--- a/ArduPlane/px4_mixer.cpp
+++ b/ArduPlane/px4_mixer.cpp
@@ -235,20 +235,23 @@ bool Plane::setup_failsafe_mixing(void)
     bool ret = false;
     char *buf = NULL;
     const uint16_t buf_size = 2048;
+    uint16_t fileSize, new_crc;
+    int px4io_fd = -1;
+    enum AP_HAL::Util::safety_state old_state = hal.util->safety_switch_state();
+    struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 8};
 
     buf = (char *)malloc(buf_size);
     if (buf == NULL) {
-        return false;
+        goto failed;
     }
 
-    uint16_t fileSize = create_mixer(buf, buf_size, mixer_filename);
+    fileSize = create_mixer(buf, buf_size, mixer_filename);
     if (!fileSize) {
         hal.console->printf("Unable to create mixer\n");
-        free(buf);
-        return false;
+        goto failed;
     }
 
-    uint16_t new_crc = crc_calculate((uint8_t *)buf, fileSize);
+    new_crc = crc_calculate((uint8_t *)buf, fileSize);
 
     if ((int32_t)new_crc == last_mixer_crc) {
         return true;
@@ -256,14 +259,10 @@ bool Plane::setup_failsafe_mixing(void)
         last_mixer_crc = new_crc;
     }
 
-    enum AP_HAL::Util::safety_state old_state = hal.util->safety_switch_state();
-    struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 8};
-
-    int px4io_fd = open("/dev/px4io", 0);
+    px4io_fd = open("/dev/px4io", 0);
     if (px4io_fd == -1) {
         // px4io isn't started, no point in setting up a mixer
-        free(buf);
-        return false;
+        goto failed;
     }
 
     if (old_state == AP_HAL::Util::SAFETY_ARMED) {