From c226f6a4126001610cd6d96d44e307d8f0713223 Mon Sep 17 00:00:00 2001
From: tracernz <michael.corcoran@outlook.com>
Date: Sat, 31 Jan 2015 18:07:05 +1300
Subject: [PATCH 01/39] Tidy up NMEA Code A Little

Still a LOT of tidying up needed in future, of the whole GPS module
really.
---
 src/main/io/gps.c | 228 +++++++++++++++++++++++-----------------------
 1 file changed, 115 insertions(+), 113 deletions(-)

diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index db0f726d3d..dfac152011 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -69,6 +69,8 @@ extern int16_t debug[4];
 #define LOG_UBLOX_POSLLH 'P'
 #define LOG_UBLOX_VELNED 'V'
 
+#define GPS_SV_MAXSATS   16
+
 char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
 static char *gpsPacketLogChar = gpsPacketLog;
 // **********************
@@ -85,11 +87,11 @@ uint16_t GPS_altitude;              // altitude in 0.1m
 uint16_t GPS_speed;                 // speed in 0.1m/s
 uint16_t GPS_ground_course = 0;     // degrees * 10
 
-uint8_t GPS_numCh;                  // Number of channels
-uint8_t GPS_svinfo_chn[16];         // Channel number
-uint8_t GPS_svinfo_svid[16];        // Satellite ID
-uint8_t GPS_svinfo_quality[16];     // Bitfield Qualtity
-uint8_t GPS_svinfo_cno[16];         // Carrier to Noise Ratio (Signal Strength)
+uint8_t GPS_numCh;                          // Number of channels
+uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS];     // Channel number
+uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS];    // Satellite ID
+uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity
+uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS];     // Carrier to Noise Ratio (Signal Strength)
 
 static gpsConfig_t *gpsConfig;
 
@@ -514,18 +516,18 @@ static uint32_t grab_fields(char *src, uint8_t mult)
     return tmp;
 }
 
+typedef struct gpsDataNmea_s {
+    int32_t latitude;
+    int32_t longitude;
+    uint8_t numSat;
+    uint16_t altitude;
+    uint16_t speed;
+    uint16_t ground_course;
+} gpsDataNmea_t;
+
 static bool gpsNewFrameNMEA(char c)
 {
-    typedef struct gpsdata_s {
-        int32_t latitude;
-        int32_t longitude;
-        uint8_t numSat;
-        uint16_t altitude;
-        uint16_t speed;
-        uint16_t ground_course;
-    } gpsdata_t;
-
-    static gpsdata_t gps_Msg;
+    static gpsDataNmea_t gps_Msg;
 
     uint8_t frameOK = 0;
     static uint8_t param = 0, offset = 0, parity = 0;
@@ -533,111 +535,111 @@ static bool gpsNewFrameNMEA(char c)
     static uint8_t checksum_param, gps_frame = NO_FRAME;
 
     switch (c) {
-    case '$':
-        param = 0;
-        offset = 0;
-        parity = 0;
-        break;
-    case ',':
-    case '*':
-        string[offset] = 0;
-        if (param == 0) {       //frame identification
-            gps_frame = NO_FRAME;
-            if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
-                gps_frame = FRAME_GGA;
-            if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
-                gps_frame = FRAME_RMC;
-        }
-
-        switch (gps_frame) {
-        case FRAME_GGA:        //************* GPGGA FRAME parsing
-            switch (param) {
-//          case 1:             // Time information
-//              break;
-            case 2:
-                gps_Msg.latitude = GPS_coord_to_degrees(string);
-                break;
-            case 3:
-                if (string[0] == 'S')
-                    gps_Msg.latitude *= -1;
-                break;
-            case 4:
-                gps_Msg.longitude = GPS_coord_to_degrees(string);
-                break;
-            case 5:
-                if (string[0] == 'W')
-                    gps_Msg.longitude *= -1;
-                break;
-            case 6:
-                if (string[0] > '0') {
-                    ENABLE_STATE(GPS_FIX);
-                } else {
-                    DISABLE_STATE(GPS_FIX);
-                }
-                break;
-            case 7:
-                gps_Msg.numSat = grab_fields(string, 0);
-                break;
-            case 9:
-                gps_Msg.altitude = grab_fields(string, 0);     // altitude in meters added by Mis
-                break;
-            }
+        case '$':
+            param = 0;
+            offset = 0;
+            parity = 0;
             break;
-        case FRAME_RMC:        //************* GPRMC FRAME parsing
-            switch (param) {
-            case 7:
-                gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
-                break;
-            case 8:
-                gps_Msg.ground_course = (grab_fields(string, 1));      // ground course deg * 10
-                break;
+        case ',':
+        case '*':
+            string[offset] = 0;
+            if (param == 0) {       //frame identification
+                gps_frame = NO_FRAME;
+                if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
+                    gps_frame = FRAME_GGA;
+                if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
+                    gps_frame = FRAME_RMC;
             }
-            break;
-        }
 
-        param++;
-        offset = 0;
-        if (c == '*')
-            checksum_param = 1;
-        else
-            parity ^= c;
-        break;
-    case '\r':
-    case '\n':
-        if (checksum_param) {   //parity checksum
-            shiftPacketLog();
-            uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
-            if (checksum == parity) {
-                *gpsPacketLogChar = LOG_IGNORED;
-                GPS_packetCount++;
-                switch (gps_frame) {
-                case FRAME_GGA:
-                  *gpsPacketLogChar = LOG_NMEA_GGA;
-                  frameOK = 1;
-                  if (STATE(GPS_FIX)) {
-                        GPS_coord[LAT] = gps_Msg.latitude;
-                        GPS_coord[LON] = gps_Msg.longitude;
-                        GPS_numSat = gps_Msg.numSat;
-                        GPS_altitude = gps_Msg.altitude;
+            switch (gps_frame) {
+                case FRAME_GGA:        //************* GPGGA FRAME parsing
+                    switch(param) {
+            //          case 1:             // Time information
+            //              break;
+                        case 2:
+                            gps_Msg.latitude = GPS_coord_to_degrees(string);
+                            break;
+                        case 3:
+                            if (string[0] == 'S')
+                                gps_Msg.latitude *= -1;
+                            break;
+                        case 4:
+                            gps_Msg.longitude = GPS_coord_to_degrees(string);
+                            break;
+                        case 5:
+                            if (string[0] == 'W')
+                                gps_Msg.longitude *= -1;
+                            break;
+                        case 6:
+                            if (string[0] > '0') {
+                                ENABLE_STATE(GPS_FIX);
+                            } else {
+                                DISABLE_STATE(GPS_FIX);
+                            }
+                            break;
+                        case 7:
+                            gps_Msg.numSat = grab_fields(string, 0);
+                            break;
+                        case 9:
+                            gps_Msg.altitude = grab_fields(string, 0);     // altitude in meters added by Mis
+                            break;
                     }
                     break;
-                case FRAME_RMC:
-                    *gpsPacketLogChar = LOG_NMEA_RMC;
-                    GPS_speed = gps_Msg.speed;
-                    GPS_ground_course = gps_Msg.ground_course;
+                case FRAME_RMC:        //************* GPRMC FRAME parsing
+                    switch(param) {
+                        case 7:
+                            gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
+                            break;
+                        case 8:
+                            gps_Msg.ground_course = (grab_fields(string, 1));      // ground course deg * 10
+                            break;
+                    }
                     break;
-                } // end switch
-            } else {
-                *gpsPacketLogChar = LOG_ERROR;
             }
-        }
-        checksum_param = 0;
-        break;
-    default:
-        if (offset < 15)
-            string[offset++] = c;
-        if (!checksum_param)
-            parity ^= c;
+
+            param++;
+            offset = 0;
+            if (c == '*')
+                checksum_param = 1;
+            else
+                parity ^= c;
+            break;
+        case '\r':
+        case '\n':
+            if (checksum_param) {   //parity checksum
+                shiftPacketLog();
+                uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
+                if (checksum == parity) {
+                    *gpsPacketLogChar = LOG_IGNORED;
+                    GPS_packetCount++;
+                    switch (gps_frame) {
+                    case FRAME_GGA:
+                      *gpsPacketLogChar = LOG_NMEA_GGA;
+                      frameOK = 1;
+                      if (STATE(GPS_FIX)) {
+                            GPS_coord[LAT] = gps_Msg.latitude;
+                            GPS_coord[LON] = gps_Msg.longitude;
+                            GPS_numSat = gps_Msg.numSat;
+                            GPS_altitude = gps_Msg.altitude;
+                        }
+                        break;
+                    case FRAME_RMC:
+                        *gpsPacketLogChar = LOG_NMEA_RMC;
+                        GPS_speed = gps_Msg.speed;
+                        GPS_ground_course = gps_Msg.ground_course;
+                        break;
+                    } // end switch
+                } else {
+                    *gpsPacketLogChar = LOG_ERROR;
+                }
+            }
+            checksum_param = 0;
+            break;
+        default:
+            if (offset < 15)
+                string[offset++] = c;
+            if (!checksum_param)
+                parity ^= c;
     }
     return frameOK;
 }

From cda2f3169e2974376b40ada5813324315c21e613 Mon Sep 17 00:00:00 2001
From: tracernz <michael.corcoran@outlook.com>
Date: Sat, 31 Jan 2015 18:08:36 +1300
Subject: [PATCH 02/39] Add NMEA $GPGSV Decoding for Sat Data

---
 src/main/io/gps.c | 48 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 48 insertions(+)

diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index dfac152011..c2896d87df 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -452,6 +452,7 @@ bool gpsNewFrame(uint8_t c)
 #define NO_FRAME   0
 #define FRAME_GGA  1
 #define FRAME_RMC  2
+#define FRAME_GSV  3
 
 
 // This code is used for parsing NMEA data
@@ -533,6 +534,8 @@ static bool gpsNewFrameNMEA(char c)
     static uint8_t param = 0, offset = 0, parity = 0;
     static char string[15];
     static uint8_t checksum_param, gps_frame = NO_FRAME;
+    static uint8_t svMessageNum = 0;
+    uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0;
 
     switch (c) {
         case '$':
@@ -549,6 +552,8 @@ static bool gpsNewFrameNMEA(char c)
                     gps_frame = FRAME_GGA;
                 if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
                     gps_frame = FRAME_RMC;
+                if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'S' && string[4] == 'V')
+                    gps_frame = FRAME_GSV;
             }
 
             switch (gps_frame) {
@@ -595,6 +600,49 @@ static bool gpsNewFrameNMEA(char c)
                             break;
                     }
                     break;
+                case FRAME_GSV:
+                    switch(param) {
+                      /*case 1:
+                            // Total number of messages of this type in this cycle
+                            break; */
+                        case 2:
+                            // Message number
+                            svMessageNum = grab_fields(string, 0);
+                            break;
+                        case 3:
+                            // Total number of SVs visible
+                            GPS_numCh = grab_fields(string, 0);
+                            break;
+                    }
+                    if(param < 4)
+                        break;
+
+                    svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4
+                    svSatNum    = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number
+                    svSatParam  = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite
+
+                    if(svSatNum > GPS_SV_MAXSATS)
+                        break;
+
+                    switch(svSatParam) {
+                        case 1:
+                            // SV PRN number
+                            GPS_svinfo_chn[svSatNum - 1]  = svSatNum;
+                            GPS_svinfo_svid[svSatNum - 1] = grab_fields(string, 0);
+                            break;
+                      /*case 2:
+                            // Elevation, in degrees, 90 maximum
+                            break;
+                        case 3:
+                            // Azimuth, degrees from True North, 000 through 359
+                            break; */
+                        case 4:
+                            // SNR, 00 through 99 dB (null when not tracking)
+                            GPS_svinfo_cno[svSatNum - 1] = grab_fields(string, 0);
+                            GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox
+                            break;
+                    }
+                    break;
             }
 
             param++;

From e56f46a7561625449ae47599c132a75090ddf044 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 11:59:10 +0100
Subject: [PATCH 03/39] Move utility macros to common/utils.h

---
 src/main/blackbox/blackbox.c | 6 +-----
 src/main/common/utils.h      | 6 ++++++
 2 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 0ee1234768..a8db32f82d 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -26,6 +26,7 @@
 #include "common/maths.h"
 #include "common/axis.h"
 #include "common/color.h"
+#include "common/utils.h"
 
 #include "drivers/gpio.h"
 #include "drivers/sensor.h"
@@ -86,11 +87,6 @@
     typedef char assert_failed_ ## name [(condition) ? 1 : -1 ]
 
 // Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter:
-#define STR_HELPER(x) #x
-#define STR(x) STR_HELPER(x)
-
-#define CONCAT_HELPER(x,y) x ## y
-#define CONCAT(x,y) CONCAT_HELPER(x, y)
 
 #define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)
 #define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x)
diff --git a/src/main/common/utils.h b/src/main/common/utils.h
index d9f22c01e9..90e12ac759 100644
--- a/src/main/common/utils.h
+++ b/src/main/common/utils.h
@@ -21,6 +21,12 @@
 
 #define ARRAYLEN(x) (sizeof(x) / sizeof((x)[0]))
 
+#define CONCAT_HELPER(x,y) x ## y
+#define CONCAT(x,y) CONCAT_HELPER(x, y)
+
+#define STR_HELPER(x) #x
+#define STR(x) STR_HELPER(x)
+
 /*
 http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html
 */

From 6b0fea5952da024c1417e60cfb03cbcbabee73f4 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 14:40:14 +0100
Subject: [PATCH 04/39] simplify `#ifdef`s

---
 src/main/config/config.c | 10 +++-------
 1 file changed, 3 insertions(+), 7 deletions(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index b489ff3bd7..03ade62284 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -723,16 +723,12 @@ void validateAndFixConfig(void)
 
 #if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
     if (feature(FEATURE_SOFTSERIAL) && (
+            0
 #ifdef USE_SOFTSERIAL1
-            (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER)
-#else
-            0
+            || (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER)
 #endif
-            ||
 #ifdef USE_SOFTSERIAL2
-            (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER)
-#else
-            0
+            || (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER)
 #endif
     )) {
         // led strip needs the same timer as softserial

From d3324a9f0dbf7856edee8f4d68edc0941522639e Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 14:43:40 +0100
Subject: [PATCH 05/39] Move ADC internal interface into new header file

---
 src/main/drivers/adc.c           |  1 +
 src/main/drivers/adc_impl.h      | 21 +++++++++++++++++++++
 src/main/drivers/adc_stm32f10x.c |  3 +--
 src/main/drivers/adc_stm32f30x.c |  4 +---
 4 files changed, 24 insertions(+), 5 deletions(-)
 create mode 100644 src/main/drivers/adc_impl.h

diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c
index 958a831b46..bdc7a67748 100644
--- a/src/main/drivers/adc.c
+++ b/src/main/drivers/adc.c
@@ -24,6 +24,7 @@
 #include "system.h"
 
 #include "adc.h"
+#include "adc_impl.h"
 
 //#define DEBUG_ADC_CHANNELS
 
diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h
new file mode 100644
index 0000000000..8007308251
--- /dev/null
+++ b/src/main/drivers/adc_impl.h
@@ -0,0 +1,21 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+extern adc_config_t adcConfig[ADC_CHANNEL_COUNT];
+extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c
index c6c97c2f45..2becc32df0 100644
--- a/src/main/drivers/adc_stm32f10x.c
+++ b/src/main/drivers/adc_stm32f10x.c
@@ -31,6 +31,7 @@
 #include "accgyro.h"
 
 #include "adc.h"
+#include "adc_impl.h"
 
 // Driver for STM32F103CB onboard ADC
 //
@@ -43,8 +44,6 @@
 //
 // CC3D Only one ADC channel supported currently, for battery on S5_IN/PA0
 
-extern adc_config_t adcConfig[ADC_CHANNEL_COUNT];
-extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
 
 void adcInit(drv_adc_config_t *init)
 {
diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c
index c18b37417b..3b2fa18592 100644
--- a/src/main/drivers/adc_stm32f30x.c
+++ b/src/main/drivers/adc_stm32f30x.c
@@ -28,9 +28,7 @@
 #include "accgyro.h"
 
 #include "adc.h"
-
-extern adc_config_t adcConfig[ADC_CHANNEL_COUNT];
-extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
+#include "adc_impl.h"
 
 #ifndef ADC_INSTANCE
 #define ADC_INSTANCE                ADC1

From 5129403c83269b06b4339019d316a5e59d3279bd Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 14:45:22 +0100
Subject: [PATCH 06/39] minor I2C cleanup

---
 src/main/drivers/bus_i2c_stm32f10x.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c
index fa0e27c439..20e1396985 100644
--- a/src/main/drivers/bus_i2c_stm32f10x.c
+++ b/src/main/drivers/bus_i2c_stm32f10x.c
@@ -184,7 +184,7 @@ bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
 static void i2c_er_handler(void)
 {
     // Read the I2Cx status register
-    volatile uint32_t SR1Register = I2Cx->SR1;
+    uint32_t SR1Register = I2Cx->SR1;
 
     if (SR1Register & 0x0F00) {                                         // an error
         error = true;
@@ -333,7 +333,7 @@ void i2cInit(I2CDevice index)
     I2C_DeInit(I2Cx);
     I2C_StructInit(&i2c);
 
-    I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE);               // Enable EVT and ERR interrupts - they are enabled by the first request
+    I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE);               // Disable EVT and ERR interrupts - they are enabled by the first request
     i2c.I2C_Mode = I2C_Mode_I2C;
     i2c.I2C_DutyCycle = I2C_DutyCycle_2;
     i2c.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;

From f9f40fb98fa499a557e3b0acf27326e3fc0858b3 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 14:48:21 +0100
Subject: [PATCH 07/39] Handle possible problem with unintentional I2C
 interrupt handler triggering

Error handler may be called repeatedly during I2C unstucking. Not sure if this change is necessary, but it is safe
---
 src/main/drivers/bus_i2c_stm32f10x.c | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c
index 20e1396985..026dccf507 100644
--- a/src/main/drivers/bus_i2c_stm32f10x.c
+++ b/src/main/drivers/bus_i2c_stm32f10x.c
@@ -325,6 +325,9 @@ void i2cInit(I2CDevice index)
     I2Cx_index = index;
     RCC_APB1PeriphClockCmd(i2cHardwareMap[index].peripheral, ENABLE);
 
+    // diable I2C interrrupts first to avoid ER handler triggering
+    I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE);
+
     // clock out stuff to make sure slaves arent stuck
     // This will also configure GPIO as AF_OD at the end
     i2cUnstick();

From fd355caca6b5118102af7bf676cc15620dba03c9 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 14:56:21 +0100
Subject: [PATCH 08/39] Modify some macros so they evaluate to single statement

Require macros to be followed with semicolon, empty version expands to dummy statement.

Fixes dangling-else problem:

```
if(1)
        INVERTER_ON;
else
        INVERTER_OFF;
```
---
 src/main/drivers/inverter.h  |  8 +++---
 src/main/drivers/light_led.h | 48 ++++++++++++++++++------------------
 2 files changed, 28 insertions(+), 28 deletions(-)

diff --git a/src/main/drivers/inverter.h b/src/main/drivers/inverter.h
index 81bd432956..017df613b0 100644
--- a/src/main/drivers/inverter.h
+++ b/src/main/drivers/inverter.h
@@ -18,11 +18,11 @@
 #pragma once
 
 #ifdef INVERTER
-#define INVERTER_OFF digitalLo(INVERTER_GPIO, INVERTER_PIN);
-#define INVERTER_ON digitalHi(INVERTER_GPIO, INVERTER_PIN);
+#define INVERTER_OFF digitalLo(INVERTER_GPIO, INVERTER_PIN)
+#define INVERTER_ON digitalHi(INVERTER_GPIO, INVERTER_PIN)
 #else
-#define INVERTER_OFF ;
-#define INVERTER_ON ;
+#define INVERTER_OFF do {} while(0)
+#define INVERTER_ON do {} while(0)
 #endif
 
 void initInverter(void);
diff --git a/src/main/drivers/light_led.h b/src/main/drivers/light_led.h
index 8f62bd5ab6..cf6a2bea97 100644
--- a/src/main/drivers/light_led.h
+++ b/src/main/drivers/light_led.h
@@ -19,49 +19,49 @@
 
 // Helpful macros
 #ifdef LED0
-#define LED0_TOGGLE              digitalToggle(LED0_GPIO, LED0_PIN);
+#define LED0_TOGGLE              digitalToggle(LED0_GPIO, LED0_PIN)
 #ifndef LED0_INVERTED
-#define LED0_OFF                 digitalHi(LED0_GPIO, LED0_PIN);
-#define LED0_ON                  digitalLo(LED0_GPIO, LED0_PIN);
+#define LED0_OFF                 digitalHi(LED0_GPIO, LED0_PIN)
+#define LED0_ON                  digitalLo(LED0_GPIO, LED0_PIN)
 #else
-#define LED0_OFF                 digitalLo(LED0_GPIO, LED0_PIN);
-#define LED0_ON                  digitalHi(LED0_GPIO, LED0_PIN);
+#define LED0_OFF                 digitalLo(LED0_GPIO, LED0_PIN)
+#define LED0_ON                  digitalHi(LED0_GPIO, LED0_PIN)
 #endif // inverted
 #else
-#define LED0_TOGGLE
-#define LED0_OFF
-#define LED0_ON
+#define LED0_TOGGLE              do {} while(0)
+#define LED0_OFF                 do {} while(0)
+#define LED0_ON                  do {} while(0)
 #endif
 
 #ifdef LED1
-#define LED1_TOGGLE              digitalToggle(LED1_GPIO, LED1_PIN);
+#define LED1_TOGGLE              digitalToggle(LED1_GPIO, LED1_PIN)
 #ifndef LED1_INVERTED
-#define LED1_OFF                 digitalHi(LED1_GPIO, LED1_PIN);
-#define LED1_ON                  digitalLo(LED1_GPIO, LED1_PIN);
+#define LED1_OFF                 digitalHi(LED1_GPIO, LED1_PIN)
+#define LED1_ON                  digitalLo(LED1_GPIO, LED1_PIN)
 #else
-#define LED1_OFF                 digitalLo(LED1_GPIO, LED1_PIN);
-#define LED1_ON                  digitalHi(LED1_GPIO, LED1_PIN);
+#define LED1_OFF                 digitalLo(LED1_GPIO, LED1_PIN)
+#define LED1_ON                  digitalHi(LED1_GPIO, LED1_PIN)
 #endif // inverted
 #else
-#define LED1_TOGGLE
-#define LED1_OFF
-#define LED1_ON
+#define LED1_TOGGLE              do {} while(0)
+#define LED1_OFF                 do {} while(0)
+#define LED1_ON                  do {} while(0)
 #endif
 
 
 #ifdef LED2
-#define LED2_TOGGLE              digitalToggle(LED2_GPIO, LED2_PIN);
+#define LED2_TOGGLE              digitalToggle(LED2_GPIO, LED2_PIN)
 #ifndef LED2_INVERTED
-#define LED2_OFF                 digitalHi(LED2_GPIO, LED2_PIN);
-#define LED2_ON                  digitalLo(LED2_GPIO, LED2_PIN);
+#define LED2_OFF                 digitalHi(LED2_GPIO, LED2_PIN)
+#define LED2_ON                  digitalLo(LED2_GPIO, LED2_PIN)
 #else
-#define LED2_OFF                 digitalLo(LED2_GPIO, LED2_PIN);
-#define LED2_ON                  digitalHi(LED2_GPIO, LED2_PIN);
+#define LED2_OFF                 digitalLo(LED2_GPIO, LED2_PIN)
+#define LED2_ON                  digitalHi(LED2_GPIO, LED2_PIN)
 #endif // inverted
 #else
-#define LED2_TOGGLE
-#define LED2_OFF
-#define LED2_ON
+#define LED2_TOGGLE              do {} while(0)
+#define LED2_OFF                 do {} while(0)
+#define LED2_ON                  do {} while(0)
 #endif
 
 void ledInit(void);

From 7c263254b2cfaeeae4e2e99f55b682209e60cd89 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 14:56:59 +0100
Subject: [PATCH 09/39] improve loop readability

---
 src/main/drivers/pwm_mapping.c | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index 6ac1e78475..533d980746 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -351,13 +351,10 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
 
     setup = hardwareMaps[i];
 
-    for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
+    for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT && setup[i] != 0xFFFF; i++) {
         uint8_t timerIndex = setup[i] & 0x00FF;
         uint8_t type = (setup[i] & 0xFF00) >> 8;
 
-        if (setup[i] == 0xFFFF) // terminator
-            break;
-
         const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex];
 
 #ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER

From 7c62ec975534326b2b8cec4be76fc2d671eb27fd Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 15:05:28 +0100
Subject: [PATCH 10/39] Move internal serial interface into separate header
 file

Declaring function from other file is quite dangerous - there is no warning if interfaces get out of sync
---
 src/main/drivers/serial_uart.c           |  5 +---
 src/main/drivers/serial_uart.h           |  2 --
 src/main/drivers/serial_uart_impl.h      | 29 ++++++++++++++++++++++++
 src/main/drivers/serial_uart_stm32f10x.c |  3 +--
 src/main/drivers/serial_uart_stm32f30x.c |  3 +--
 5 files changed, 32 insertions(+), 10 deletions(-)
 create mode 100644 src/main/drivers/serial_uart_impl.h

diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c
index 0057b7ba92..90b7648f3a 100644
--- a/src/main/drivers/serial_uart.c
+++ b/src/main/drivers/serial_uart.c
@@ -34,10 +34,7 @@
 
 #include "serial.h"
 #include "serial_uart.h"
-
-uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode);
-uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode);
-uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode);
+#include "serial_uart_impl.h"
 
 static void usartConfigurePinInversion(uartPort_t *uartPort) {
 #if !defined(INVERTER) && !defined(STM32F303xC)
diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h
index 817929f288..6a7357d26d 100644
--- a/src/main/drivers/serial_uart.h
+++ b/src/main/drivers/serial_uart.h
@@ -44,8 +44,6 @@ typedef struct {
     USART_TypeDef *USARTx;
 } uartPort_t;
 
-extern const struct serialPortVTable uartVTable[];
-
 serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion);
 
 // serialPort API
diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h
new file mode 100644
index 0000000000..713405bc09
--- /dev/null
+++ b/src/main/drivers/serial_uart_impl.h
@@ -0,0 +1,29 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+// device specific uart implementation is defined here
+
+extern const struct serialPortVTable uartVTable[];
+
+void uartStartTxDMA(uartPort_t *s);
+
+uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode);
+uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode);
+uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode);
+
diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c
index 5369206dc3..02a99b3f66 100644
--- a/src/main/drivers/serial_uart_stm32f10x.c
+++ b/src/main/drivers/serial_uart_stm32f10x.c
@@ -34,6 +34,7 @@
 
 #include "serial.h"
 #include "serial_uart.h"
+#include "serial_uart_impl.h"
 
 #ifdef USE_USART1
 static uartPort_t uartPort1;
@@ -54,8 +55,6 @@ static uartPort_t uartPort3;
 #undef USE_USART1_RX_DMA
 #endif
 
-void uartStartTxDMA(uartPort_t *s);
-
 void usartIrqCallback(uartPort_t *s)
 {
     uint16_t SR = s->USARTx->SR;
diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c
index f7bc087e08..4436fc291b 100644
--- a/src/main/drivers/serial_uart_stm32f30x.c
+++ b/src/main/drivers/serial_uart_stm32f30x.c
@@ -35,6 +35,7 @@
 
 #include "serial.h"
 #include "serial_uart.h"
+#include "serial_uart_impl.h"
 
 // Using RX DMA disables the use of receive callbacks
 #define USE_USART1_RX_DMA
@@ -74,8 +75,6 @@ static uartPort_t uartPort1;
 static uartPort_t uartPort2;
 static uartPort_t uartPort3;
 
-void uartStartTxDMA(uartPort_t *s);
-
 uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
 {
     uartPort_t *s;

From 7875b97aae9689b9dce72373f817245d472cc766 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 15:11:27 +0100
Subject: [PATCH 11/39] Compile serial code conditionally on stm32f303

---
 src/main/drivers/serial_uart_stm32f30x.c | 19 ++++++++++++++++++-
 1 file changed, 18 insertions(+), 1 deletion(-)

diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c
index 4436fc291b..10d8cdd9eb 100644
--- a/src/main/drivers/serial_uart_stm32f30x.c
+++ b/src/main/drivers/serial_uart_stm32f30x.c
@@ -71,10 +71,17 @@
 #define UART3_RX_PINSOURCE  GPIO_PinSource11
 #endif
 
+#ifdef USE_USART1
 static uartPort_t uartPort1;
+#endif
+#ifdef USE_USART2
 static uartPort_t uartPort2;
+#endif
+#ifdef USE_USART3
 static uartPort_t uartPort3;
+#endif
 
+#ifdef USE_USART1
 uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
 {
     uartPort_t *s;
@@ -147,7 +154,9 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
 
     return s;
 }
+#endif
 
+#ifdef USE_USART2
 uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode)
 {
     uartPort_t *s;
@@ -226,7 +235,9 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode)
 
     return s;
 }
+#endif
 
+#ifdef USE_USART3
 uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode)
 {
     uartPort_t *s;
@@ -305,6 +316,7 @@ uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode)
 
     return s;
 }
+#endif
 
 static void handleUsartTxDma(uartPort_t *s)
 {
@@ -380,24 +392,29 @@ void usartIrqHandler(uartPort_t *s)
     }
 }
 
+#ifdef USE_USART1
 void USART1_IRQHandler(void)
 {
     uartPort_t *s = &uartPort1;
 
     usartIrqHandler(s);
 }
+#endif
 
+#ifdef USE_USART2
 void USART2_IRQHandler(void)
 {
     uartPort_t *s = &uartPort2;
 
     usartIrqHandler(s);
 }
+#endif
 
+#ifdef USE_USART3
 void USART3_IRQHandler(void)
 {
     uartPort_t *s = &uartPort3;
 
     usartIrqHandler(s);
 }
-
+#endif

From 9dd7faeefafb999555ec404e212c5293dc4c4a59 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 15:05:56 +0100
Subject: [PATCH 12/39] Remove unneeded volatile

---
 src/main/drivers/system.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index d5a4860fd8..2de34f3072 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -31,7 +31,7 @@
 #include "system.h"
 
 // cycles per microsecond
-static volatile uint32_t usTicks = 0;
+static uint32_t usTicks = 0;
 // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
 static volatile uint32_t sysTickUptime = 0;
 

From 6b37b960481a9c20150802d5abea19d0961463e2 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 15:11:27 +0100
Subject: [PATCH 13/39] Simplify #ifdef nesting

---
 src/main/io/serial.c | 22 +++++++---------------
 1 file changed, 7 insertions(+), 15 deletions(-)

diff --git a/src/main/io/serial.c b/src/main/io/serial.c
index 4c9b5c4d75..a7e8495a1b 100644
--- a/src/main/io/serial.c
+++ b/src/main/io/serial.c
@@ -101,9 +101,8 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
 #endif
 };
 
-#else
+#elif defined(CC3D)
 
-#ifdef CC3D
 static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
 #ifdef USE_VCP
     {SERIAL_PORT_USB_VCP,     NULL, SCENARIO_UNUSED, FUNCTION_NONE},
@@ -140,7 +139,7 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
     {SERIAL_PORT_SOFTSERIAL2,   9600, 19200,    SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}
 #endif
 };
-#endif
+
 #endif
 
 const functionConstraint_t functionConstraints[] = {
@@ -271,25 +270,18 @@ serialPortSearchResult_t *findNextSerialPort(serialPortFunction_e function, cons
         uint8_t serialPortIndex = lookupSerialPortIndexByIdentifier(serialPortFunction->identifier);
         const serialPortConstraint_t *serialPortConstraint = &serialPortConstraints[serialPortIndex];
 
-#if defined(CC3D)
-        if (!feature(FEATURE_SOFTSERIAL) && (
-                serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL1)) {
+#if defined(USE_SOFTSERIAL1)
+        if (!feature(FEATURE_SOFTSERIAL) && serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL1)
             continue;
-        }
-#else
-#if defined(USE_SOFTSERIAL1) ||(defined(USE_SOFTSERIAL2))
-        if (!feature(FEATURE_SOFTSERIAL) && (
-                serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL1 ||
-                serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL2
-        )) {
+#endif
+#if defined(USE_SOFTSERIAL2)
+        if (!feature(FEATURE_SOFTSERIAL) && serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL2)
             continue;
-        }
 #endif
 #if (defined(NAZE) || defined(OLIMEXINO)) && defined(SONAR)
         if (feature(FEATURE_SONAR) && !feature(FEATURE_RX_PARALLEL_PWM) && (serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL2)) {
             continue;
         }
-#endif
 #endif
 
         if ((serialPortConstraint->feature & functionConstraint->requiredSerialPortFeatures) != functionConstraint->requiredSerialPortFeatures) {

From 1ecbdf3daeef4b73941a0e0fb05bc20ab909c902 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 15:11:27 +0100
Subject: [PATCH 14/39] Minor code cleanup

---
 src/main/io/serial_cli.c       |  8 ++++----
 src/main/sensors/battery.c     | 15 ++++++---------
 src/main/telemetry/smartport.c |  5 ++---
 3 files changed, 12 insertions(+), 16 deletions(-)

diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 29c2f5497c..4ee885942f 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -1437,16 +1437,16 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value)
     switch (var->type & VALUE_TYPE_MASK) {
         case VAR_UINT8:
         case VAR_INT8:
-            *(char *)ptr = (char)value.int_value;
+            *(int8_t *)ptr = value.int_value;
             break;
 
         case VAR_UINT16:
         case VAR_INT16:
-            *(short *)ptr = (short)value.int_value;
+            *(int16_t *)ptr = value.int_value;
             break;
 
         case VAR_UINT32:
-            *(int *)ptr = (int)value.int_value;
+            *(uint32_t *)ptr = value.int_value;
             break;
 
         case VAR_FLOAT:
@@ -1639,7 +1639,7 @@ void cliProcess(void)
             }
             for (; i < bufferIndex; i++)
                 cliWrite(cliBuffer[i]);
-        } else if (!bufferIndex && c == 4) {
+        } else if (!bufferIndex && c == 4) {   // CTRL-D
             cliExit(cliBuffer);
             return;
         } else if (c == 12) {
diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c
index a367cb8f85..83fb29d82b 100644
--- a/src/main/sensors/battery.c
+++ b/src/main/sensors/battery.c
@@ -92,23 +92,20 @@ void batteryInit(batteryConfig_t *initialBatteryConfig)
         delay((32 / BATTERY_SAMPLE_COUNT) * 10);
     }
 
-    // autodetect cell count, going from 1S..8S
-    for (i = 1; i < 8; i++) {
-        if (vbat < i * batteryConfig->vbatmaxcellvoltage)
-            break;
-    }
-
-    batteryCellCount = i;
+    unsigned cells = (vbat / batteryConfig->vbatmaxcellvoltage) + 1;
+    if(cells > 8)            // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
+        cells = 8;
+    batteryCellCount = cells;
     batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage;
     batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage;
 }
 
-#define ADCVREF 33L
+#define ADCVREF 3300   // in mV
 int32_t currentSensorToCentiamps(uint16_t src)
 {
     int32_t millivolts;
 
-    millivolts = ((uint32_t)src * ADCVREF * 100) / 4095;
+    millivolts = ((uint32_t)src * ADCVREF) / 4096;
     millivolts -= batteryConfig->currentMeterOffset;
 
     return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 01c8d74a6f..c17eec1c17 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -322,13 +322,12 @@ void handleSmartPortTelemetry(void)
         smartPortIdCnt++;
 
         int32_t tmpi;
-        uint32_t tmpui;
         static uint8_t t1Cnt = 0;
 
         switch(id) {
             case FSSP_DATAID_SPEED      :
                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
-                    tmpui = (GPS_speed * 36 + 36 / 2) / 100;
+                    uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100;
                     smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H
                     smartPortHasRequest = 0;
                 }
@@ -355,7 +354,7 @@ void handleSmartPortTelemetry(void)
             //case FSSP_DATAID_ADC2       :
             case FSSP_DATAID_LATLONG    :
                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
-                    tmpui = 0;
+                    uint32_t tmpui = 0;
                     // the same ID is sent twice, one for longitude, one for latitude
                     // the MSB of the sent uint32_t helps FrSky keep track
                     // the even/odd bit of our counter helps us keep track

From f1ac4f8461b389a2e04e8a4ac557c7257a2d73a6 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 15:11:27 +0100
Subject: [PATCH 15/39] fix #ifdef typo

---
 src/main/drivers/serial_uart_stm32f30x.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c
index 10d8cdd9eb..6565e27fb0 100644
--- a/src/main/drivers/serial_uart_stm32f30x.c
+++ b/src/main/drivers/serial_uart_stm32f30x.c
@@ -349,7 +349,7 @@ void DMA1_Channel7_IRQHandler(void)
 #endif
 
 // USART3 Tx DMA Handler
-#ifdef USE_USART2_TX_DMA
+#ifdef USE_USART3_TX_DMA
 void DMA1_Channel2_IRQHandler(void)
 {
     uartPort_t *s = &uartPort3;

From a67d2f8a9e30c54a6a1c38e307d5547471ebfd28 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 15:11:27 +0100
Subject: [PATCH 16/39] Whitespace fixes

---
 src/main/main.c           | 2 +-
 src/main/telemetry/hott.c | 8 ++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/main.c b/src/main/main.c
index 7e530b3f32..5c48aee3c9 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -162,7 +162,7 @@ void init(void)
 
     ledInit();
 
-    #ifdef SPEKTRUM_BIND
+#ifdef SPEKTRUM_BIND
     if (feature(FEATURE_RX_SERIAL)) {
         switch (masterConfig.rxConfig.serialrx_provider) {
             case SERIALRX_SPEKTRUM1024:
diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c
index d0bfd56bfd..e81359d293 100644
--- a/src/main/telemetry/hott.c
+++ b/src/main/telemetry/hott.c
@@ -219,15 +219,15 @@ static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage)
 
 static inline void hottEAMUpdateCurrentMeter(HOTT_EAM_MSG_t *hottEAMMessage)
 {
-	int32_t amp = amperage / 10;
-	hottEAMMessage->current_L = amp & 0xFF;
+    int32_t amp = amperage / 10;
+    hottEAMMessage->current_L = amp & 0xFF;
     hottEAMMessage->current_H = amp >> 8;
 }
 
 static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMessage)
 {
-	int32_t mAh = mAhDrawn / 10;
-	hottEAMMessage->batt_cap_L = mAh & 0xFF;
+    int32_t mAh = mAhDrawn / 10;
+    hottEAMMessage->batt_cap_L = mAh & 0xFF;
     hottEAMMessage->batt_cap_H = mAh >> 8;
 }
 

From a96a12bd479f2b3403a7772dd841fd2df54c6c12 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 15:10:25 +0100
Subject: [PATCH 17/39] compile some GPS code conditionally

---
 src/main/telemetry/frsky.c     | 5 ++++-
 src/main/telemetry/smartport.c | 8 ++++++++
 2 files changed, 12 insertions(+), 1 deletion(-)

diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c
index 25691fb47d..e88bacaadb 100644
--- a/src/main/telemetry/frsky.c
+++ b/src/main/telemetry/frsky.c
@@ -172,6 +172,7 @@ static void sendBaro(void)
     serialize16(ABS(BaroAlt % 100));
 }
 
+#ifdef GPS
 static void sendGpsAltitude(void)
 {
     uint16_t altitude = GPS_altitude;
@@ -184,7 +185,7 @@ static void sendGpsAltitude(void)
     sendDataHead(ID_GPS_ALTIDUTE_AP);
     serialize16(0);
 }
-
+#endif
 
 static void sendThrottleOrBatterySizeAsRpm(void)
 {
@@ -207,6 +208,7 @@ static void sendTemperature1(void)
 #endif
 }
 
+#ifdef GPS
 static void sendSatalliteSignalQualityAsTemperature2(void)
 {
     uint16_t satellite = GPS_numSat;
@@ -236,6 +238,7 @@ static void sendSpeed(void)
     sendDataHead(ID_GPS_SPEED_AP);
     serialize16(0); //Not dipslayed
 }
+#endif
 
 static void sendTime(void)
 {
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index c17eec1c17..f82b3d5273 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -325,6 +325,7 @@ void handleSmartPortTelemetry(void)
         static uint8_t t1Cnt = 0;
 
         switch(id) {
+#ifdef GPS
             case FSSP_DATAID_SPEED      :
                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
                     uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100;
@@ -332,6 +333,7 @@ void handleSmartPortTelemetry(void)
                     smartPortHasRequest = 0;
                 }
                 break;
+#endif
             case FSSP_DATAID_VFAS       :
                 smartPortSendPackage(id, vbat * 83); // supposedly given in 0.1V, unknown requested unit
                 // multiplying by 83 seems to make Taranis read correctly
@@ -352,6 +354,7 @@ void handleSmartPortTelemetry(void)
                 break;
             //case FSSP_DATAID_ADC1       :
             //case FSSP_DATAID_ADC2       :
+#ifdef GPS
             case FSSP_DATAID_LATLONG    :
                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
                     uint32_t tmpui = 0;
@@ -377,6 +380,7 @@ void handleSmartPortTelemetry(void)
                     smartPortHasRequest = 0;
                 }
                 break;
+#endif
             //case FSSP_DATAID_CAP_USED   :
             case FSSP_DATAID_VARIO      :
                 smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s
@@ -448,21 +452,25 @@ void handleSmartPortTelemetry(void)
                 break;
             case FSSP_DATAID_T2         :
                 if (sensors(SENSOR_GPS)) {
+#ifdef GPS
                     // provide GPS lock status
                     smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat);
                     smartPortHasRequest = 0;
+#endif
                 }
                 else {
                     smartPortSendPackage(id, 0);
                     smartPortHasRequest = 0;
                 }
                 break;
+#ifdef GPS
             case FSSP_DATAID_GPS_ALT    :
                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
                     smartPortSendPackage(id, GPS_altitude * 1000); // given in 0.1m , requested in 100 = 1m
                     smartPortHasRequest = 0;
                 }
                 break;
+#endif
             default:
                 break;
                 // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just wait for the next loop

From 3abcbace72645de22140de8e1e5661cc70dcd488 Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Thu, 29 Jan 2015 15:11:27 +0100
Subject: [PATCH 18/39] Generate phony targets in auto-dependencies

This will fix problem with missing dependencies where #include changes
---
 Makefile | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Makefile b/Makefile
index f3f29edd4d..a13ddb07f0 100644
--- a/Makefile
+++ b/Makefile
@@ -555,12 +555,12 @@ CFLAGS		 = $(ARCH_FLAGS) \
 		   -D'__TARGET__="$(TARGET)"' \
 		   -D'__REVISION__="$(REVISION)"' \
 		   -save-temps=obj \
-		   -MMD
+		   -MMD -MP
 
 ASFLAGS		 = $(ARCH_FLAGS) \
 		   -x assembler-with-cpp \
 		   $(addprefix -I,$(INCLUDE_DIRS)) \
-		  -MMD
+		  -MMD -MP
 
 LDFLAGS		 = -lm \
 		   -nostartfiles \

From 737fbe02ed716e1c292b803a1fead8efbe27079f Mon Sep 17 00:00:00 2001
From: Petr Ledvina <ledvinap@gmail.com>
Date: Fri, 30 Jan 2015 12:35:01 +0100
Subject: [PATCH 19/39] Fix battery unittest

New code uses divition to get cell count, so nonzero vbatmaxcellvoltage is neccessary. Also added remaining fields to avoid future problems (and g++ doesn't support non-trivial initializers)
---
 src/test/unit/battery_unittest.cc | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/src/test/unit/battery_unittest.cc b/src/test/unit/battery_unittest.cc
index 2a67017439..8eccb32e54 100644
--- a/src/test/unit/battery_unittest.cc
+++ b/src/test/unit/battery_unittest.cc
@@ -37,7 +37,17 @@ TEST(BatteryTest, BatteryADCToVoltage)
 {
     // given
 
-    batteryConfig_t batteryConfig;
+    batteryConfig_t batteryConfig = {
+        .vbatscale = 110,
+        .vbatmaxcellvoltage = 43,
+        .vbatmincellvoltage = 33,
+        .vbatwarningcellvoltage = 35,
+        .currentMeterScale = 400,
+        .currentMeterOffset = 0,
+        .currentMeterType = CURRENT_SENSOR_NONE,
+        .multiwiiCurrentMeterOutput = 0,
+        .batteryCapacity = 2200,
+    };
 
     // batteryInit() reads a bunch of fields including vbatscale, so set up the config with useful initial values:
     memset(&batteryConfig, 0, sizeof(batteryConfig));

From 97fae9405364a745fd2c218f7e049bec245daf47 Mon Sep 17 00:00:00 2001
From: Ben Hitchcock <benh@perthweb.com.au>
Date: Tue, 3 Mar 2015 11:47:35 +0800
Subject: [PATCH 20/39] Bugfix for 3D inverted flight using a tricopter.  The
 yaw servo now should reverse when the throttle is lower than the midpoint.

---
 src/main/flight/mixer.c | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index ff779782ed..2563e8a2a0 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -546,6 +546,12 @@ static void airplaneMixer(void)
 void mixTable(void)
 {
     uint32_t i;
+    int8_t yawDirection3D = 1;
+
+	// Reverse yaw servo when inverted in 3D mode
+	if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
+		yawDirection3D = -1;
+	}
 
     if (motorCount > 3) {
         // prevent "yaw jump" during yaw correction
@@ -572,7 +578,7 @@ void mixTable(void)
             break;
 
         case MIXER_TRI:
-            servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + determineServoMiddleOrForwardFromChannel(5); // REAR
+            servo[5] = (servoDirection(5, 1) * axisPID[YAW] * yawDirection3D) + determineServoMiddleOrForwardFromChannel(5); // REAR
             break;
 
         case MIXER_GIMBAL:

From 8a541a9fa28197da2d3f2b858ba837be5b9cea1d Mon Sep 17 00:00:00 2001
From: Nicholas Sherlock <n.sherlock@gmail.com>
Date: Sat, 7 Mar 2015 01:18:05 +1300
Subject: [PATCH 21/39] Fix upper limit on mag_hardware and acc_hardware CLI
 variables

---
 src/main/io/serial_cli.c        | 4 ++--
 src/main/sensors/acceleration.h | 2 ++
 src/main/sensors/compass.h      | 2 ++
 3 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 35317b7a84..3a90d07266 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -407,7 +407,7 @@ const clivalue_t valueTable[] = {
     { "gimbal_flags",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].gimbalConfig.gimbal_flags, 0, 255},
 #endif
 
-    { "acc_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.acc_hardware, 0, ACC_NONE },
+    { "acc_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.acc_hardware, 0, ACC_MAX },
     { "acc_lpf_factor",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].acc_lpf_factor, 0, 250 },
     { "accxy_deadband",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.xy, 0, 100 },
     { "accz_deadband",              VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.z, 0, 100 },
@@ -421,7 +421,7 @@ const clivalue_t valueTable[] = {
     { "baro_cf_vel",                VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_vel, 0, 1 },
     { "baro_cf_alt",                VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_alt, 0, 1 },
 
-    { "mag_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.mag_hardware, 0, MAG_NONE },
+    { "mag_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.mag_hardware, 0, MAG_MAX },
     { "mag_declination",            VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 },
 
     { "pid_controller",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidController, 0, 5 },
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index 198b12778a..d1403c71cf 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -29,6 +29,8 @@ typedef enum {
     ACC_SPI_MPU6000 = 7,
     ACC_SPI_MPU6500 = 8,
     ACC_FAKE = 9,
+
+    ACC_MAX = ACC_FAKE
 } accelerationSensor_e;
 
 extern sensor_align_e accAlign;
diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h
index 502c187285..00cba565c1 100644
--- a/src/main/sensors/compass.h
+++ b/src/main/sensors/compass.h
@@ -23,6 +23,8 @@ typedef enum {
     MAG_NONE = 1,
     MAG_HMC5883 = 2,
     MAG_AK8975 = 3,
+
+    MAG_MAX = MAG_AK8975
 } magSensor_e;
 
 #ifdef MAG

From 0c8adf25c2c0c35945f3889aa16310138ed2d96e Mon Sep 17 00:00:00 2001
From: Nicholas Sherlock <n.sherlock@gmail.com>
Date: Sat, 7 Mar 2015 01:57:45 +1300
Subject: [PATCH 22/39] Use defines for MAG_MAX and ACC_MAX

---
 src/main/sensors/acceleration.h | 4 ++--
 src/main/sensors/compass.h      | 8 ++++----
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index d1403c71cf..cd13a01a4d 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -29,10 +29,10 @@ typedef enum {
     ACC_SPI_MPU6000 = 7,
     ACC_SPI_MPU6500 = 8,
     ACC_FAKE = 9,
-
-    ACC_MAX = ACC_FAKE
 } accelerationSensor_e;
 
+#define ACC_MAX  ACC_FAKE
+
 extern sensor_align_e accAlign;
 extern acc_t acc;
 extern uint16_t acc_1G;
diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h
index 00cba565c1..8c1eb71994 100644
--- a/src/main/sensors/compass.h
+++ b/src/main/sensors/compass.h
@@ -17,16 +17,16 @@
 
 #pragma once
 
-// Type of accelerometer used/detected
+// Type of magnetometer used/detected
 typedef enum {
     MAG_DEFAULT = 0,
     MAG_NONE = 1,
     MAG_HMC5883 = 2,
-    MAG_AK8975 = 3,
-
-    MAG_MAX = MAG_AK8975
+    MAG_AK8975 = 3
 } magSensor_e;
 
+#define MAG_MAX  MAG_AK8975
+
 #ifdef MAG
 void compassInit(void);
 void updateCompass(flightDynamicsTrims_t *magZero);

From 7331bdbd4f4f49a917dfd2ef0fdc7d37031b5327 Mon Sep 17 00:00:00 2001
From: Alex Gorbatchev <alex.gorbatchev@gmail.com>
Date: Fri, 6 Mar 2015 21:08:22 -0800
Subject: [PATCH 23/39] Update Inflight Adjustments.md

Adds separators between example images so that it's possible to tell what's what.
---
 docs/Inflight Adjustments.md | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/docs/Inflight Adjustments.md b/docs/Inflight Adjustments.md
index ce0caffa67..f834d836b3 100644
--- a/docs/Inflight Adjustments.md	
+++ b/docs/Inflight Adjustments.md	
@@ -197,9 +197,21 @@ When the switch is high, rate profile 2 is selcted.
 The following 5 images show valid configurations.  In all cales the enture usable range for the Range Channel is used.
 
 ![Configurator example 1](Screenshots/adjustments-rate-profile-selection-via-3pos.png)
+
+---
+
 ![Configurator example 2](Screenshots/adjustments-pitch-and-roll-rate-adjustment-via-3pos.png)
+
+---
+
 ![Configurator example 3](Screenshots/adjustments-pid-via-two-3pos.png)
+
+---
+
 ![Configurator example 4](Screenshots/adjustments-pid-via-6pos-and-3pos.png)
+
+---
+
 ![Configurator example 5](Screenshots/adjustments-rates-via-a-2pos-and-3pos.png)
 
 The following examples shows __incorrect__ configuration - the entire usable range for the Range Channel is not used in both cases.

From 26bb86898e98bb9e34f69d328835cd46891f4a37 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Sat, 7 Mar 2015 14:15:38 +0000
Subject: [PATCH 24/39] Updating comment in mixer.c

---
 src/main/flight/mixer.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 2563e8a2a0..c766ad665c 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -202,7 +202,7 @@ static const motorMixer_t mixerDualcopter[] = {
     { 1.0f,  0.0f,  0.0f,  1.0f },          // RIGHT
 };
 
-// Keep this synced with MultiType struct in mw.h!
+// Keep synced with mixerMode_e
 const mixer_t mixers[] = {
 //    Mo Se Mixtable
     { 0, 0, NULL },                // entry 0

From b595b49ca842a352c1cb8334892d2193dcd4cfa0 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Sat, 7 Mar 2015 15:09:42 +0000
Subject: [PATCH 25/39] Decouple roll and pitch rates.  MSP clients take note
 of updated MSP_RC_TUNING/MSP_SET_RC_TUNING commands.

---
 src/main/config/config.c  |  7 +++++--
 src/main/flight/mixer.c   | 13 +++++++------
 src/main/flight/pid.c     | 31 +++++++++++++++++++++----------
 src/main/io/display.c     | 22 +++++++++-------------
 src/main/io/rc_controls.c | 10 ++++++----
 src/main/io/rc_controls.h |  3 +--
 src/main/io/serial_cli.c  |  5 +++--
 src/main/io/serial_msp.c  | 26 ++++++++++++++++----------
 src/main/mw.c             |  4 ++--
 9 files changed, 70 insertions(+), 51 deletions(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index 179dd0895e..3f8ff440dc 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -275,12 +275,15 @@ void resetSerialConfig(serialConfig_t *serialConfig)
 static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
     controlRateConfig->rcRate8 = 90;
     controlRateConfig->rcExpo8 = 65;
-    controlRateConfig->rollPitchRate = 0;
-    controlRateConfig->yawRate = 0;
     controlRateConfig->thrMid8 = 50;
     controlRateConfig->thrExpo8 = 0;
     controlRateConfig->dynThrPID = 0;
     controlRateConfig->tpa_breakpoint = 1500;
+
+    for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
+        controlRateConfig->rates[axis] = 0;
+    }
+
 }
 
 void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index c766ad665c..d79db0ef71 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -546,12 +546,6 @@ static void airplaneMixer(void)
 void mixTable(void)
 {
     uint32_t i;
-    int8_t yawDirection3D = 1;
-
-	// Reverse yaw servo when inverted in 3D mode
-	if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
-		yawDirection3D = -1;
-	}
 
     if (motorCount > 3) {
         // prevent "yaw jump" during yaw correction
@@ -570,6 +564,13 @@ void mixTable(void)
     }
 
 #if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS)
+    int8_t yawDirection3D = 1;
+
+    // Reverse yaw servo when inverted in 3D mode
+    if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
+        yawDirection3D = -1;
+    }
+
     // airplane / servo mixes
     switch (currentMixerMode) {
         case MIXER_BI:
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 0f95588745..a009a5e527 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -139,9 +139,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     // ----------PID controller----------
     for (axis = 0; axis < 3; axis++) {
         // -----Get the desired angle rate depending on flight mode
+        uint8_t rate = controlRateConfig->rates[axis];
+
         if (axis == FD_YAW) {
             // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
-            AngleRate = (float)((controlRateConfig->yawRate + 10) * rcCommand[YAW]) / 50.0f;
+            AngleRate = (float)((rate + 10) * rcCommand[YAW]) / 50.0f;
          } else {
             // calculate error and limit the angle to the max inclination
 #ifdef GPS
@@ -163,7 +165,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
                 AngleRate = errorAngle * pidProfile->A_level;
             } else {
                 //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
-                AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate
+                AngleRate = (float)((rate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate
                 if (FLIGHT_MODE(HORIZON_MODE)) {
                     // mix up angle error to desired AngleRate to add a little auto-level feel
                     AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength;
@@ -310,14 +312,21 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
     static int32_t delta1[2] = { 0, 0 }, delta2[2] = { 0, 0 };
     int32_t delta;
 
-    if (FLIGHT_MODE(HORIZON_MODE)) prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512);
+    if (FLIGHT_MODE(HORIZON_MODE)) {
+        prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512);
+    }
 
     // PITCH & ROLL
     for (axis = 0; axis < 2; axis++) {
+
         rc = rcCommand[axis] << 1;
+
         error = rc - (gyroData[axis] / 4);
         errorGyroI[axis]  = constrain(errorGyroI[axis] + error, -16000, +16000);   // WindUp   16 bits is ok here
-        if (ABS(gyroData[axis]) > (640 * 4)) errorGyroI[axis] = 0;
+
+        if (ABS(gyroData[axis]) > (640 * 4)) {
+            errorGyroI[axis] = 0;
+        }
 
         ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6;   // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
 
@@ -372,7 +381,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
     }
 
     //YAW
-    rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30)  >> 5;
+    rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30)  >> 5;
 #ifdef ALIENWII32
     error = rc - gyroData[FD_YAW];
 #else
@@ -483,7 +492,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
 #endif
     }
     //YAW
-    rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30)  >> 5;
+    rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30)  >> 5;
 #ifdef ALIENWII32
     error = rc - gyroData[FD_YAW];
 #else
@@ -603,7 +612,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
     Mwii3msTimescale /= 3000.0f;
 
     if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
-        PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
+        PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->rates[FD_YAW] * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
         int32_t tmp = lrintf(gyroData[FD_YAW] * 0.25f);
         PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80;
         if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
@@ -614,7 +623,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
             ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6;
         }
     } else {
-        int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5;
+        int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->rates[FD_YAW] << 1) + 40)) >> 5;
         error = tmp - lrintf(gyroData[FD_YAW] * 0.25f);                       // Less Gyrojitter works actually better
 
         if (ABS(tmp) > 50) {
@@ -657,9 +666,11 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
 
     // ----------PID controller----------
     for (axis = 0; axis < 3; axis++) {
+        uint8_t rate = controlRateConfig->rates[axis];
+
         // -----Get the desired angle rate depending on flight mode
         if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
-            AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[YAW]) >> 5);
+            AngleRateTmp = (((int32_t)(rate + 27) * rcCommand[YAW]) >> 5);
         } else {
             // calculate error and limit the angle to max configured inclination
 #ifdef GPS
@@ -677,7 +688,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
 #endif
 
             if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
-                AngleRateTmp = ((int32_t)(controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4;
+                AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
                 if (FLIGHT_MODE(HORIZON_MODE)) {
                     // mix up angle error to desired AngleRateTmp to add a little auto-level feel
                     AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8;
diff --git a/src/main/io/display.c b/src/main/io/display.c
index 65f94d2178..11566c76b5 100644
--- a/src/main/io/display.c
+++ b/src/main/io/display.c
@@ -271,26 +271,22 @@ void showProfilePage(void)
 
     controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex);
 
-    tfp_sprintf(lineBuffer, "RC Expo: %d", controlRateConfig->rcExpo8);
+    tfp_sprintf(lineBuffer, "Expo: %d, Rate: %d",
+        controlRateConfig->rcExpo8,
+        controlRateConfig->rcRate8
+    );
     padLineBuffer();
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
-    tfp_sprintf(lineBuffer, "RC Rate: %d", controlRateConfig->rcRate8);
+    tfp_sprintf(lineBuffer, "Rates R:%d P:%d Y:%d",
+        controlRateConfig->rates[FD_ROLL],
+        controlRateConfig->rates[FD_PITCH],
+        controlRateConfig->rates[FD_YAW]
+    );
     padLineBuffer();
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
-
-    tfp_sprintf(lineBuffer, "R&P Rate: %d", controlRateConfig->rollPitchRate);
-    padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
-
-    tfp_sprintf(lineBuffer, "Yaw Rate: %d", controlRateConfig->yawRate);
-    padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
-
 }
 #define SATELLITE_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0]))
 #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c
index e14683dc0e..efd0761d2d 100644
--- a/src/main/io/rc_controls.c
+++ b/src/main/io/rc_controls.c
@@ -402,12 +402,14 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
             generateThrottleCurve(controlRateConfig, escAndServoConfig);
             break;
         case ADJUSTMENT_PITCH_ROLL_RATE:
-            newValue = (int)controlRateConfig->rollPitchRate + delta;
-            controlRateConfig->rollPitchRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            newValue = (int)controlRateConfig->rates[FD_PITCH] + delta;
+            controlRateConfig->rates[FD_PITCH] = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            newValue = (int)controlRateConfig->rates[FD_ROLL] + delta;
+            controlRateConfig->rates[FD_ROLL] = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
             break;
         case ADJUSTMENT_YAW_RATE:
-            newValue = (int)controlRateConfig->yawRate + delta;
-            controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            newValue = (int)controlRateConfig->rates[FD_YAW] + delta;
+            controlRateConfig->rates[FD_YAW] = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
             break;
         case ADJUSTMENT_PITCH_ROLL_P:
             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index a1549f5c5f..56180c9af0 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -122,8 +122,7 @@ typedef struct controlRateConfig_s {
     uint8_t rcExpo8;
     uint8_t thrMid8;
     uint8_t thrExpo8;
-    uint8_t rollPitchRate;
-    uint8_t yawRate;
+    uint8_t rates[3];
     uint8_t dynThrPID;
     uint16_t tpa_breakpoint;                // Breakpoint where TPA is activated
 } controlRateConfig_t;
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 3a90d07266..fb92400959 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -392,8 +392,9 @@ const clivalue_t valueTable[] = {
     { "rc_expo",                    VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, 0, 100 },
     { "thr_mid",                    VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrMid8, 0, 100 },
     { "thr_expo",                   VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrExpo8, 0, 100 },
-    { "roll_pitch_rate",            VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rollPitchRate, 0, 100 },
-    { "yaw_rate",                   VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].yawRate, 0, 100 },
+    { "roll_rate",                  VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_ROLL], 0, 100 },
+    { "pitch_rate",                 VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_PITCH], 0, 100 },
+    { "yaw_rate",                   VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_YAW], 0, 100 },
     { "tpa_rate",                   VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].dynThrPID, 0, 100},
     { "tpa_breakpoint",             VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_breakpoint, PWM_RANGE_MIN, PWM_RANGE_MAX},
 
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 19e75a8745..1861bc48a9 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -890,11 +890,12 @@ static bool processOutCommand(uint8_t cmdMSP)
             serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
         break;
     case MSP_RC_TUNING:
-        headSerialReply(7);
+        headSerialReply(8);
         serialize8(currentControlRateProfile->rcRate8);
         serialize8(currentControlRateProfile->rcExpo8);
-        serialize8(currentControlRateProfile->rollPitchRate);
-        serialize8(currentControlRateProfile->yawRate);
+        for (i = 0 ; i < 3; i++) {
+            serialize8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
+        }
         serialize8(currentControlRateProfile->dynThrPID);
         serialize8(currentControlRateProfile->thrMid8);
         serialize8(currentControlRateProfile->thrExpo8);
@@ -1325,13 +1326,18 @@ static bool processInCommand(void)
         break;
 
     case MSP_SET_RC_TUNING:
-        currentControlRateProfile->rcRate8 = read8();
-        currentControlRateProfile->rcExpo8 = read8();
-        currentControlRateProfile->rollPitchRate = read8();
-        currentControlRateProfile->yawRate = read8();
-        currentControlRateProfile->dynThrPID = read8();
-        currentControlRateProfile->thrMid8 = read8();
-        currentControlRateProfile->thrExpo8 = read8();
+        if (currentPort->dataSize == 8) {
+            currentControlRateProfile->rcRate8 = read8();
+            currentControlRateProfile->rcExpo8 = read8();
+            for (i = 0; i < 3; i++) {
+                currentControlRateProfile->rates[i] = read8();
+            }
+            currentControlRateProfile->dynThrPID = read8();
+            currentControlRateProfile->thrMid8 = read8();
+            currentControlRateProfile->thrExpo8 = read8();
+        } else {
+            headSerialError(0);
+        }
         break;
     case MSP_SET_MISC:
         tmp = read16();
diff --git a/src/main/mw.c b/src/main/mw.c
index db8d34e4ee..e678848ecd 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -201,7 +201,7 @@ void annexCode(void)
 
             tmp2 = tmp / 100;
             rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
-            prop1 = 100 - (uint16_t)currentControlRateProfile->rollPitchRate * tmp / 500;
+            prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500;
             prop1 = (uint16_t)prop1 * prop2 / 100;
         } else if (axis == YAW) {
             if (currentProfile->rcControlsConfig.yaw_deadband) {
@@ -212,7 +212,7 @@ void annexCode(void)
                 }
             }
             rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
-            prop1 = 100 - (uint16_t)currentControlRateProfile->yawRate * ABS(tmp) / 500;
+            prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
         }
         // FIXME axis indexes into pids.  use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
         dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;

From e4a68862661a97acf35e16adbac9d5592bdebd23 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Sat, 7 Mar 2015 15:14:52 +0000
Subject: [PATCH 26/39] Allow independent in-flight adjustment of roll and
 pitch.

---
 src/main/io/rc_controls.c | 6 ++++++
 src/main/io/rc_controls.h | 6 ++++--
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c
index efd0761d2d..450a63b1a4 100644
--- a/src/main/io/rc_controls.c
+++ b/src/main/io/rc_controls.c
@@ -402,8 +402,14 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
             generateThrottleCurve(controlRateConfig, escAndServoConfig);
             break;
         case ADJUSTMENT_PITCH_ROLL_RATE:
+        case ADJUSTMENT_PITCH_RATE:
             newValue = (int)controlRateConfig->rates[FD_PITCH] + delta;
             controlRateConfig->rates[FD_PITCH] = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
+                break;
+            }
+            // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
+        case ADJUSTMENT_ROLL_RATE:
             newValue = (int)controlRateConfig->rates[FD_ROLL] + delta;
             controlRateConfig->rates[FD_ROLL] = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
             break;
diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index 56180c9af0..3a8a5e846a 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -156,10 +156,12 @@ typedef enum {
     ADJUSTMENT_YAW_P,
     ADJUSTMENT_YAW_I,
     ADJUSTMENT_YAW_D,
-    ADJUSTMENT_RATE_PROFILE
+    ADJUSTMENT_RATE_PROFILE,
+    ADJUSTMENT_PITCH_RATE,
+    ADJUSTMENT_ROLL_RATE,
 } adjustmentFunction_e;
 
-#define ADJUSTMENT_FUNCTION_COUNT 13
+#define ADJUSTMENT_FUNCTION_COUNT 15
 
 typedef enum {
     ADJUSTMENT_MODE_STEP,

From 2e5fe06a6afee5cf740ae6fac682750bc522aacc Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Sun, 8 Mar 2015 00:48:47 +0000
Subject: [PATCH 27/39] Update AlienWii32 defaults for pitch/roll/yaw rates

---
 src/main/config/config.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index 3f8ff440dc..81645078d3 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -502,9 +502,9 @@ static void resetConf(void)
     currentProfile->failsafeConfig.failsafe_off_delay = 0;
     currentProfile->failsafeConfig.failsafe_throttle = 1000;
     currentControlRateProfile->rcRate8 = 130;
-    currentControlRateProfile->rollPitchRate = 20;
-    currentControlRateProfile->yawRate = 60;
-    currentControlRateProfile->yawRate = 100;
+    currentControlRateProfile->rates[FD_PITCH] = 20;
+    currentControlRateProfile->rates[FD_ROLL] = 20;
+    currentControlRateProfile->rates[FD_YAW] = 100;
     parseRcChannels("TAER1234", &masterConfig.rxConfig);
 
     //  { 1.0f, -0.5f,  1.0f, -1.0f },          // REAR_R

From 9d0e464aaffec82d0817120f23b157e2be0fe1fe Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Sun, 8 Mar 2015 00:51:29 +0000
Subject: [PATCH 28/39] Updating unit tests to match roll/pid/yaw rate
 decoupling.

---
 src/test/unit/rc_controls_unittest.cc | 9 +++------
 1 file changed, 3 insertions(+), 6 deletions(-)

diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc
index eeb1b16ec7..a99596e329 100644
--- a/src/test/unit/rc_controls_unittest.cc
+++ b/src/test/unit/rc_controls_unittest.cc
@@ -229,8 +229,7 @@ TEST(RcControlsTest, processRcAdjustmentsSticksInMiddle)
             .rcExpo8 = 0,
             .thrMid8 = 0,
             .thrExpo8 = 0,
-            .rollPitchRate = 0,
-            .yawRate = 0,
+            .rates = {0,0,0},
             .dynThrPID = 0,
             .tpa_breakpoint = 0
     };
@@ -273,8 +272,7 @@ TEST(RcControlsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp)
             .rcExpo8 = 0,
             .thrMid8 = 0,
             .thrExpo8 = 0,
-            .rollPitchRate = 0,
-            .yawRate = 0,
+            .rates = {0,0,0},
             .dynThrPID = 0,
             .tpa_breakpoint = 0
     };
@@ -440,8 +438,7 @@ TEST(RcControlsTest, processRcRateProfileAdjustments)
             .rcExpo8 = 0,
             .thrMid8 = 0,
             .thrExpo8 = 0,
-            .rollPitchRate = 0,
-            .yawRate = 0,
+            .rates = {0,0,0},
             .dynThrPID = 0,
             .tpa_breakpoint = 0
     };

From 60a95f1d22728639b52dc48e6d43ec83604eb9d2 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 9 Mar 2015 22:17:32 +0000
Subject: [PATCH 29/39] Removing noise from unit tests.

---
 src/test/unit/altitude_hold_unittest.cc  |  4 ++++
 src/test/unit/battery_unittest.cc        |  5 ++++-
 src/test/unit/gps_conversion_unittest.cc |  5 ++++-
 src/test/unit/ledstrip_unittest.cc       | 10 +++++++++-
 src/test/unit/lowpass_unittest.cc        |  5 ++++-
 src/test/unit/rc_controls_unittest.cc    |  6 ++++++
 6 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/src/test/unit/altitude_hold_unittest.cc b/src/test/unit/altitude_hold_unittest.cc
index b895a1b1bd..3b9ba887cd 100644
--- a/src/test/unit/altitude_hold_unittest.cc
+++ b/src/test/unit/altitude_hold_unittest.cc
@@ -20,6 +20,8 @@
 
 #include <limits.h>
 
+//#define DEBUG_ALTITUDE_HOLD
+
 #define BARO
 
 extern "C" {
@@ -87,7 +89,9 @@ TEST(AltitudeHoldTest, IsThrustFacingDownwards)
 
     for (uint8_t index = 0; index < testIterationCount; index ++) {
         inclinationExpectation_t *angleInclinationExpectation = &inclinationExpectations[index];
+#ifdef DEBUG_ALTITUDE_HOLD
         printf("iteration: %d\n", index);
+#endif
         bool result = isThrustFacingDownwards(&angleInclinationExpectation->inclination);
         EXPECT_EQ(angleInclinationExpectation->expectDownwardsThrust, result);
     }
diff --git a/src/test/unit/battery_unittest.cc b/src/test/unit/battery_unittest.cc
index 8eccb32e54..9ea7131832 100644
--- a/src/test/unit/battery_unittest.cc
+++ b/src/test/unit/battery_unittest.cc
@@ -18,6 +18,8 @@
 
 #include <limits.h>
 
+//#define DEBUG_BATTERY
+
 extern "C" {
     #include "sensors/battery.h"
 }
@@ -76,11 +78,12 @@ TEST(BatteryTest, BatteryADCToVoltage)
     for (uint8_t index = 0; index < testIterationCount; index ++) {
         batteryAdcToVoltageExpectation_t *batteryAdcToVoltageExpectation = &batteryAdcToVoltageExpectations[index];
         batteryConfig.vbatscale = batteryAdcToVoltageExpectation->scale;
+#ifdef DEBUG_BATTERY
         printf("adcReading: %d, vbatscale: %d\n",
                 batteryAdcToVoltageExpectation->adcReading,
                 batteryAdcToVoltageExpectation->scale
         );
-
+#endif
         uint16_t pointOneVoltSteps = batteryAdcToVoltage(batteryAdcToVoltageExpectation->adcReading);
 
         EXPECT_EQ(pointOneVoltSteps, batteryAdcToVoltageExpectation->expectedVoltageInDeciVoltSteps);
diff --git a/src/test/unit/gps_conversion_unittest.cc b/src/test/unit/gps_conversion_unittest.cc
index 8f0c20e25d..0249b04611 100644
--- a/src/test/unit/gps_conversion_unittest.cc
+++ b/src/test/unit/gps_conversion_unittest.cc
@@ -19,6 +19,8 @@
 
 #include <limits.h>
 
+//#ifdef DEBUG_GPS_CONVERSION
+
 extern "C" {
     #include "flight/gps_conversion.h"
 }
@@ -63,8 +65,9 @@ TEST(GpsConversionTest, GPSCoordToDegrees_NMEA_Values)
 
     for (uint8_t index = 0; index < testIterationCount; index ++) {
         const gpsConversionExpectation_t *expectation = &gpsConversionExpectations[index];
+#ifdef DEBUG_GPS_CONVERSION
         printf("iteration: %d\n", index);
-
+#endif
         uint32_t result = GPS_coord_to_degrees(expectation->coord);
         EXPECT_EQ(result, expectation->degrees);
     }
diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc
index 6cd1bf9dcf..bf955b3646 100644
--- a/src/test/unit/ledstrip_unittest.cc
+++ b/src/test/unit/ledstrip_unittest.cc
@@ -19,6 +19,8 @@
 
 #include <limits.h>
 
+//#define DEBUG_LEDSTRIP
+
 extern "C" {
     #include "build_config.h"
 
@@ -165,8 +167,9 @@ TEST(LedStripTest, parseLedStripConfig)
 
     // and
     for (uint8_t index = 0; index < WS2811_LED_STRIP_LENGTH; index++) {
+#ifdef DEBUG_LEDSTRIP
         printf("iteration: %d\n", index);
-
+#endif
         EXPECT_EQ(expectedLedStripConfig[index].xy, ledConfigs[index].xy);
         EXPECT_EQ(expectedLedStripConfig[index].flags, ledConfigs[index].flags);
         EXPECT_EQ(expectedLedStripConfig[index].color, ledConfigs[index].color);
@@ -324,14 +327,19 @@ TEST(ColorTest, parseColor)
 
     // when
     for (uint8_t index = 0; index < TEST_COLOR_COUNT; index++) {
+#ifdef DEBUG_LEDSTRIP
         printf("parse iteration: %d\n", index);
+#endif
+
         parseColor(index, testColors[index]);
     }
 
     // then
 
     for (uint8_t index = 0; index < TEST_COLOR_COUNT; index++) {
+#ifdef DEBUG_LEDSTRIP
         printf("iteration: %d\n", index);
+#endif
 
         EXPECT_EQ(expectedColors[index].h, colors[index].h);
         EXPECT_EQ(expectedColors[index].s, colors[index].s);
diff --git a/src/test/unit/lowpass_unittest.cc b/src/test/unit/lowpass_unittest.cc
index 1b07987b79..9506bd6e27 100644
--- a/src/test/unit/lowpass_unittest.cc
+++ b/src/test/unit/lowpass_unittest.cc
@@ -17,6 +17,8 @@
 #include <stdint.h>
 #include <limits.h>
 
+//#define DEBUG_LOWPASS
+
 extern "C" {
     #include "flight/lowpass.h" 
 }
@@ -97,8 +99,9 @@ TEST(LowpassTest, Lowpass)
 
     // Test all frequencies
     for (freq = 10; freq <= 400; freq++) {
+#ifdef DEBUG_LOWPASS
         printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f));
-
+#endif
         // Run tests
         for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) 
         {
diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc
index a99596e329..4e8e33e513 100644
--- a/src/test/unit/rc_controls_unittest.cc
+++ b/src/test/unit/rc_controls_unittest.cc
@@ -18,6 +18,8 @@
 
 #include <limits.h>
 
+//#define DEBUG_RC_CONTROLS
+
 extern "C" {
     #include "platform.h"
 
@@ -69,7 +71,9 @@ TEST(RcControlsTest, updateActivatedModesWithAllInputsAtMidde)
 
     // then
     for (index = 0; index < CHECKBOX_ITEM_COUNT; index++) {
+#ifdef DEBUG_RC_CONTROLS
         printf("iteration: %d\n", index);
+#endif
         EXPECT_EQ(false, IS_RC_MODE_ACTIVE(index));
     }
 }
@@ -160,7 +164,9 @@ TEST(RcControlsTest, updateActivatedModesUsingValidAuxConfigurationAndRXValues)
 
     // then
     for (index = 0; index < CHECKBOX_ITEM_COUNT; index++) {
+#ifdef DEBUG_RC_CONTROLS
         printf("iteration: %d\n", index);
+#endif
         EXPECT_EQ(expectedMask & (1 << index), rcModeActivationMask & (1 << index));
     }
 }

From e40a3663d2178851b3d31f78309127fe78f539a0 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 9 Mar 2015 22:19:09 +0000
Subject: [PATCH 30/39] Remove failsafe vtable usage.

---
 src/main/flight/failsafe.c         | 105 +++++++++++++----------------
 src/main/flight/failsafe.h         |  33 +++++----
 src/main/io/beeper.c               |  13 ++--
 src/main/io/ledstrip.c             |   7 +-
 src/main/main.c                    |  18 +++--
 src/main/mw.c                      |   9 ++-
 src/main/rx/rx.c                   |  14 ++--
 src/test/unit/ledstrip_unittest.cc |   2 +
 8 files changed, 88 insertions(+), 113 deletions(-)

diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index 97521c6f84..01edaf28fd 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -38,17 +38,15 @@
  * enable() should be called after system initialisation.
  */
 
-static failsafe_t failsafe;
+static failsafeState_t failsafeState;
 
 static failsafeConfig_t *failsafeConfig;
 
 static rxConfig_t *rxConfig;
 
-const failsafeVTable_t failsafeVTable[];
-
-void reset(void)
+void failsafeReset(void)
 {
-    failsafe.counter = 0;
+    failsafeState.counter = 0;
 }
 
 /*
@@ -57,129 +55,118 @@ void reset(void)
 void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
 {
     failsafeConfig = failsafeConfigToUse;
-    reset();
+    failsafeReset();
 }
 
-failsafe_t* failsafeInit(rxConfig_t *intialRxConfig)
+failsafeState_t* failsafeInit(rxConfig_t *intialRxConfig)
 {
     rxConfig = intialRxConfig;
 
-    failsafe.vTable = failsafeVTable;
-    failsafe.events = 0;
-    failsafe.enabled = false;
+    failsafeState.events = 0;
+    failsafeState.enabled = false;
 
-    return &failsafe;
+    return &failsafeState;
 }
 
-bool isIdle(void)
+bool failsafeIsIdle(void)
 {
-    return failsafe.counter == 0;
+    return failsafeState.counter == 0;
 }
 
-bool isEnabled(void)
+bool failsafeIsEnabled(void)
 {
-    return failsafe.enabled;
+    return failsafeState.enabled;
 }
 
-void enable(void)
+void failsafeEnable(void)
 {
-    failsafe.enabled = true;
+    failsafeState.enabled = true;
 }
 
-bool hasTimerElapsed(void)
+bool failsafeHasTimerElapsed(void)
 {
-    return failsafe.counter > (5 * failsafeConfig->failsafe_delay);
+    return failsafeState.counter > (5 * failsafeConfig->failsafe_delay);
 }
 
-bool shouldForceLanding(bool armed)
+bool failsafeShouldForceLanding(bool armed)
 {
-    return hasTimerElapsed() && armed;
+    return failsafeHasTimerElapsed() && armed;
 }
 
-bool shouldHaveCausedLandingByNow(void)
+bool failsafeShouldHaveCausedLandingByNow(void)
 {
-    return failsafe.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
+    return failsafeState.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
 }
 
-void failsafeAvoidRearm(void)
+static void failsafeAvoidRearm(void)
 {
     // This will prevent the automatic rearm if failsafe shuts it down and prevents
     // to restart accidently by just reconnect to the tx - you will have to switch off first to rearm
     ENABLE_ARMING_FLAG(PREVENT_ARMING);
 }
 
-void onValidDataReceived(void)
+static void failsafeOnValidDataReceived(void)
 {
-    if (failsafe.counter > 20)
-        failsafe.counter -= 20;
+    if (failsafeState.counter > 20)
+        failsafeState.counter -= 20;
     else
-        failsafe.counter = 0;
+        failsafeState.counter = 0;
 }
 
-void updateState(void)
+void failsafeUpdateState(void)
 {
     uint8_t i;
 
-    if (!hasTimerElapsed()) {
+    if (!failsafeHasTimerElapsed()) {
         return;
     }
 
-    if (!isEnabled()) {
-        reset();
+    if (!failsafeIsEnabled()) {
+        failsafeReset();
         return;
     }
 
-    if (shouldForceLanding(ARMING_FLAG(ARMED))) { // Stabilize, and set Throttle to specified level
+    if (failsafeShouldForceLanding(ARMING_FLAG(ARMED))) { // Stabilize, and set Throttle to specified level
         failsafeAvoidRearm();
 
         for (i = 0; i < 3; i++) {
             rcData[i] = rxConfig->midrc;      // after specified guard time after RC signal is lost (in 0.1sec)
         }
         rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
-        failsafe.events++;
+        failsafeState.events++;
     }
 
-    if (shouldHaveCausedLandingByNow() || !ARMING_FLAG(ARMED)) {
+    if (failsafeShouldHaveCausedLandingByNow() || !ARMING_FLAG(ARMED)) {
         mwDisarm();
     }
 }
 
-void incrementCounter(void)
+/**
+ * Should be called once each time RX data is processed by the system.
+ */
+void failsafeOnRxCycle(void)
 {
-    failsafe.counter++;
+    failsafeState.counter++;
 }
 
+#define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels
+
 // pulse duration is in micro secons (usec)
 void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration)
 {
-    static uint8_t goodChannelMask;
+    static uint8_t goodChannelMask = 0;
 
     if (channel < 4 &&
         pulseDuration > failsafeConfig->failsafe_min_usec &&
         pulseDuration < failsafeConfig->failsafe_max_usec
-    )
-        goodChannelMask |= (1 << channel);       // if signal is valid - mark channel as OK
+    ) {
+        // if signal is valid - mark channel as OK
+        goodChannelMask |= (1 << channel);
+    }
 
-    if (goodChannelMask == 0x0F) {               // If first four channels have good pulses, clear FailSafe counter
+    if (goodChannelMask == REQUIRED_CHANNEL_MASK) {
         goodChannelMask = 0;
-        onValidDataReceived();
+        failsafeOnValidDataReceived();
     }
 }
 
-
-const failsafeVTable_t failsafeVTable[] = {
-    {
-        reset,
-        shouldForceLanding,
-        hasTimerElapsed,
-        shouldHaveCausedLandingByNow,
-        incrementCounter,
-        updateState,
-        isIdle,
-        failsafeCheckPulse,
-        isEnabled,
-        enable
-    }
-};
-
-
diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h
index 1f2fe6ca26..35a94e0a34 100644
--- a/src/main/flight/failsafe.h
+++ b/src/main/flight/failsafe.h
@@ -27,27 +27,26 @@ typedef struct failsafeConfig_s {
     uint16_t failsafe_max_usec;
 } failsafeConfig_t;
 
-typedef struct failsafeVTable_s {
-    void (*reset)(void);
-    bool (*shouldForceLanding)(bool armed);
-    bool (*hasTimerElapsed)(void);
-    bool (*shouldHaveCausedLandingByNow)(void);
-    void (*incrementCounter)(void);
-    void (*updateState)(void);
-    bool (*isIdle)(void);
-    void (*checkPulse)(uint8_t channel, uint16_t pulseDuration);
-    bool (*isEnabled)(void);
-    void (*enable)(void);
-
-} failsafeVTable_t;
-
-typedef struct failsafe_s {
-    const failsafeVTable_t *vTable;
 
+typedef struct failsafeState_s {
     int16_t counter;
     int16_t events;
     bool enabled;
-} failsafe_t;
+} failsafeState_t;
 
 void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse);
 
+void failsafeEnable(void);
+void failsafeOnRxCycle(void);
+void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration);
+void failsafeUpdateState(void);
+
+void failsafeReset(void);
+
+
+bool failsafeIsEnabled(void);
+bool failsafeIsIdle(void);
+bool failsafeHasTimerElapsed(void);
+bool failsafeShouldForceLanding(bool armed);
+bool failsafeShouldHaveCausedLandingByNow(void);
+
diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c
index 9d4f33c548..93af618a43 100644
--- a/src/main/io/beeper.c
+++ b/src/main/io/beeper.c
@@ -54,11 +54,8 @@ typedef enum {
     FAILSAFE_FIND_ME
 } failsafeBeeperWarnings_e;
 
-static failsafe_t* failsafe;
-
-void beepcodeInit(failsafe_t *initialFailsafe)
+void beepcodeInit(void)
 {
-    failsafe = initialFailsafe;
 }
 
 void beepcodeUpdateState(batteryState_e batteryState)
@@ -77,19 +74,19 @@ void beepcodeUpdateState(batteryState_e batteryState)
     }
     //===================== Beeps for failsafe =====================
     if (feature(FEATURE_FAILSAFE)) {
-        if (failsafe->vTable->shouldForceLanding(ARMING_FLAG(ARMED))) {
+        if (failsafeShouldForceLanding(ARMING_FLAG(ARMED))) {
             warn_failsafe = FAILSAFE_LANDING;
 
-            if (failsafe->vTable->shouldHaveCausedLandingByNow()) {
+            if (failsafeShouldHaveCausedLandingByNow()) {
                 warn_failsafe = FAILSAFE_FIND_ME;
             }
         }
 
-        if (failsafe->vTable->hasTimerElapsed() && !ARMING_FLAG(ARMED)) {
+        if (failsafeHasTimerElapsed() && !ARMING_FLAG(ARMED)) {
             warn_failsafe = FAILSAFE_FIND_ME;
         }
 
-        if (failsafe->vTable->isIdle()) {
+        if (failsafeIsIdle()) {
             warn_failsafe = FAILSAFE_IDLE;      // turn off alarm if TX is okay
         }
     }
diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c
index 5d40671a5c..bd8cb20029 100644
--- a/src/main/io/ledstrip.c
+++ b/src/main/io/ledstrip.c
@@ -52,8 +52,6 @@
 static bool ledStripInitialised = false;
 static bool ledStripEnabled = true;
 
-static failsafe_t* failsafe;
-
 static void ledStripDisable(void);
 
 //#define USE_LED_ANIMATION
@@ -663,7 +661,7 @@ void applyLedWarningLayer(uint8_t updateNow)
         if (feature(FEATURE_VBAT) && calculateBatteryState() != BATTERY_OK) {
             warningFlags |= WARNING_FLAG_LOW_BATTERY;
         }
-        if (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed()) {
+        if (feature(FEATURE_FAILSAFE) && failsafeHasTimerElapsed()) {
             warningFlags |= WARNING_FLAG_FAILSAFE;
         }
         if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) {
@@ -1031,11 +1029,10 @@ void applyDefaultLedStripConfig(ledConfig_t *ledConfigs)
     reevalulateLedConfig();
 }
 
-void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse)
+void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse)
 {
     ledConfigs = ledConfigsToUse;
     colors = colorsToUse;
-    failsafe = failsafeToUse;
     ledStripInitialised = false;
 }
 
diff --git a/src/main/main.c b/src/main/main.c
index 8fbe3e6638..fa060ce5e2 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -97,26 +97,24 @@ extern uint32_t previousTime;
 serialPort_t *loopbackPort;
 #endif
 
-failsafe_t *failsafe;
-
 void printfSupportInit(void);
 void timerInit(void);
 void telemetryInit(void);
 void serialInit(serialConfig_t *initialSerialConfig);
 void mspInit(serialConfig_t *serialConfig);
 void cliInit(serialConfig_t *serialConfig);
-failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
+void failsafeInit(rxConfig_t *intialRxConfig);
 pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
 void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMixers);
 void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
-void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
-void beepcodeInit(failsafe_t *initialFailsafe);
+void rxInit(rxConfig_t *rxConfig);
+void beepcodeInit(void);
 void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
 void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
 bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig);
 void imuInit(void);
 void displayInit(rxConfig_t *intialRxConfig);
-void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse);
+void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
 void loop(void);
 void spektrumBind(rxConfig_t *rxConfig);
 
@@ -347,11 +345,11 @@ void init(void)
     mspInit(&masterConfig.serialConfig);
     cliInit(&masterConfig.serialConfig);
 
-    failsafe = failsafeInit(&masterConfig.rxConfig);
+    failsafeInit(&masterConfig.rxConfig);
 
-    beepcodeInit(failsafe);
+    beepcodeInit();
 
-    rxInit(&masterConfig.rxConfig, failsafe);
+    rxInit(&masterConfig.rxConfig);
 
 #ifdef GPS
     if (feature(FEATURE_GPS)) {
@@ -373,7 +371,7 @@ void init(void)
 #endif
 
 #ifdef LED_STRIP
-    ledStripInit(masterConfig.ledConfigs, masterConfig.colors, failsafe);
+    ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
 
     if (feature(FEATURE_LED_STRIP)) {
         ledStripEnable();
diff --git a/src/main/mw.c b/src/main/mw.c
index e678848ecd..0437c58b58 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -102,7 +102,6 @@ int16_t telemTemperature1;      // gyro sensor temperature
 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 
 extern uint8_t dynP8[3], dynI8[3], dynD8[3];
-extern failsafe_t *failsafe;
 
 typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);            // pid controller function prototype
@@ -511,11 +510,11 @@ void processRx(void)
 
     if (feature(FEATURE_FAILSAFE)) {
 
-        if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafe->vTable->isEnabled()) {
-            failsafe->vTable->enable();
+        if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsEnabled()) {
+            failsafeEnable();
         }
 
-        failsafe->vTable->updateState();
+        failsafeUpdateState();
     }
 
     throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
@@ -555,7 +554,7 @@ void processRx(void)
 
     bool canUseHorizonMode = true;
 
-    if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {
+    if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeHasTimerElapsed())) && (sensors(SENSOR_ACC))) {
         // bumpless transfer to Level mode
     	canUseHorizonMode = false;
 
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 4fb62bd9e1..1af7a24b92 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -79,8 +79,6 @@ static rxConfig_t *rxConfig;
 
 void serialRxInit(rxConfig_t *rxConfig);
 
-static failsafe_t *failsafe;
-
 void useRxConfig(rxConfig_t *rxConfigToUse)
 {
     rxConfig = rxConfigToUse;
@@ -88,7 +86,7 @@ void useRxConfig(rxConfig_t *rxConfigToUse)
 
 #define STICK_CHANNEL_COUNT 4
 
-void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe)
+void rxInit(rxConfig_t *rxConfig)
 {
     uint8_t i;
 
@@ -98,8 +96,6 @@ void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe)
         rcData[i] = rxConfig->midrc;
     }
 
-    failsafe = initialFailsafe;
-
 #ifdef SERIAL_RX
     if (feature(FEATURE_RX_SERIAL)) {
         serialRxInit(rxConfig);
@@ -205,7 +201,7 @@ void updateRx(void)
 
     if (rcDataReceived) {
         if (feature(FEATURE_FAILSAFE)) {
-            failsafe->vTable->reset();
+            failsafeReset();
         }
     }
 }
@@ -277,7 +273,7 @@ void processRxChannels(void)
         uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel);
 
         if (feature(FEATURE_FAILSAFE) && shouldCheckPulse) {
-            failsafe->vTable->checkPulse(chan, sample);
+            failsafeCheckPulse(chan, sample);
         }
 
         // validate the range
@@ -295,7 +291,7 @@ void processRxChannels(void)
 void processDataDrivenRx(void)
 {
     if (rcDataReceived) {
-        failsafe->vTable->reset();
+        failsafeReset();
     }
 
     processRxChannels();
@@ -315,7 +311,7 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)
     rxUpdateAt = currentTime + DELAY_50_HZ;
 
     if (feature(FEATURE_FAILSAFE)) {
-        failsafe->vTable->incrementCounter();
+        failsafeOnRxCycle();
     }
 
     if (isRxDataDriven()) {
diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc
index bf955b3646..b650efebcd 100644
--- a/src/test/unit/ledstrip_unittest.cc
+++ b/src/test/unit/ledstrip_unittest.cc
@@ -418,4 +418,6 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
     return 0;
 }
 
+bool failsafeHasTimerElapsed(void) { }
+
 }

From 5e3734946ebb3950848a9213537fc4f191d6a6af Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 9 Mar 2015 23:36:45 +0100
Subject: [PATCH 31/39] # This is a combination of 2 commits. # The first
 commit's message is:

Previously, at minimum throttle, the quad would do absolutely no self-leveling
and simply run the motors at constant minthrottle.  This allowed the chance
for the quad to lose control during flight if the throttle was set to minimum,
say, to drop from a high altitude to a lower one.

With this edit, the quad will still self-level at minimum throttle when armed,
allowing for safe decents from altitude.  To prevent motors spinning when
arming/disarming, the yaw input is ignored if the throttle is at minimum and
we're using the sticks to arm/disarm.

Conflicts:
	src/main/flight/mixer.c

# This is the 2nd commit message:

added cli command disable_pid_at_min_throttle
---
 src/main/config/config.c        |  1 +
 src/main/config/config_master.h |  1 +
 src/main/flight/mixer.c         | 12 ++++++++----
 src/main/io/rc_controls.c       |  2 +-
 src/main/io/rc_controls.h       |  2 ++
 src/main/io/serial_cli.c        |  3 ++-
 src/main/mw.c                   | 10 +++++++++-
 7 files changed, 24 insertions(+), 7 deletions(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index c8d44d675f..55150f758a 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -378,6 +378,7 @@ static void resetConf(void)
     masterConfig.disarm_kill_switch = 1;
     masterConfig.auto_disarm_delay = 5;
     masterConfig.small_angle = 25;
+    masterConfig.disable_pid_at_min_throttle = 0;
 
     masterConfig.airplaneConfig.flaps_speed = 0;
     masterConfig.airplaneConfig.fixedwing_althold_dir = 1;
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index 79a093cf44..357ecbc0f8 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -65,6 +65,7 @@ typedef struct master_t {
     uint8_t disarm_kill_switch;             // allow disarm via AUX switch regardless of throttle value
     uint8_t auto_disarm_delay;              // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
     uint8_t small_angle;
+    uint8_t disable_pid_at_min_throttle;   // when enabled (nonzero), ignore pids at minimum throttle
 
     airplaneConfig_t airplaneConfig;
 
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index d79db0ef71..ea74f20217 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -671,8 +671,12 @@ void mixTable(void)
 
     if (ARMING_FLAG(ARMED)) {
 
+        // Find the maximum motor output.
         int16_t maxMotor = motor[0];
         for (i = 1; i < motorCount; i++) {
+            // If one motor is above the maxthrottle threshold, we reduce the value
+            // of all motors by the amount of overshoot.  That way, only one motor
+            // is at max and the relative power of each motor is preserved.
             if (motor[i] > maxMotor) {
                 maxMotor = motor[i];
             }
@@ -691,16 +695,16 @@ void mixTable(void)
                     motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low);
                 }
             } else {
+                // Don't spin the motors if FEATURE_MOTOR_STOP is enabled and we're
+                // at minimum throttle.
                 motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
                 if ((rcData[THROTTLE]) < rxConfig->mincheck) {
-                    if (!feature(FEATURE_MOTOR_STOP))
-                        motor[i] = escAndServoConfig->minthrottle;
-                    else
+                    if (feature(FEATURE_MOTOR_STOP)) {
                         motor[i] = escAndServoConfig->mincommand;
+                    }
                 }
             }
         }
-
     } else {
         for (i = 0; i < motorCount; i++) {
             motor[i] = motor_disarmed[i];
diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c
index 450a63b1a4..27dfcd251f 100644
--- a/src/main/io/rc_controls.c
+++ b/src/main/io/rc_controls.c
@@ -64,7 +64,7 @@ int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+
 
 uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
 
-bool isUsingSticksForArming(void)
+bool areUsingSticksToArm(void)
 {
     return isUsingSticksToArm;
 }
diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index 3a8a5e846a..33821938c0 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -136,6 +136,8 @@ typedef struct rcControlsConfig_s {
     uint8_t alt_hold_fast_change;           // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement
 } rcControlsConfig_t;
 
+bool areUsingSticksToArm(void);
+
 bool areSticksInApModePosition(uint16_t ap_mode);
 throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
 void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch);
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index e04b6187ad..511d0c12f5 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -254,7 +254,7 @@ const clivalue_t valueTable[] = {
     { "min_check",                  VAR_UINT16 | MASTER_VALUE,  &masterConfig.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX },
     { "max_check",                  VAR_UINT16 | MASTER_VALUE,  &masterConfig.rxConfig.maxcheck, PWM_RANGE_ZERO, PWM_RANGE_MAX },
     { "rssi_channel",               VAR_INT8   | MASTER_VALUE,  &masterConfig.rxConfig.rssi_channel, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT },
-    { "rssi_scale",                 VAR_UINT8   | MASTER_VALUE,  &masterConfig.rxConfig.rssi_scale, RSSI_SCALE_MIN, RSSI_SCALE_MAX },
+    { "rssi_scale",                 VAR_UINT8  | MASTER_VALUE,  &masterConfig.rxConfig.rssi_scale, RSSI_SCALE_MIN, RSSI_SCALE_MAX },
     { "input_filtering_mode",       VAR_INT8   | MASTER_VALUE,  &masterConfig.inputFilteringMode, 0, 1 },
 
     { "min_throttle",               VAR_UINT16 | MASTER_VALUE,  &masterConfig.escAndServoConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX },
@@ -274,6 +274,7 @@ const clivalue_t valueTable[] = {
     { "disarm_kill_switch",         VAR_UINT8  | MASTER_VALUE,  &masterConfig.disarm_kill_switch, 0, 1 },
     { "auto_disarm_delay",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.auto_disarm_delay, 0, 60 },
     { "small_angle",                VAR_UINT8  | MASTER_VALUE,  &masterConfig.small_angle, 0, 180 },
+    { "disable_pid_at_min_throttle", VAR_UINT8  | MASTER_VALUE,  &masterConfig.disable_pid_at_min_throttle, 0, 1 },
 
     { "flaps_speed",                VAR_UINT8  | MASTER_VALUE,  &masterConfig.airplaneConfig.flaps_speed, 0, 100 },
 
diff --git a/src/main/mw.c b/src/main/mw.c
index 0437c58b58..fb5f854569 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -529,7 +529,7 @@ void processRx(void)
         ARMING_FLAG(ARMED)
         && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING)
         && masterConfig.auto_disarm_delay != 0
-        && isUsingSticksForArming()
+        && areUsingSticksToArm()
     ) {
         if (throttleStatus == THROTTLE_LOW) {
             if ((int32_t)(disarmAt - millis()) < 0)  // delay is over
@@ -702,6 +702,14 @@ void loop(void)
         }
 #endif
 
+        // If we're armed, at minimum throttle, and we do arming via the
+        // sticks, do not process yaw input from the rx.  We do this so the
+        // motors do not spin up while we are trying to arm or disarm.
+        if (areUsingSticksToArm() &&
+                rcData[THROTTLE] <= masterConfig.rxConfig.mincheck) {
+            rcCommand[YAW] = 0;
+        }
+
         if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
             rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value);
         }

From 1b1163da102c858b099426a3499af59459b72c82 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 9 Mar 2015 23:42:18 +0100
Subject: [PATCH 32/39] Previously, at minimum throttle, the quad would do
 absolutely no self-leveling and simply run the motors at constant
 minthrottle.  This allowed the chance for the quad to lose control during
 flight if the throttle was set to minimum, say, to drop from a high altitude
 to a lower one.

With this edit, the quad will still self-level at minimum throttle when armed,
allowing for safe decents from altitude.  To prevent motors spinning when
arming/disarming, the yaw input is ignored if the throttle is at minimum and
we're using the sticks to arm/disarm.

Conflicts:
	src/main/flight/mixer.c

added cli command disable_pid_at_min_throttle

(same as previous)
---
 src/main/config/config.c        | 1 +
 src/main/config/config_master.h | 2 +-
 src/main/flight/mixer.c         | 2 ++
 3 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index 55150f758a..45861c92e8 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -380,6 +380,7 @@ static void resetConf(void)
     masterConfig.small_angle = 25;
     masterConfig.disable_pid_at_min_throttle = 0;
 
+
     masterConfig.airplaneConfig.flaps_speed = 0;
     masterConfig.airplaneConfig.fixedwing_althold_dir = 1;
 
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index 357ecbc0f8..6de9f68a1f 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -65,7 +65,7 @@ typedef struct master_t {
     uint8_t disarm_kill_switch;             // allow disarm via AUX switch regardless of throttle value
     uint8_t auto_disarm_delay;              // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
     uint8_t small_angle;
-    uint8_t disable_pid_at_min_throttle;   // when enabled (nonzero), ignore pids at minimum throttle
+    uint8_t disable_pid_at_min_throttle;    // when enabled (nonzero), ignore pids at minimum throttle
 
     airplaneConfig_t airplaneConfig;
 
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index ea74f20217..f222d78874 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -701,6 +701,8 @@ void mixTable(void)
                 if ((rcData[THROTTLE]) < rxConfig->mincheck) {
                     if (feature(FEATURE_MOTOR_STOP)) {
                         motor[i] = escAndServoConfig->mincommand;
+                    } else if (masterConfig.disable_pid_at_min_throttle != 0) {
+                        motor[i] = escAndServoConfig->minthrottle;
                     }
                 }
             }

From acabbf41db4537697280d813c21e08c19749ee20 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 9 Mar 2015 23:44:03 +0100
Subject: [PATCH 33/39] Previously, at minimum throttle, the quad would do
 absolutely no self-leveling and simply run the motors at constant
 minthrottle.  This allowed the chance for the quad to lose control during
 flight if the throttle was set to minimum, say, to drop from a high altitude
 to a lower one.

With this edit, the quad will still self-level at minimum throttle when armed,
allowing for safe decents from altitude.  To prevent motors spinning when
arming/disarming, the yaw input is ignored if the throttle is at minimum and
we're using the sticks to arm/disarm.

Conflicts:
	src/main/flight/mixer.c
---
 docs/Cli.md               |  1 +
 src/main/flight/mixer.c   | 23 +++++++++++++++++++++--
 src/main/io/rc_controls.c |  5 ++++-
 src/main/mw.c             |  9 ++++-----
 4 files changed, 30 insertions(+), 8 deletions(-)

diff --git a/docs/Cli.md b/docs/Cli.md
index 129b9bcd66..7c140be87a 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -118,6 +118,7 @@ Re-apply any new defaults as desired.
 | disarm_kill_switch            | Enabled by default. Disarms the motors independently of throttle value. Setting to 0 reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel.                                                                                                                                                                                                                                                                                                                                                                                                                              | 0      | 1      | 1             | Master       | UINT8    |
 | auto_disarm_delay             |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 60     | 5             | Master       | UINT8    |
 | small_angle                   | If the copter tilt angle exceed this value the copter will refuse to arm. default is 25°.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              | 0      | 180    | 25            | Master       | UINT8    |
+| disable_pid_at_min_throttle   | If enabled, the copter will not process the pid algorithm at minimum throttle.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          | 0      | 1      | 0             | Master       | UINT8    |
 | flaps_speed                   |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 100    | 0             | Master       | UINT8    |
 | fixedwing_althold_dir         |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | -1     | 1      | 1             | Master       | INT8     |
 | reboot_character              |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 48     | 126    | 82            | Master       | UINT8    |
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index f222d78874..d504bc473c 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -25,31 +25,48 @@
 
 #include "common/axis.h"
 #include "common/maths.h"
+#include "common/color.h"
 
 #include "drivers/system.h"
 #include "drivers/gpio.h"
 #include "drivers/timer.h"
 #include "drivers/pwm_output.h"
 #include "drivers/pwm_mapping.h"
+#include "drivers/pwm_rx.h"
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/serial.h"
+#include "drivers/system.h"
 
 #include "rx/rx.h"
 
 #include "io/gimbal.h"
 #include "io/escservo.h"
 #include "io/rc_controls.h"
+#include "io/gps.h"
+#include "io/serial.h"
+#include "io/ledstrip.h"
 
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
+#include "sensors/boardalignment.h"
+#include "sensors/gyro.h"
+#include "sensors/battery.h"
+#include "sensors/barometer.h"
 
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/imu.h"
 #include "flight/lowpass.h"
+#include "flight/failsafe.h"
+#include "flight/navigation.h"
+
+#include "telemetry/telemetry.h"
 
 #include "config/runtime_config.h"
 #include "config/config.h"
+#include "config/config_profile.h"
+#include "config/config_master.h"
 
 #define GIMBAL_SERVO_PITCH 0
 #define GIMBAL_SERVO_ROLL 1
@@ -695,8 +712,10 @@ void mixTable(void)
                     motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low);
                 }
             } else {
-                // Don't spin the motors if FEATURE_MOTOR_STOP is enabled and we're
-                // at minimum throttle.
+                // If we're at minimum throttle and FEATURE_MOTOR_STOP enabled,
+                // do not spin the motors.
+                // If we're at minimum throttle and disable_pid_at_min_throttle
+                // is enabled, spin motors at minimum throttle.
                 motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
                 if ((rcData[THROTTLE]) < rxConfig->mincheck) {
                     if (feature(FEATURE_MOTOR_STOP)) {
diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c
index 27dfcd251f..1c4a968f36 100644
--- a/src/main/io/rc_controls.c
+++ b/src/main/io/rc_controls.c
@@ -58,17 +58,20 @@
 static escAndServoConfig_t *escAndServoConfig;
 static pidProfile_t *pidProfile;
 
+// true if arming is done via the sticks (as opposed to a switch)
 static bool isUsingSticksToArm = true;
 
 int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
 
 uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
 
-bool areUsingSticksToArm(void)
+
+bool isUsingSticksForArming(void)
 {
     return isUsingSticksToArm;
 }
 
+
 bool areSticksInApModePosition(uint16_t ap_mode)
 {
     return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
diff --git a/src/main/mw.c b/src/main/mw.c
index fb5f854569..0344bc595e 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -525,12 +525,10 @@ void processRx(void)
     }
     // When armed and motors aren't spinning, disarm board after delay so users without buzzer won't lose fingers.
     // mixTable constrains motor commands, so checking  throttleStatus is enough
-    if (
-        ARMING_FLAG(ARMED)
+    if (ARMING_FLAG(ARMED)
         && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING)
         && masterConfig.auto_disarm_delay != 0
-        && areUsingSticksToArm()
-    ) {
+        && isUsingSticksForArming()) {
         if (throttleStatus == THROTTLE_LOW) {
             if ((int32_t)(disarmAt - millis()) < 0)  // delay is over
                 mwDisarm();
@@ -705,11 +703,12 @@ void loop(void)
         // If we're armed, at minimum throttle, and we do arming via the
         // sticks, do not process yaw input from the rx.  We do this so the
         // motors do not spin up while we are trying to arm or disarm.
-        if (areUsingSticksToArm() &&
+        if (isUsingSticksForArming() &&
                 rcData[THROTTLE] <= masterConfig.rxConfig.mincheck) {
             rcCommand[YAW] = 0;
         }
 
+
         if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
             rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value);
         }

From ed434fe47b5865dd55907a59c0bfb84aac62dce5 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 9 Mar 2015 22:50:27 +0000
Subject: [PATCH 34/39] Use a positive named setting and variable instead of a
 negative one to simplify the logic and aid understanding.

---
 docs/Cli.md                     | 2 +-
 src/main/config/config.c        | 2 +-
 src/main/config/config_master.h | 2 +-
 src/main/flight/mixer.c         | 4 +---
 src/main/io/serial_cli.c        | 2 +-
 5 files changed, 5 insertions(+), 7 deletions(-)

diff --git a/docs/Cli.md b/docs/Cli.md
index 7c140be87a..9980dc1c36 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -118,7 +118,7 @@ Re-apply any new defaults as desired.
 | disarm_kill_switch            | Enabled by default. Disarms the motors independently of throttle value. Setting to 0 reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel.                                                                                                                                                                                                                                                                                                                                                                                                                              | 0      | 1      | 1             | Master       | UINT8    |
 | auto_disarm_delay             |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 60     | 5             | Master       | UINT8    |
 | small_angle                   | If the copter tilt angle exceed this value the copter will refuse to arm. default is 25°.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              | 0      | 180    | 25            | Master       | UINT8    |
-| disable_pid_at_min_throttle   | If enabled, the copter will not process the pid algorithm at minimum throttle.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          | 0      | 1      | 0             | Master       | UINT8    |
+| pid_at_min_throttle   | If enabled, the copter will process the pid algorithm at minimum throttle.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          | 0      | 1      | 0             | Master       | UINT8    |
 | flaps_speed                   |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 100    | 0             | Master       | UINT8    |
 | fixedwing_althold_dir         |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | -1     | 1      | 1             | Master       | INT8     |
 | reboot_character              |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 48     | 126    | 82            | Master       | UINT8    |
diff --git a/src/main/config/config.c b/src/main/config/config.c
index 45861c92e8..91620e0a59 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -378,7 +378,7 @@ static void resetConf(void)
     masterConfig.disarm_kill_switch = 1;
     masterConfig.auto_disarm_delay = 5;
     masterConfig.small_angle = 25;
-    masterConfig.disable_pid_at_min_throttle = 0;
+    masterConfig.pid_at_min_throttle = 1;
 
 
     masterConfig.airplaneConfig.flaps_speed = 0;
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index 6de9f68a1f..ef6f584c18 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -65,7 +65,7 @@ typedef struct master_t {
     uint8_t disarm_kill_switch;             // allow disarm via AUX switch regardless of throttle value
     uint8_t auto_disarm_delay;              // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
     uint8_t small_angle;
-    uint8_t disable_pid_at_min_throttle;    // when enabled (nonzero), ignore pids at minimum throttle
+    uint8_t pid_at_min_throttle;            // when enabled pids are used at minimum throttle
 
     airplaneConfig_t airplaneConfig;
 
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index d504bc473c..961b6675b1 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -714,13 +714,11 @@ void mixTable(void)
             } else {
                 // If we're at minimum throttle and FEATURE_MOTOR_STOP enabled,
                 // do not spin the motors.
-                // If we're at minimum throttle and disable_pid_at_min_throttle
-                // is enabled, spin motors at minimum throttle.
                 motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
                 if ((rcData[THROTTLE]) < rxConfig->mincheck) {
                     if (feature(FEATURE_MOTOR_STOP)) {
                         motor[i] = escAndServoConfig->mincommand;
-                    } else if (masterConfig.disable_pid_at_min_throttle != 0) {
+                    } else if (masterConfig.pid_at_min_throttle == 0) {
                         motor[i] = escAndServoConfig->minthrottle;
                     }
                 }
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 511d0c12f5..3624e34ce3 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -274,7 +274,7 @@ const clivalue_t valueTable[] = {
     { "disarm_kill_switch",         VAR_UINT8  | MASTER_VALUE,  &masterConfig.disarm_kill_switch, 0, 1 },
     { "auto_disarm_delay",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.auto_disarm_delay, 0, 60 },
     { "small_angle",                VAR_UINT8  | MASTER_VALUE,  &masterConfig.small_angle, 0, 180 },
-    { "disable_pid_at_min_throttle", VAR_UINT8  | MASTER_VALUE,  &masterConfig.disable_pid_at_min_throttle, 0, 1 },
+    { "pid_at_min_throttle",        VAR_UINT8  | MASTER_VALUE,  &masterConfig.pid_at_min_throttle, 0, 1 },
 
     { "flaps_speed",                VAR_UINT8  | MASTER_VALUE,  &masterConfig.airplaneConfig.flaps_speed, 0, 100 },
 

From 4a12d00d1e23fbab5f7c0effda5bf07b79f7a1d5 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 9 Mar 2015 23:00:22 +0000
Subject: [PATCH 35/39] Moving mixer config out of the profile.  It doesn't
 really make sense.

---
 src/main/config/config.c         | 21 +++++++++++++--------
 src/main/config/config_master.h  |  4 +++-
 src/main/config/config_profile.h |  3 ---
 src/main/flight/mixer.c          | 18 +-----------------
 src/main/flight/mixer.h          |  1 +
 src/main/io/serial_cli.c         | 12 +++++++-----
 6 files changed, 25 insertions(+), 34 deletions(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index 91620e0a59..6994e231dc 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -119,7 +119,7 @@ profile_t *currentProfile;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 93;
+static const uint8_t EEPROM_CONF_VERSION = 94;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -293,6 +293,16 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {
     rcControlsConfig->alt_hold_fast_change = 1;
 }
 
+void resetMixerConfig(mixerConfig_t *mixerConfig) {
+    mixerConfig->pid_at_min_throttle = 1;
+    mixerConfig->yaw_direction = 1;
+#ifdef USE_SERVOS
+    mixerConfig->tri_unarmed_servo = 1;
+    mixerConfig->servo_lowpass_freq = 400;
+    mixerConfig->servo_lowpass_enable = 0;
+#endif
+}
+
 uint8_t getCurrentProfile(void)
 {
     return masterConfig.current_profile_index;
@@ -378,8 +388,8 @@ static void resetConf(void)
     masterConfig.disarm_kill_switch = 1;
     masterConfig.auto_disarm_delay = 5;
     masterConfig.small_angle = 25;
-    masterConfig.pid_at_min_throttle = 1;
 
+    resetMixerConfig(&masterConfig.mixerConfig);
 
     masterConfig.airplaneConfig.flaps_speed = 0;
     masterConfig.airplaneConfig.fixedwing_althold_dir = 1;
@@ -452,11 +462,6 @@ static void resetConf(void)
         currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
     }
 
-    currentProfile->mixerConfig.yaw_direction = 1;
-    currentProfile->mixerConfig.tri_unarmed_servo = 1;
-    currentProfile->mixerConfig.servo_lowpass_freq = 400;
-    currentProfile->mixerConfig.servo_lowpass_enable = 0;
-
     // gimbal
     currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL;
 #endif
@@ -648,7 +653,7 @@ void activateConfig(void)
 #endif
         &masterConfig.flight3DConfig,
         &masterConfig.escAndServoConfig,
-        &currentProfile->mixerConfig,
+        &masterConfig.mixerConfig,
         &masterConfig.airplaneConfig,
         &masterConfig.rxConfig
     );
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index ef6f584c18..895fcf6c1a 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -65,7 +65,9 @@ typedef struct master_t {
     uint8_t disarm_kill_switch;             // allow disarm via AUX switch regardless of throttle value
     uint8_t auto_disarm_delay;              // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
     uint8_t small_angle;
-    uint8_t pid_at_min_throttle;            // when enabled pids are used at minimum throttle
+
+    // mixer-related configuration
+    mixerConfig_t mixerConfig;
 
     airplaneConfig_t airplaneConfig;
 
diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h
index 1d7d285b59..7b2ccb5103 100644
--- a/src/main/config/config_profile.h
+++ b/src/main/config/config_profile.h
@@ -57,9 +57,6 @@ typedef struct profile_s {
     // Failsafe related configuration
     failsafeConfig_t failsafeConfig;
 
-    // mixer-related configuration
-    mixerConfig_t mixerConfig;
-
 #ifdef GPS
     gpsProfile_t gpsProfile;
 #endif
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 961b6675b1..42dcc4ca4d 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -25,17 +25,14 @@
 
 #include "common/axis.h"
 #include "common/maths.h"
-#include "common/color.h"
 
 #include "drivers/system.h"
 #include "drivers/gpio.h"
 #include "drivers/timer.h"
 #include "drivers/pwm_output.h"
 #include "drivers/pwm_mapping.h"
-#include "drivers/pwm_rx.h"
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
-#include "drivers/serial.h"
 #include "drivers/system.h"
 
 #include "rx/rx.h"
@@ -43,30 +40,17 @@
 #include "io/gimbal.h"
 #include "io/escservo.h"
 #include "io/rc_controls.h"
-#include "io/gps.h"
-#include "io/serial.h"
-#include "io/ledstrip.h"
 
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
-#include "sensors/boardalignment.h"
-#include "sensors/gyro.h"
-#include "sensors/battery.h"
-#include "sensors/barometer.h"
 
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/imu.h"
 #include "flight/lowpass.h"
-#include "flight/failsafe.h"
-#include "flight/navigation.h"
-
-#include "telemetry/telemetry.h"
 
 #include "config/runtime_config.h"
 #include "config/config.h"
-#include "config/config_profile.h"
-#include "config/config_master.h"
 
 #define GIMBAL_SERVO_PITCH 0
 #define GIMBAL_SERVO_ROLL 1
@@ -718,7 +702,7 @@ void mixTable(void)
                 if ((rcData[THROTTLE]) < rxConfig->mincheck) {
                     if (feature(FEATURE_MOTOR_STOP)) {
                         motor[i] = escAndServoConfig->mincommand;
-                    } else if (masterConfig.pid_at_min_throttle == 0) {
+                    } else if (mixerConfig->pid_at_min_throttle == 0) {
                         motor[i] = escAndServoConfig->minthrottle;
                     }
                 }
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 627199af3b..121ab8fa88 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -64,6 +64,7 @@ typedef struct mixer_t {
 } mixer_t;
 
 typedef struct mixerConfig_s {
+    uint8_t pid_at_min_throttle;            // when enabled pids are used at minimum throttle
     int8_t yaw_direction;
 #ifdef USE_SERVOS
     uint8_t tri_unarmed_servo;              // send tail servo correction pulses even when unarmed
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 3624e34ce3..cbdd3ad723 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -274,7 +274,6 @@ const clivalue_t valueTable[] = {
     { "disarm_kill_switch",         VAR_UINT8  | MASTER_VALUE,  &masterConfig.disarm_kill_switch, 0, 1 },
     { "auto_disarm_delay",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.auto_disarm_delay, 0, 60 },
     { "small_angle",                VAR_UINT8  | MASTER_VALUE,  &masterConfig.small_angle, 0, 180 },
-    { "pid_at_min_throttle",        VAR_UINT8  | MASTER_VALUE,  &masterConfig.pid_at_min_throttle, 0, 1 },
 
     { "flaps_speed",                VAR_UINT8  | MASTER_VALUE,  &masterConfig.airplaneConfig.flaps_speed, 0, 100 },
 
@@ -382,12 +381,15 @@ const clivalue_t valueTable[] = {
     { "throttle_correction_angle",  VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].throttle_correction_angle, 1, 900 },
 
     { "yaw_control_direction",      VAR_INT8   | MASTER_VALUE,  &masterConfig.yaw_control_direction, -1, 1 },
-    { "yaw_direction",              VAR_INT8   | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 },
+
+    { "pid_at_min_throttle",        VAR_UINT8  | MASTER_VALUE,  &masterConfig.mixerConfig.pid_at_min_throttle, 0, 1 },
+    { "yaw_direction",              VAR_INT8   | PROFILE_VALUE, &masterConfig.mixerConfig.yaw_direction, -1, 1 },
 #ifdef USE_SERVOS
-    { "tri_unarmed_servo",          VAR_INT8   | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 },
-    { "servo_lowpass_freq",         VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_freq, 10, 400},
-    { "servo_lowpass_enable",       VAR_INT8   | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_enable, 0, 1 },
+    { "tri_unarmed_servo",          VAR_INT8   | PROFILE_VALUE, &masterConfig.mixerConfig.tri_unarmed_servo, 0, 1 },
+    { "servo_lowpass_freq",         VAR_INT16  | PROFILE_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, 10, 400},
+    { "servo_lowpass_enable",       VAR_INT8   | PROFILE_VALUE, &masterConfig.mixerConfig.servo_lowpass_enable, 0, 1 },
 #endif
+
     { "default_rate_profile",       VAR_UINT8  | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 },
     { "rc_rate",                    VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, 0, 250 },
     { "rc_expo",                    VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, 0, 100 },

From c0c2f8e285c9d3834d917f780b3db5bb609258f9 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 9 Mar 2015 23:04:54 +0000
Subject: [PATCH 36/39] Remove a suprious newline.

---
 src/main/mw.c | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/main/mw.c b/src/main/mw.c
index 0344bc595e..4edad40cda 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -703,8 +703,7 @@ void loop(void)
         // If we're armed, at minimum throttle, and we do arming via the
         // sticks, do not process yaw input from the rx.  We do this so the
         // motors do not spin up while we are trying to arm or disarm.
-        if (isUsingSticksForArming() &&
-                rcData[THROTTLE] <= masterConfig.rxConfig.mincheck) {
+        if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck) {
             rcCommand[YAW] = 0;
         }
 

From bc8e53a9d82fa99258059111a50e79fa07b3e877 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 9 Mar 2015 23:26:52 +0000
Subject: [PATCH 37/39] Rename some PID controller methods.

See #461.
---
 src/main/config/config.c | 3 +--
 src/main/flight/pid.c    | 6 +++---
 src/main/flight/pid.h    | 6 +++---
 src/main/io/serial_msp.c | 2 +-
 src/main/mw.c            | 8 ++++----
 5 files changed, 12 insertions(+), 13 deletions(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index 6994e231dc..3398523c11 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -73,7 +73,6 @@
 #define BRUSHED_MOTORS_PWM_RATE 16000
 #define BRUSHLESS_MOTORS_PWM_RATE 400
 
-void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h
 void mixerUseConfigs(
 #ifdef USE_SERVOS
         servoParam_t *servoConfToUse,
@@ -636,7 +635,7 @@ void activateConfig(void)
     useTelemetryConfig(&masterConfig.telemetryConfig);
 #endif
 
-    setPIDController(currentProfile->pidProfile.pidController);
+    pidSetController(currentProfile->pidProfile.pidController);
 
 #ifdef GPS
     gpsUseProfile(&currentProfile->gpsProfile);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index a009a5e527..339fab4541 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -70,7 +70,7 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig
 
 pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
 
-void resetErrorAngle(void)
+void pidResetErrorAngle(void)
 {
     errorAngleI[ROLL] = 0;
     errorAngleI[PITCH] = 0;
@@ -79,7 +79,7 @@ void resetErrorAngle(void)
     errorAngleIf[PITCH] = 0.0f;
 }
 
-void resetErrorGyro(void)
+void pidResetErrorGyro(void)
 {
     errorGyroI[ROLL] = 0;
     errorGyroI[PITCH] = 0;
@@ -742,7 +742,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
     }
 }
 
-void setPIDController(int type)
+void pidSetController(int type)
 {
     switch (type) {
         case 0:
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index b702734169..e1ef9d34e3 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -55,8 +55,8 @@ typedef struct pidProfile_s {
 extern int16_t axisPID[XYZ_AXIS_COUNT];
 extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 
-void setPIDController(int type);
-void resetErrorAngle(void);
-void resetErrorGyro(void);
+void pidSetController(int type);
+void pidResetErrorAngle(void);
+void pidResetErrorGyro(void);
 
 
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 1861bc48a9..fd5e070c73 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -1257,7 +1257,7 @@ static bool processInCommand(void)
         break;
     case MSP_SET_PID_CONTROLLER:
         currentProfile->pidProfile.pidController = read8();
-        setPIDController(currentProfile->pidProfile.pidController);
+        pidSetController(currentProfile->pidProfile.pidController);
         break;
     case MSP_SET_PID:
         if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
diff --git a/src/main/mw.c b/src/main/mw.c
index 4edad40cda..7085a8a7a0 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -520,8 +520,8 @@ void processRx(void)
     throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 
     if (throttleStatus == THROTTLE_LOW) {
-        resetErrorAngle();
-        resetErrorGyro();
+        pidResetErrorAngle();
+        pidResetErrorGyro();
     }
     // When armed and motors aren't spinning, disarm board after delay so users without buzzer won't lose fingers.
     // mixTable constrains motor commands, so checking  throttleStatus is enough
@@ -557,7 +557,7 @@ void processRx(void)
     	canUseHorizonMode = false;
 
         if (!FLIGHT_MODE(ANGLE_MODE)) {
-            resetErrorAngle();
+            pidResetErrorAngle();
             ENABLE_FLIGHT_MODE(ANGLE_MODE);
         }
     } else {
@@ -569,7 +569,7 @@ void processRx(void)
         DISABLE_FLIGHT_MODE(ANGLE_MODE);
 
         if (!FLIGHT_MODE(HORIZON_MODE)) {
-            resetErrorAngle();
+            pidResetErrorAngle();
             ENABLE_FLIGHT_MODE(HORIZON_MODE);
         }
     } else {

From 44b36107a6495634686449fa2cb295bea9151014 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 9 Mar 2015 23:41:08 +0000
Subject: [PATCH 38/39] Documenting the ID's used for modes by the CLI.

See #509, #510, #518
---
 docs/Modes.md | 48 ++++++++++++++++++++++++------------------------
 1 file changed, 24 insertions(+), 24 deletions(-)

diff --git a/docs/Modes.md b/docs/Modes.md
index 145997bfb4..0c2c7ec10b 100644
--- a/docs/Modes.md
+++ b/docs/Modes.md
@@ -3,30 +3,30 @@
 Cleanflight has various modes that can be toggled on or off.  Modes can be enabled/disabled by stick positions,
 auxillary receiver channels and other events such as failsafe detection.
 
-| MSP ID  | Short Name | Function                                                             |
-| ------- | ---------- | -------------------------------------------------------------------- |
-| 0       | ARM        | Enables motors and flight stabilisation                              |
-| 1       | ANGLE      | Legacy auto-level flight mode                                        |
-| 2       | HORIZON    | Auto-level flight mode                                               |
-| 3       | BARO       | Altitude hold mode (Requires barometer sensor)                       |
-| 5       | MAG        | Heading lock                                                         |
-| 6       | HEADFREE   | Head Free - When enabled yaw has no effect on pitch/roll inputs      |
-| 7       | HEADADJ    | Heading Adjust - Sets a new yaw origin for HEADFREE mode             |
-| 8       | CAMSTAB    | Camera Stabilisation                                                 |
-| 9       | CAMTRIG    |                                                                      |
-| 10      | GPSHOME    | Autonomous flight to HOME position                                   |
-| 11      | GPSHOLD    | Maintain the same longitude/lattitude                                |
-| 12      | PASSTHRU   |                                                                      |
-| 13      | BEEPERON   | Enable beeping - useful for locating a crashed aircraft              |
-| 14      | LEDMAX     |                                                                      |
-| 15      | LEDLOW     |                                                                      |
-| 16      | LLIGHTS    |                                                                      |
-| 17      | CALIB      |                                                                      |
-| 18      | GOV        |                                                                      |
-| 19      | OSD        | Enable/Disable On-Screen-Display (OSD)                               |
-| 20      | TELEMETRY  | Enable telemetry via switch                                          |
-| 21      | AUTOTUNE   | Autotune Pitch/Roll PIDs                                             |
-| 22      | SONAR      | Altitude hold mode (sonar sensor only)                               |
+| MSP ID  | CLI ID | Short Name | Function                                                             |
+| ------- | ------ | ---------- | -------------------------------------------------------------------- |
+| 0       | 0      | ARM        | Enables motors and flight stabilisation                              |
+| 1       | 1      | ANGLE      | Legacy auto-level flight mode                                        |
+| 2       | 2      | HORIZON    | Auto-level flight mode                                               |
+| 3       | 3      | BARO       | Altitude hold mode (Requires barometer sensor)                       |
+| 5       | 4      | MAG        | Heading lock                                                         |
+| 6       | 5      | HEADFREE   | Head Free - When enabled yaw has no effect on pitch/roll inputs      |
+| 7       | 6      | HEADADJ    | Heading Adjust - Sets a new yaw origin for HEADFREE mode             |
+| 8       | 7      | CAMSTAB    | Camera Stabilisation                                                 |
+| 9       | 8      | CAMTRIG    |                                                                      |
+| 10      | 9      | GPSHOME    | Autonomous flight to HOME position                                   |
+| 11      | 10     | GPSHOLD    | Maintain the same longitude/lattitude                                |
+| 12      | 11     | PASSTHRU   |                                                                      |
+| 13      | 12     | BEEPERON   | Enable beeping - useful for locating a crashed aircraft              |
+| 14      | 13     | LEDMAX     |                                                                      |
+| 15      | 14     | LEDLOW     |                                                                      |
+| 16      | 15     | LLIGHTS    |                                                                      |
+| 17      | 16     | CALIB      |                                                                      |
+| 18      | 17     | GOV        |                                                                      |
+| 19      | 18     | OSD        | Enable/Disable On-Screen-Display (OSD)                               |
+| 20      | 19     | TELEMETRY  | Enable telemetry via switch                                          |
+| 21      | 20     | AUTOTUNE   | Autotune Pitch/Roll PIDs                                             |
+| 22      | 21     | SONAR      | Altitude hold mode (sonar sensor only)                               |
 
 ## Mode details
 

From 4746b336b9870bf98f0df237438ec4db9bbb7c69 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Tue, 10 Mar 2015 23:01:06 +0000
Subject: [PATCH 39/39] Tweak to GPS page to show when Space Vehicle updates
 are received via a ticker next to the bargraphs which updates each time SV
 info is received.

Some code-size improvements optimizations could be done.
---
 src/main/io/display.c | 77 ++++++++++++++++++++++++++++++++-----------
 src/main/io/gps.c     |  5 +++
 src/main/io/gps.h     |  1 +
 3 files changed, 64 insertions(+), 19 deletions(-)

diff --git a/src/main/io/display.c b/src/main/io/display.c
index 11566c76b5..2e8915da99 100644
--- a/src/main/io/display.c
+++ b/src/main/io/display.c
@@ -80,6 +80,9 @@ static rxConfig_t *rxConfig;
 
 static char lineBuffer[SCREEN_CHARACTER_COLUMN_COUNT + 1];
 
+#define HALF_SCREEN_CHARACTER_COLUMN_COUNT (SCREEN_CHARACTER_COLUMN_COUNT / 2)
+#define IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD (SCREEN_CHARACTER_COLUMN_COUNT & 1)
+
 const char* pageTitles[] = {
     "CLEANFLIGHT",
     "ARMED",
@@ -112,7 +115,7 @@ const uint8_t cyclePageIds[] = {
 
 #define CYCLE_PAGE_ID_COUNT (sizeof(cyclePageIds) / sizeof(cyclePageIds[0]))
 
-static const char* tickerCharacters = "|/-\\";
+static const char* tickerCharacters = "|/-\\"; // use 2/4/8 characters so that the divide is optimal.
 #define TICKER_CHARACTER_COUNT (sizeof(tickerCharacters) / sizeof(char))
 
 typedef enum {
@@ -146,6 +149,17 @@ void padLineBuffer(void)
     while (length < sizeof(lineBuffer) - 1) {
         lineBuffer[length++] = ' ';
     }
+    lineBuffer[length] = 0;
+}
+
+void padHalfLineBuffer(void)
+{
+    uint8_t halfLineIndex = sizeof(lineBuffer) / 2;
+    uint8_t length = strlen(lineBuffer);
+    while (length < halfLineIndex - 1) {
+        lineBuffer[length++] = ' ';
+    }
+    lineBuffer[length] = 0;
 }
 
 // LCDbar(n,v) : draw a bar graph - n number of chars for width, v value in % to display
@@ -179,6 +193,7 @@ void fillScreenWithCharacters()
 }
 #endif
 
+
 void updateTicker(void)
 {
     static uint8_t tickerIndex = 0;
@@ -215,9 +230,6 @@ void drawRxChannel(uint8_t channelIndex, uint8_t width)
     drawHorizonalPercentageBar(width - 1, percentage);
 }
 
-#define HALF_SCREEN_CHARACTER_COLUMN_COUNT (SCREEN_CHARACTER_COLUMN_COUNT / 2)
-#define IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD (SCREEN_CHARACTER_COLUMN_COUNT & 1)
-
 #define RX_CHANNELS_PER_PAGE_COUNT 14
 void showRxPage(void)
 {
@@ -295,43 +307,70 @@ void showProfilePage(void)
 void showGpsPage() {
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 
+    static uint8_t gpsTicker = 0;
+    static uint32_t lastGPSSvInfoReceivedCount = 0;
+    if (GPS_svInfoReceivedCount != lastGPSSvInfoReceivedCount) {
+        lastGPSSvInfoReceivedCount = GPS_svInfoReceivedCount;
+        gpsTicker++;
+        gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT;
+    }
+
+    i2c_OLED_set_xy(0, rowIndex);
+    i2c_OLED_send_char(tickerCharacters[gpsTicker]);
+
     i2c_OLED_set_xy(MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
 
     uint32_t index;
     for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) {
-        uint8_t bargraphValue = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
-        bargraphValue = MIN(bargraphValue, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
-        i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphValue);
+        uint8_t bargraphOffset = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
+        bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
+        i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
     }
 
+
     char fixChar = STATE(GPS_FIX) ? 'Y' : 'N';
-    tfp_sprintf(lineBuffer, "Satellites: %d Fix: %c", GPS_numSat, fixChar);
+    tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", GPS_numSat, fixChar);
     padLineBuffer();
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
-    tfp_sprintf(lineBuffer, "Lat: %d Lon: %d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER);
+    tfp_sprintf(lineBuffer, "La/Lo: %d/%d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER);
     padLineBuffer();
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
-    tfp_sprintf(lineBuffer, "Spd: %d cm/s GC: %d", GPS_speed, GPS_ground_course);
-    padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
+    tfp_sprintf(lineBuffer, "Spd: %d", GPS_speed);
+    padHalfLineBuffer();
+    i2c_OLED_set_line(rowIndex);
     i2c_OLED_send_string(lineBuffer);
 
-    tfp_sprintf(lineBuffer, "RX: %d Delta: %d", GPS_packetCount, gpsData.lastMessage - gpsData.lastLastMessage);
-    padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
+    tfp_sprintf(lineBuffer, "GC: %d", GPS_ground_course);
+    padHalfLineBuffer();
+    i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
-    tfp_sprintf(lineBuffer, "ERRs: %d TOs: %d", gpsData.errors, gpsData.timeouts);
-    padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
+    tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount);
+    padHalfLineBuffer();
+    i2c_OLED_set_line(rowIndex);
+    i2c_OLED_send_string(lineBuffer);
+
+    tfp_sprintf(lineBuffer, "ERRs: %d", gpsData.errors, gpsData.timeouts);
+    padHalfLineBuffer();
+    i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
+    i2c_OLED_send_string(lineBuffer);
+
+    tfp_sprintf(lineBuffer, "Delta: %d", gpsData.lastMessage - gpsData.lastLastMessage);
+    padHalfLineBuffer();
+    i2c_OLED_set_line(rowIndex);
+    i2c_OLED_send_string(lineBuffer);
+
+    tfp_sprintf(lineBuffer, "TOs: %d", gpsData.timeouts);
+    padHalfLineBuffer();
+    i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
     strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT);
-    padLineBuffer();
+    padHalfLineBuffer();
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 5609fd03f9..5b919f1856 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -77,6 +77,7 @@ int32_t GPS_coord[2];               // LAT/LON
 uint8_t GPS_numSat;
 uint16_t GPS_hdop = 9999;           // Compute GPS quality signal
 uint32_t GPS_packetCount = 0;
+uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
 uint8_t GPS_update = 0;             // it's a binary toggle to distinct a GPS position update
 
 uint16_t GPS_altitude;              // altitude in 0.1m
@@ -644,6 +645,9 @@ static bool gpsNewFrameNMEA(char c)
                             GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox
                             break;
                     }
+
+                    GPS_svInfoReceivedCount++;
+
                     break;
             }
 
@@ -915,6 +919,7 @@ static bool UBLOX_parse_gps(void)
             GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality;
             GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno;
         }
+        GPS_svInfoReceivedCount++;
         break;
     default:
         return false;
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index 34ad83625a..ee90f09b23 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -104,6 +104,7 @@ extern uint8_t GPS_numSat;
 extern uint16_t GPS_hdop;                  // GPS signal quality
 extern uint8_t GPS_update;                 // it's a binary toogle to distinct a GPS position update
 extern uint32_t GPS_packetCount;
+extern uint32_t GPS_svInfoReceivedCount;
 extern uint16_t GPS_altitude;              // altitude in 0.1m
 extern uint16_t GPS_speed;                 // speed in 0.1m/s
 extern uint16_t GPS_ground_course;         // degrees * 10