From eaff64d25f56c2041226bccc51cbb8053bc6da7c Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Wed, 15 Jul 2015 17:23:57 +0200
Subject: [PATCH 01/41] RC Smoothing

fix loopcount

Improve AUX logic // initial factor set to 1

Better Delta approach

FIX AUX CALC

Update refresh rates
---
 src/main/mw.c    | 113 +++++++++++++++++++++++++++++++++++++++++++++++
 src/main/rx/rx.c |   2 +-
 2 files changed, 114 insertions(+), 1 deletion(-)

diff --git a/src/main/mw.c b/src/main/mw.c
index 5a376f8f2..708dc006a 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -93,6 +93,8 @@ enum {
 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
 #define IBATINTERVAL (6 * 3500)       
 
+#define LOOP_DEADBAND 400 // Dead band for loop to modify to rcInterpolationFactor in RC Filtering for unstable looptimes
+
 uint32_t currentTime = 0;
 uint32_t previousTime = 0;
 uint16_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
@@ -107,6 +109,8 @@ static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the m
 
 extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
 
+static bool isRXdataNew;
+
 typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);            // pid controller function prototype
 
@@ -681,6 +685,112 @@ void processRx(void)
 
 }
 
+void getArmingChannel(modeActivationCondition_t *modeActivationConditions, uint8_t *armingChannel) {
+    for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
+        modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
+        if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
+            *armingChannel = modeActivationCondition->auxChannelIndex +  NON_AUX_CHANNEL_COUNT;
+            break;
+        }
+    }
+}
+
+void filterRc(void){
+    static int16_t lastCommand[4] = { 0, 0, 0, 0 };
+    static int16_t deltaRC[4] = { 0, 0, 0, 0 };
+    static int16_t loop[5] = { 0, 0, 0, 0, 0 };
+    static int16_t factor, rcInterpolationFactor, loopAvg;
+    static uint32_t rxRefreshRate;
+    static int16_t lastAux, deltaAux;                            // last arming AUX position and delta for arming AUX
+	static uint8_t auxChannelToFilter;                           // AUX channel used for arming needs filtering when used
+	static int loopCount;
+
+    // Set RC refresh rate for sampling and channels to filter
+    if (!rxRefreshRate) {
+        if (feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) {
+            rxRefreshRate = 20000;
+
+            // AUX Channels to filter to replace PPM/PWM averaging
+            getArmingChannel(currentProfile->modeActivationConditions,&auxChannelToFilter);
+
+        }
+
+        // TODO Are there more different refresh rates?
+        else {
+    	    switch (masterConfig.rxConfig.serialrx_provider) {
+    	        case SERIALRX_SPEKTRUM1024:
+    	            rxRefreshRate = 22000;
+    	            break;
+    	        case SERIALRX_SPEKTRUM2048:
+    	            rxRefreshRate = 11000;
+    	            break;
+    	        case SERIALRX_SBUS:
+    	            rxRefreshRate = 11000;
+    	            break;
+    	        default:
+    	            rxRefreshRate = 10000;
+    	            break;
+            }
+        }
+
+        rcInterpolationFactor = 1; // Initial Factor before looptime average is calculated
+
+    }
+
+    // Averaging of cycleTime for more precise sampling
+    loop[loopCount] = cycleTime;
+    loopCount++;
+
+    // Start recalculating new rcInterpolationFactor every 5 loop iterations
+    if (loopCount > 4) {
+        uint16_t tmp = (loop[0] + loop[1] + loop[2] + loop[3] + loop[4]) / 5;
+
+        // Jitter tolerance to prevent rcInterpolationFactor jump too much
+        if (tmp > (loopAvg + LOOP_DEADBAND) || tmp < (loopAvg - LOOP_DEADBAND))  {
+            loopAvg = tmp;
+            rcInterpolationFactor = rxRefreshRate / loopAvg + 1;
+        }
+
+        loopCount = 0;
+     }
+
+    if (isRXdataNew) {
+        for (int channel=0; channel < 4; channel++) {
+        	deltaRC[channel] = rcData[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
+            lastCommand[channel] = rcData[channel];
+        }
+
+        // Read AUX channel (arm/disarm guard enhancement)
+        if (auxChannelToFilter) {
+            deltaAux = rcData[auxChannelToFilter] - (lastAux - deltaAux * factor/rcInterpolationFactor);
+            lastAux = rcData[auxChannelToFilter];
+        }
+
+        isRXdataNew = false;
+        factor = rcInterpolationFactor - 1;
+        }
+
+    else {
+        factor--;
+    }
+
+    // Interpolate steps of rcData
+    if (factor > 0) {
+        for (int channel=0; channel < 4; channel++) {
+            rcData[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
+         }
+
+        // Interpolate steps of Aux
+        if (auxChannelToFilter) {
+            rcData[auxChannelToFilter] = lastAux - deltaAux * factor/rcInterpolationFactor;
+        }
+    }
+
+    else {
+        factor = 0;
+    }
+}
+
 void loop(void)
 {
     static uint32_t loopTime;
@@ -692,6 +802,7 @@ void loop(void)
 
     if (shouldProcessRx(currentTime)) {
         processRx();
+        isRXdataNew = true;
 
 #ifdef BARO
         // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
@@ -746,6 +857,8 @@ void loop(void)
             }
         }
 
+        filterRc();
+
         annexCode();
 #if defined(BARO) || defined(SONAR)
         haveProcessedAnnexCodeOnce = true;
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 78016c43b..c252feb77 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -75,7 +75,7 @@ static uint32_t needRxSignalBefore = 0;
 int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 
-#define PPM_AND_PWM_SAMPLE_COUNT 4
+#define PPM_AND_PWM_SAMPLE_COUNT 3
 
 #define DELAY_50_HZ (1000000 / 50)
 #define DELAY_10_HZ (1000000 / 10)

From b69bab33b2d04b933674f5f126b51e3a978542db Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Sun, 16 Aug 2015 23:25:16 +0200
Subject: [PATCH 02/41] Configurable RC Smoothing

---
 src/main/config/config.c | 1 +
 src/main/io/serial_cli.c | 1 +
 src/main/mw.c            | 4 +++-
 src/main/rx/rx.h         | 1 +
 4 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index 6764ad4ab..3f615ecf5 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -414,6 +414,7 @@ static void resetConf(void)
     masterConfig.rxConfig.rssi_channel = 0;
     masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
     masterConfig.rxConfig.rssi_ppm_invert = 0;
+    masterConfig.rxConfig.rcSmoothing = 1;
 
     resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);
 
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index eddbf960a..fbc6ca197 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -316,6 +316,7 @@ const clivalue_t valueTable[] = {
     { "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_ppm_invert",            VAR_INT8   | MASTER_VALUE,  &masterConfig.rxConfig.rssi_ppm_invert, 0, 1 },
+    { "rc_smoothing",               VAR_INT8   | MASTER_VALUE,  &masterConfig.rxConfig.rcSmoothing, 0, 1 },
     { "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 },
diff --git a/src/main/mw.c b/src/main/mw.c
index 708dc006a..5fc1e4159 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -857,7 +857,9 @@ void loop(void)
             }
         }
 
-        filterRc();
+        if (masterConfig.rxConfig.rcSmoothing) {
+            filterRc();
+        }
 
         annexCode();
 #if defined(BARO) || defined(SONAR)
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 7ab45f106..058c3b089 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -106,6 +106,7 @@ typedef struct rxConfig_s {
     uint8_t rssi_channel;
     uint8_t rssi_scale;
     uint8_t rssi_ppm_invert;
+    uint8_t rcSmoothing;                    // Enable/Disable RC filtering
     uint16_t midrc;                         // Some radios have not a neutral point centered on 1500. can be changed here
     uint16_t mincheck;                      // minimum rc end
     uint16_t maxcheck;                      // maximum rc end

From ff23cab2a8e30d4b90acb8de3558b6bbf0775d1c Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Thu, 20 Aug 2015 21:59:07 +0200
Subject: [PATCH 03/41] Remove Aux filter // more accurate cycleTime // Code
 cleanup

Change datatypes
---
 src/main/mw.c    | 97 ++++++------------------------------------------
 src/main/rx/rx.c | 13 +++++++
 src/main/rx/rx.h |  1 +
 3 files changed, 26 insertions(+), 85 deletions(-)

diff --git a/src/main/mw.c b/src/main/mw.c
index 5fc1e4159..a826988ad 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -93,8 +93,6 @@ enum {
 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
 #define IBATINTERVAL (6 * 3500)       
 
-#define LOOP_DEADBAND 400 // Dead band for loop to modify to rcInterpolationFactor in RC Filtering for unstable looptimes
-
 uint32_t currentTime = 0;
 uint32_t previousTime = 0;
 uint16_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
@@ -109,7 +107,7 @@ static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the m
 
 extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
 
-static bool isRXdataNew;
+static bool isRXDataNew;
 
 typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);            // pid controller function prototype
@@ -685,92 +683,28 @@ void processRx(void)
 
 }
 
-void getArmingChannel(modeActivationCondition_t *modeActivationConditions, uint8_t *armingChannel) {
-    for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
-        modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
-        if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
-            *armingChannel = modeActivationCondition->auxChannelIndex +  NON_AUX_CHANNEL_COUNT;
-            break;
-        }
-    }
-}
-
 void filterRc(void){
     static int16_t lastCommand[4] = { 0, 0, 0, 0 };
     static int16_t deltaRC[4] = { 0, 0, 0, 0 };
-    static int16_t loop[5] = { 0, 0, 0, 0, 0 };
-    static int16_t factor, rcInterpolationFactor, loopAvg;
-    static uint32_t rxRefreshRate;
-    static int16_t lastAux, deltaAux;                            // last arming AUX position and delta for arming AUX
-	static uint8_t auxChannelToFilter;                           // AUX channel used for arming needs filtering when used
-	static int loopCount;
+    static int16_t factor, rcInterpolationFactor;
+    static filterStatePt1_t filteredCycleTimeState;
+    uint16_t rxRefreshRate, filteredCycleTime;
 
     // Set RC refresh rate for sampling and channels to filter
-    if (!rxRefreshRate) {
-        if (feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) {
-            rxRefreshRate = 20000;
+   	initRxRefreshRate(&rxRefreshRate);
 
-            // AUX Channels to filter to replace PPM/PWM averaging
-            getArmingChannel(currentProfile->modeActivationConditions,&auxChannelToFilter);
+    filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1);
+    rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
 
-        }
-
-        // TODO Are there more different refresh rates?
-        else {
-    	    switch (masterConfig.rxConfig.serialrx_provider) {
-    	        case SERIALRX_SPEKTRUM1024:
-    	            rxRefreshRate = 22000;
-    	            break;
-    	        case SERIALRX_SPEKTRUM2048:
-    	            rxRefreshRate = 11000;
-    	            break;
-    	        case SERIALRX_SBUS:
-    	            rxRefreshRate = 11000;
-    	            break;
-    	        default:
-    	            rxRefreshRate = 10000;
-    	            break;
-            }
-        }
-
-        rcInterpolationFactor = 1; // Initial Factor before looptime average is calculated
-
-    }
-
-    // Averaging of cycleTime for more precise sampling
-    loop[loopCount] = cycleTime;
-    loopCount++;
-
-    // Start recalculating new rcInterpolationFactor every 5 loop iterations
-    if (loopCount > 4) {
-        uint16_t tmp = (loop[0] + loop[1] + loop[2] + loop[3] + loop[4]) / 5;
-
-        // Jitter tolerance to prevent rcInterpolationFactor jump too much
-        if (tmp > (loopAvg + LOOP_DEADBAND) || tmp < (loopAvg - LOOP_DEADBAND))  {
-            loopAvg = tmp;
-            rcInterpolationFactor = rxRefreshRate / loopAvg + 1;
-        }
-
-        loopCount = 0;
-     }
-
-    if (isRXdataNew) {
+    if (isRXDataNew) {
         for (int channel=0; channel < 4; channel++) {
         	deltaRC[channel] = rcData[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
             lastCommand[channel] = rcData[channel];
         }
 
-        // Read AUX channel (arm/disarm guard enhancement)
-        if (auxChannelToFilter) {
-            deltaAux = rcData[auxChannelToFilter] - (lastAux - deltaAux * factor/rcInterpolationFactor);
-            lastAux = rcData[auxChannelToFilter];
-        }
-
-        isRXdataNew = false;
+        isRXDataNew = false;
         factor = rcInterpolationFactor - 1;
-        }
-
-    else {
+    } else {
         factor--;
     }
 
@@ -779,14 +713,7 @@ void filterRc(void){
         for (int channel=0; channel < 4; channel++) {
             rcData[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
          }
-
-        // Interpolate steps of Aux
-        if (auxChannelToFilter) {
-            rcData[auxChannelToFilter] = lastAux - deltaAux * factor/rcInterpolationFactor;
-        }
-    }
-
-    else {
+    } else {
         factor = 0;
     }
 }
@@ -802,7 +729,7 @@ void loop(void)
 
     if (shouldProcessRx(currentTime)) {
         processRx();
-        isRXdataNew = true;
+        isRXDataNew = true;
 
 #ifdef BARO
         // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index c252feb77..7fbfdf99e 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -91,6 +91,7 @@ static uint16_t nullReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channe
 }
 
 static rcReadRawDataPtr rcReadRawFunc = nullReadRawRC;
+static uint16_t rxRefreshRate;
 
 void serialRxInit(rxConfig_t *rxConfig);
 
@@ -161,6 +162,7 @@ void rxInit(rxConfig_t *rxConfig)
     }
 
     if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
+        rxRefreshRate = 20000;
         rxPwmInit(&rxRuntimeConfig, &rcReadRawFunc);
     }
 
@@ -173,20 +175,28 @@ void serialRxInit(rxConfig_t *rxConfig)
     bool enabled = false;
     switch (rxConfig->serialrx_provider) {
         case SERIALRX_SPEKTRUM1024:
+            rxRefreshRate = 22000;
+            enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
+            break;
         case SERIALRX_SPEKTRUM2048:
+            rxRefreshRate = 11000;
             enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
             break;
         case SERIALRX_SBUS:
+            rxRefreshRate = 11000;
             enabled = sbusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
             break;
         case SERIALRX_SUMD:
+            rxRefreshRate = 11000;
             enabled = sumdInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
             break;
         case SERIALRX_SUMH:
+            rxRefreshRate = 11000;
             enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
             break;
         case SERIALRX_XBUS_MODE_B:
         case SERIALRX_XBUS_MODE_B_RJ01:
+            rxRefreshRate = 11000;
             enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
             break;
     }
@@ -545,4 +555,7 @@ void updateRSSI(uint32_t currentTime)
     }
 }
 
+void initRxRefreshRate(uint16_t *rxRefreshRatePtr) {
+    *rxRefreshRatePtr = rxRefreshRate;
+}
 
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 058c3b089..31a8f5628 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -143,3 +143,4 @@ uint8_t serialRxFrameStatus(rxConfig_t *rxConfig);
 void updateRSSI(uint32_t currentTime);
 void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChannelRangeConfiguration);
 
+void initRxRefreshRate(uint16_t *rxRefreshRatePtr);

From 656cfb07742a9adfe6916478fca10ad1411aa62a Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Thu, 20 Aug 2015 22:24:29 +0200
Subject: [PATCH 04/41] cli doc for rc_smoothing

---
 docs/Cli.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/docs/Cli.md b/docs/Cli.md
index b8b9c9880..e25191add 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -111,6 +111,7 @@ Re-apply any new defaults as desired.
 | `rssi_scale`                    |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 1      | 255    | 30            | Master       | UINT8    |
 | `rssi_ppm_invert`               |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 1      | 0             | Master       | UINT8    |
 | `input_filtering_mode`          |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 1      | 0             | Master       | INT8     |
+| `rc_smoothing        `          | Interpolation of Rc data during looptimes when there are no new updates. This gives smoother RC input to PID controller and cleaner PIDsum                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             | 0      | 1      | 1             | Master       | INT8     |
 | `min_throttle`                  | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     | 0      | 2000   | 1150          | Master       | UINT16   |
 | `max_throttle`                  | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     | 0      | 2000   | 1850          | Master       | UINT16   |
 | `min_command`                   | This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once.                                                                                                                                                                                                                                                                                                                                                                                                                                                                      | 0      | 2000   | 1000          | Master       | UINT16   |

From a5348684083c031e2519f58d08b7f2fd0d2d04b2 Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Thu, 9 Jul 2015 23:56:11 +0200
Subject: [PATCH 05/41] Luxfloat Optimisation and new defaults

Remove constraining of delta

Yaw D default to 10

Default change and rebase with master
---
 src/main/config/config.c | 18 +++++++++---------
 src/main/flight/pid.c    | 15 ++++++++-------
 2 files changed, 17 insertions(+), 16 deletions(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index 6764ad4ab..73cc5807a 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -175,15 +175,15 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->pterm_cut_hz = 0;
     pidProfile->gyro_cut_hz = 0;
 
-    pidProfile->P_f[ROLL] = 2.5f;     // new PID with preliminary defaults test carefully
-    pidProfile->I_f[ROLL] = 0.6f;
-    pidProfile->D_f[ROLL] = 0.06f;
-    pidProfile->P_f[PITCH] = 2.5f;
-    pidProfile->I_f[PITCH] = 0.6f;
-    pidProfile->D_f[PITCH] = 0.06f;
-    pidProfile->P_f[YAW] = 8.0f;
-    pidProfile->I_f[YAW] = 0.5f;
-    pidProfile->D_f[YAW] = 0.05f;
+    pidProfile->P_f[ROLL] = 1.5f;     // new PID with preliminary defaults test carefully
+    pidProfile->I_f[ROLL] = 0.4f;
+    pidProfile->D_f[ROLL] = 0.03f;
+    pidProfile->P_f[PITCH] = 1.5f;
+    pidProfile->I_f[PITCH] = 0.4f;
+    pidProfile->D_f[PITCH] = 0.03f;
+    pidProfile->P_f[YAW] = 2.5f;
+    pidProfile->I_f[YAW] = 1.0f;
+    pidProfile->D_f[YAW] = 0.00f;
     pidProfile->A_level = 5.0f;
     pidProfile->H_level = 3.0f;
     pidProfile->H_sensitivity = 75;
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 21b73a649..899015ce4 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -109,7 +109,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     float RateError, errorAngle, AngleRate, gyroRate;
     float ITerm,PTerm,DTerm;
     int32_t stickPosAil, stickPosEle, mostDeflectedPos;
-    static float lastGyroRate[3];
+    static float lastError[3];
     static float delta1[3], delta2[3];
     float delta, deltaSum;
     float dT;
@@ -192,16 +192,17 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         if (pidProfile->pterm_cut_hz) {
             PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
         }
-        // -----calculate I component. Note that PIDweight is divided by 10, because it is simplified formule from the previous multiply by 10
-        errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * PIDweight[axis] / 10, -250.0f, 250.0f);
+
+        // -----calculate I component.
+        errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
 
         // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
         // I coefficient (I8) moved before integration to make limiting independent from PID settings
         ITerm = errorGyroIf[axis];
 
         //-----calculate D-term
-        delta = gyroRate - lastGyroRate[axis];  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
-        lastGyroRate[axis] = gyroRate;
+        delta = RateError - lastError[axis];
+        lastError[axis] = RateError;
 
         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
         // would be scaled by different dt each time. Division by dT fixes that.
@@ -219,12 +220,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
 
         // -----calculate total PID output
-        axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000);
+        axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
 
 #ifdef BLACKBOX
         axisPID_P[axis] = PTerm;
         axisPID_I[axis] = ITerm;
-        axisPID_D[axis] = -DTerm;
+        axisPID_D[axis] = DTerm;
 #endif
     }
 }

From 210d6bcd119c92afb7a22b9e208ae47b726579b5 Mon Sep 17 00:00:00 2001
From: Echelon9 <rhyskidd@gmail.com>
Date: Mon, 17 Aug 2015 22:37:09 +1000
Subject: [PATCH 06/41] Add new Makefile feature, try 'make help'

---
 Makefile | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index f22bde515..00de825b5 100755
--- a/Makefile
+++ b/Makefile
@@ -666,6 +666,11 @@ $(OBJECT_DIR)/$(TARGET)/%.o: %.S
 	@echo %% $(notdir $<)
 	@$(CC) -c -o $@ $(ASFLAGS) $<
 
+
+## all         : default task; compile C code, build firmware
+all: binary
+
+## clean       : clean up all temporary / machine-generated files
 clean:
 	rm -f $(TARGET_BIN) $(TARGET_HEX) $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP)
 	rm -rf $(OBJECT_DIR)/$(TARGET)
@@ -676,11 +681,13 @@ flash_$(TARGET): $(TARGET_HEX)
 	echo -n 'R' >$(SERIAL_DEVICE)
 	stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE)
 
+## flash       : flash firmware (.hex) onto flight controller
 flash: flash_$(TARGET)
 
 st-flash_$(TARGET): $(TARGET_BIN)
 	st-flash --reset write $< 0x08000000
 
+## st-flash    : flash firmware (.bin) onto flight controller
 st-flash: st-flash_$(TARGET)
 
 binary: $(TARGET_BIN)
@@ -689,6 +696,7 @@ unbrick_$(TARGET): $(TARGET_HEX)
 	stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon
 	stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE)
 
+## unbrick     : unbrick flight controller
 unbrick: unbrick_$(TARGET)
 
 ## cppcheck    : run static analysis on C source code
@@ -698,7 +706,8 @@ cppcheck: $(CSOURCES)
 cppcheck-result.xml: $(CSOURCES)
 	$(CPPCHECK) --xml-version=2 2> cppcheck-result.xml
 
-help:
+## help        : print this help message and exit
+help: Makefile
 	@echo ""
 	@echo "Makefile for the $(FORKNAME) firmware"
 	@echo ""
@@ -707,6 +716,7 @@ help:
 	@echo ""
 	@echo "Valid TARGET values are: $(VALID_TARGETS)"
 	@echo ""
+	@sed -n 's/^## //p' $<
 
 ## test        : run the cleanflight test suite
 test:

From 8fb2cf66492b326bce445c817da7987be96c5d3b Mon Sep 17 00:00:00 2001
From: Richard Marko <rissko@gmail.com>
Date: Fri, 2 Oct 2015 00:46:07 +0200
Subject: [PATCH 07/41] Fix broken LedStrip.md link in docs

---
 docs/Getting Started.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Getting Started.md b/docs/Getting Started.md
index 0e52141ad..b7d4a452f 100644
--- a/docs/Getting Started.md	
+++ b/docs/Getting Started.md	
@@ -105,5 +105,5 @@ Some advanced configurations and features are documented in the following pages,
 * [Spektrum Bind](Spektrum bind.md)
 * [Telemetry](Telemetry.md)
 * [Using a Display](Display.md)
-* [Using a LED strip](Ledstrip.md)
+* [Using a LED strip](LedStrip.md)
 * [Migrating from baseflight](Migrating from baseflight.md)

From b46d56a5bd691ee435bae53a1d78123c5fe6923e Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Fri, 18 Sep 2015 20:23:50 +0100
Subject: [PATCH 08/41] Relocate some of the common MPU code from MPU6050 into
 accgyro_mpu.c.

---
 Makefile                           |   7 +
 src/main/drivers/accgyro.h         |   2 +-
 src/main/drivers/accgyro_mpu.c     | 279 ++++++++++++++++++++++++++++
 src/main/drivers/accgyro_mpu.h     |  75 ++++++++
 src/main/drivers/accgyro_mpu6050.c | 285 ++---------------------------
 src/main/drivers/accgyro_mpu6050.h |  22 +--
 src/main/drivers/exti.h            |  35 ++++
 src/main/sensors/initialisation.c  |  36 ++--
 8 files changed, 441 insertions(+), 300 deletions(-)
 create mode 100644 src/main/drivers/accgyro_mpu.c
 create mode 100644 src/main/drivers/accgyro_mpu.h
 create mode 100644 src/main/drivers/exti.h

diff --git a/Makefile b/Makefile
index 00de825b5..40ec0b1c1 100755
--- a/Makefile
+++ b/Makefile
@@ -292,6 +292,7 @@ NAZE_SRC	 = startup_stm32f10x_md_gcc.S \
 		   drivers/accgyro_bma280.c \
 		   drivers/accgyro_l3g4200d.c \
 		   drivers/accgyro_mma845x.c \
+		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_mpu3050.c \
 		   drivers/accgyro_mpu6050.c \
 		   drivers/accgyro_spi_mpu6500.c \
@@ -332,6 +333,7 @@ EUSTM32F103RC_SRC	 = startup_stm32f10x_hd_gcc.S \
 		   drivers/accgyro_bma280.c \
 		   drivers/accgyro_l3g4200d.c \
 		   drivers/accgyro_mma845x.c \
+		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_mpu3050.c \
 		   drivers/accgyro_mpu6050.c \
 		   drivers/accgyro_spi_mpu6000.c \
@@ -369,6 +371,7 @@ EUSTM32F103RC_SRC	 = startup_stm32f10x_hd_gcc.S \
 PORT103R_SRC = $(EUSTM32F103RC_SRC)
 
 OLIMEXINO_SRC	 = startup_stm32f10x_md_gcc.S \
+		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_mpu6050.c \
 		   drivers/adc.c \
 		   drivers/adc_stm32f10x.c \
@@ -408,6 +411,7 @@ CJMCU_SRC	 = \
 		   startup_stm32f10x_md_gcc.S \
 		   drivers/adc.c \
 		   drivers/adc_stm32f10x.c \
+		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_mpu6050.c \
 		   drivers/bus_i2c_stm32f10x.c \
 		   drivers/compass_hmc5883l.c \
@@ -499,6 +503,7 @@ STM32F3DISCOVERY_SRC	 = \
 		   drivers/accgyro_adxl345.c \
 		   drivers/accgyro_bma280.c \
 		   drivers/accgyro_mma845x.c \
+		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_mpu3050.c \
 		   drivers/accgyro_mpu6050.c \
 		   drivers/accgyro_l3g4200d.c \
@@ -527,6 +532,7 @@ COLIBRI_RACE_SRC        = \
 SPARKY_SRC	 = \
 		   $(STM32F30x_COMMON_SRC) \
 		   drivers/display_ug2864hsweg01.c \
+		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_mpu6050.c \
 		   drivers/barometer_ms5611.c \
 		   drivers/compass_ak8975.c \
@@ -539,6 +545,7 @@ ALIENWIIF3_SRC	 = $(SPARKY_SRC)
 
 SPRACINGF3_SRC	 = \
 		   $(STM32F30x_COMMON_SRC) \
+		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_mpu6050.c \
 		   drivers/barometer_ms5611.c \
 		   drivers/compass_ak8975.c \
diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h
index 90ddf510f..8ebbe3414 100644
--- a/src/main/drivers/accgyro.h
+++ b/src/main/drivers/accgyro.h
@@ -17,7 +17,7 @@
 
 #pragma once
 
-extern uint16_t acc_1G;
+extern uint16_t acc_1G; // FIXME move into acc_t
 
 typedef struct gyro_s {
     sensorInitFuncPtr init;                                 // initialize function
diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
new file mode 100644
index 000000000..064ba5d54
--- /dev/null
+++ b/src/main/drivers/accgyro_mpu.c
@@ -0,0 +1,279 @@
+/*
+ * 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/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "platform.h"
+#include "build_config.h"
+#include "debug.h"
+
+#include "common/maths.h"
+
+#include "nvic.h"
+
+#include "system.h"
+#include "gpio.h"
+#include "exti.h"
+#include "bus_i2c.h"
+
+#include "sensor.h"
+#include "accgyro.h"
+#include "accgyro_mpu6050.h"
+#include "accgyro_mpu.h"
+
+//#define DEBUG_MPU_DATA_READY_INTERRUPT
+
+// MPU6050, Standard address 0x68
+// MPU_INT on PB13 on rev4 Naze32 hardware
+#define MPU6050_ADDRESS         0x68
+
+
+uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
+
+void mpu6050AccInit(void);
+bool mpu6050AccRead(int16_t *accData);
+void mpu6050GyroInit(void);
+bool mpu6050GyroRead(int16_t *gyroADC);
+
+mpuDetectionResult_t mpuDetectionResult;
+
+static const extiConfig_t *mpuIntExtiConfig = NULL;
+
+// MPU6050
+#define MPU_RA_WHO_AM_I         0x75
+#define MPU_RA_XA_OFFS_H        0x06    //[15:0] XA_OFFS
+#define MPU_RA_PRODUCT_ID       0x0C    // Product ID Register
+#define MPU_RA_ACCEL_XOUT_H     0x3B
+#define MPU_RA_GYRO_XOUT_H      0x43
+
+mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
+{
+    memset(&mpuDetectionResult, 0, sizeof(mpuDetectionResult));
+
+    mpuIntExtiConfig = configToUse;
+
+    bool ack;
+    uint8_t sig;
+    uint8_t readBuffer[6];
+    uint8_t revision;
+    uint8_t productId;
+
+    delay(35);          // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe
+
+    ack = i2cRead(MPU6050_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
+    if (!ack)
+        return &mpuDetectionResult;
+
+    // So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
+    // The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0�s 7-bit I2C address.
+    // The least significant bit of the MPU-60X0�s I2C address is determined by the value of the AD0 pin. (we know that already).
+    // But here's the best part: The value of the AD0 pin is not reflected in this register.
+
+    if (sig != (MPU6050_ADDRESS & 0x7e))
+
+        return &mpuDetectionResult;
+
+
+    // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
+    // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
+
+    // determine product ID and accel revision
+    i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, readBuffer);
+    revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
+    if (revision) {
+        /* Congrats, these parts are better. */
+        if (revision == 1) {
+            mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
+        } else if (revision == 2) {
+            mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
+        } else {
+            failureMode(FAILURE_ACC_INCOMPATIBLE);
+        }
+    } else {
+        i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId);
+        revision = productId & 0x0F;
+        if (!revision) {
+            failureMode(FAILURE_ACC_INCOMPATIBLE);
+        } else if (revision == 4) {
+            mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
+        } else {
+            mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
+        }
+    }
+
+    mpuDetectionResult.sensor = MPU_60x0;
+    return &mpuDetectionResult;
+}
+
+void MPU_DATA_READY_EXTI_Handler(void)
+{
+    if (EXTI_GetITStatus(mpuIntExtiConfig->exti_line) == RESET) {
+        return;
+    }
+
+    EXTI_ClearITPendingBit(mpuIntExtiConfig->exti_line);
+
+#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
+    // Measure the delta in micro seconds between calls to the interrupt handler
+    static uint32_t lastCalledAt = 0;
+    static int32_t callDelta = 0;
+
+    uint32_t now = micros();
+    callDelta = now - lastCalledAt;
+
+    //UNUSED(callDelta);
+    debug[0] = callDelta;
+
+    lastCalledAt = now;
+#endif
+}
+
+void configureMPUDataReadyInterruptHandling(void)
+{
+#ifdef USE_MPU_DATA_READY_SIGNAL
+
+#ifdef STM32F10X
+    // enable AFIO for EXTI support
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
+#endif
+
+#ifdef STM32F303xC
+    /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
+#endif
+
+#ifdef STM32F10X
+    gpioExtiLineConfig(mpuIntExtiConfig->exti_port_source, mpuIntExtiConfig->exti_pin_source);
+#endif
+
+#ifdef STM32F303xC
+    gpioExtiLineConfig(mpuIntExtiConfig->exti_port_source, mpuIntExtiConfig->exti_pin_source);
+#endif
+
+#ifdef ENSURE_MPU_DATA_READY_IS_LOW
+    uint8_t status = GPIO_ReadInputDataBit(mpuIntExtiConfig->gpioPort, mpuIntExtiConfig->gpioPin);
+    if (status) {
+        return;
+    }
+#endif
+
+    registerExti15_10_CallbackHandler(MPU_DATA_READY_EXTI_Handler);
+
+    EXTI_ClearITPendingBit(mpuIntExtiConfig->exti_line);
+
+    EXTI_InitTypeDef EXTIInit;
+    EXTIInit.EXTI_Line = mpuIntExtiConfig->exti_line;
+    EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
+    EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising;
+    EXTIInit.EXTI_LineCmd = ENABLE;
+    EXTI_Init(&EXTIInit);
+
+    NVIC_InitTypeDef NVIC_InitStructure;
+
+    NVIC_InitStructure.NVIC_IRQChannel = mpuIntExtiConfig->exti_irqn;
+    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MPU_DATA_READY);
+    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MPU_DATA_READY);
+    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+    NVIC_Init(&NVIC_InitStructure);
+#endif
+}
+
+void mpuIntExtiInit(void)
+{
+    gpio_config_t gpio;
+
+    static bool mpuExtiInitDone = false;
+
+    if (mpuExtiInitDone || !mpuIntExtiConfig) {
+        return;
+    }
+
+#ifdef STM32F303
+        if (mpuIntExtiConfig->gpioAHBPeripherals) {
+            RCC_AHBPeriphClockCmd(mpuIntExtiConfig->gpioAHBPeripherals, ENABLE);
+        }
+#endif
+#ifdef STM32F10X
+        if (mpuIntExtiConfig->gpioAPB2Peripherals) {
+            RCC_APB2PeriphClockCmd(mpuIntExtiConfig->gpioAPB2Peripherals, ENABLE);
+        }
+#endif
+
+    gpio.pin = mpuIntExtiConfig->gpioPin;
+    gpio.speed = Speed_2MHz;
+    gpio.mode = Mode_IN_FLOATING;
+    gpioInit(mpuIntExtiConfig->gpioPort, &gpio);
+
+    configureMPUDataReadyInterruptHandling();
+
+    mpuExtiInitDone = true;
+}
+
+void configureMPULPF(uint16_t lpf)
+{
+    if (lpf == 256)
+        mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
+    else if (lpf >= 188)
+        mpuLowPassFilter = INV_FILTER_188HZ;
+    else if (lpf >= 98)
+        mpuLowPassFilter = INV_FILTER_98HZ;
+    else if (lpf >= 42)
+        mpuLowPassFilter = INV_FILTER_42HZ;
+    else if (lpf >= 20)
+        mpuLowPassFilter = INV_FILTER_20HZ;
+    else if (lpf >= 10)
+        mpuLowPassFilter = INV_FILTER_10HZ;
+    else if (lpf > 0)
+        mpuLowPassFilter = INV_FILTER_5HZ;
+    else
+        mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
+}
+
+bool mpuAccRead(int16_t *accData)
+{
+    uint8_t buf[6];
+
+    bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
+    if (!ack) {
+        return false;
+    }
+
+    accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
+    accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
+    accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
+
+    return true;
+}
+
+bool mpuGyroRead(int16_t *gyroADC)
+{
+    uint8_t buf[6];
+
+    bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
+    if (!ack) {
+        return false;
+    }
+
+    gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
+    gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
+    gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
+
+    return true;
+}
diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h
new file mode 100644
index 000000000..b36c78eee
--- /dev/null
+++ b/src/main/drivers/accgyro_mpu.h
@@ -0,0 +1,75 @@
+/*
+ * 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
+
+
+enum lpf_e {
+    INV_FILTER_256HZ_NOLPF2 = 0,
+    INV_FILTER_188HZ,
+    INV_FILTER_98HZ,
+    INV_FILTER_42HZ,
+    INV_FILTER_20HZ,
+    INV_FILTER_10HZ,
+    INV_FILTER_5HZ,
+    INV_FILTER_2100HZ_NOLPF,
+    NUM_FILTER
+};
+enum gyro_fsr_e {
+    INV_FSR_250DPS = 0,
+    INV_FSR_500DPS,
+    INV_FSR_1000DPS,
+    INV_FSR_2000DPS,
+    NUM_GYRO_FSR
+};
+enum clock_sel_e {
+    INV_CLK_INTERNAL = 0,
+    INV_CLK_PLL,
+    NUM_CLK
+};
+enum accel_fsr_e {
+    INV_FSR_2G = 0,
+    INV_FSR_4G,
+    INV_FSR_8G,
+    INV_FSR_16G,
+    NUM_ACCEL_FSR
+};
+
+typedef enum {
+    MPU_NONE,
+    MPU_3050,
+    MPU_60x0,
+    MPU_65xx_I2C,
+    MPU_65xx_SPI
+} detectedMPUSensor_e;
+
+typedef enum {
+    MPU_HALF_RESOLUTION,
+    MPU_FULL_RESOLUTION
+} mpu6050Resolution_e;
+
+typedef struct mpuDetectionResult_s {
+    detectedMPUSensor_e sensor;
+    mpu6050Resolution_e resolution;
+} mpuDetectionResult_t;
+
+void configureMPULPF(uint16_t lpf);
+void configureMPUDataReadyInterruptHandling(void);
+void mpuIntExtiInit(void);
+bool mpuAccRead(int16_t *accData);
+bool mpuGyroRead(int16_t *gyroADC);
+mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c
index 15d2c28fe..b790e6bc9 100644
--- a/src/main/drivers/accgyro_mpu6050.c
+++ b/src/main/drivers/accgyro_mpu6050.c
@@ -29,12 +29,17 @@
 
 #include "system.h"
 #include "gpio.h"
+#include "exti.h"
 #include "bus_i2c.h"
 
 #include "sensor.h"
 #include "accgyro.h"
+#include "accgyro_mpu.h"
 #include "accgyro_mpu6050.h"
 
+extern mpuDetectionResult_t mpuDetectionResult;
+extern uint8_t mpuLowPassFilter;
+
 //#define DEBUG_MPU_DATA_READY_INTERRUPT
 
 // MPU6050, Standard address 0x68
@@ -140,291 +145,55 @@
 
 #define MPU6050_SMPLRT_DIV      0       // 8000Hz
 
-enum lpf_e {
-    INV_FILTER_256HZ_NOLPF2 = 0,
-    INV_FILTER_188HZ,
-    INV_FILTER_98HZ,
-    INV_FILTER_42HZ,
-    INV_FILTER_20HZ,
-    INV_FILTER_10HZ,
-    INV_FILTER_5HZ,
-    INV_FILTER_2100HZ_NOLPF,
-    NUM_FILTER
-};
-enum gyro_fsr_e {
-    INV_FSR_250DPS = 0,
-    INV_FSR_500DPS,
-    INV_FSR_1000DPS,
-    INV_FSR_2000DPS,
-    NUM_GYRO_FSR
-};
-enum clock_sel_e {
-    INV_CLK_INTERNAL = 0,
-    INV_CLK_PLL,
-    NUM_CLK
-};
-enum accel_fsr_e {
-    INV_FSR_2G = 0,
-    INV_FSR_4G,
-    INV_FSR_8G,
-    INV_FSR_16G,
-    NUM_ACCEL_FSR
-};
-
-static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
 static void mpu6050AccInit(void);
-static bool mpu6050AccRead(int16_t *accData);
 static void mpu6050GyroInit(void);
-static bool mpu6050GyroRead(int16_t *gyroADC);
 
-typedef enum {
-    MPU_6050_HALF_RESOLUTION,
-    MPU_6050_FULL_RESOLUTION
-} mpu6050Resolution_e;
-
-static mpu6050Resolution_e mpuAccelTrim;
-
-static const mpu6050Config_t *mpu6050Config = NULL;
-
-void MPU_DATA_READY_EXTI_Handler(void)
+bool mpu6050AccDetect(acc_t *acc)
 {
-    if (EXTI_GetITStatus(mpu6050Config->exti_line) == RESET) {
-        return;
-    }
-
-    EXTI_ClearITPendingBit(mpu6050Config->exti_line);
-
-#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
-    // Measure the delta in micro seconds between calls to the interrupt handler
-    static uint32_t lastCalledAt = 0;
-    static int32_t callDelta = 0;
-
-    uint32_t now = micros();
-    callDelta = now - lastCalledAt;
-
-    //UNUSED(callDelta);
-    debug[0] = callDelta;
-
-    lastCalledAt = now;
-#endif
-}
-
-void configureMPUDataReadyInterruptHandling(void)
-{
-#ifdef USE_MPU_DATA_READY_SIGNAL
-
-#ifdef STM32F10X
-    // enable AFIO for EXTI support
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
-#endif
-
-#ifdef STM32F303xC
-    /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
-#endif
-
-#ifdef STM32F10X
-    gpioExtiLineConfig(mpu6050Config->exti_port_source, mpu6050Config->exti_pin_source);
-#endif
-
-#ifdef STM32F303xC
-    gpioExtiLineConfig(mpu6050Config->exti_port_source, mpu6050Config->exti_pin_source);
-#endif
-
-#ifdef ENSURE_MPU_DATA_READY_IS_LOW
-    uint8_t status = GPIO_ReadInputDataBit(mpu6050Config->gpioPort, mpu6050Config->gpioPin);
-    if (status) {
-        return;
-    }
-#endif
-
-    registerExti15_10_CallbackHandler(MPU_DATA_READY_EXTI_Handler);
-
-    EXTI_ClearITPendingBit(mpu6050Config->exti_line);
-
-    EXTI_InitTypeDef EXTIInit;
-    EXTIInit.EXTI_Line = mpu6050Config->exti_line;
-    EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
-    EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising;
-    EXTIInit.EXTI_LineCmd = ENABLE;
-    EXTI_Init(&EXTIInit);
-
-    NVIC_InitTypeDef NVIC_InitStructure;
-
-    NVIC_InitStructure.NVIC_IRQChannel = mpu6050Config->exti_irqn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MPU_DATA_READY);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MPU_DATA_READY);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
-#endif
-}
-
-void mpu6050GpioInit(void) {
-    gpio_config_t gpio;
-
-    static bool mpu6050GpioInitDone = false;
-
-    if (mpu6050GpioInitDone || !mpu6050Config) {
-        return;
-    }
-
-#ifdef STM32F303
-        if (mpu6050Config->gpioAHBPeripherals) {
-            RCC_AHBPeriphClockCmd(mpu6050Config->gpioAHBPeripherals, ENABLE);
-        }
-#endif
-#ifdef STM32F10X
-        if (mpu6050Config->gpioAPB2Peripherals) {
-            RCC_APB2PeriphClockCmd(mpu6050Config->gpioAPB2Peripherals, ENABLE);
-        }
-#endif
-
-    gpio.pin = mpu6050Config->gpioPin;
-    gpio.speed = Speed_2MHz;
-    gpio.mode = Mode_IN_FLOATING;
-    gpioInit(mpu6050Config->gpioPort, &gpio);
-
-    configureMPUDataReadyInterruptHandling();
-
-    mpu6050GpioInitDone = true;
-}
-
-static bool mpu6050Detect(void)
-{
-    bool ack;
-    uint8_t sig;
-
-    delay(35);          // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe
-
-    ack = i2cRead(MPU6050_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
-    if (!ack)
+    if (mpuDetectionResult.sensor != MPU_60x0) {
         return false;
-
-    // So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
-    // The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0�s 7-bit I2C address.
-    // The least significant bit of the MPU-60X0�s I2C address is determined by the value of the AD0 pin. (we know that already).
-    // But here's the best part: The value of the AD0 pin is not reflected in this register.
-
-    if (sig != (MPU6050_ADDRESS & 0x7e))
-        return false;
-
-    return true;
-}
-
-bool mpu6050AccDetect(const mpu6050Config_t *configToUse, acc_t *acc)
-{
-    uint8_t readBuffer[6];
-    uint8_t revision;
-    uint8_t productId;
-
-    mpu6050Config = configToUse;
-
-    if (!mpu6050Detect()) {
-        return false;
-    }
-
-    // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
-    // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
-
-    // determine product ID and accel revision
-    i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, readBuffer);
-    revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
-    if (revision) {
-        /* Congrats, these parts are better. */
-        if (revision == 1) {
-            mpuAccelTrim = MPU_6050_HALF_RESOLUTION;
-        } else if (revision == 2) {
-            mpuAccelTrim = MPU_6050_FULL_RESOLUTION;
-        } else {
-            failureMode(FAILURE_ACC_INCOMPATIBLE);
-        }
-    } else {
-        i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId);
-        revision = productId & 0x0F;
-        if (!revision) {
-            failureMode(FAILURE_ACC_INCOMPATIBLE);
-        } else if (revision == 4) {
-            mpuAccelTrim = MPU_6050_HALF_RESOLUTION;
-        } else {
-            mpuAccelTrim = MPU_6050_FULL_RESOLUTION;
-        }
     }
 
     acc->init = mpu6050AccInit;
-    acc->read = mpu6050AccRead;
-    acc->revisionCode = (mpuAccelTrim == MPU_6050_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES.
+    acc->read = mpuAccRead;
+    acc->revisionCode = (mpuDetectionResult.resolution == MPU_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES.
 
     return true;
 }
 
-bool mpu6050GyroDetect(const mpu6050Config_t *configToUse, gyro_t *gyro, uint16_t lpf)
+bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf)
 {
-    mpu6050Config = configToUse;
-
-    if (!mpu6050Detect()) {
-        return false;
+    if (mpuDetectionResult.sensor != MPU_60x0) {
+        return false;;
     }
-
-
     gyro->init = mpu6050GyroInit;
-    gyro->read = mpu6050GyroRead;
+    gyro->read = mpuGyroRead;
 
     // 16.4 dps/lsb scalefactor
     gyro->scale = 1.0f / 16.4f;
 
-    if (lpf == 256)
-        mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
-    else if (lpf >= 188)
-        mpuLowPassFilter = INV_FILTER_188HZ;
-    else if (lpf >= 98)
-        mpuLowPassFilter = INV_FILTER_98HZ;
-    else if (lpf >= 42)
-        mpuLowPassFilter = INV_FILTER_42HZ;
-    else if (lpf >= 20)
-        mpuLowPassFilter = INV_FILTER_20HZ;
-    else if (lpf >= 10)
-        mpuLowPassFilter = INV_FILTER_10HZ;
-    else if (lpf > 0)
-        mpuLowPassFilter = INV_FILTER_5HZ;
-    else
-        mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
+    configureMPULPF(lpf);
 
     return true;
 }
 
 static void mpu6050AccInit(void)
 {
-    mpu6050GpioInit();
+    mpuIntExtiInit();
 
-    switch (mpuAccelTrim) {
-        case MPU_6050_HALF_RESOLUTION:
+    switch (mpuDetectionResult.resolution) {
+        case MPU_HALF_RESOLUTION:
             acc_1G = 256 * 8;
             break;
-        case MPU_6050_FULL_RESOLUTION:
+        case MPU_FULL_RESOLUTION:
             acc_1G = 512 * 8;
             break;
     }
 }
 
-static bool mpu6050AccRead(int16_t *accData)
-{
-    uint8_t buf[6];
-
-    bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
-    if (!ack) {
-        return false;
-    }
-
-    accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
-    accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
-    accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
-
-    return true;
-}
-
 static void mpu6050GyroInit(void)
 {
-    mpu6050GpioInit();
+    mpuIntExtiInit();
 
     bool ack;
     ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
@@ -447,19 +216,3 @@ static void mpu6050GyroInit(void)
 #endif
     UNUSED(ack);
 }
-
-static bool mpu6050GyroRead(int16_t *gyroADC)
-{
-    uint8_t buf[6];
-
-    bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
-    if (!ack) {
-        return false;
-    }
-
-    gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
-    gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
-    gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
-
-    return true;
-}
diff --git a/src/main/drivers/accgyro_mpu6050.h b/src/main/drivers/accgyro_mpu6050.h
index da25684cb..83c1caab8 100644
--- a/src/main/drivers/accgyro_mpu6050.h
+++ b/src/main/drivers/accgyro_mpu6050.h
@@ -17,23 +17,5 @@
 
 #pragma once
 
-typedef struct mpu6050Config_s {
-#ifdef STM32F303
-    uint32_t gpioAHBPeripherals;
-#endif
-#ifdef STM32F10X
-    uint32_t gpioAPB2Peripherals;
-#endif
-    uint16_t gpioPin;
-    GPIO_TypeDef *gpioPort;
-
-    uint8_t exti_port_source;
-    uint32_t exti_line;
-    uint8_t exti_pin_source;
-    IRQn_Type exti_irqn;
-} mpu6050Config_t;
-
-bool mpu6050AccDetect(const mpu6050Config_t *config,acc_t *acc);
-bool mpu6050GyroDetect(const mpu6050Config_t *config, gyro_t *gyro, uint16_t lpf);
-void mpu6050DmpLoop(void);
-void mpu6050DmpResetFifo(void);
+bool mpu6050AccDetect(acc_t *acc);
+bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf);
diff --git a/src/main/drivers/exti.h b/src/main/drivers/exti.h
new file mode 100644
index 000000000..4e48d9052
--- /dev/null
+++ b/src/main/drivers/exti.h
@@ -0,0 +1,35 @@
+/*
+ * 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
+
+typedef struct extiConfig_s {
+#ifdef STM32F303
+    uint32_t gpioAHBPeripherals;
+#endif
+#ifdef STM32F10X
+    uint32_t gpioAPB2Peripherals;
+#endif
+    uint16_t gpioPin;
+    GPIO_TypeDef *gpioPort;
+
+    uint8_t exti_port_source;
+    uint32_t exti_line;
+    uint8_t exti_pin_source;
+    IRQn_Type exti_irqn;
+} extiConfig_t;
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 333e39c0a..0c3487ad7 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -24,6 +24,10 @@
 
 #include "common/axis.h"
 
+#include "drivers/gpio.h"
+#include "drivers/system.h"
+#include "drivers/exti.h"
+
 #include "drivers/sensor.h"
 
 #include "drivers/accgyro.h"
@@ -31,6 +35,7 @@
 #include "drivers/accgyro_bma280.h"
 #include "drivers/accgyro_l3g4200d.h"
 #include "drivers/accgyro_mma845x.h"
+#include "drivers/accgyro_mpu.h"
 #include "drivers/accgyro_mpu3050.h"
 #include "drivers/accgyro_mpu6050.h"
 #include "drivers/accgyro_l3gd20.h"
@@ -50,9 +55,6 @@
 
 #include "drivers/sonar_hcsr04.h"
 
-#include "drivers/gpio.h"
-#include "drivers/system.h"
-
 #include "config/runtime_config.h"
 
 #include "sensors/sensors.h"
@@ -76,11 +78,11 @@ extern acc_t acc;
 uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
 
 
-const mpu6050Config_t *selectMPU6050Config(void)
+const extiConfig_t *selectMPUIntExtiConfig(void)
 {
 #ifdef NAZE
     // MPU_INT output on rev4 PB13
-    static const mpu6050Config_t nazeRev4MPU6050Config = {
+    static const extiConfig_t nazeRev4MPUIntExtiConfig = {
             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
             .gpioPin = Pin_13,
             .gpioPort = GPIOB,
@@ -90,7 +92,7 @@ const mpu6050Config_t *selectMPU6050Config(void)
             .exti_irqn = EXTI15_10_IRQn
     };
     // MPU_INT output on rev5 hardware PC13
-    static const mpu6050Config_t nazeRev5MPU6050Config = {
+    static const extiConfig_t nazeRev5MPUIntExtiConfig = {
             .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
             .gpioPin = Pin_13,
             .gpioPort = GPIOC,
@@ -101,14 +103,14 @@ const mpu6050Config_t *selectMPU6050Config(void)
     };
 
     if (hardwareRevision < NAZE32_REV5) {
-        return &nazeRev4MPU6050Config;
+        return &nazeRev4MPUIntExtiConfig;
     } else {
-        return &nazeRev5MPU6050Config;
+        return &nazeRev5MPUIntExtiConfig;
     }
 #endif
 
 #ifdef SPRACINGF3
-    static const mpu6050Config_t spRacingF3MPU6050Config = {
+    static const extiConfig_t spRacingF3MPUIntExtiConfig = {
             .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
             .gpioPort = GPIOC,
             .gpioPin = Pin_13,
@@ -117,7 +119,7 @@ const mpu6050Config_t *selectMPU6050Config(void)
             .exti_line = EXTI_Line13,
             .exti_irqn = EXTI15_10_IRQn
     };
-    return &spRacingF3MPU6050Config;
+    return &spRacingF3MPUIntExtiConfig;
 #endif
 
     return NULL;
@@ -171,7 +173,7 @@ bool detectGyro(uint16_t gyroLpf)
             ; // fallthrough
         case GYRO_MPU6050:
 #ifdef USE_GYRO_MPU6050
-            if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
+            if (mpu6050GyroDetect(&gyro, gyroLpf)) {
 #ifdef GYRO_MPU6050_ALIGN
                 gyroHardware = GYRO_MPU6050;
                 gyroAlign = GYRO_MPU6050_ALIGN;
@@ -279,7 +281,7 @@ static void detectAcc(accelerationSensor_e accHardwareToUse)
 {
     accelerationSensor_e accHardware;
 
-    #ifdef USE_ACC_ADXL345
+#ifdef USE_ACC_ADXL345
     drv_adxl345_config_t acc_params;
 #endif
 
@@ -319,7 +321,7 @@ retry:
             ; // fallthrough
         case ACC_MPU6050: // MPU6050
 #ifdef USE_ACC_MPU6050
-            if (mpu6050AccDetect(selectMPU6050Config(), &acc)) {
+            if (mpu6050AccDetect(&acc)) {
 #ifdef ACC_MPU6050_ALIGN
                 accAlign = ACC_MPU6050_ALIGN;
 #endif
@@ -597,6 +599,14 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
     memset(&acc, 0, sizeof(acc));
     memset(&gyro, 0, sizeof(gyro));
 
+#if defined(USE_GYRO_MPU6050) || defined(USE_ACC_MPU6050)
+
+    const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
+
+    mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig);
+    UNUSED(mpuDetectionResult);
+#endif
+
     if (!detectGyro(gyroLpf)) {
         return false;
     }

From da46d9f1d215101ae89640653e4381d821128097 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Sat, 19 Sep 2015 14:17:19 +0100
Subject: [PATCH 09/41] Relocate and use some of the common MPU code from
 MPU3050 into accgyro_mpu.c.

---
 src/main/drivers/accgyro_mpu.c     | 78 +++++++++++++++++++++---------
 src/main/drivers/accgyro_mpu3050.c | 73 +++++-----------------------
 src/main/drivers/accgyro_mpu3050.h | 10 ++++
 src/main/sensors/initialisation.c  |  2 +-
 4 files changed, 76 insertions(+), 87 deletions(-)

diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
index 064ba5d54..9708ece29 100644
--- a/src/main/drivers/accgyro_mpu.c
+++ b/src/main/drivers/accgyro_mpu.c
@@ -35,67 +35,100 @@
 
 #include "sensor.h"
 #include "accgyro.h"
+#include "accgyro_mpu3050.h"
 #include "accgyro_mpu6050.h"
 #include "accgyro_mpu.h"
 
 //#define DEBUG_MPU_DATA_READY_INTERRUPT
 
-// MPU6050, Standard address 0x68
-// MPU_INT on PB13 on rev4 Naze32 hardware
-#define MPU6050_ADDRESS         0x68
 
 
 uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
 
-void mpu6050AccInit(void);
-bool mpu6050AccRead(int16_t *accData);
-void mpu6050GyroInit(void);
-bool mpu6050GyroRead(int16_t *gyroADC);
+void mpu6050FindRevision(void);
 
 mpuDetectionResult_t mpuDetectionResult;
 
+typedef struct mpuConfiguration_s {
+    uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
+} mpuConfiguration_t;
+
+static mpuConfiguration_t mpuConfiguration;
 static const extiConfig_t *mpuIntExtiConfig = NULL;
 
+
+#define MPU_ADDRESS             0x68
+
 // MPU6050
 #define MPU_RA_WHO_AM_I         0x75
+#define MPU_RA_WHO_AM_I_LEGACY  0x00
 #define MPU_RA_XA_OFFS_H        0x06    //[15:0] XA_OFFS
 #define MPU_RA_PRODUCT_ID       0x0C    // Product ID Register
 #define MPU_RA_ACCEL_XOUT_H     0x3B
 #define MPU_RA_GYRO_XOUT_H      0x43
 
+// WHO_AM_I register contents for MPU3050, 6050 and 6500
+#define MPU6500_WHO_AM_I_CONST              (0x70)
+#define MPUx0x0_WHO_AM_I_CONST              (0x68)
+
+#define MPU_INQUIRY_MASK   0x7E
+
 mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
 {
     memset(&mpuDetectionResult, 0, sizeof(mpuDetectionResult));
+    memset(&mpuConfiguration, 0, sizeof(mpuConfiguration));
 
     mpuIntExtiConfig = configToUse;
 
     bool ack;
     uint8_t sig;
-    uint8_t readBuffer[6];
-    uint8_t revision;
-    uint8_t productId;
+    uint8_t inquiryResult;
 
-    delay(35);          // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe
+    // MPU datasheet specifies 30ms.
+    delay(35);
 
-    ack = i2cRead(MPU6050_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
+    ack = i2cRead(MPU_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
     if (!ack)
         return &mpuDetectionResult;
 
-    // So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
-    // The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0�s 7-bit I2C address.
-    // The least significant bit of the MPU-60X0�s I2C address is determined by the value of the AD0 pin. (we know that already).
-    // But here's the best part: The value of the AD0 pin is not reflected in this register.
+    mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
 
-    if (sig != (MPU6050_ADDRESS & 0x7e))
+    // If an MPU3050 is connected sig will contain 0.
+    ack = i2cRead(MPU_ADDRESS, MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
+    inquiryResult &= MPU_INQUIRY_MASK;
+    if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
+        mpuDetectionResult.sensor = MPU_3050;
+        mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
+        return &mpuDetectionResult;
+    }
 
+    sig &= MPU_INQUIRY_MASK;
+
+    if (sig != MPUx0x0_WHO_AM_I_CONST)
         return &mpuDetectionResult;
 
+    mpuDetectionResult.sensor = MPU_60x0;
+
+    mpu6050FindRevision();
+
+    return &mpuDetectionResult;
+}
+
+
+void mpu6050FindRevision(void)
+{
+    bool ack;
+    UNUSED(ack);
+
+    uint8_t readBuffer[6];
+    uint8_t revision;
+    uint8_t productId;
 
     // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
     // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
 
     // determine product ID and accel revision
-    i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, readBuffer);
+    ack = i2cRead(MPU_ADDRESS, MPU_RA_XA_OFFS_H, 6, readBuffer);
     revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
     if (revision) {
         /* Congrats, these parts are better. */
@@ -107,7 +140,7 @@ mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
             failureMode(FAILURE_ACC_INCOMPATIBLE);
         }
     } else {
-        i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId);
+        ack = i2cRead(MPU_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId);
         revision = productId & 0x0F;
         if (!revision) {
             failureMode(FAILURE_ACC_INCOMPATIBLE);
@@ -117,9 +150,6 @@ mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
             mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
         }
     }
-
-    mpuDetectionResult.sensor = MPU_60x0;
-    return &mpuDetectionResult;
 }
 
 void MPU_DATA_READY_EXTI_Handler(void)
@@ -250,7 +280,7 @@ bool mpuAccRead(int16_t *accData)
 {
     uint8_t buf[6];
 
-    bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
+    bool ack = i2cRead(MPU_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
     if (!ack) {
         return false;
     }
@@ -266,7 +296,7 @@ bool mpuGyroRead(int16_t *gyroADC)
 {
     uint8_t buf[6];
 
-    bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
+    bool ack = i2cRead(MPU_ADDRESS, mpuConfiguration.gyroReadXRegister, 6, buf);
     if (!ack) {
         return false;
     }
diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c
index 055edd940..a45f1a751 100644
--- a/src/main/drivers/accgyro_mpu3050.c
+++ b/src/main/drivers/accgyro_mpu3050.c
@@ -23,26 +23,20 @@
 #include "common/maths.h"
 
 #include "system.h"
+#include "exti.h"
 #include "bus_i2c.h"
 
 #include "sensor.h"
 #include "accgyro.h"
+#include "accgyro_mpu.h"
 #include "accgyro_mpu3050.h"
 
-
+extern mpuDetectionResult_t mpuDetectionResult;
+extern uint8_t mpuLowPassFilter;
 
 // MPU3050, Standard address 0x68
 #define MPU3050_ADDRESS         0x68
 
-// Registers
-#define MPU3050_SMPLRT_DIV      0x15
-#define MPU3050_DLPF_FS_SYNC    0x16
-#define MPU3050_INT_CFG         0x17
-#define MPU3050_TEMP_OUT        0x1B
-#define MPU3050_GYRO_OUT        0x1D
-#define MPU3050_USER_CTRL       0x3D
-#define MPU3050_PWR_MGM         0x3E
-
 // Bits
 #define MPU3050_FS_SEL_2000DPS  0x18
 #define MPU3050_DLPF_10HZ       0x05
@@ -55,56 +49,27 @@
 #define MPU3050_USER_RESET      0x01
 #define MPU3050_CLK_SEL_PLL_GX  0x01
 
-static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
-
-static void mpu3050Init(void);
-static bool mpu3050Read(int16_t *gyroADC);
+void mpu3050Init(void);
 static bool mpu3050ReadTemp(int16_t *tempData);
 
 bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
 {
-    bool ack;
-
-    delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
-
-    ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0);
-    if (!ack)
-        return false;
-
+    if (mpuDetectionResult.sensor != MPU_3050) {
+        return false;;
+    }
     gyro->init = mpu3050Init;
-    gyro->read = mpu3050Read;
+    gyro->read = mpuGyroRead;
     gyro->temperature = mpu3050ReadTemp;
 
     // 16.4 dps/lsb scalefactor
     gyro->scale = 1.0f / 16.4f;
 
-    // default lpf is 42Hz
-    switch (lpf) {
-        case 256:
-            mpuLowPassFilter = MPU3050_DLPF_256HZ;
-            break;
-        case 188:
-            mpuLowPassFilter = MPU3050_DLPF_188HZ;
-            break;
-        case 98:
-            mpuLowPassFilter = MPU3050_DLPF_98HZ;
-            break;
-        default:
-        case 42:
-            mpuLowPassFilter = MPU3050_DLPF_42HZ;
-            break;
-        case 20:
-            mpuLowPassFilter = MPU3050_DLPF_20HZ;
-            break;
-        case 10:
-            mpuLowPassFilter = MPU3050_DLPF_10HZ;
-            break;
-    }
+    configureMPULPF(lpf);
 
     return true;
 }
 
-static void mpu3050Init(void)
+void mpu3050Init(void)
 {
     bool ack;
 
@@ -120,22 +85,6 @@ static void mpu3050Init(void)
     i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
 }
 
-// Read 3 gyro values into user-provided buffer. No overrun checking is done.
-static bool mpu3050Read(int16_t *gyroADC)
-{
-    uint8_t buf[6];
-
-    if (!i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf)) {
-        return false;
-    }
-
-    gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
-    gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
-    gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
-
-    return true;
-}
-
 static bool mpu3050ReadTemp(int16_t *tempData)
 {
     uint8_t buf[2];
diff --git a/src/main/drivers/accgyro_mpu3050.h b/src/main/drivers/accgyro_mpu3050.h
index bccaa0659..c2f125be5 100644
--- a/src/main/drivers/accgyro_mpu3050.h
+++ b/src/main/drivers/accgyro_mpu3050.h
@@ -17,4 +17,14 @@
 
 #pragma once
 
+// Registers
+#define MPU3050_SMPLRT_DIV      0x15
+#define MPU3050_DLPF_FS_SYNC    0x16
+#define MPU3050_INT_CFG         0x17
+#define MPU3050_TEMP_OUT        0x1B
+#define MPU3050_GYRO_OUT        0x1D
+#define MPU3050_USER_CTRL       0x3D
+#define MPU3050_PWR_MGM         0x3E
+
+void mpu3050Init(void);
 bool mpu3050Detect(gyro_t *gyro, uint16_t lpf);
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 0c3487ad7..2fdc24d2b 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -599,7 +599,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
     memset(&acc, 0, sizeof(acc));
     memset(&gyro, 0, sizeof(gyro));
 
-#if defined(USE_GYRO_MPU6050) || defined(USE_ACC_MPU6050)
+#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_ACC_MPU6050)
 
     const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
 

From 0361d161fb3bd60b170678a0da319b00b3e52e09 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Sat, 19 Sep 2015 16:21:50 +0100
Subject: [PATCH 10/41] Relocate and use some of the common MPU code from
 MPU6500 into accgyro_mpu.c.

---
 Makefile                               |   1 +
 src/main/drivers/accgyro_mpu.c         |  85 +++++++++++++++-----
 src/main/drivers/accgyro_mpu.h         |  10 +++
 src/main/drivers/accgyro_mpu3050.c     |  12 +--
 src/main/drivers/accgyro_mpu6050.c     |  16 ++--
 src/main/drivers/accgyro_spi_mpu6500.c | 105 +++++--------------------
 src/main/drivers/accgyro_spi_mpu6500.h |  18 +++--
 src/main/sensors/initialisation.c      |   2 +-
 8 files changed, 122 insertions(+), 127 deletions(-)

diff --git a/Makefile b/Makefile
index 40ec0b1c1..45db036b4 100755
--- a/Makefile
+++ b/Makefile
@@ -520,6 +520,7 @@ CHEBUZZF3_SRC	 = \
 COLIBRI_RACE_SRC        = \
 		   $(STM32F30x_COMMON_SRC) \
 		   drivers/display_ug2864hsweg01.c \
+		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_spi_mpu6500.c \
 		   drivers/barometer_ms5611.c \
 		   drivers/compass_ak8975.c \
diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
index 9708ece29..e9745617a 100644
--- a/src/main/drivers/accgyro_mpu.c
+++ b/src/main/drivers/accgyro_mpu.c
@@ -37,25 +37,29 @@
 #include "accgyro.h"
 #include "accgyro_mpu3050.h"
 #include "accgyro_mpu6050.h"
+#include "accgyro_spi_mpu6500.h"
 #include "accgyro_mpu.h"
 
 //#define DEBUG_MPU_DATA_READY_INTERRUPT
 
 
+static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data);
+static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
 
-uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
+static void mpu6050FindRevision(void);
 
-void mpu6050FindRevision(void);
+#ifdef USE_SPI
+static bool detectSPISensorsAndUpdateDetectionResult(void);
+#endif
 
 mpuDetectionResult_t mpuDetectionResult;
 
-typedef struct mpuConfiguration_s {
-    uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
-} mpuConfiguration_t;
-
-static mpuConfiguration_t mpuConfiguration;
+mpuConfiguration_t mpuConfiguration;
 static const extiConfig_t *mpuIntExtiConfig = NULL;
 
+// FIXME move into mpuConfiguration
+uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
+
 
 #define MPU_ADDRESS             0x68
 
@@ -87,14 +91,23 @@ mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
     // MPU datasheet specifies 30ms.
     delay(35);
 
-    ack = i2cRead(MPU_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
-    if (!ack)
+    ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
+    if (ack) {
+        mpuConfiguration.read = mpuReadRegisterI2C;
+        mpuConfiguration.write = mpuWriteRegisterI2C;
+    } else {
+#ifdef USE_SPI
+        bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult();
+        UNUSED(detectedSpiSensor);
+#endif
+
         return &mpuDetectionResult;
+    }
 
     mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
 
     // If an MPU3050 is connected sig will contain 0.
-    ack = i2cRead(MPU_ADDRESS, MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
+    ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
     inquiryResult &= MPU_INQUIRY_MASK;
     if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
         mpuDetectionResult.sensor = MPU_3050;
@@ -114,8 +127,26 @@ mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
     return &mpuDetectionResult;
 }
 
+#ifdef USE_SPI
+static bool detectSPISensorsAndUpdateDetectionResult(void)
+{
+    bool found = false;
 
-void mpu6050FindRevision(void)
+#ifdef USE_GYRO_SPI_MPU6500
+    found = mpu6500Detect();
+    if (found) {
+        mpuDetectionResult.sensor = MPU_65xx_SPI;
+        mpuConfiguration.gyroReadXRegister = MPU6500_RA_GYRO_XOUT_H;
+        mpuConfiguration.read = mpu6500ReadRegister;
+        mpuConfiguration.write = mpu6500WriteRegister;
+    }
+#endif
+
+    return found;
+}
+#endif
+
+static void mpu6050FindRevision(void)
 {
     bool ack;
     UNUSED(ack);
@@ -276,34 +307,46 @@ void configureMPULPF(uint16_t lpf)
         mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
 }
 
+static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
+{
+    bool ack = i2cRead(MPU_ADDRESS, reg, length, data);
+    return ack;
+}
+
+static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data)
+{
+    bool ack = i2cWrite(MPU_ADDRESS, reg, data);
+    return ack;
+}
+
 bool mpuAccRead(int16_t *accData)
 {
-    uint8_t buf[6];
+    uint8_t data[6];
 
-    bool ack = i2cRead(MPU_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
+    bool ack = mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data);
     if (!ack) {
         return false;
     }
 
-    accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
-    accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
-    accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
+    accData[0] = (int16_t)((data[0] << 8) | data[1]);
+    accData[1] = (int16_t)((data[2] << 8) | data[3]);
+    accData[2] = (int16_t)((data[4] << 8) | data[5]);
 
     return true;
 }
 
 bool mpuGyroRead(int16_t *gyroADC)
 {
-    uint8_t buf[6];
+    uint8_t data[6];
 
-    bool ack = i2cRead(MPU_ADDRESS, mpuConfiguration.gyroReadXRegister, 6, buf);
+    bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
     if (!ack) {
         return false;
     }
 
-    gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
-    gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
-    gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
+    gyroADC[0] = (int16_t)((data[0] << 8) | data[1]);
+    gyroADC[1] = (int16_t)((data[2] << 8) | data[3]);
+    gyroADC[2] = (int16_t)((data[4] << 8) | data[5]);
 
     return true;
 }
diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h
index b36c78eee..6d946c1b4 100644
--- a/src/main/drivers/accgyro_mpu.h
+++ b/src/main/drivers/accgyro_mpu.h
@@ -17,6 +17,16 @@
 
 #pragma once
 
+typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data);
+typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
+
+typedef struct mpuConfiguration_s {
+    uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
+    mpuReadRegisterFunc read;
+    mpuWriteRegisterFunc write;
+} mpuConfiguration_t;
+
+extern mpuConfiguration_t mpuConfiguration;
 
 enum lpf_e {
     INV_FILTER_256HZ_NOLPF2 = 0,
diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c
index a45f1a751..77061ee5b 100644
--- a/src/main/drivers/accgyro_mpu3050.c
+++ b/src/main/drivers/accgyro_mpu3050.c
@@ -75,20 +75,20 @@ void mpu3050Init(void)
 
     delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
 
-    ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0);
+    ack = mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0);
     if (!ack)
         failureMode(FAILURE_ACC_INIT);
 
-    i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
-    i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
-    i2cWrite(MPU3050_ADDRESS, MPU3050_USER_CTRL, MPU3050_USER_RESET);
-    i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
+    mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
+    mpuConfiguration.write(MPU3050_INT_CFG, 0);
+    mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
+    mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
 }
 
 static bool mpu3050ReadTemp(int16_t *tempData)
 {
     uint8_t buf[2];
-    if (!i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf)) {
+    if (!mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) {
         return false;
     }
 
diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c
index b790e6bc9..84881cc9b 100644
--- a/src/main/drivers/accgyro_mpu6050.c
+++ b/src/main/drivers/accgyro_mpu6050.c
@@ -196,23 +196,23 @@ static void mpu6050GyroInit(void)
     mpuIntExtiInit();
 
     bool ack;
-    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
+    ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
     delay(100);
-    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
-    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
+    ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
+    ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
     delay(15); //PLL Settling time when changing CLKSEL is max 10ms.  Use 15ms to be sure 
-    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
-    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);   //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
+    ack = mpuConfiguration.write(MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
+    ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);   //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
 
     // ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
     // Accel scale 8g (4096 LSB/g)
-    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
+    ack = mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
 
-    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG,
+    ack = mpuConfiguration.write(MPU_RA_INT_PIN_CFG,
             0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG   -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
 
 #ifdef USE_MPU_DATA_READY_SIGNAL
-    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
+    ack = mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
 #endif
     UNUSED(ack);
 }
diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c
index 08b14ebbf..95bbcfa57 100755
--- a/src/main/drivers/accgyro_spi_mpu6500.c
+++ b/src/main/drivers/accgyro_spi_mpu6500.c
@@ -25,73 +25,45 @@
 #include "common/maths.h"
 
 #include "system.h"
+#include "exti.h"
 #include "gpio.h"
 #include "bus_spi.h"
 
 #include "sensor.h"
 #include "accgyro.h"
+#include "accgyro_mpu.h"
 #include "accgyro_spi_mpu6500.h"
 
-enum lpf_e {
-    INV_FILTER_256HZ_NOLPF2 = 0,
-    INV_FILTER_188HZ,
-    INV_FILTER_98HZ,
-    INV_FILTER_42HZ,
-    INV_FILTER_20HZ,
-    INV_FILTER_10HZ,
-    INV_FILTER_5HZ,
-    INV_FILTER_2100HZ_NOLPF,
-    NUM_FILTER
-};
-
-enum gyro_fsr_e {
-    INV_FSR_250DPS = 0,
-    INV_FSR_500DPS,
-    INV_FSR_1000DPS,
-    INV_FSR_2000DPS,
-    NUM_GYRO_FSR
-};
-
-enum clock_sel_e {
-    INV_CLK_INTERNAL = 0,
-    INV_CLK_PLL,
-    NUM_CLK
-};
-
-enum accel_fsr_e {
-    INV_FSR_2G = 0,
-    INV_FSR_4G,
-    INV_FSR_8G,
-    INV_FSR_16G,
-    NUM_ACCEL_FSR
-};
-
 #define DISABLE_MPU6500       GPIO_SetBits(MPU6500_CS_GPIO,   MPU6500_CS_PIN)
 #define ENABLE_MPU6500        GPIO_ResetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
 
-static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
+extern mpuDetectionResult_t mpuDetectionResult;
+extern uint8_t mpuLowPassFilter;
+
 
 static void mpu6500AccInit(void);
-static bool mpu6500AccRead(int16_t *accData);
 static void mpu6500GyroInit(void);
-static bool mpu6500GyroRead(int16_t *gyroADC);
 
 extern uint16_t acc_1G;
 
-static void mpu6500WriteRegister(uint8_t reg, uint8_t data)
+bool mpu6500WriteRegister(uint8_t reg, uint8_t data)
 {
     ENABLE_MPU6500;
     spiTransferByte(MPU6500_SPI_INSTANCE, reg);
     spiTransferByte(MPU6500_SPI_INSTANCE, data);
     DISABLE_MPU6500;
+
+    return true;
 }
 
-static void mpu6500ReadRegister(uint8_t reg, uint8_t *data, int length)
+bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
 {
     ENABLE_MPU6500;
     spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
     spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
     DISABLE_MPU6500;
+
+    return true;
 }
 
 static void mpu6500SpiInit(void)
@@ -133,13 +105,13 @@ static void mpu6500SpiInit(void)
     hardwareInitialised = true;
 }
 
-static bool mpu6500Detect(void)
+bool mpu6500Detect(void)
 {
     uint8_t tmp;
 
     mpu6500SpiInit();
 
-    mpu6500ReadRegister(MPU6500_RA_WHOAMI, &tmp, 1);
+    mpu6500ReadRegister(MPU6500_RA_WHOAMI, 1, &tmp);
 
     if (tmp != MPU6500_WHO_AM_I_CONST)
         return false;
@@ -149,42 +121,29 @@ static bool mpu6500Detect(void)
 
 bool mpu6500SpiAccDetect(acc_t *acc)
 {
-    if (!mpu6500Detect()) {
+    if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
         return false;
     }
 
     acc->init = mpu6500AccInit;
-    acc->read = mpu6500AccRead;
+    acc->read = mpuAccRead;
 
     return true;
 }
 
 bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
 {
-    if (!mpu6500Detect()) {
+    if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
         return false;
     }
 
     gyro->init = mpu6500GyroInit;
-    gyro->read = mpu6500GyroRead;
+    gyro->read = mpuGyroRead;
 
     // 16.4 dps/lsb scalefactor
     gyro->scale = 1.0f / 16.4f;
-    //gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
 
-    // default lpf is 42Hz
-    if (lpf >= 188)
-        mpuLowPassFilter = INV_FILTER_188HZ;
-    else if (lpf >= 98)
-        mpuLowPassFilter = INV_FILTER_98HZ;
-    else if (lpf >= 42)
-        mpuLowPassFilter = INV_FILTER_42HZ;
-    else if (lpf >= 20)
-        mpuLowPassFilter = INV_FILTER_20HZ;
-    else if (lpf >= 10)
-        mpuLowPassFilter = INV_FILTER_10HZ;
-    else
-        mpuLowPassFilter = INV_FILTER_5HZ;
+    configureMPULPF(lpf);
 
     return true;
 }
@@ -194,19 +153,6 @@ static void mpu6500AccInit(void)
     acc_1G = 512 * 8;
 }
 
-static bool mpu6500AccRead(int16_t *accData)
-{
-    uint8_t buf[6];
-
-    mpu6500ReadRegister(MPU6500_RA_ACCEL_XOUT_H, buf, 6);
-
-    accData[X] = (int16_t)((buf[0] << 8) | buf[1]);
-    accData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
-    accData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
-
-    return true;
-}
-
 static void mpu6500GyroInit(void)
 {
 #ifdef NAZE
@@ -222,6 +168,8 @@ static void mpu6500GyroInit(void)
 
     mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
     delay(100);
+    mpu6500WriteRegister(MPU6500_RA_SIGNAL_PATH_RST, 0x07);
+    delay(100);
     mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, 0);
     delay(100);
     mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, INV_CLK_PLL);
@@ -230,16 +178,3 @@ static void mpu6500GyroInit(void)
     mpu6500WriteRegister(MPU6500_RA_LPF, mpuLowPassFilter);
     mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R
 }
-
-static bool mpu6500GyroRead(int16_t *gyroADC)
-{
-    uint8_t buf[6];
-
-    mpu6500ReadRegister(MPU6500_RA_GYRO_XOUT_H, buf, 6);
-
-    gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
-    gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
-    gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
-
-    return true;
-}
diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h
index 20f4e06b9..11a3c3e3d 100755
--- a/src/main/drivers/accgyro_spi_mpu6500.h
+++ b/src/main/drivers/accgyro_spi_mpu6500.h
@@ -15,16 +15,17 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#define MPU6500_RA_WHOAMI                   (0x75)
+#define MPU6500_RA_RATE_DIV                 (0x19)
+#define MPU6500_RA_LPF                      (0x1A)
+#define MPU6500_RA_GYRO_CFG                 (0x1B)
+#define MPU6500_RA_ACCEL_CFG                (0x1C)
 #define MPU6500_RA_ACCEL_XOUT_H             (0x3B)
 #define MPU6500_RA_GYRO_XOUT_H              (0x43)
+#define MPU6500_RA_SIGNAL_PATH_RST          (0x68)
+#define MPU6500_RA_PWR_MGMT_1               (0x6B)
 #define MPU6500_RA_BANK_SEL                 (0x6D)
 #define MPU6500_RA_MEM_RW                   (0x6F)
-#define MPU6500_RA_GYRO_CFG                 (0x1B)
-#define MPU6500_RA_PWR_MGMT_1               (0x6B)
-#define MPU6500_RA_ACCEL_CFG                (0x1C)
-#define MPU6500_RA_LPF                      (0x1A)
-#define MPU6500_RA_RATE_DIV                 (0x19)
+#define MPU6500_RA_WHOAMI                   (0x75)
 
 #define MPU6500_WHO_AM_I_CONST              (0x70)
 
@@ -32,5 +33,10 @@
 
 #pragma once
 
+bool mpu6500Detect(void);
+
 bool mpu6500SpiAccDetect(acc_t *acc);
 bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
+
+bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
+bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 2fdc24d2b..fa43959d8 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -599,7 +599,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
     memset(&acc, 0, sizeof(acc));
     memset(&gyro, 0, sizeof(gyro));
 
-#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_ACC_MPU6050)
+#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_ACC_MPU6050)
 
     const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
 

From 678c0413cbdb3d5653a547e1003d35d928c8cde3 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Sat, 19 Sep 2015 17:11:04 +0100
Subject: [PATCH 11/41] Relocate and use some of the common MPU code from
 MPU6000 into accgyro_mpu.c.

---
 Makefile                               |   1 +
 src/main/drivers/accgyro_adxl345.c     |   2 +-
 src/main/drivers/accgyro_mpu.c         |  15 ++-
 src/main/drivers/accgyro_mpu.h         |   1 +
 src/main/drivers/accgyro_mpu3050.c     |   2 +-
 src/main/drivers/accgyro_mpu6050.c     |   2 +-
 src/main/drivers/accgyro_spi_mpu6000.c | 159 +++++++------------------
 src/main/drivers/accgyro_spi_mpu6000.h |  37 +++++-
 src/main/drivers/system.h              |   3 +-
 src/main/sensors/initialisation.c      |   2 +-
 10 files changed, 98 insertions(+), 126 deletions(-)

diff --git a/Makefile b/Makefile
index 45db036b4..a5edba6e1 100755
--- a/Makefile
+++ b/Makefile
@@ -433,6 +433,7 @@ CJMCU_SRC	 = \
 
 CC3D_SRC	 = \
 		   startup_stm32f10x_md_gcc.S \
+		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_spi_mpu6000.c \
 		   drivers/adc.c \
 		   drivers/adc_stm32f10x.c \
diff --git a/src/main/drivers/accgyro_adxl345.c b/src/main/drivers/accgyro_adxl345.c
index 82082e161..feada6597 100644
--- a/src/main/drivers/accgyro_adxl345.c
+++ b/src/main/drivers/accgyro_adxl345.c
@@ -111,7 +111,7 @@ static bool adxl345Read(int16_t *accelData)
             i++;
 
             if (!i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf)) {
-                return false;;
+                return false;
             }
 
             x += (int16_t)(buf[0] + (buf[1] << 8));
diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
index e9745617a..af66683eb 100644
--- a/src/main/drivers/accgyro_mpu.c
+++ b/src/main/drivers/accgyro_mpu.c
@@ -37,6 +37,7 @@
 #include "accgyro.h"
 #include "accgyro_mpu3050.h"
 #include "accgyro_mpu6050.h"
+#include "accgyro_spi_mpu6000.h"
 #include "accgyro_spi_mpu6500.h"
 #include "accgyro_mpu.h"
 
@@ -139,10 +140,22 @@ static bool detectSPISensorsAndUpdateDetectionResult(void)
         mpuConfiguration.gyroReadXRegister = MPU6500_RA_GYRO_XOUT_H;
         mpuConfiguration.read = mpu6500ReadRegister;
         mpuConfiguration.write = mpu6500WriteRegister;
+        return true;
     }
 #endif
 
-    return found;
+#ifdef USE_GYRO_SPI_MPU6000
+    found = mpu6000SpiDetect();
+    if (found) {
+        mpuDetectionResult.sensor = MPU_60x0_SPI;
+        mpuConfiguration.gyroReadXRegister = MPU6000_GYRO_XOUT_H;
+        mpuConfiguration.read = mpu6000ReadRegister;
+        mpuConfiguration.write = mpu6000WriteRegister;
+        return true;
+    }
+#endif
+
+    return false;
 }
 #endif
 
diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h
index 6d946c1b4..5808e65ec 100644
--- a/src/main/drivers/accgyro_mpu.h
+++ b/src/main/drivers/accgyro_mpu.h
@@ -63,6 +63,7 @@ typedef enum {
     MPU_NONE,
     MPU_3050,
     MPU_60x0,
+    MPU_60x0_SPI,
     MPU_65xx_I2C,
     MPU_65xx_SPI
 } detectedMPUSensor_e;
diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c
index 77061ee5b..4a621109c 100644
--- a/src/main/drivers/accgyro_mpu3050.c
+++ b/src/main/drivers/accgyro_mpu3050.c
@@ -55,7 +55,7 @@ static bool mpu3050ReadTemp(int16_t *tempData);
 bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
 {
     if (mpuDetectionResult.sensor != MPU_3050) {
-        return false;;
+        return false;
     }
     gyro->init = mpu3050Init;
     gyro->read = mpuGyroRead;
diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c
index 84881cc9b..06e91540d 100644
--- a/src/main/drivers/accgyro_mpu6050.c
+++ b/src/main/drivers/accgyro_mpu6050.c
@@ -164,7 +164,7 @@ bool mpu6050AccDetect(acc_t *acc)
 bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf)
 {
     if (mpuDetectionResult.sensor != MPU_60x0) {
-        return false;;
+        return false;
     }
     gyro->init = mpu6050GyroInit;
     gyro->read = mpuGyroRead;
diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c
index 3bee15fbe..e71e35014 100644
--- a/src/main/drivers/accgyro_spi_mpu6000.c
+++ b/src/main/drivers/accgyro_spi_mpu6000.c
@@ -33,45 +33,20 @@
 
 #include "system.h"
 #include "gpio.h"
+#include "exti.h"
 #include "bus_spi.h"
 
 #include "sensor.h"
 #include "accgyro.h"
+#include "accgyro_mpu.h"
 #include "accgyro_spi_mpu6000.h"
 
-static bool mpuSpi6000InitDone = false;
+static void mpu6000AccAndGyroInit(void);
 
-// Registers
-#define MPU6000_PRODUCT_ID      	0x0C
-#define MPU6000_SMPLRT_DIV	    	0x19
-#define MPU6000_GYRO_CONFIG	    	0x1B
-#define MPU6000_ACCEL_CONFIG  		0x1C
-#define MPU6000_FIFO_EN		    	0x23
-#define MPU6000_INT_PIN_CFG	    	0x37
-#define MPU6000_INT_ENABLE	    	0x38
-#define MPU6000_INT_STATUS	    	0x3A
-#define MPU6000_ACCEL_XOUT_H 		0x3B
-#define MPU6000_ACCEL_XOUT_L 		0x3C
-#define MPU6000_ACCEL_YOUT_H 		0x3D
-#define MPU6000_ACCEL_YOUT_L 		0x3E
-#define MPU6000_ACCEL_ZOUT_H 		0x3F
-#define MPU6000_ACCEL_ZOUT_L    	0x40
-#define MPU6000_TEMP_OUT_H	    	0x41
-#define MPU6000_TEMP_OUT_L	    	0x42
-#define MPU6000_GYRO_XOUT_H	    	0x43
-#define MPU6000_GYRO_XOUT_L	    	0x44
-#define MPU6000_GYRO_YOUT_H	    	0x45
-#define MPU6000_GYRO_YOUT_L	     	0x46
-#define MPU6000_GYRO_ZOUT_H	    	0x47
-#define MPU6000_GYRO_ZOUT_L	    	0x48
-#define MPU6000_USER_CTRL	    	0x6A
-#define MPU6000_SIGNAL_PATH_RESET   0x68
-#define MPU6000_PWR_MGMT_1	    	0x6B
-#define MPU6000_PWR_MGMT_2	    	0x6C
-#define MPU6000_FIFO_COUNTH	    	0x72
-#define MPU6000_FIFO_COUNTL	    	0x73
-#define MPU6000_FIFO_R_W		   	0x74
-#define MPU6000_WHOAMI		    	0x75
+extern mpuDetectionResult_t mpuDetectionResult;
+extern uint8_t mpuLowPassFilter;
+
+static bool mpuSpi6000InitDone = false;
 
 // Bits
 #define BIT_SLEEP				    0x40
@@ -125,27 +100,46 @@ static bool mpuSpi6000InitDone = false;
 #define DISABLE_MPU6000       GPIO_SetBits(MPU6000_CS_GPIO,   MPU6000_CS_PIN)
 #define ENABLE_MPU6000        GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
 
-bool mpu6000SpiGyroRead(int16_t *gyroADC);
-bool mpu6000SpiAccRead(int16_t *gyroADC);
 
-static void mpu6000WriteRegister(uint8_t reg, uint8_t data)
+bool mpu6000WriteRegister(uint8_t reg, uint8_t data)
 {
     ENABLE_MPU6000;
     spiTransferByte(MPU6000_SPI_INSTANCE, reg);
     spiTransferByte(MPU6000_SPI_INSTANCE, data);
     DISABLE_MPU6000;
+
+    return true;
 }
 
-static void mpu6000ReadRegister(uint8_t reg, uint8_t *data, int length)
+bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
 {
     ENABLE_MPU6000;
     spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction
     spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length);
     DISABLE_MPU6000;
+
+    return true;
 }
 
 void mpu6000SpiGyroInit(void)
 {
+    mpu6000AccAndGyroInit();
+
+    spiResetErrorCounter(MPU6000_SPI_INSTANCE);
+
+    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
+
+    // Accel and Gyro DLPF Setting
+    mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
+    delayMicroseconds(1);
+
+    int16_t data[3];
+    mpuGyroRead(data);
+
+    if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
+        spiResetErrorCounter(MPU6000_SPI_INSTANCE);
+        failureMode(FAILURE_GYRO_INIT_FAILED);
+    }
 }
 
 void mpu6000SpiAccInit(void)
@@ -157,9 +151,6 @@ bool mpu6000SpiDetect(void)
 {
     uint8_t in;
     uint8_t attemptsRemaining = 5;
-    if (mpuSpi6000InitDone) {
-        return true;
-    }
 
     spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
 
@@ -168,7 +159,7 @@ bool mpu6000SpiDetect(void)
     do {
         delay(150);
 
-        mpu6000ReadRegister(MPU6000_WHOAMI, &in, 1);
+        mpu6000ReadRegister(MPU6000_WHOAMI, 1, &in);
         if (in == MPU6000_WHO_AM_I_CONST) {
             break;
         }
@@ -178,7 +169,7 @@ bool mpu6000SpiDetect(void)
     } while (attemptsRemaining--);
 
 
-    mpu6000ReadRegister(MPU6000_PRODUCT_ID, &in, 1);
+    mpu6000ReadRegister(MPU6000_PRODUCT_ID, 1, &in);
 
     /* look for a product ID we recognise */
 
@@ -202,7 +193,7 @@ bool mpu6000SpiDetect(void)
     return false;
 }
 
-void mpu6000AccAndGyroInit() {
+static void mpu6000AccAndGyroInit(void) {
 
     if (mpuSpi6000InitDone) {
         return;
@@ -241,104 +232,36 @@ void mpu6000AccAndGyroInit() {
     mpu6000WriteRegister(MPU6000_GYRO_CONFIG, BITS_FS_2000DPS);
     delayMicroseconds(1);
 
+    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER);  // 18 MHz SPI clock
+
     mpuSpi6000InitDone = true;
 }
 
 bool mpu6000SpiAccDetect(acc_t *acc)
 {
-    if (!mpu6000SpiDetect()) {
+    if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
         return false;
     }
 
-    spiResetErrorCounter(MPU6000_SPI_INSTANCE);
-
-    mpu6000AccAndGyroInit();
-
     acc->init = mpu6000SpiAccInit;
-    acc->read = mpu6000SpiAccRead;
+    acc->read = mpuAccRead;
 
-    delay(100);
     return true;
 }
 
 bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
 {
-    if (!mpu6000SpiDetect()) {
+    if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
         return false;
     }
 
-    spiResetErrorCounter(MPU6000_SPI_INSTANCE);
-
-    mpu6000AccAndGyroInit();
-
-    uint8_t mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
-    int16_t data[3];
-
-    // default lpf is 42Hz
-    if (lpf == 256)
-        mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
-    else if (lpf >= 188)
-        mpuLowPassFilter = BITS_DLPF_CFG_188HZ;
-    else if (lpf >= 98)
-        mpuLowPassFilter = BITS_DLPF_CFG_98HZ;
-    else if (lpf >= 42)
-        mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
-    else if (lpf >= 20)
-        mpuLowPassFilter = BITS_DLPF_CFG_20HZ;
-    else if (lpf >= 10)
-        mpuLowPassFilter = BITS_DLPF_CFG_10HZ;
-    else if (lpf > 0)
-        mpuLowPassFilter = BITS_DLPF_CFG_5HZ;
-    else
-        mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
-
-    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
-
-    // Accel and Gyro DLPF Setting
-    mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
-    delayMicroseconds(1);
-
-    mpu6000SpiGyroRead(data);
-
-    if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
-        spiResetErrorCounter(MPU6000_SPI_INSTANCE);
-        return false;
-    }
     gyro->init = mpu6000SpiGyroInit;
-    gyro->read = mpu6000SpiGyroRead;
+    gyro->read = mpuGyroRead;
+
     // 16.4 dps/lsb scalefactor
     gyro->scale = 1.0f / 16.4f;
-    //gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
-    delay(100);
-    return true;
-}
 
-bool mpu6000SpiGyroRead(int16_t *gyroData)
-{
-    uint8_t buf[6];
-
-    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER);  // 18 MHz SPI clock
-
-    mpu6000ReadRegister(MPU6000_GYRO_XOUT_H, buf, 6);
-
-    gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
-    gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
-    gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
-
-    return true;
-}
-
-bool mpu6000SpiAccRead(int16_t *gyroData)
-{
-    uint8_t buf[6];
-
-    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER);  // 18 MHz SPI clock
-
-    mpu6000ReadRegister(MPU6000_ACCEL_XOUT_H, buf, 6);
-
-    gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
-    gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
-    gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
+    configureMPULPF(lpf);
 
     return true;
 }
diff --git a/src/main/drivers/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro_spi_mpu6000.h
index 26e5ba037..67618d8f8 100644
--- a/src/main/drivers/accgyro_spi_mpu6000.h
+++ b/src/main/drivers/accgyro_spi_mpu6000.h
@@ -3,6 +3,38 @@
 
 #define MPU6000_CONFIG		    	0x1A
 
+// Registers
+#define MPU6000_PRODUCT_ID          0x0C
+#define MPU6000_SMPLRT_DIV          0x19
+#define MPU6000_GYRO_CONFIG         0x1B
+#define MPU6000_ACCEL_CONFIG        0x1C
+#define MPU6000_FIFO_EN             0x23
+#define MPU6000_INT_PIN_CFG         0x37
+#define MPU6000_INT_ENABLE          0x38
+#define MPU6000_INT_STATUS          0x3A
+#define MPU6000_ACCEL_XOUT_H        0x3B
+#define MPU6000_ACCEL_XOUT_L        0x3C
+#define MPU6000_ACCEL_YOUT_H        0x3D
+#define MPU6000_ACCEL_YOUT_L        0x3E
+#define MPU6000_ACCEL_ZOUT_H        0x3F
+#define MPU6000_ACCEL_ZOUT_L        0x40
+#define MPU6000_TEMP_OUT_H          0x41
+#define MPU6000_TEMP_OUT_L          0x42
+#define MPU6000_GYRO_XOUT_H         0x43
+#define MPU6000_GYRO_XOUT_L         0x44
+#define MPU6000_GYRO_YOUT_H         0x45
+#define MPU6000_GYRO_YOUT_L         0x46
+#define MPU6000_GYRO_ZOUT_H         0x47
+#define MPU6000_GYRO_ZOUT_L         0x48
+#define MPU6000_USER_CTRL           0x6A
+#define MPU6000_SIGNAL_PATH_RESET   0x68
+#define MPU6000_PWR_MGMT_1          0x6B
+#define MPU6000_PWR_MGMT_2          0x6C
+#define MPU6000_FIFO_COUNTH         0x72
+#define MPU6000_FIFO_COUNTL         0x73
+#define MPU6000_FIFO_R_W            0x74
+#define MPU6000_WHOAMI              0x75
+
 #define BITS_DLPF_CFG_256HZ         0x00
 #define BITS_DLPF_CFG_188HZ         0x01
 #define BITS_DLPF_CFG_98HZ          0x02
@@ -12,9 +44,10 @@
 
 #define MPU6000_WHO_AM_I_CONST              (0x68)
 
+bool mpu6000SpiDetect(void);
 
 bool mpu6000SpiAccDetect(acc_t *acc);
 bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
 
-bool mpu6000SpiGyroRead(int16_t *gyroADC);
-bool mpu6000SpiAccRead(int16_t *gyroADC);
+bool mpu6000WriteRegister(uint8_t reg, uint8_t data);
+bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h
index e6b62d730..65d0f18a7 100644
--- a/src/main/drivers/system.h
+++ b/src/main/drivers/system.h
@@ -49,6 +49,7 @@ typedef enum {
     FAILURE_ACC_INIT,
     FAILURE_ACC_INCOMPATIBLE,
     FAILURE_INVALID_EEPROM_CONTENTS,
-    FAILURE_FLASH_WRITE_FAILED
+    FAILURE_FLASH_WRITE_FAILED,
+    FAILURE_GYRO_INIT_FAILED
 } failureMode_e;
 
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index fa43959d8..c3f7f668f 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -599,7 +599,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
     memset(&acc, 0, sizeof(acc));
     memset(&gyro, 0, sizeof(gyro));
 
-#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_ACC_MPU6050)
+#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
 
     const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
 

From 85ba1eb0bdfb6174318e41c1cf6f82075c26f32b Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Sat, 19 Sep 2015 18:30:34 +0100
Subject: [PATCH 12/41] Add support for MPU6500 connected via I2C.

---
 Makefile                               |   1 +
 docs/Cli.md                            |   2 +-
 src/main/drivers/accgyro_mpu.c         |  13 +--
 src/main/drivers/accgyro_mpu6050.c     |   2 +-
 src/main/drivers/accgyro_mpu6500.c     | 110 +++++++++++++++++++++++++
 src/main/drivers/accgyro_mpu6500.h     |  42 ++++++++++
 src/main/drivers/accgyro_spi_mpu6500.c |  37 +--------
 src/main/drivers/accgyro_spi_mpu6500.h |   4 +-
 src/main/sensors/acceleration.h        |   4 +-
 src/main/sensors/gyro.h                |   4 +-
 src/main/sensors/initialisation.c      |  57 +++++--------
 src/main/target/CC3D/target.h          |   4 +-
 src/main/target/COLIBRI_RACE/target.h  |   4 +-
 src/main/target/NAZE/target.h          |   6 +-
 14 files changed, 202 insertions(+), 88 deletions(-)
 create mode 100644 src/main/drivers/accgyro_mpu6500.c
 create mode 100644 src/main/drivers/accgyro_mpu6500.h

diff --git a/Makefile b/Makefile
index a5edba6e1..a71801486 100755
--- a/Makefile
+++ b/Makefile
@@ -295,6 +295,7 @@ NAZE_SRC	 = startup_stm32f10x_md_gcc.S \
 		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_mpu3050.c \
 		   drivers/accgyro_mpu6050.c \
+		   drivers/accgyro_mpu6500.c \
 		   drivers/accgyro_spi_mpu6500.c \
 		   drivers/adc.c \
 		   drivers/adc_stm32f10x.c \
diff --git a/docs/Cli.md b/docs/Cli.md
index 5cdb98de4..3070c6d90 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -204,7 +204,7 @@ Re-apply any new defaults as desired.
 | `rx_min_usec`                   | Defines the shortest pulse width value used when ensuring the channel value is valid.  If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of `mid_rc`.                                                                                                                                                                                                                                                                                                                                                                                                                     | 100    | 2000   | 985           | Profile      | UINT16   |
 | `rx_max_usec`                   | Defines the longest pulse width value used when ensuring the channel value is valid.  If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of `mid_rc`.                                                                                                                                                                                                                                                                                                                                                                                                                     | 100    | 3000   | 2115          | Profile      | UINT16   |
 | `gimbal_mode`                  | When feature SERVO_TILT is enabled, this can be either 0=normal gimbal (default) or 1=tiltmix gimbal                                                                                                                                                                                                                                                                                                    | 0      | 1      | 0             | Profile      | UINT8    |
-| `acc_hardware`                  | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for SPI_MPU6000, 8 for SPI_MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation.                                                                                                                                                                            | 0      | 9      | 0             | Master       | UINT8    |
+| `acc_hardware`                  | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for MPU6000, 8 for MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation.                                                                                                                                                                            | 0      | 9      | 0             | Master       | UINT8    |
 | `acc_lpf_factor`                | This setting controls the Low Pass Filter factor for ACC.  Increasing this value reduces ACC noise (visible in GUI), but would increase ACC lag time.  Zero = no filter                                                                                                                                                                                                                                                                                                                                                                                                                                                                                | 0      | 250    | 4             | Profile      | UINT8    |
 | `accxy_deadband`                |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 100    | 40            | Profile      | UINT8    |
 | `accz_deadband`                 |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 100    | 40            | Profile      | UINT8    |
diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
index af66683eb..f5b77c2a0 100644
--- a/src/main/drivers/accgyro_mpu.c
+++ b/src/main/drivers/accgyro_mpu.c
@@ -37,6 +37,7 @@
 #include "accgyro.h"
 #include "accgyro_mpu3050.h"
 #include "accgyro_mpu6050.h"
+#include "accgyro_mpu6500.h"
 #include "accgyro_spi_mpu6000.h"
 #include "accgyro_spi_mpu6500.h"
 #include "accgyro_mpu.h"
@@ -118,12 +119,14 @@ mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
 
     sig &= MPU_INQUIRY_MASK;
 
-    if (sig != MPUx0x0_WHO_AM_I_CONST)
-        return &mpuDetectionResult;
+    if (sig == MPUx0x0_WHO_AM_I_CONST) {
 
-    mpuDetectionResult.sensor = MPU_60x0;
+        mpuDetectionResult.sensor = MPU_60x0;
 
-    mpu6050FindRevision();
+        mpu6050FindRevision();
+    } else if (sig == MPU6500_WHO_AM_I_CONST) {
+        mpuDetectionResult.sensor = MPU_65xx_I2C;
+    }
 
     return &mpuDetectionResult;
 }
@@ -134,7 +137,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(void)
     bool found = false;
 
 #ifdef USE_GYRO_SPI_MPU6500
-    found = mpu6500Detect();
+    found = mpu6500SpiDetect();
     if (found) {
         mpuDetectionResult.sensor = MPU_65xx_SPI;
         mpuConfiguration.gyroReadXRegister = MPU6500_RA_GYRO_XOUT_H;
diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c
index 06e91540d..c69e9f105 100644
--- a/src/main/drivers/accgyro_mpu6050.c
+++ b/src/main/drivers/accgyro_mpu6050.c
@@ -204,7 +204,7 @@ static void mpu6050GyroInit(void)
     ack = mpuConfiguration.write(MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
     ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);   //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
 
-    // ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
+    // ACC Init stuff.
     // Accel scale 8g (4096 LSB/g)
     ack = mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
 
diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c
new file mode 100644
index 000000000..a0d114a9e
--- /dev/null
+++ b/src/main/drivers/accgyro_mpu6500.c
@@ -0,0 +1,110 @@
+/*
+ * 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/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include "platform.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "system.h"
+#include "exti.h"
+#include "gpio.h"
+
+#include "sensor.h"
+#include "accgyro.h"
+#include "accgyro_mpu.h"
+#include "accgyro_mpu6500.h"
+
+extern mpuDetectionResult_t mpuDetectionResult;
+extern uint8_t mpuLowPassFilter;
+
+
+extern uint16_t acc_1G;
+
+
+bool mpu6500AccDetect(acc_t *acc)
+{
+    if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
+        return false;
+    }
+
+    acc->init = mpu6500AccInit;
+    acc->read = mpuAccRead;
+
+    return true;
+}
+
+bool mpu6500GyroDetect(gyro_t *gyro, uint16_t lpf)
+{
+    if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
+        return false;
+    }
+
+    gyro->init = mpu6500GyroInit;
+    gyro->read = mpuGyroRead;
+
+    // 16.4 dps/lsb scalefactor
+    gyro->scale = 1.0f / 16.4f;
+
+    configureMPULPF(lpf);
+
+    return true;
+}
+
+void mpu6500AccInit(void)
+{
+    acc_1G = 512 * 8;
+}
+
+void mpu6500GyroInit(void)
+{
+#ifdef NAZE
+    // FIXME target specific code in driver code.
+
+    gpio_config_t gpio;
+    // MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices
+    if (hse_value == 12000000) {
+        gpio.pin = Pin_13;
+        gpio.speed = Speed_2MHz;
+        gpio.mode = Mode_IN_FLOATING;
+        gpioInit(GPIOC, &gpio);
+    }
+#endif
+
+    mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
+    delay(100);
+    mpuConfiguration.write(MPU6500_RA_SIGNAL_PATH_RST, 0x07);
+    delay(100);
+    mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, 0);
+    delay(100);
+    mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, INV_CLK_PLL);
+    mpuConfiguration.write(MPU6500_RA_GYRO_CFG, INV_FSR_2000DPS << 3);
+    mpuConfiguration.write(MPU6500_RA_ACCEL_CFG, INV_FSR_8G << 3);
+    mpuConfiguration.write(MPU6500_RA_LPF, mpuLowPassFilter);
+    mpuConfiguration.write(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R
+
+    // Data ready interrupt configuration
+    mpuConfiguration.write(MPU6500_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR, BYPASS_EN
+#ifdef USE_MPU_DATA_READY_SIGNAL
+    mpuConfiguration.write(MPU6500_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
+#endif
+
+}
diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h
new file mode 100644
index 000000000..3a8a389c8
--- /dev/null
+++ b/src/main/drivers/accgyro_mpu6500.h
@@ -0,0 +1,42 @@
+/*
+ * 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/>.
+ */
+
+#define MPU6500_RA_RATE_DIV                 (0x19)
+#define MPU6500_RA_LPF                      (0x1A)
+#define MPU6500_RA_GYRO_CFG                 (0x1B)
+#define MPU6500_RA_ACCEL_CFG                (0x1C)
+#define MPU6500_RA_ACCEL_XOUT_H             (0x3B)
+#define MPU6500_RA_INT_PIN_CFG              (0x37)
+#define MPU6500_RA_INT_ENABLE               (0x38)
+#define MPU6500_RA_GYRO_XOUT_H              (0x43)
+#define MPU6500_RA_SIGNAL_PATH_RST          (0x68)
+#define MPU6500_RA_PWR_MGMT_1               (0x6B)
+#define MPU6500_RA_BANK_SEL                 (0x6D)
+#define MPU6500_RA_MEM_RW                   (0x6F)
+#define MPU6500_RA_WHOAMI                   (0x75)
+
+#define MPU6500_WHO_AM_I_CONST              (0x70)
+
+#define MPU6500_BIT_RESET                   (0x80)
+
+#pragma once
+
+bool mpu6500AccDetect(acc_t *acc);
+bool mpu6500GyroDetect(gyro_t *gyro, uint16_t lpf);
+
+void mpu6500AccInit(void);
+void mpu6500GyroInit(void);
diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c
index 95bbcfa57..6eb7864b5 100755
--- a/src/main/drivers/accgyro_spi_mpu6500.c
+++ b/src/main/drivers/accgyro_spi_mpu6500.c
@@ -32,6 +32,7 @@
 #include "sensor.h"
 #include "accgyro.h"
 #include "accgyro_mpu.h"
+#include "accgyro_mpu6500.h"
 #include "accgyro_spi_mpu6500.h"
 
 #define DISABLE_MPU6500       GPIO_SetBits(MPU6500_CS_GPIO,   MPU6500_CS_PIN)
@@ -40,10 +41,6 @@
 extern mpuDetectionResult_t mpuDetectionResult;
 extern uint8_t mpuLowPassFilter;
 
-
-static void mpu6500AccInit(void);
-static void mpu6500GyroInit(void);
-
 extern uint16_t acc_1G;
 
 bool mpu6500WriteRegister(uint8_t reg, uint8_t data)
@@ -105,7 +102,7 @@ static void mpu6500SpiInit(void)
     hardwareInitialised = true;
 }
 
-bool mpu6500Detect(void)
+bool mpu6500SpiDetect(void)
 {
     uint8_t tmp;
 
@@ -148,33 +145,3 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
     return true;
 }
 
-static void mpu6500AccInit(void)
-{
-    acc_1G = 512 * 8;
-}
-
-static void mpu6500GyroInit(void)
-{
-#ifdef NAZE
-    gpio_config_t gpio;
-    // MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices
-    if (hse_value == 12000000) {
-        gpio.pin = Pin_13;
-        gpio.speed = Speed_2MHz;
-        gpio.mode = Mode_IN_FLOATING;
-        gpioInit(GPIOC, &gpio);
-    }
-#endif
-
-    mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
-    delay(100);
-    mpu6500WriteRegister(MPU6500_RA_SIGNAL_PATH_RST, 0x07);
-    delay(100);
-    mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, 0);
-    delay(100);
-    mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, INV_CLK_PLL);
-    mpu6500WriteRegister(MPU6500_RA_GYRO_CFG, INV_FSR_2000DPS << 3);
-    mpu6500WriteRegister(MPU6500_RA_ACCEL_CFG, INV_FSR_8G << 3);
-    mpu6500WriteRegister(MPU6500_RA_LPF, mpuLowPassFilter);
-    mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R
-}
diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h
index 11a3c3e3d..7c1c6e88f 100755
--- a/src/main/drivers/accgyro_spi_mpu6500.h
+++ b/src/main/drivers/accgyro_spi_mpu6500.h
@@ -20,6 +20,8 @@
 #define MPU6500_RA_GYRO_CFG                 (0x1B)
 #define MPU6500_RA_ACCEL_CFG                (0x1C)
 #define MPU6500_RA_ACCEL_XOUT_H             (0x3B)
+#define MPU6500_RA_INT_PIN_CFG              (0x37)
+#define MPU6500_RA_INT_ENABLE               (0x38)
 #define MPU6500_RA_GYRO_XOUT_H              (0x43)
 #define MPU6500_RA_SIGNAL_PATH_RST          (0x68)
 #define MPU6500_RA_PWR_MGMT_1               (0x6B)
@@ -33,7 +35,7 @@
 
 #pragma once
 
-bool mpu6500Detect(void);
+bool mpu6500SpiDetect(void);
 
 bool mpu6500SpiAccDetect(acc_t *acc);
 bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index cd13a01a4..5a66f581c 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -26,8 +26,8 @@ typedef enum {
     ACC_MMA8452 = 4,
     ACC_BMA280 = 5,
     ACC_LSM303DLHC = 6,
-    ACC_SPI_MPU6000 = 7,
-    ACC_SPI_MPU6500 = 8,
+    ACC_MPU6000 = 7,
+    ACC_MPU6500 = 8,
     ACC_FAKE = 9,
 } accelerationSensor_e;
 
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index f09e2af5e..d9bc6e210 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -24,8 +24,8 @@ typedef enum {
     GYRO_L3G4200D,
     GYRO_MPU3050,
     GYRO_L3GD20,
-    GYRO_SPI_MPU6000,
-    GYRO_SPI_MPU6500,
+    GYRO_MPU6000,
+    GYRO_MPU6500,
     GYRO_FAKE
 } gyroSensor_e;
 
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index c3f7f668f..a45d94750 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -38,6 +38,7 @@
 #include "drivers/accgyro_mpu.h"
 #include "drivers/accgyro_mpu3050.h"
 #include "drivers/accgyro_mpu6050.h"
+#include "drivers/accgyro_mpu6500.h"
 #include "drivers/accgyro_l3gd20.h"
 #include "drivers/accgyro_lsm303dlhc.h"
 
@@ -218,40 +219,30 @@ bool detectGyro(uint16_t gyroLpf)
 #endif
             ; // fallthrough
 
-        case GYRO_SPI_MPU6000:
+        case GYRO_MPU6000:
 #ifdef USE_GYRO_SPI_MPU6000
             if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
-#ifdef GYRO_SPI_MPU6000_ALIGN
-                gyroHardware = GYRO_SPI_MPU6000;
-                gyroAlign = GYRO_SPI_MPU6000_ALIGN;
+#ifdef GYRO_MPU6000_ALIGN
+                gyroHardware = GYRO_MPU6000;
+                gyroAlign = GYRO_MPU6000_ALIGN;
 #endif
                 break;
             }
 #endif
             ; // fallthrough
 
-        case GYRO_SPI_MPU6500:
-#ifdef USE_GYRO_SPI_MPU6500
+        case GYRO_MPU6500:
+#ifdef USE_GYRO_MPU6500
 #ifdef USE_HARDWARE_REVISION_DETECTION
 		  spiBusInit();
 #endif
-#ifdef NAZE
-            if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
-#ifdef GYRO_SPI_MPU6500_ALIGN
-                gyroHardware = GYRO_SPI_MPU6500;
-                gyroAlign = GYRO_SPI_MPU6500_ALIGN;
+            if (mpu6500GyroDetect(&gyro, gyroLpf) || mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
+#ifdef GYRO_MPU6500_ALIGN
+                gyroHardware = GYRO_MPU6500;
+                gyroAlign = GYRO_MPU6500_ALIGN;
 #endif
                 break;
             }
-#else
-            if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
-#ifdef GYRO_SPI_MPU6500_ALIGN
-                gyroHardware = GYRO_SPI_MPU6500;
-                gyroAlign = GYRO_SPI_MPU6500_ALIGN;
-#endif
-                break;
-            }
-#endif
 #endif
             ; // fallthrough
 
@@ -357,28 +348,24 @@ retry:
             }
 #endif
             ; // fallthrough
-        case ACC_SPI_MPU6000:
+        case ACC_MPU6000:
 #ifdef USE_ACC_SPI_MPU6000
             if (mpu6000SpiAccDetect(&acc)) {
-#ifdef ACC_SPI_MPU6000_ALIGN
-                accAlign = ACC_SPI_MPU6000_ALIGN;
+#ifdef ACC_MPU6000_ALIGN
+                accAlign = ACC_MPU6000_ALIGN;
 #endif
-                accHardware = ACC_SPI_MPU6000;
+                accHardware = ACC_MPU6000;
                 break;
             }
 #endif
             ; // fallthrough
-        case ACC_SPI_MPU6500:
-#ifdef USE_ACC_SPI_MPU6500
-#ifdef NAZE
-            if (hardwareRevision == NAZE32_SP && mpu6500SpiAccDetect(&acc)) {
-#else
-            if (mpu6500SpiAccDetect(&acc)) {
+        case ACC_MPU6500:
+#ifdef USE_ACC_MPU6500
+            if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc)) {
+#ifdef ACC_MPU6500_ALIGN
+                accAlign = ACC_MPU6500_ALIGN;
 #endif
-#ifdef ACC_SPI_MPU6500_ALIGN
-                accAlign = ACC_SPI_MPU6500_ALIGN;
-#endif
-                accHardware = ACC_SPI_MPU6500;
+                accHardware = ACC_MPU6500;
                 break;
             }
 #endif
@@ -599,7 +586,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
     memset(&acc, 0, sizeof(acc));
     memset(&gyro, 0, sizeof(gyro));
 
-#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
+#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
 
     const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
 
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index a79dab592..7f0043d7e 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -47,12 +47,12 @@
 #define GYRO
 #define USE_GYRO_SPI_MPU6000
 
-#define GYRO_SPI_MPU6000_ALIGN CW270_DEG
+#define GYRO_MPU6000_ALIGN CW270_DEG
 
 #define ACC
 #define USE_ACC_SPI_MPU6000
 
-#define ACC_SPI_MPU6000_ALIGN CW270_DEG
+#define ACC_MPU6000_ALIGN CW270_DEG
 
 // External I2C BARO
 #define BARO
diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h
index 3aae6f748..cb35d1074 100755
--- a/src/main/target/COLIBRI_RACE/target.h
+++ b/src/main/target/COLIBRI_RACE/target.h
@@ -57,11 +57,11 @@
 
 #define GYRO
 #define USE_GYRO_SPI_MPU6500
-#define GYRO_SPI_MPU6500_ALIGN CW270_DEG
+#define GYRO_MPU6500_ALIGN CW270_DEG
 
 #define ACC
 #define USE_ACC_SPI_MPU6500
-#define ACC_SPI_MPU6500_ALIGN CW270_DEG
+#define ACC_MPU6500_ALIGN CW270_DEG
 
 #define BARO
 #define USE_BARO_MS5611
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index 8d2c66575..66a5db3b3 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -84,25 +84,27 @@
 #define GYRO
 #define USE_GYRO_MPU3050
 #define USE_GYRO_MPU6050
+#define USE_GYRO_MPU6500
 #define USE_GYRO_SPI_MPU6500
 
 
 #define GYRO_MPU3050_ALIGN CW0_DEG
 #define GYRO_MPU6050_ALIGN CW0_DEG
-#define GYRO_SPI_MPU6500_ALIGN CW0_DEG
+#define GYRO_MPU6500_ALIGN CW0_DEG
 
 #define ACC
 #define USE_ACC_ADXL345
 #define USE_ACC_BMA280
 #define USE_ACC_MMA8452
 #define USE_ACC_MPU6050
+#define USE_ACC_MPU6500
 #define USE_ACC_SPI_MPU6500
 
 #define ACC_ADXL345_ALIGN CW270_DEG
 #define ACC_MPU6050_ALIGN CW0_DEG
 #define ACC_MMA8452_ALIGN CW90_DEG
 #define ACC_BMA280_ALIGN CW0_DEG
-#define ACC_SPI_MPU6500_ALIGN CW0_DEG
+#define ACC_MPU6500_ALIGN CW0_DEG
 
 #define BARO
 #define USE_BARO_MS5611

From 501c83f85199f9207a547b0e951723e66eacf429 Mon Sep 17 00:00:00 2001
From: Richard Lehey <rlehey+gh@gmail.com>
Date: Tue, 2 Jun 2015 01:48:13 +0900
Subject: [PATCH 13/41] Add support for new BMP280 barometer

---
 src/main/drivers/barometer_bmp280.c | 212 ++++++++++++++++++++++++++++
 src/main/drivers/barometer_bmp280.h |  21 +++
 src/main/io/serial_cli.c            |   2 +-
 src/main/sensors/barometer.h        |   3 +-
 src/main/sensors/initialisation.c   |   9 ++
 5 files changed, 245 insertions(+), 2 deletions(-)
 create mode 100644 src/main/drivers/barometer_bmp280.c
 create mode 100644 src/main/drivers/barometer_bmp280.h

diff --git a/src/main/drivers/barometer_bmp280.c b/src/main/drivers/barometer_bmp280.c
new file mode 100644
index 000000000..3edb55c5f
--- /dev/null
+++ b/src/main/drivers/barometer_bmp280.c
@@ -0,0 +1,212 @@
+/*
+ * 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/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <platform.h>
+
+#include "build_config.h"
+
+#include "barometer.h"
+
+#include "system.h"
+#include "bus_i2c.h"
+
+#include "barometer_bmp280.h"
+
+#ifdef BARO
+
+// BMP280, address 0x76
+
+#define BMP280_I2C_ADDR                      (0x76)
+#define BMP280_DEFAULT_CHIP_ID               (0x58)
+
+#define BMP280_CHIP_ID_REG                   (0xD0)  /* Chip ID Register */
+#define BMP280_RST_REG                       (0xE0)  /* Softreset Register */
+#define BMP280_STAT_REG                      (0xF3)  /* Status Register */
+#define BMP280_CTRL_MEAS_REG                 (0xF4)  /* Ctrl Measure Register */
+#define BMP280_CONFIG_REG                    (0xF5)  /* Configuration Register */
+#define BMP280_PRESSURE_MSB_REG              (0xF7)  /* Pressure MSB Register */
+#define BMP280_PRESSURE_LSB_REG              (0xF8)  /* Pressure LSB Register */
+#define BMP280_PRESSURE_XLSB_REG             (0xF9)  /* Pressure XLSB Register */
+#define BMP280_TEMPERATURE_MSB_REG           (0xFA)  /* Temperature MSB Reg */
+#define BMP280_TEMPERATURE_LSB_REG           (0xFB)  /* Temperature LSB Reg */
+#define BMP280_TEMPERATURE_XLSB_REG          (0xFC)  /* Temperature XLSB Reg */
+#define BMP280_FORCED_MODE                   (0x01)
+
+#define BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG             (0x88)
+#define BMP280_PRESSURE_TEMPERATURE_CALIB_DATA_LENGTH       (24)
+#define BMP280_DATA_FRAME_SIZE               (6)
+
+#define BMP280_OVERSAMP_SKIPPED          (0x00)
+#define BMP280_OVERSAMP_1X               (0x01)
+#define BMP280_OVERSAMP_2X               (0x02)
+#define BMP280_OVERSAMP_4X               (0x03)
+#define BMP280_OVERSAMP_8X               (0x04)
+#define BMP280_OVERSAMP_16X              (0x05)
+
+// configure pressure and temperature oversampling, forced sampling mode
+#define BMP280_PRESSURE_OSR              (BMP280_OVERSAMP_8X)
+#define BMP280_TEMPERATURE_OSR           (BMP280_OVERSAMP_1X)
+#define BMP280_MODE                      (BMP280_PRESSURE_OSR << 2 | BMP280_TEMPERATURE_OSR << 5 | BMP280_FORCED_MODE)
+
+#define T_INIT_MAX                       (20)
+// 20/16 = 1.25 ms
+#define T_MEASURE_PER_OSRS_MAX           (37)
+// 37/16 = 2.3125 ms
+#define T_SETUP_PRESSURE_MAX             (10)
+// 10/16 = 0.625 ms
+
+typedef struct bmp280_calib_param_t {
+    uint16_t dig_T1; /* calibration T1 data */
+    int16_t dig_T2; /* calibration T2 data */
+    int16_t dig_T3; /* calibration T3 data */
+    uint16_t dig_P1; /* calibration P1 data */
+    int16_t dig_P2; /* calibration P2 data */
+    int16_t dig_P3; /* calibration P3 data */
+    int16_t dig_P4; /* calibration P4 data */
+    int16_t dig_P5; /* calibration P5 data */
+    int16_t dig_P6; /* calibration P6 data */
+    int16_t dig_P7; /* calibration P7 data */
+    int16_t dig_P8; /* calibration P8 data */
+    int16_t dig_P9; /* calibration P9 data */
+    int32_t t_fine; /* calibration t_fine data */
+} bmp280_calib_param_t;
+
+static uint8_t bmp280_chip_id = 0;
+static bool bmp280InitDone = false;
+static bmp280_calib_param_t bmp280_cal;
+// uncompensated pressure and temperature
+static int32_t bmp280_up = 0;
+static int32_t bmp280_ut = 0;
+
+static void bmp280_start_ut(void);
+static void bmp280_get_ut(void);
+static void bmp280_start_up(void);
+static void bmp280_get_up(void);
+static void bmp280_calculate(int32_t *pressure, int32_t *temperature);
+
+bool bmp280Detect(baro_t *baro)
+{
+    if (bmp280InitDone)
+        return true;
+
+    delay(20);
+
+    i2cRead(BMP280_I2C_ADDR, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id);  /* read Chip Id */
+    if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID)
+        return false;
+
+    // read calibration
+    i2cRead(BMP280_I2C_ADDR, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal);
+    // set oversampling + power mode (forced), and start sampling
+    i2cWrite(BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
+
+    bmp280InitDone = true;
+
+    // these are dummy as temperature is measured as part of pressure
+    baro->ut_delay = 0;
+    baro->get_ut = bmp280_get_ut;
+    baro->start_ut = bmp280_start_ut;
+
+    // only _up part is executed, and gets both temperature and pressure
+    baro->up_delay = ((T_INIT_MAX + T_MEASURE_PER_OSRS_MAX * (((1 << BMP280_TEMPERATURE_OSR) >> 1) + ((1 << BMP280_PRESSURE_OSR) >> 1)) + (BMP280_PRESSURE_OSR ? T_SETUP_PRESSURE_MAX : 0) + 15) / 16) * 1000;
+    baro->start_up = bmp280_start_up;
+    baro->get_up = bmp280_get_up;
+    baro->calculate = bmp280_calculate;
+
+    return true;
+}
+
+static void bmp280_start_ut(void)
+{
+    // dummy
+}
+
+static void bmp280_get_ut(void)
+{
+    // dummy
+}
+
+static void bmp280_start_up(void)
+{
+    // start measurement
+    // set oversampling + power mode (forced), and start sampling
+    i2cWrite(BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
+}
+
+static void bmp280_get_up(void)
+{
+    uint8_t data[BMP280_DATA_FRAME_SIZE];
+
+    // read data from sensor
+    i2cRead(BMP280_I2C_ADDR, BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data);
+    bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4));
+    bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4));
+}
+
+// Returns temperature in DegC, float precision. Output value of “51.23” equals 51.23 DegC.
+// t_fine carries fine temperature as global value
+float bmp280_compensate_T(int32_t adc_T)
+{
+    float var1, var2, T;
+
+    var1 = (((float)adc_T) / 16384.0f - ((float)bmp280_cal.dig_T1) / 1024.0f) * ((float)bmp280_cal.dig_T2);
+    var2 = ((((float)adc_T) / 131072.0f - ((float)bmp280_cal.dig_T1) / 8192.0f) * (((float)adc_T) / 131072.0f - ((float)bmp280_cal.dig_T1) / 8192.0f)) * ((float)bmp280_cal.dig_T3);
+    bmp280_cal.t_fine = (int32_t)(var1 + var2);
+    T = (var1 + var2) / 5120.0f;
+
+    return T;
+}
+
+// Returns pressure in Pa as float. Output value of “96386.2” equals 96386.2 Pa = 963.862 hPa
+float bmp280_compensate_P(int32_t adc_P)
+{
+    float var1, var2, p;
+    var1 = ((float)bmp280_cal.t_fine / 2.0f) - 64000.0f;
+    var2 = var1 * var1 * ((float)bmp280_cal.dig_P6) / 32768.0f;
+    var2 = var2 + var1 * ((float)bmp280_cal.dig_P5) * 2.0f;
+    var2 = (var2 / 4.0f) + (((float)bmp280_cal.dig_P4) * 65536.0f);
+    var1 = (((float)bmp280_cal.dig_P3) * var1 * var1 / 524288.0f + ((float)bmp280_cal.dig_P2) * var1) / 524288.0f;
+    var1 = (1.0f + var1 / 32768.0f) * ((float)bmp280_cal.dig_P1);
+    if (var1 == 0.0f)
+        return 0.0f; // avoid exception caused by division by zero
+
+    p = 1048576.0f - (float)adc_P;
+    p = (p - (var2 / 4096.0f)) * 6250.0f / var1;
+    var1 = ((float)bmp280_cal.dig_P9) * p * p / 2147483648.0f;
+    var2 = p * ((float)bmp280_cal.dig_P8) / 32768.0f;
+    p = p + (var1 + var2 + ((float)bmp280_cal.dig_P7)) / 16.0f;
+
+    return p;
+}
+
+static void bmp280_calculate(int32_t *pressure, int32_t *temperature)
+{
+    // calculate
+    float t, p;
+    t = bmp280_compensate_T(bmp280_ut);
+    p = bmp280_compensate_P(bmp280_up);
+
+    if (pressure)
+        *pressure = (int32_t)p;
+    if (temperature)
+        *temperature = (int32_t)t * 100;
+}
+
+#endif
diff --git a/src/main/drivers/barometer_bmp280.h b/src/main/drivers/barometer_bmp280.h
new file mode 100644
index 000000000..e6858ed10
--- /dev/null
+++ b/src/main/drivers/barometer_bmp280.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
+
+bool bmp280Detect(baro_t *baro);
+
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index eb083821d..812d69470 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -192,7 +192,7 @@ static const char * const sensorTypeNames[] = {
 static const char * const sensorHardwareNames[4][11] = {
     { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE", NULL },
     { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", NULL },
-    { "", "None", "BMP085", "MS5611", NULL },
+    { "", "None", "BMP085", "MS5611", "BMP280", NULL },
     { "", "None", "HMC5883", "AK8975", NULL }
 };
 #endif
diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h
index f8befc2f1..b992df63d 100644
--- a/src/main/sensors/barometer.h
+++ b/src/main/sensors/barometer.h
@@ -21,7 +21,8 @@ typedef enum {
     BARO_DEFAULT = 0,
     BARO_NONE = 1,
     BARO_BMP085 = 2,
-    BARO_MS5611 = 3
+    BARO_MS5611 = 3,
+    BARO_BMP280 = 4
 } baroSensor_e;
 
 #define BARO_SAMPLE_COUNT_MAX   48
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index a45d94750..4edc5d70c 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -48,6 +48,7 @@
 
 #include "drivers/barometer.h"
 #include "drivers/barometer_bmp085.h"
+#include "drivers/barometer_bmp280.h"
 #include "drivers/barometer_ms5611.h"
 
 #include "drivers/compass.h"
@@ -448,6 +449,14 @@ static void detectBaro(baroSensor_e baroHardwareToUse)
                 baroHardware = BARO_BMP085;
                 break;
             }
+#endif
+	    ; // fallthough
+        case BARO_BMP280:
+#ifdef USE_BARO_BMP280
+            if (bmp280Detect(&baro)) {
+                baroHardware = BARO_BMP280;
+                break;
+            }
 #endif
         case BARO_NONE:
             baroHardware = BARO_NONE;

From 6fb500f58033f04a2685bb753ac81d9376ecc30c Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Sat, 19 Sep 2015 18:38:02 +0100
Subject: [PATCH 14/41] NAZE32 - Add support for BMP280.

---
 Makefile                      | 1 +
 src/main/target/NAZE/target.h | 1 +
 2 files changed, 2 insertions(+)

diff --git a/Makefile b/Makefile
index a71801486..893b97fea 100755
--- a/Makefile
+++ b/Makefile
@@ -301,6 +301,7 @@ NAZE_SRC	 = startup_stm32f10x_md_gcc.S \
 		   drivers/adc_stm32f10x.c \
 		   drivers/barometer_bmp085.c \
 		   drivers/barometer_ms5611.c \
+		   drivers/barometer_bmp280.c \
 		   drivers/bus_spi.c \
 		   drivers/bus_i2c_stm32f10x.c \
 		   drivers/compass_hmc5883l.c \
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index 66a5db3b3..52d9c5955 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -109,6 +109,7 @@
 #define BARO
 #define USE_BARO_MS5611
 #define USE_BARO_BMP085
+#define USE_BARO_BMP280
 
 #define MAG
 #define USE_MAG_HMC5883

From 06ceac06146452b3c041e65056c48aae73e82ad2 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Sat, 19 Sep 2015 19:01:35 +0100
Subject: [PATCH 15/41] Remove unused variable warnings when no SPI sensors are
 used.

---
 src/main/drivers/accgyro_mpu.c | 8 ++------
 1 file changed, 2 insertions(+), 6 deletions(-)

diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
index f5b77c2a0..1c61e00ba 100644
--- a/src/main/drivers/accgyro_mpu.c
+++ b/src/main/drivers/accgyro_mpu.c
@@ -134,11 +134,8 @@ mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
 #ifdef USE_SPI
 static bool detectSPISensorsAndUpdateDetectionResult(void)
 {
-    bool found = false;
-
 #ifdef USE_GYRO_SPI_MPU6500
-    found = mpu6500SpiDetect();
-    if (found) {
+    if (mpu6500SpiDetect()) {
         mpuDetectionResult.sensor = MPU_65xx_SPI;
         mpuConfiguration.gyroReadXRegister = MPU6500_RA_GYRO_XOUT_H;
         mpuConfiguration.read = mpu6500ReadRegister;
@@ -148,8 +145,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(void)
 #endif
 
 #ifdef USE_GYRO_SPI_MPU6000
-    found = mpu6000SpiDetect();
-    if (found) {
+    if (mpu6000SpiDetect()) {
         mpuDetectionResult.sensor = MPU_60x0_SPI;
         mpuConfiguration.gyroReadXRegister = MPU6000_GYRO_XOUT_H;
         mpuConfiguration.read = mpu6000ReadRegister;

From aac13914f9e631c5ff56caa637d29a333480c5d2 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 21 Sep 2015 14:16:21 +0100
Subject: [PATCH 16/41] LPF setting is not needed to detect the gyro sensor,
 only when it's initialised; now the lpf setting is passed to gyroInit().

This saves a bit of code size and ram as well as making the code
cleaner.
---
 src/main/drivers/accgyro.h             |  2 +-
 src/main/drivers/accgyro_l3g4200d.c    | 23 +++++++--------
 src/main/drivers/accgyro_l3g4200d.h    |  2 +-
 src/main/drivers/accgyro_l3gd20.c      |  8 +++---
 src/main/drivers/accgyro_l3gd20.h      |  2 +-
 src/main/drivers/accgyro_mpu.c         | 10 +++----
 src/main/drivers/accgyro_mpu.h         |  4 ++-
 src/main/drivers/accgyro_mpu3050.c     | 13 ++++-----
 src/main/drivers/accgyro_mpu3050.h     |  3 +-
 src/main/drivers/accgyro_mpu6050.c     | 14 ++++-----
 src/main/drivers/accgyro_mpu6050.h     |  2 +-
 src/main/drivers/accgyro_mpu6500.c     | 13 +++------
 src/main/drivers/accgyro_mpu6500.h     |  4 +--
 src/main/drivers/accgyro_spi_mpu6000.c |  9 +++---
 src/main/drivers/accgyro_spi_mpu6000.h |  2 +-
 src/main/drivers/accgyro_spi_mpu6500.c |  7 +----
 src/main/drivers/accgyro_spi_mpu6500.h |  2 +-
 src/main/drivers/sensor.h              |  1 +
 src/main/flight/mixer.h                |  7 +++--
 src/main/sensors/initialisation.c      | 40 +++++++++++++++-----------
 20 files changed, 83 insertions(+), 85 deletions(-)

diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h
index 8ebbe3414..07157cbed 100644
--- a/src/main/drivers/accgyro.h
+++ b/src/main/drivers/accgyro.h
@@ -20,7 +20,7 @@
 extern uint16_t acc_1G; // FIXME move into acc_t
 
 typedef struct gyro_s {
-    sensorInitFuncPtr init;                                 // initialize function
+    sensorGyroInitFuncPtr init;                             // initialize function
     sensorReadFuncPtr read;                                 // read 3 axis data function
     sensorReadFuncPtr temperature;                          // read temperature if available
     float scale;                                            // scalefactor
diff --git a/src/main/drivers/accgyro_l3g4200d.c b/src/main/drivers/accgyro_l3g4200d.c
index 82bd43e76..f4814bc67 100644
--- a/src/main/drivers/accgyro_l3g4200d.c
+++ b/src/main/drivers/accgyro_l3g4200d.c
@@ -54,12 +54,10 @@
 #define L3G4200D_DLPF_78HZ       0x80
 #define L3G4200D_DLPF_93HZ       0xC0
 
-static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
-
-static void l3g4200dInit(void);
+static void l3g4200dInit(uint16_t lpf);
 static bool l3g4200dRead(int16_t *gyroADC);
 
-bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
+bool l3g4200dDetect(gyro_t *gyro)
 {
     uint8_t deviceid;
 
@@ -75,7 +73,15 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
     // 14.2857dps/lsb scalefactor
     gyro->scale = 1.0f / 14.2857f;
 
-    // default LPF is set to 32Hz
+    return true;
+}
+
+static void l3g4200dInit(uint16_t lpf)
+{
+    bool ack;
+
+    uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
+
     switch (lpf) {
         default:
             case 32:
@@ -92,13 +98,6 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
             break;
     }
 
-    return true;
-}
-
-static void l3g4200dInit(void)
-{
-    bool ack;
-
     delay(100);
 
     ack = i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);
diff --git a/src/main/drivers/accgyro_l3g4200d.h b/src/main/drivers/accgyro_l3g4200d.h
index 8cbd51593..40c05f6e4 100644
--- a/src/main/drivers/accgyro_l3g4200d.h
+++ b/src/main/drivers/accgyro_l3g4200d.h
@@ -17,4 +17,4 @@
 
 #pragma once
 
-bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf);
+bool l3g4200dDetect(gyro_t *gyro);
diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c
index fd56d2512..6cc0b9ae9 100644
--- a/src/main/drivers/accgyro_l3gd20.c
+++ b/src/main/drivers/accgyro_l3gd20.c
@@ -86,8 +86,10 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
     spiSetDivisor(L3GD20_SPI, SPI_9MHZ_CLOCK_DIVIDER);
 }
 
-void l3gd20GyroInit(void)
+void l3gd20GyroInit(uint16_t lpf)
 {
+    UNUSED(lpf); // FIXME use it!
+
     l3gd20SpiInit(L3GD20_SPI);
 
     GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN);
@@ -150,10 +152,8 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
 // Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
 #define L3GD20_GYRO_SCALE_FACTOR  0.07f
 
-bool l3gd20Detect(gyro_t *gyro, uint16_t lpf)
+bool l3gd20Detect(gyro_t *gyro)
 {
-    UNUSED(lpf);
-
     gyro->init = l3gd20GyroInit;
     gyro->read = l3gd20GyroRead;
 
diff --git a/src/main/drivers/accgyro_l3gd20.h b/src/main/drivers/accgyro_l3gd20.h
index 0e1766dd5..11b1241e9 100644
--- a/src/main/drivers/accgyro_l3gd20.h
+++ b/src/main/drivers/accgyro_l3gd20.h
@@ -17,4 +17,4 @@
 
 #pragma once
 
-bool l3gd20Detect(gyro_t *gyro, uint16_t lpf);
+bool l3gd20Detect(gyro_t *gyro);
diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
index 1c61e00ba..d381540fc 100644
--- a/src/main/drivers/accgyro_mpu.c
+++ b/src/main/drivers/accgyro_mpu.c
@@ -59,10 +59,6 @@ mpuDetectionResult_t mpuDetectionResult;
 mpuConfiguration_t mpuConfiguration;
 static const extiConfig_t *mpuIntExtiConfig = NULL;
 
-// FIXME move into mpuConfiguration
-uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
-
-
 #define MPU_ADDRESS             0x68
 
 // MPU6050
@@ -299,8 +295,10 @@ void mpuIntExtiInit(void)
     mpuExtiInitDone = true;
 }
 
-void configureMPULPF(uint16_t lpf)
+uint8_t determineMPULPF(uint16_t lpf)
 {
+    uint8_t mpuLowPassFilter;
+
     if (lpf == 256)
         mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
     else if (lpf >= 188)
@@ -317,6 +315,8 @@ void configureMPULPF(uint16_t lpf)
         mpuLowPassFilter = INV_FILTER_5HZ;
     else
         mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
+
+    return mpuLowPassFilter;
 }
 
 static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h
index 5808e65ec..c7af60b3e 100644
--- a/src/main/drivers/accgyro_mpu.h
+++ b/src/main/drivers/accgyro_mpu.h
@@ -78,7 +78,9 @@ typedef struct mpuDetectionResult_s {
     mpu6050Resolution_e resolution;
 } mpuDetectionResult_t;
 
-void configureMPULPF(uint16_t lpf);
+extern mpuDetectionResult_t mpuDetectionResult;
+
+uint8_t determineMPULPF(uint16_t lpf);
 void configureMPUDataReadyInterruptHandling(void);
 void mpuIntExtiInit(void);
 bool mpuAccRead(int16_t *accData);
diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c
index 4a621109c..c0970cd39 100644
--- a/src/main/drivers/accgyro_mpu3050.c
+++ b/src/main/drivers/accgyro_mpu3050.c
@@ -31,9 +31,6 @@
 #include "accgyro_mpu.h"
 #include "accgyro_mpu3050.h"
 
-extern mpuDetectionResult_t mpuDetectionResult;
-extern uint8_t mpuLowPassFilter;
-
 // MPU3050, Standard address 0x68
 #define MPU3050_ADDRESS         0x68
 
@@ -49,10 +46,10 @@ extern uint8_t mpuLowPassFilter;
 #define MPU3050_USER_RESET      0x01
 #define MPU3050_CLK_SEL_PLL_GX  0x01
 
-void mpu3050Init(void);
+static void mpu3050Init(uint16_t lpf);
 static bool mpu3050ReadTemp(int16_t *tempData);
 
-bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
+bool mpu3050Detect(gyro_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_3050) {
         return false;
@@ -64,15 +61,15 @@ bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
     // 16.4 dps/lsb scalefactor
     gyro->scale = 1.0f / 16.4f;
 
-    configureMPULPF(lpf);
-
     return true;
 }
 
-void mpu3050Init(void)
+static void mpu3050Init(uint16_t lpf)
 {
     bool ack;
 
+    uint8_t mpuLowPassFilter = determineMPULPF(lpf);
+
     delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
 
     ack = mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0);
diff --git a/src/main/drivers/accgyro_mpu3050.h b/src/main/drivers/accgyro_mpu3050.h
index c2f125be5..fd4c28e10 100644
--- a/src/main/drivers/accgyro_mpu3050.h
+++ b/src/main/drivers/accgyro_mpu3050.h
@@ -26,5 +26,4 @@
 #define MPU3050_USER_CTRL       0x3D
 #define MPU3050_PWR_MGM         0x3E
 
-void mpu3050Init(void);
-bool mpu3050Detect(gyro_t *gyro, uint16_t lpf);
+bool mpu3050Detect(gyro_t *gyro);
diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c
index c69e9f105..c235fe5fb 100644
--- a/src/main/drivers/accgyro_mpu6050.c
+++ b/src/main/drivers/accgyro_mpu6050.c
@@ -37,7 +37,6 @@
 #include "accgyro_mpu.h"
 #include "accgyro_mpu6050.h"
 
-extern mpuDetectionResult_t mpuDetectionResult;
 extern uint8_t mpuLowPassFilter;
 
 //#define DEBUG_MPU_DATA_READY_INTERRUPT
@@ -146,7 +145,7 @@ extern uint8_t mpuLowPassFilter;
 #define MPU6050_SMPLRT_DIV      0       // 8000Hz
 
 static void mpu6050AccInit(void);
-static void mpu6050GyroInit(void);
+static void mpu6050GyroInit(uint16_t lpf);
 
 bool mpu6050AccDetect(acc_t *acc)
 {
@@ -161,7 +160,7 @@ bool mpu6050AccDetect(acc_t *acc)
     return true;
 }
 
-bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf)
+bool mpu6050GyroDetect(gyro_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_60x0) {
         return false;
@@ -172,8 +171,6 @@ bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf)
     // 16.4 dps/lsb scalefactor
     gyro->scale = 1.0f / 16.4f;
 
-    configureMPULPF(lpf);
-
     return true;
 }
 
@@ -191,11 +188,14 @@ static void mpu6050AccInit(void)
     }
 }
 
-static void mpu6050GyroInit(void)
+static void mpu6050GyroInit(uint16_t lpf)
 {
+    bool ack;
+
     mpuIntExtiInit();
 
-    bool ack;
+    uint8_t mpuLowPassFilter = determineMPULPF(lpf);
+
     ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
     delay(100);
     ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
diff --git a/src/main/drivers/accgyro_mpu6050.h b/src/main/drivers/accgyro_mpu6050.h
index 83c1caab8..d8649a3c6 100644
--- a/src/main/drivers/accgyro_mpu6050.h
+++ b/src/main/drivers/accgyro_mpu6050.h
@@ -18,4 +18,4 @@
 #pragma once
 
 bool mpu6050AccDetect(acc_t *acc);
-bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf);
+bool mpu6050GyroDetect(gyro_t *gyro);
diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c
index a0d114a9e..e614a1856 100644
--- a/src/main/drivers/accgyro_mpu6500.c
+++ b/src/main/drivers/accgyro_mpu6500.c
@@ -33,13 +33,8 @@
 #include "accgyro_mpu.h"
 #include "accgyro_mpu6500.h"
 
-extern mpuDetectionResult_t mpuDetectionResult;
-extern uint8_t mpuLowPassFilter;
-
-
 extern uint16_t acc_1G;
 
-
 bool mpu6500AccDetect(acc_t *acc)
 {
     if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
@@ -52,7 +47,7 @@ bool mpu6500AccDetect(acc_t *acc)
     return true;
 }
 
-bool mpu6500GyroDetect(gyro_t *gyro, uint16_t lpf)
+bool mpu6500GyroDetect(gyro_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
         return false;
@@ -64,8 +59,6 @@ bool mpu6500GyroDetect(gyro_t *gyro, uint16_t lpf)
     // 16.4 dps/lsb scalefactor
     gyro->scale = 1.0f / 16.4f;
 
-    configureMPULPF(lpf);
-
     return true;
 }
 
@@ -74,7 +67,7 @@ void mpu6500AccInit(void)
     acc_1G = 512 * 8;
 }
 
-void mpu6500GyroInit(void)
+void mpu6500GyroInit(uint16_t lpf)
 {
 #ifdef NAZE
     // FIXME target specific code in driver code.
@@ -89,6 +82,8 @@ void mpu6500GyroInit(void)
     }
 #endif
 
+    uint8_t mpuLowPassFilter = determineMPULPF(lpf);
+
     mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
     delay(100);
     mpuConfiguration.write(MPU6500_RA_SIGNAL_PATH_RST, 0x07);
diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h
index 3a8a389c8..4c8455ff6 100644
--- a/src/main/drivers/accgyro_mpu6500.h
+++ b/src/main/drivers/accgyro_mpu6500.h
@@ -36,7 +36,7 @@
 #pragma once
 
 bool mpu6500AccDetect(acc_t *acc);
-bool mpu6500GyroDetect(gyro_t *gyro, uint16_t lpf);
+bool mpu6500GyroDetect(gyro_t *gyro);
 
 void mpu6500AccInit(void);
-void mpu6500GyroInit(void);
+void mpu6500GyroInit(uint16_t lpf);
diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c
index e71e35014..177b646e7 100644
--- a/src/main/drivers/accgyro_spi_mpu6000.c
+++ b/src/main/drivers/accgyro_spi_mpu6000.c
@@ -43,7 +43,6 @@
 
 static void mpu6000AccAndGyroInit(void);
 
-extern mpuDetectionResult_t mpuDetectionResult;
 extern uint8_t mpuLowPassFilter;
 
 static bool mpuSpi6000InitDone = false;
@@ -121,8 +120,10 @@ bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
     return true;
 }
 
-void mpu6000SpiGyroInit(void)
+void mpu6000SpiGyroInit(uint16_t lpf)
 {
+    uint8_t mpuLowPassFilter = determineMPULPF(lpf);
+
     mpu6000AccAndGyroInit();
 
     spiResetErrorCounter(MPU6000_SPI_INSTANCE);
@@ -249,7 +250,7 @@ bool mpu6000SpiAccDetect(acc_t *acc)
     return true;
 }
 
-bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
+bool mpu6000SpiGyroDetect(gyro_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
         return false;
@@ -261,7 +262,5 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
     // 16.4 dps/lsb scalefactor
     gyro->scale = 1.0f / 16.4f;
 
-    configureMPULPF(lpf);
-
     return true;
 }
diff --git a/src/main/drivers/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro_spi_mpu6000.h
index 67618d8f8..ecadc23c7 100644
--- a/src/main/drivers/accgyro_spi_mpu6000.h
+++ b/src/main/drivers/accgyro_spi_mpu6000.h
@@ -47,7 +47,7 @@
 bool mpu6000SpiDetect(void);
 
 bool mpu6000SpiAccDetect(acc_t *acc);
-bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
+bool mpu6000SpiGyroDetect(gyro_t *gyro);
 
 bool mpu6000WriteRegister(uint8_t reg, uint8_t data);
 bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c
index 6eb7864b5..faafd69a5 100755
--- a/src/main/drivers/accgyro_spi_mpu6500.c
+++ b/src/main/drivers/accgyro_spi_mpu6500.c
@@ -38,9 +38,6 @@
 #define DISABLE_MPU6500       GPIO_SetBits(MPU6500_CS_GPIO,   MPU6500_CS_PIN)
 #define ENABLE_MPU6500        GPIO_ResetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
 
-extern mpuDetectionResult_t mpuDetectionResult;
-extern uint8_t mpuLowPassFilter;
-
 extern uint16_t acc_1G;
 
 bool mpu6500WriteRegister(uint8_t reg, uint8_t data)
@@ -128,7 +125,7 @@ bool mpu6500SpiAccDetect(acc_t *acc)
     return true;
 }
 
-bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
+bool mpu6500SpiGyroDetect(gyro_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
         return false;
@@ -140,8 +137,6 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
     // 16.4 dps/lsb scalefactor
     gyro->scale = 1.0f / 16.4f;
 
-    configureMPULPF(lpf);
-
     return true;
 }
 
diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h
index 7c1c6e88f..967f65cd4 100755
--- a/src/main/drivers/accgyro_spi_mpu6500.h
+++ b/src/main/drivers/accgyro_spi_mpu6500.h
@@ -38,7 +38,7 @@
 bool mpu6500SpiDetect(void);
 
 bool mpu6500SpiAccDetect(acc_t *acc);
-bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
+bool mpu6500SpiGyroDetect(gyro_t *gyro);
 
 bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
 bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h
index e2e30b779..ff0327f14 100644
--- a/src/main/drivers/sensor.h
+++ b/src/main/drivers/sensor.h
@@ -19,3 +19,4 @@
 
 typedef void (*sensorInitFuncPtr)(void);                    // sensor init prototype
 typedef bool (*sensorReadFuncPtr)(int16_t *data);           // sensor read and align prototype
+typedef void (*sensorGyroInitFuncPtr)(uint16_t lpf);         // gyro sensor init prototype
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 026e7d335..07f31c4cf 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -194,16 +194,19 @@ void filterServos(void);
 extern int16_t motor[MAX_SUPPORTED_MOTORS];
 extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 
+struct escAndServoConfig_s;
+struct rxConfig_s;
+
 void mixerUseConfigs(
 #ifdef USE_SERVOS
         servoParam_t *servoConfToUse,
         struct gimbalConfig_s *gimbalConfigToUse,
 #endif
         flight3DConfig_t *flight3DConfigToUse,
-		struct escAndServoConfig_s *escAndServoConfigToUse,
+        struct escAndServoConfig_s *escAndServoConfigToUse,
         mixerConfig_t *mixerConfigToUse,
         airplaneConfig_t *airplaneConfigToUse,
-		struct rxConfig_s *rxConfigToUse);
+        struct rxConfig_s *rxConfigToUse);
 
 void writeAllMotors(int16_t mc);
 void mixerLoadMix(int index, motorMixer_t *customMixers);
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 4edc5d70c..4e0dd330e 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -128,19 +128,25 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
 }
 
 #ifdef USE_FAKE_GYRO
-static void fakeGyroInit(void) {}
-static bool fakeGyroRead(int16_t *gyroADC) {
+static void fakeGyroInit(uint16_t lpf)
+{
+    UNUSED(lpf);
+}
+
+static bool fakeGyroRead(int16_t *gyroADC)
+{
     memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
     return true;
 }
-static bool fakeGyroReadTemp(int16_t *tempData) {
+
+static bool fakeGyroReadTemp(int16_t *tempData)
+{
     UNUSED(tempData);
     return true;
 }
 
-bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
+bool fakeGyroDetect(gyro_t *gyro)
 {
-    UNUSED(lpf);
     gyro->init = fakeGyroInit;
     gyro->read = fakeGyroRead;
     gyro->temperature = fakeGyroReadTemp;
@@ -164,7 +170,7 @@ bool fakeAccDetect(acc_t *acc)
 }
 #endif
 
-bool detectGyro(uint16_t gyroLpf)
+bool detectGyro(void)
 {
     gyroSensor_e gyroHardware = GYRO_DEFAULT;
 
@@ -175,7 +181,7 @@ bool detectGyro(uint16_t gyroLpf)
             ; // fallthrough
         case GYRO_MPU6050:
 #ifdef USE_GYRO_MPU6050
-            if (mpu6050GyroDetect(&gyro, gyroLpf)) {
+            if (mpu6050GyroDetect(&gyro)) {
 #ifdef GYRO_MPU6050_ALIGN
                 gyroHardware = GYRO_MPU6050;
                 gyroAlign = GYRO_MPU6050_ALIGN;
@@ -186,7 +192,7 @@ bool detectGyro(uint16_t gyroLpf)
             ; // fallthrough
         case GYRO_L3G4200D:
 #ifdef USE_GYRO_L3G4200D
-            if (l3g4200dDetect(&gyro, gyroLpf)) {
+            if (l3g4200dDetect(&gyro)) {
 #ifdef GYRO_L3G4200D_ALIGN
                 gyroHardware = GYRO_L3G4200D;
                 gyroAlign = GYRO_L3G4200D_ALIGN;
@@ -198,7 +204,7 @@ bool detectGyro(uint16_t gyroLpf)
 
         case GYRO_MPU3050:
 #ifdef USE_GYRO_MPU3050
-            if (mpu3050Detect(&gyro, gyroLpf)) {
+            if (mpu3050Detect(&gyro)) {
 #ifdef GYRO_MPU3050_ALIGN
                 gyroHardware = GYRO_MPU3050;
                 gyroAlign = GYRO_MPU3050_ALIGN;
@@ -210,7 +216,7 @@ bool detectGyro(uint16_t gyroLpf)
 
         case GYRO_L3GD20:
 #ifdef USE_GYRO_L3GD20
-            if (l3gd20Detect(&gyro, gyroLpf)) {
+            if (l3gd20Detect(&gyro)) {
 #ifdef GYRO_L3GD20_ALIGN
                 gyroHardware = GYRO_L3GD20;
                 gyroAlign = GYRO_L3GD20_ALIGN;
@@ -222,7 +228,7 @@ bool detectGyro(uint16_t gyroLpf)
 
         case GYRO_MPU6000:
 #ifdef USE_GYRO_SPI_MPU6000
-            if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
+            if (mpu6000SpiGyroDetect(&gyro)) {
 #ifdef GYRO_MPU6000_ALIGN
                 gyroHardware = GYRO_MPU6000;
                 gyroAlign = GYRO_MPU6000_ALIGN;
@@ -237,7 +243,7 @@ bool detectGyro(uint16_t gyroLpf)
 #ifdef USE_HARDWARE_REVISION_DETECTION
 		  spiBusInit();
 #endif
-            if (mpu6500GyroDetect(&gyro, gyroLpf) || mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
+            if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro)) {
 #ifdef GYRO_MPU6500_ALIGN
                 gyroHardware = GYRO_MPU6500;
                 gyroAlign = GYRO_MPU6500_ALIGN;
@@ -249,7 +255,7 @@ bool detectGyro(uint16_t gyroLpf)
 
         case GYRO_FAKE:
 #ifdef USE_FAKE_GYRO
-            if (fakeGyroDetect(&gyro, gyroLpf)) {
+            if (fakeGyroDetect(&gyro)) {
                 gyroHardware = GYRO_FAKE;
                 break;
             }
@@ -403,7 +409,9 @@ retry:
 
 static void detectBaro(baroSensor_e baroHardwareToUse)
 {
-#ifdef BARO
+#ifndef BARO
+    UNUSED(baroHardwareToUse);
+#else
     // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
 
     baroSensor_e baroHardware = baroHardwareToUse;
@@ -603,7 +611,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
     UNUSED(mpuDetectionResult);
 #endif
 
-    if (!detectGyro(gyroLpf)) {
+    if (!detectGyro()) {
         return false;
     }
     detectAcc(accHardwareToUse);
@@ -614,7 +622,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
     if (sensors(SENSOR_ACC))
         acc.init();
     // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
-    gyro.init();
+    gyro.init(gyroLpf);
 
     detectMag(magHardwareToUse);
 

From 07d985ba5c9e54a31bdf18171bfdd3282166e73c Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Wed, 23 Sep 2015 01:02:39 +0100
Subject: [PATCH 17/41] More MPU cleanups.

---
 src/main/drivers/accgyro_mpu.c            |  4 ++--
 src/main/drivers/accgyro_mpu6500.h        |  2 ++
 src/main/drivers/accgyro_spi_mpu6500.h    | 18 ------------------
 src/main/sensors/initialisation.c         | 20 ++++++++++++++------
 src/main/target/CJMCU/hardware_revision.c |  4 ----
 src/main/target/NAZE/hardware_revision.c  |  6 +-----
 6 files changed, 19 insertions(+), 35 deletions(-)

diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
index d381540fc..92139974a 100644
--- a/src/main/drivers/accgyro_mpu.c
+++ b/src/main/drivers/accgyro_mpu.c
@@ -167,7 +167,7 @@ static void mpu6050FindRevision(void)
     // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
 
     // determine product ID and accel revision
-    ack = i2cRead(MPU_ADDRESS, MPU_RA_XA_OFFS_H, 6, readBuffer);
+    ack = mpuConfiguration.read(MPU_RA_XA_OFFS_H, 6, readBuffer);
     revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
     if (revision) {
         /* Congrats, these parts are better. */
@@ -179,7 +179,7 @@ static void mpu6050FindRevision(void)
             failureMode(FAILURE_ACC_INCOMPATIBLE);
         }
     } else {
-        ack = i2cRead(MPU_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId);
+        ack = mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId);
         revision = productId & 0x0F;
         if (!revision) {
             failureMode(FAILURE_ACC_INCOMPATIBLE);
diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h
index 4c8455ff6..fbd3245e2 100644
--- a/src/main/drivers/accgyro_mpu6500.h
+++ b/src/main/drivers/accgyro_mpu6500.h
@@ -24,10 +24,12 @@
 #define MPU6500_RA_INT_ENABLE               (0x38)
 #define MPU6500_RA_GYRO_XOUT_H              (0x43)
 #define MPU6500_RA_SIGNAL_PATH_RST          (0x68)
+#define MPU6500_RA_USER_CTRL                (0x6A)
 #define MPU6500_RA_PWR_MGMT_1               (0x6B)
 #define MPU6500_RA_BANK_SEL                 (0x6D)
 #define MPU6500_RA_MEM_RW                   (0x6F)
 #define MPU6500_RA_WHOAMI                   (0x75)
+#define MPU6500_RA_XA_OFFS_H                (0x77)
 
 #define MPU6500_WHO_AM_I_CONST              (0x70)
 
diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h
index 967f65cd4..a8d5cf514 100755
--- a/src/main/drivers/accgyro_spi_mpu6500.h
+++ b/src/main/drivers/accgyro_spi_mpu6500.h
@@ -15,24 +15,6 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#define MPU6500_RA_RATE_DIV                 (0x19)
-#define MPU6500_RA_LPF                      (0x1A)
-#define MPU6500_RA_GYRO_CFG                 (0x1B)
-#define MPU6500_RA_ACCEL_CFG                (0x1C)
-#define MPU6500_RA_ACCEL_XOUT_H             (0x3B)
-#define MPU6500_RA_INT_PIN_CFG              (0x37)
-#define MPU6500_RA_INT_ENABLE               (0x38)
-#define MPU6500_RA_GYRO_XOUT_H              (0x43)
-#define MPU6500_RA_SIGNAL_PATH_RST          (0x68)
-#define MPU6500_RA_PWR_MGMT_1               (0x6B)
-#define MPU6500_RA_BANK_SEL                 (0x6D)
-#define MPU6500_RA_MEM_RW                   (0x6F)
-#define MPU6500_RA_WHOAMI                   (0x75)
-
-#define MPU6500_WHO_AM_I_CONST              (0x70)
-
-#define MPU6500_BIT_RESET                   (0x80)
-
 #pragma once
 
 bool mpu6500SpiDetect(void);
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 4e0dd330e..f1dc52791 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -111,7 +111,7 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
     }
 #endif
 
-#ifdef SPRACINGF3
+#if defined(SPRACINGF3)
     static const extiConfig_t spRacingF3MPUIntExtiConfig = {
             .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
             .gpioPort = GPIOC,
@@ -240,14 +240,17 @@ bool detectGyro(void)
 
         case GYRO_MPU6500:
 #ifdef USE_GYRO_MPU6500
-#ifdef USE_HARDWARE_REVISION_DETECTION
-		  spiBusInit();
+#ifdef USE_GYRO_SPI_MPU6500
+            if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro))
+#else
+            if (mpu6500GyroDetect(&gyro))
 #endif
-            if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro)) {
-#ifdef GYRO_MPU6500_ALIGN
+            {
                 gyroHardware = GYRO_MPU6500;
+#ifdef GYRO_MPU6500_ALIGN
                 gyroAlign = GYRO_MPU6500_ALIGN;
 #endif
+
                 break;
             }
 #endif
@@ -368,7 +371,12 @@ retry:
             ; // fallthrough
         case ACC_MPU6500:
 #ifdef USE_ACC_MPU6500
-            if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc)) {
+#ifdef USE_ACC_SPI_MPU6500
+            if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc))
+#else
+            if (mpu6500AccDetect(&acc))
+#endif
+            {
 #ifdef ACC_MPU6500_ALIGN
                 accAlign = ACC_MPU6500_ALIGN;
 #endif
diff --git a/src/main/target/CJMCU/hardware_revision.c b/src/main/target/CJMCU/hardware_revision.c
index 88cbbae1e..f424ee2af 100755
--- a/src/main/target/CJMCU/hardware_revision.c
+++ b/src/main/target/CJMCU/hardware_revision.c
@@ -51,7 +51,3 @@ void detectHardwareRevision(void)
 void updateHardwareRevision(void)
 {
 }
-
-void spiBusInit(void)
-{
-}
diff --git a/src/main/target/NAZE/hardware_revision.c b/src/main/target/NAZE/hardware_revision.c
index fca6d6e55..26ade1a33 100755
--- a/src/main/target/NAZE/hardware_revision.c
+++ b/src/main/target/NAZE/hardware_revision.c
@@ -27,7 +27,7 @@
 #include "drivers/bus_spi.h"
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
-#include "drivers/accgyro_spi_mpu6500.h"
+#include "drivers/accgyro_mpu6500.h"
 
 #include "hardware_revision.h"
 
@@ -101,7 +101,3 @@ void updateHardwareRevision(void)
         hardwareRevision = NAZE32_SP;
 #endif
 }
-
-void spiBusInit(void)
-{
-}

From b20dc77a7428aeba237d215789160a065bdcbf24 Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Thu, 26 Feb 2015 20:40:28 +0100
Subject: [PATCH 18/41] Harakiri PID controller make hardcoded parameters
 configurable

---
 src/main/config/config.c | 3 +++
 src/main/flight/pid.c    | 8 ++++++--
 src/main/flight/pid.h    | 2 ++
 src/main/io/serial_cli.c | 3 +++
 4 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index 5c324f420..83be96640 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -187,6 +187,9 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->A_level = 5.0f;
     pidProfile->H_level = 3.0f;
     pidProfile->H_sensitivity = 75;
+
+    pidProfile->pid5_oldyw = 0;
+    pidProfile->pid5_maincuthz = 12;
 }
 
 #ifdef GPS
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 899015ce4..8fa3e424d 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -577,7 +577,11 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
     uint8_t axis;
     float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale;
 
-    MainDptCut = RCconstPI / constrain(pidProfile->dterm_cut_hz, 1, 50);       // maincuthz (default 0 (disabled), Range 1-50Hz)
+    if (pidProfile->dterm_cut_hz) {
+        MainDptCut = RCconstPI / constrain(pidProfile->dterm_cut_hz, 1, 50);   // dterm_cut_hz (default 0, Range 1-50Hz)
+    } else {
+        MainDptCut = RCconstPI / 12.0f;                                        // default is 12Hz to maintain initial behavior of PID5
+    }
     FLOATcycleTime  = (float)constrain(cycleTime, 1, 100000);                  // 1us - 100ms
     ACCDeltaTimeINS = FLOATcycleTime * 0.000001f;                              // ACCDeltaTimeINS is in seconds now
     RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS);               // used for pt1 element
@@ -589,7 +593,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
     for (axis = 0; axis < 2; axis++) {
         int32_t tmp = (int32_t)((float)gyroADC[axis] * 0.3125f);              // Multiwii masks out the last 2 bits, this has the same idea
         gyroADCQuant = (float)tmp * 3.2f;                                     // but delivers more accuracy and also reduces jittery flight
-        rcCommandAxis = (float)rcCommand[axis];                                // Calculate common values for pid controllers
+        rcCommandAxis = (float)rcCommand[axis];                               // Calculate common values for pid controllers
         if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 #ifdef GPS
             error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 3ec0004e9..6dc552726 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -61,7 +61,9 @@ typedef struct pidProfile_s {
     float A_level;
     float H_level;
     uint8_t H_sensitivity;
+
     uint16_t yaw_p_limit;                   // set P term limit (fixed value was 300)
+    uint8_t pid5_oldyw;                     // Old yaw behavior for PID5
     uint8_t dterm_cut_hz;                   // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
     uint8_t pterm_cut_hz;                   // Used for fitlering Pterm noise on noisy frames
     uint8_t gyro_cut_hz;                    // Used for soft gyro filtering
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index eb083821d..1f3499d42 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -514,6 +514,9 @@ const clivalue_t valueTable[] = {
 	{ "pterm_cut_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, 0, 200 },
 	{ "gyro_cut_hz",                VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 },
 
+    { "pid5_oldyw",                 VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.pid5_oldyw, 0, 1 },
+    { "pid5_maincuthz",             VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.pid5_maincuthz, 1, 50 },
+
 #ifdef BLACKBOX
     { "blackbox_rate_num",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_num, 1, 32 },
     { "blackbox_rate_denom",        VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_denom, 1, 32 },

From 43f5792a6167d108191e322282d0d3944cc1a770 Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Fri, 27 Feb 2015 07:38:51 +0100
Subject: [PATCH 19/41] G-Tune port from Harakiri Enabled for NAZE, ALIENWWIIF1
 and ALIENWIIF3 targets Implement G-Tune for all PID controllers The G-Tune
 tuning results will be save if G-Tune mode will be disabled during copter is
 disarmed. Update PID controller and G-Tune documentation

---
 Makefile                            |   1 +
 docs/Gtune.md                       |  46 ++++++++
 docs/PID tuning.md                  |   6 +-
 src/main/config/config.c            |  10 ++
 src/main/config/runtime_config.h    |   1 +
 src/main/flight/gtune.c             | 164 ++++++++++++++++++++++++++++
 src/main/flight/gtune.h             |  21 ++++
 src/main/flight/pid.c               |  69 ++++++++++++
 src/main/flight/pid.h               |   9 +-
 src/main/io/rc_controls.h           |   1 +
 src/main/io/serial_cli.c            |  10 ++
 src/main/io/serial_msp.c            |   6 +
 src/main/mw.c                       |  29 +++++
 src/main/target/ALIENWIIF3/target.h |   1 +
 src/main/target/NAZE/target.h       |   1 +
 15 files changed, 371 insertions(+), 4 deletions(-)
 create mode 100644 docs/Gtune.md
 create mode 100644 src/main/flight/gtune.c
 create mode 100644 src/main/flight/gtune.h

diff --git a/Makefile b/Makefile
index 00de825b5..84df74f39 100755
--- a/Makefile
+++ b/Makefile
@@ -261,6 +261,7 @@ COMMON_SRC	 = build_config.c \
 		   $(DEVICE_STDPERIPH_SRC)
 
 HIGHEND_SRC  = flight/autotune.c \
+		   flight/gtune.c \
 		   flight/navigation.c \
 		   flight/gps_conversion.c \
 		   common/colorconversion.c \
diff --git a/docs/Gtune.md b/docs/Gtune.md
new file mode 100644
index 000000000..46ac2c8c0
--- /dev/null
+++ b/docs/Gtune.md
@@ -0,0 +1,46 @@
+# G-Tune instructions.
+
+The algorithm has been originally developed by Mohammad Hefny (mohammad.hefny@gmail.com)
+http://technicaladventure.blogspot.com/2014/06/zero-pids-tuner-for-multirotors.html
+http://diydrones.com/profiles/blogs/zero-pid-tunes-for-multirotors-part-2
+http://www.multiwii.com/forum/viewtopic.php?f=8&t=5190
+
+The G-Tune functionality for Cleanflight is ported from the Harakiri firmware.
+
+- Safety preamble: Use at your own risk -
+
+The implementation you have here is quiet different and just for adjusting the P values of ROLL/PITCH/YAW in Acro mode.
+When flying in Acro mode (yaw tune in other modes possible as well - see below) you can activate G-Tune with an AUX box (switch).
+It will start tuning the wanted / possible axes (see below) in a predefined range (see below).
+After activation you will probably notice nothing! That means G-Tune will not start shaking your copter, you will have to do it (or simply fly and let it work).
+The G-Tune is based on the gyro error so it is only active when you give no RC input (that would be an additional error). So if you just roll only pitch
+and yaw are tuned. If you stop rolling G-Tune will wait ca. 400ms to let the axis settle and then start tuning that axis again. All axes are treated independently.
+The easiest way to tune all axes at once is to do some air-jumps with the copter in Acro (RC centered and G-Tune activated... of course..).
+You can set a too high P for the axes as default in the GUI, when the copter starts shaking the wobbles will be detected and P tuned down (be careful with the strength setting though - see below).
+Yaw tune is disabled in any copter with less than 4 motors (like tricopters).
+G-Tune in Horizon or Level mode will just affect Yaw axis (if more than 3 motors...)
+You will see the results in the GUI - the the tuning results will only be saved if you disable G-Tune mode while the copter is disarmed or you save the configuration in an alternative way (like hitting save button in the GUI, casting an eepromwrite with trimming, acc calibration etc.)
+TPA and G-Tune: It is not tested and will most likely not result into something good. However G-Tune might be able to replace TPA for you.
+
+## Parameters and their function:
+
+gtune_loP_rll  = 20 [10..200] Lower limit of ROLL P during G-Tune.  Note "20" means "2.0" in the GUI.
+gtune_loP_ptch = 20 [10..200] Lower limit of PITCH P during G-Tune. Note "20" means "2.0" in the GUI.
+gtune_loP_yw   = 20 [10..200] Lower limit of YAW P during G-Tune.   Note "20" means "2.0" in the GUI.
+gtune_hiP_rll  = 70 [0..200]  Higher limit of ROLL P during G-Tune. 0 Disables tuning for that axis.  Note "70" means "7.0" in the GUI.
+gtune_hiP_ptch = 70 [0..200]  Higher limit of PITCH P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI.
+gtune_hiP_yw   = 70 [0..200]  Higher limit of YAW P during G-Tune. 0 Disables tuning for that axis.   Note "70" means "7.0" in the GUI.
+gtune_pwr      = 0  [0..10]   Strength of adjustment
+
+So you have lower and higher limits for each P for every axis. The preset range (GUI: 2.0 - 7.0) is quiet broad to represent most setups.
+If you want tighter ranges change them here. The gtune_loP_XXX can not be lower than "10" that means a P of "1.0" in the GUI. So you can not have "Zero P",
+you are in maybe sluggish initial control.
+If you want to exclude one axis from the tuning you must set gtune_hiP_XXX to zero. Let's say you want to disable yaw tuning write in CLI
+"set gtune_hiP_yw = 0". Note: The MultiWii Wiki advises you to trim the yaw axis on your transmitter. If you have done so (yaw not neutral on your RC)
+yaw tuning will be disabled.
+
+Setting the strength of tuning:
+gtune_pwr [0..10] Strength of adjustment.
+My small copter works fine with 0 and doesn't like a value of "3". My big copter likes "gtune_pwr = 5". It shifts the tuning to higher values and if too high can
+diminish the wobble blocking! So start with 0 (default). If you feel your resulting P is always too low for you increase gtune_pwr. You will see it getting a little shaky
+if value too high.
diff --git a/docs/PID tuning.md b/docs/PID tuning.md
index 8c41f4ce8..972584c5e 100644
--- a/docs/PID tuning.md	
+++ b/docs/PID tuning.md	
@@ -150,10 +150,10 @@ For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This
 
 PID Controller 5 is an port of the PID controller from the Harakiri firmware.
 
-The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don't need retuning of the PID values when looptime is changing. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri:
+The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don't need retuning of the PID values when looptime is changing. There are two additional settings which are configurable via the CLI in Harakiri:
 
-        OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw.
-        MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
+        set pid5_maincuthz = 12     [1-50Hz] Cut Off Frequency for D term of main Pid controller
+        set pid5_oldyw = 0          [0/1] 0 = multiwii 2.3 yaw (default), 1 = older yaw
 
 The PID controller is flight tested and running well with the default PID settings. If you want do acrobatics start slowly.
 
diff --git a/src/main/config/config.c b/src/main/config/config.c
index 83be96640..8dc232f34 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -190,6 +190,16 @@ static void resetPidProfile(pidProfile_t *pidProfile)
 
     pidProfile->pid5_oldyw = 0;
     pidProfile->pid5_maincuthz = 12;
+
+#ifdef GTUNE
+    pidProfile->gtune_lolimP[ROLL] = 20;          // [10..200] Lower limit of ROLL P during G tune.
+    pidProfile->gtune_lolimP[PITCH] = 20;         // [10..200] Lower limit of PITCH P during G tune.
+    pidProfile->gtune_lolimP[YAW] = 20;           // [10..200] Lower limit of YAW P during G tune.
+    pidProfile->gtune_hilimP[ROLL] = 70;          // [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axis.
+    pidProfile->gtune_hilimP[PITCH] = 70;         // [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axis.
+    pidProfile->gtune_hilimP[YAW] = 70;           // [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axis.
+    pidProfile->gtune_pwr = 0;                    // [0..10] Strength of adjustment
+#endif
 }
 
 #ifdef GPS
diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h
index c5a1bff6a..14e8b0fc3 100644
--- a/src/main/config/runtime_config.h
+++ b/src/main/config/runtime_config.h
@@ -42,6 +42,7 @@ typedef enum {
     PASSTHRU_MODE   = (1 << 8),
     SONAR_MODE      = (1 << 9),
     FAILSAFE_MODE   = (1 << 10),
+    GTUNE_MODE      = (1 << 11),
 } flightModeFlags_e;
 
 extern uint16_t flightModeFlags;
diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c
new file mode 100644
index 000000000..b0011d2e5
--- /dev/null
+++ b/src/main/flight/gtune.c
@@ -0,0 +1,164 @@
+/*
+ * 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/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+
+#include "platform.h"
+
+#ifdef GTUNE
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "drivers/system.h"
+#include "drivers/sensor.h"
+#include "drivers/accgyro.h"
+
+#include "sensors/sensors.h"
+#include "sensors/acceleration.h"
+
+#include "flight/pid.h"
+#include "flight/imu.h"
+
+#include "config/config.h"
+#include "blackbox/blackbox.h"
+
+#include "io/rc_controls.h"
+
+#include "config/runtime_config.h"
+
+extern uint8_t motorCount;
+
+/*
+ ****************************************************************************
+ ***                    G_Tune                                            ***
+ ****************************************************************************
+	G_Tune Mode
+	This is the multiwii implementation of ZERO-PID Algorithm
+	http://technicaladventure.blogspot.com/2014/06/zero-pids-tuner-for-multirotors.html
+	The algorithm has been originally developed by Mohammad Hefny (mohammad.hefny@gmail.com)
+
+	You may use/modify this algorithm on your own risk, kindly refer to above link in any future distribution.
+ */
+
+/*
+   version 1.0.0: MIN & Maxis & Tuned Band
+   version 1.0.1:
+                a. error is gyro reading not rc - gyro.
+                b. OldError = Error no averaging.
+                c. No Min Maxis BOUNDRY
+    version 1.0.2:
+                a. no boundaries
+                b. I - Factor tune.
+                c. time_skip
+
+   Crashpilot: Reduced to just P tuning in a predefined range - so it is not "zero pid" anymore.
+   Tuning is limited to just work when stick is centered besides that YAW is tuned in non Acro as well.
+   See also:
+   http://diydrones.com/profiles/blogs/zero-pid-tunes-for-multirotors-part-2
+   http://www.multiwii.com/forum/viewtopic.php?f=8&t=5190
+   Gyrosetting 2000DPS
+   GyroScale = (1 / 16,4 ) * RADX(see board.h) = 0,001064225154 digit per rad/s
+
+    pidProfile->gtune_lolimP[ROLL]   = 20; [10..200] Lower limit of ROLL P during G tune.
+    pidProfile->gtune_lolimP[PITCH]  = 20; [10..200] Lower limit of PITCH P during G tune.
+    pidProfile->gtune_lolimP[YAW]    = 20; [10..200] Lower limit of YAW P during G tune.
+    pidProfile->gtune_hilimP[ROLL]   = 70; [0..200]  Higher limit of ROLL P during G tune. 0 Disables tuning for that axisis.
+    pidProfile->gtune_hilimP[PITCH]  = 70; [0..200]  Higher limit of PITCH P during G tune. 0 Disables tuning for that axisis.
+    pidProfile->gtune_hilimP[YAW]    = 70; [0..200]  Higher limit of YAW P during G tune. 0 Disables tuning for that axisis.
+    pidProfile->gtune_pwr            = 0;  [0..10] Strength of adjustment
+*/
+
+static pidProfile_t *pidProfile;
+static int8_t time_skip[3];
+static int16_t OldError[3], result_P64[3];
+static int32_t AvgGyro[3];
+static bool floatPID;
+
+void init_Gtune(pidProfile_t *pidProfileToTune)
+{
+    uint8_t i;
+
+    pidProfile = pidProfileToTune;
+	if (pidProfile->pidController == 2) floatPID = true;                        // LuxFloat is using float values for PID settings
+	else floatPID = false;
+	for (i = 0; i < 3; i++) {
+        if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || // User config error disable axisis for tuning
+            (motorCount < 4 && i == FD_YAW)) pidProfile->gtune_hilimP[i] = 0;   // Disable Yawtuning for everything below a quadcopter
+        if (floatPID) {
+            if(pidProfile->P_f[i] < pidProfile->gtune_lolimP[i]) pidProfile->P_f[i] = pidProfile->gtune_lolimP[i];
+            result_P64[i] = (int16_t)pidProfile->P_f[i] << 6;                   // 6 bit extra resolution for P.
+        } else {
+            if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) pidProfile->P8[i] = pidProfile->gtune_lolimP[i];
+            result_P64[i] = (int16_t)pidProfile->P8[i] << 6;                    // 6 bit extra resolution for P.
+        }
+        OldError[i] = 0;
+        time_skip[i] = -125;
+    }
+}
+
+void calculate_Gtune(uint8_t axis)
+{
+    int16_t error, diff_G, threshP;
+
+    if(rcCommand[axis] || (axis != FD_YAW && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)))) {  // Block Tuning on stickinput. Always allow Gtune on YAW, Roll & Pitch only in acromode
+        OldError[axis] = 0;
+        time_skip[axis] = -125;                                                 // Some settletime after stick center. (125 + 16)* 3ms clycle = 423ms (ca.)
+    } else {
+        if (!time_skip[axis]) AvgGyro[axis] = 0;
+        time_skip[axis]++;
+        if (time_skip[axis] > 0) {
+            if (axis == FD_YAW) AvgGyro[axis] += 32 * ((int16_t)gyroData[axis] / 32); // Chop some jitter and average
+            else AvgGyro[axis] += 128 * ((int16_t)gyroData[axis] / 128);        // Chop some jitter and average
+        }
+        if (time_skip[axis] == 16) {                                            // ca 48 ms
+            AvgGyro[axis] /= time_skip[axis];                                   // AvgGyro[axis] has now very clean gyrodata
+            time_skip[axis] = 0;
+            if (axis == FD_YAW) {
+                threshP = 20;
+                error = -AvgGyro[axis];
+            } else {
+                threshP = 10;
+                error = AvgGyro[axis];
+            }
+            if (pidProfile->gtune_hilimP[axis] && error && OldError[axis] && error != OldError[axis]) {  // Don't run when not needed or pointless to do so
+                diff_G = ABS(error) - ABS(OldError[axis]);
+                if ((error > 0 && OldError[axis] > 0) || (error < 0 && OldError[axis] < 0)) {
+                    if (diff_G > threshP) result_P64[axis] += 64 + pidProfile->gtune_pwr; // Shift balance a little on the plus side.
+                    else {
+                        if (diff_G < -threshP) {
+                            if (axis == FD_YAW) result_P64[axis] -= 64 + pidProfile->gtune_pwr;
+                            else result_P64[axis] -= 32;
+                        }
+                    }
+                } else {
+                    if (ABS(diff_G) > threshP && axis != FD_YAW) result_P64[axis] -= 32; // Don't use antiwobble for YAW
+                }
+                result_P64[axis] = constrain(result_P64[axis], (int16_t)pidProfile->gtune_lolimP[axis] << 6, (int16_t)pidProfile->gtune_hilimP[axis] << 6);
+                if (floatPID) pidProfile->P_f[axis] = (float)(result_P64[axis] >> 6); // new P value for float PID
+                else pidProfile->P8[axis] = result_P64[axis] >> 6;              // new P value
+            }
+            OldError[axis] = error;
+        }
+    }
+}
+
+#endif
+
diff --git a/src/main/flight/gtune.h b/src/main/flight/gtune.h
new file mode 100644
index 000000000..28f39fa92
--- /dev/null
+++ b/src/main/flight/gtune.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
+
+void init_Gtune(pidProfile_t *pidProfileToTune);
+void calculate_Gtune(uint8_t axis);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 8fa3e424d..a9fc9ff97 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -42,6 +42,7 @@
 #include "flight/imu.h"
 #include "flight/navigation.h"
 #include "flight/autotune.h"
+#include "flight/gtune.h"
 #include "flight/filter.h"
 
 #include "config/runtime_config.h"
@@ -222,6 +223,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         // -----calculate total PID output
         axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
 
+#ifdef GTUNE
+        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+            calculate_Gtune(axis);
+        }
+#endif
+
 #ifdef BLACKBOX
         axisPID_P[axis] = PTerm;
         axisPID_I[axis] = ITerm;
@@ -317,6 +324,12 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         DTerm = (deltaSum * dynD8[axis]) / 32;
         axisPID[axis] = PTerm + ITerm - DTerm;
 
+#ifdef GTUNE
+        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+            calculate_Gtune(axis);
+        }
+#endif
+
 #ifdef BLACKBOX
         axisPID_P[axis] = PTerm;
         axisPID_I[axis] = ITerm;
@@ -408,6 +421,12 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
 
         axisPID[axis] = PTerm + ITerm - DTerm;
 
+#ifdef GTUNE
+        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+            calculate_Gtune(axis);
+        }
+#endif
+
 #ifdef BLACKBOX
         axisPID_P[axis] = PTerm;
         axisPID_I[axis] = ITerm;
@@ -437,6 +456,17 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
 
     axisPID[FD_YAW] =  PTerm + ITerm;
 
+    if (motorCount >= 4) {                                                     // prevent "yaw jump" during yaw correction
+        int16_t limit = ABS(rcCommand[FD_YAW]) + 100;
+        axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit);
+    }
+
+#ifdef GTUNE
+    if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+        calculate_Gtune(FD_YAW);
+    }
+#endif
+
 #ifdef BLACKBOX
     axisPID_P[FD_YAW] = PTerm;
     axisPID_I[FD_YAW] = ITerm;
@@ -530,6 +560,12 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
         DTerm = (deltaSum * dynD8[axis]) / 32;
         axisPID[axis] = PTerm + ITerm - DTerm;
 
+#ifdef GTUNE
+        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+            calculate_Gtune(axis);
+        }
+#endif
+
 #ifdef BLACKBOX
         axisPID_P[axis] = PTerm;
         axisPID_I[axis] = ITerm;
@@ -558,6 +594,16 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
 
     axisPID[FD_YAW] =  PTerm + ITerm;
 
+    if (motorCount >= 4) {                                                     // prevent "yaw jump" during yaw correction
+        int16_t limit = ABS(rcCommand[FD_YAW]) + 100;
+        axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit);
+    }
+
+#ifdef GTUNE
+    if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+        calculate_Gtune(FD_YAW);
+    }
+#endif
 
 #ifdef BLACKBOX
     axisPID_P[FD_YAW] = PTerm;
@@ -650,6 +696,12 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 
         axisPID[axis] = lrintf(PTerm + ITerm - DTerm);                         // Round up result.
 
+#ifdef GTUNE
+        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+            calculate_Gtune(axis);
+        }
+#endif
+
 #ifdef BLACKBOX
         axisPID_P[axis] = PTerm;
         axisPID_I[axis] = ITerm;
@@ -697,6 +749,17 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
     axisPID[FD_YAW] = PTerm + ITerm;
     axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]);                                 // Round up result.
 
+    if (motorCount >= 4) {                                                     // prevent "yaw jump" during yaw correction
+        int16_t limit = ABS(rcCommand[FD_YAW]) + 100;
+        axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit);
+    }
+
+#ifdef GTUNE
+    if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+        calculate_Gtune(FD_YAW);
+    }
+#endif
+
 #ifdef BLACKBOX
     axisPID_P[FD_YAW] = PTerm;
     axisPID_I[FD_YAW] = ITerm;
@@ -823,6 +886,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         // -----calculate total PID output
         axisPID[axis] = PTerm + ITerm + DTerm;
 
+#ifdef GTUNE
+        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+             calculate_Gtune(axis);
+        }
+#endif
+
 #ifdef BLACKBOX
         axisPID_P[axis] = PTerm;
         axisPID_I[axis] = ITerm;
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 6dc552726..21a6e048f 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -63,10 +63,17 @@ typedef struct pidProfile_s {
     uint8_t H_sensitivity;
 
     uint16_t yaw_p_limit;                   // set P term limit (fixed value was 300)
-    uint8_t pid5_oldyw;                     // Old yaw behavior for PID5
     uint8_t dterm_cut_hz;                   // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
     uint8_t pterm_cut_hz;                   // Used for fitlering Pterm noise on noisy frames
     uint8_t gyro_cut_hz;                    // Used for soft gyro filtering
+
+    uint8_t pid5_oldyw;                     // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw
+
+#ifdef GTUNE
+    uint8_t  gtune_lolimP[3];               // [10..200] Lower limit of P during G tune
+    uint8_t  gtune_hilimP[3];               // [0..200] Higher limit of P during G tune. 0 Disables tuning for that axis.
+    int8_t   gtune_pwr;                     // [0..10] Strength of adjustment
+#endif
 } pidProfile_t;
 
 #define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index 8be258648..6d5f28be2 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -48,6 +48,7 @@ typedef enum {
     BOXSERVO3,
     BOXBLACKBOX,
     BOXFAILSAFE,
+    BOXGTUNE,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 1f3499d42..7ee8e4314 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -517,6 +517,16 @@ const clivalue_t valueTable[] = {
     { "pid5_oldyw",                 VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.pid5_oldyw, 0, 1 },
     { "pid5_maincuthz",             VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.pid5_maincuthz, 1, 50 },
 
+#ifdef GTUNE
+    { "gtune_loP_rll",              VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], 10, 200 },
+    { "gtune_loP_ptch",             VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], 10, 200 },
+    { "gtune_loP_yw",               VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_YAW], 10, 200 },
+    { "gtune_hiP_rll",              VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_ROLL], 0, 200 },
+    { "gtune_hiP_ptch",             VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_PITCH], 0, 200 },
+    { "gtune_hiP_yw",               VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_YAW], 0, 200 },
+    { "gtune_pwr",                  VAR_INT8   | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_pwr, 0, 10 },
+#endif
+
 #ifdef BLACKBOX
     { "blackbox_rate_num",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_num, 1, 32 },
     { "blackbox_rate_denom",        VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_denom, 1, 32 },
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index f383acbf8..c662b4c3d 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -347,6 +347,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXSERVO3, "SERVO3;", 25 },
     { BOXBLACKBOX, "BLACKBOX;", 26 },
     { BOXFAILSAFE, "FAILSAFE;", 27 },
+    { BOXGTUNE, "GTUNE;", 28 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -703,6 +704,10 @@ void mspInit(serialConfig_t *serialConfig)
         activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
     }
 
+#ifdef GTUNE
+    activeBoxIds[activeBoxIdCount++] = BOXGTUNE;
+#endif
+
     memset(mspPorts, 0x00, sizeof(mspPorts));
     mspAllocateSerialPorts(serialConfig);
 }
@@ -822,6 +827,7 @@ static bool processOutCommand(uint8_t cmdMSP)
             IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
             IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
             IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) << BOXAUTOTUNE |
+            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
             IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
             IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
             IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
diff --git a/src/main/mw.c b/src/main/mw.c
index 4174d0e17..7553f7d69 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -71,6 +71,7 @@
 #include "flight/altitudehold.h"
 #include "flight/failsafe.h"
 #include "flight/autotune.h"
+#include "flight/gtune.h"
 #include "flight/navigation.h"
 #include "flight/filter.h"
 
@@ -160,6 +161,30 @@ void updateAutotuneState(void)
 }
 #endif
 
+#ifdef GTUNE
+
+void updateGtuneState(void)
+{
+    static bool GTuneWasUsed = false;
+
+    if (IS_RC_MODE_ACTIVE(BOXGTUNE)) {
+        if (!FLIGHT_MODE(GTUNE_MODE)) {
+        	ENABLE_FLIGHT_MODE(GTUNE_MODE);
+            init_Gtune(&currentProfile->pidProfile);
+            GTuneWasUsed = true;
+        }
+    } else {
+        if (FLIGHT_MODE(GTUNE_MODE)) {
+    	    DISABLE_FLIGHT_MODE(GTUNE_MODE);
+            if (!ARMING_FLAG(ARMED) && GTuneWasUsed) {
+                saveConfigAndNotify();
+                GTuneWasUsed = false;
+            }
+        }
+    }
+}
+#endif
+
 bool isCalibrating()
 {
 #ifdef BARO
@@ -806,6 +831,10 @@ void loop(void)
         }
 #endif
 
+#ifdef GTUNE
+        updateGtuneState();
+#endif
+
 #if defined(BARO) || defined(SONAR)
         if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
             if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
diff --git a/src/main/target/ALIENWIIF3/target.h b/src/main/target/ALIENWIIF3/target.h
index 70b3367ee..f9291e367 100644
--- a/src/main/target/ALIENWIIF3/target.h
+++ b/src/main/target/ALIENWIIF3/target.h
@@ -115,6 +115,7 @@
 //#define GPS
 //#define DISPLAY
 #define AUTOTUNE
+#define GTUNE
 #define USE_SERVOS
 #define USE_CLI
 
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index 8d2c66575..bba37c7e9 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -175,6 +175,7 @@
 #define TELEMETRY
 #define SERIAL_RX
 #define AUTOTUNE
+#define GTUNE
 #define USE_SERVOS
 #define USE_CLI
 

From cb5f81ca988078635c07f4052b5fa79fd991b5cb Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Sun, 1 Mar 2015 01:01:11 +0100
Subject: [PATCH 20/41] G-Tune documentation update

---
 docs/Gtune.md | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/docs/Gtune.md b/docs/Gtune.md
index 46ac2c8c0..3fff96aa4 100644
--- a/docs/Gtune.md
+++ b/docs/Gtune.md
@@ -24,13 +24,13 @@ TPA and G-Tune: It is not tested and will most likely not result into something
 
 ## Parameters and their function:
 
-gtune_loP_rll  = 20 [10..200] Lower limit of ROLL P during G-Tune.  Note "20" means "2.0" in the GUI.
-gtune_loP_ptch = 20 [10..200] Lower limit of PITCH P during G-Tune. Note "20" means "2.0" in the GUI.
-gtune_loP_yw   = 20 [10..200] Lower limit of YAW P during G-Tune.   Note "20" means "2.0" in the GUI.
-gtune_hiP_rll  = 70 [0..200]  Higher limit of ROLL P during G-Tune. 0 Disables tuning for that axis.  Note "70" means "7.0" in the GUI.
-gtune_hiP_ptch = 70 [0..200]  Higher limit of PITCH P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI.
-gtune_hiP_yw   = 70 [0..200]  Higher limit of YAW P during G-Tune. 0 Disables tuning for that axis.   Note "70" means "7.0" in the GUI.
-gtune_pwr      = 0  [0..10]   Strength of adjustment
+    gtune_loP_rll  = 20 [10..200] Lower limit of ROLL P during G-Tune.  Note "20" means "2.0" in the GUI.
+    gtune_loP_ptch = 20 [10..200] Lower limit of PITCH P during G-Tune. Note "20" means "2.0" in the GUI.
+    gtune_loP_yw   = 20 [10..200] Lower limit of YAW P during G-Tune.   Note "20" means "2.0" in the GUI.
+    gtune_hiP_rll  = 70 [0..200]  Higher limit of ROLL P during G-Tune. 0 Disables tuning for that axis.  Note "70" means "7.0" in the GUI.
+    gtune_hiP_ptch = 70 [0..200]  Higher limit of PITCH P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI.
+    gtune_hiP_yw   = 70 [0..200]  Higher limit of YAW P during G-Tune. 0 Disables tuning for that axis.   Note "70" means "7.0" in the GUI.
+    gtune_pwr      = 0  [0..10]   Strength of adjustment
 
 So you have lower and higher limits for each P for every axis. The preset range (GUI: 2.0 - 7.0) is quiet broad to represent most setups.
 If you want tighter ranges change them here. The gtune_loP_XXX can not be lower than "10" that means a P of "1.0" in the GUI. So you can not have "Zero P",

From daceb2db9a61d0df3fd86ec17490182107831fd6 Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Mon, 2 Mar 2015 00:12:01 +0100
Subject: [PATCH 21/41] Fix G-Tune for LuxFloat PID controller G-Tune
 documentation fix

---
 docs/Gtune.md           | 2 +-
 src/main/flight/gtune.c | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/docs/Gtune.md b/docs/Gtune.md
index 3fff96aa4..a58100325 100644
--- a/docs/Gtune.md
+++ b/docs/Gtune.md
@@ -19,7 +19,7 @@ The easiest way to tune all axes at once is to do some air-jumps with the copter
 You can set a too high P for the axes as default in the GUI, when the copter starts shaking the wobbles will be detected and P tuned down (be careful with the strength setting though - see below).
 Yaw tune is disabled in any copter with less than 4 motors (like tricopters).
 G-Tune in Horizon or Level mode will just affect Yaw axis (if more than 3 motors...)
-You will see the results in the GUI - the the tuning results will only be saved if you disable G-Tune mode while the copter is disarmed or you save the configuration in an alternative way (like hitting save button in the GUI, casting an eepromwrite with trimming, acc calibration etc.)
+You will see the results in the GUI - the tuning results will only be saved if you disable G-Tune mode while the copter is disarmed or you save the configuration in an alternative way (like hitting save button in the GUI, casting an eepromwrite with trimming, acc calibration etc.)
 TPA and G-Tune: It is not tested and will most likely not result into something good. However G-Tune might be able to replace TPA for you.
 
 ## Parameters and their function:
diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c
index b0011d2e5..b3236a3ef 100644
--- a/src/main/flight/gtune.c
+++ b/src/main/flight/gtune.c
@@ -103,7 +103,7 @@ void init_Gtune(pidProfile_t *pidProfileToTune)
         if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || // User config error disable axisis for tuning
             (motorCount < 4 && i == FD_YAW)) pidProfile->gtune_hilimP[i] = 0;   // Disable Yawtuning for everything below a quadcopter
         if (floatPID) {
-            if(pidProfile->P_f[i] < pidProfile->gtune_lolimP[i]) pidProfile->P_f[i] = pidProfile->gtune_lolimP[i];
+            if((pidProfile->P_f[i] * 10) < pidProfile->gtune_lolimP[i]) pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10);
             result_P64[i] = (int16_t)pidProfile->P_f[i] << 6;                   // 6 bit extra resolution for P.
         } else {
             if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) pidProfile->P8[i] = pidProfile->gtune_lolimP[i];
@@ -152,7 +152,7 @@ void calculate_Gtune(uint8_t axis)
                     if (ABS(diff_G) > threshP && axis != FD_YAW) result_P64[axis] -= 32; // Don't use antiwobble for YAW
                 }
                 result_P64[axis] = constrain(result_P64[axis], (int16_t)pidProfile->gtune_lolimP[axis] << 6, (int16_t)pidProfile->gtune_hilimP[axis] << 6);
-                if (floatPID) pidProfile->P_f[axis] = (float)(result_P64[axis] >> 6); // new P value for float PID
+                if (floatPID) pidProfile->P_f[axis] = (float)((result_P64[axis] >> 6) / 10); // new P value for float PID
                 else pidProfile->P8[axis] = result_P64[axis] >> 6;              // new P value
             }
             OldError[axis] = error;

From 6c022455c510b9d85731990203324d583d4a5f1f Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Mon, 2 Mar 2015 13:31:37 +0100
Subject: [PATCH 22/41] Add BlackBox recording for G-Tune

---
 src/main/blackbox/blackbox.c           |  4 ++++
 src/main/blackbox/blackbox_fielddefs.h | 13 +++++++++++++
 src/main/flight/gtune.c                | 19 ++++++++++++++++---
 3 files changed, 33 insertions(+), 3 deletions(-)

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 2f7ac5174..fb993d246 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1162,6 +1162,10 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
                 blackboxWrite(data->inflightAdjustment.adjustmentFunction);
                 blackboxWriteSignedVB(data->inflightAdjustment.newValue);
             }
+        case FLIGHT_LOG_EVENT_GTUNE_RESULT:
+            blackboxWrite(data->gtuneCycleResult.gtuneAxis);
+            blackboxWrite(data->gtuneCycleResult.gtuneGyroAVG);
+            blackboxWrite(data->gtuneCycleResult.gtuneNewP);
         break;
         case FLIGHT_LOG_EVENT_LOGGING_RESUME:
             blackboxWriteUnsignedVB(data->loggingResume.logIteration);
diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h
index 72ffa8148..f74e0bb52 100644
--- a/src/main/blackbox/blackbox_fielddefs.h
+++ b/src/main/blackbox/blackbox_fielddefs.h
@@ -108,6 +108,7 @@ typedef enum FlightLogEvent {
     FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12,
     FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13,
     FLIGHT_LOG_EVENT_LOGGING_RESUME = 14,
+    FLIGHT_LOG_EVENT_GTUNE_RESULT = 20,
     FLIGHT_LOG_EVENT_LOG_END = 255
 } FlightLogEvent;
 
@@ -148,10 +149,18 @@ typedef struct flightLogEvent_inflightAdjustment_t {
     float newFloatValue;
 } flightLogEvent_inflightAdjustment_t;
 
+<<<<<<< Upstream, based on origin/master
 typedef struct flightLogEvent_loggingResume_t {
     uint32_t logIteration;
     uint32_t currentTime;
 } flightLogEvent_loggingResume_t;
+=======
+typedef struct flightLogEvent_gtuneCycleResult_t {
+    uint8_t gtuneAxis;
+    uint32_t gtuneGyroAVG;
+    uint8_t gtuneNewP;
+} flightLogEvent_gtuneCycleResult_t;
+>>>>>>> 6f60c52 Add BlackBox recording for G-Tune
 
 typedef union flightLogEventData_t
 {
@@ -160,7 +169,11 @@ typedef union flightLogEventData_t
     flightLogEvent_autotuneCycleResult_t autotuneCycleResult;
     flightLogEvent_autotuneTargets_t autotuneTargets;
     flightLogEvent_inflightAdjustment_t inflightAdjustment;
+<<<<<<< Upstream, based on origin/master
     flightLogEvent_loggingResume_t loggingResume;
+=======
+    flightLogEvent_gtuneCycleResult_t gtuneCycleResult;
+>>>>>>> 6f60c52 Add BlackBox recording for G-Tune
 } flightLogEventData_t;
 
 typedef struct flightLogEvent_t
diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c
index b3236a3ef..d228c4aa8 100644
--- a/src/main/flight/gtune.c
+++ b/src/main/flight/gtune.c
@@ -151,9 +151,22 @@ void calculate_Gtune(uint8_t axis)
                 } else {
                     if (ABS(diff_G) > threshP && axis != FD_YAW) result_P64[axis] -= 32; // Don't use antiwobble for YAW
                 }
-                result_P64[axis] = constrain(result_P64[axis], (int16_t)pidProfile->gtune_lolimP[axis] << 6, (int16_t)pidProfile->gtune_hilimP[axis] << 6);
-                if (floatPID) pidProfile->P_f[axis] = (float)((result_P64[axis] >> 6) / 10); // new P value for float PID
-                else pidProfile->P8[axis] = result_P64[axis] >> 6;              // new P value
+                int16_t newP = constrain((result_P64[axis] >> 6), (int16_t)pidProfile->gtune_lolimP[axis], (int16_t)pidProfile->gtune_hilimP[axis]);
+
+#ifdef BLACKBOX
+                if (feature(FEATURE_BLACKBOX)) {
+                	flightLogEvent_gtuneCycleResult_t eventData;
+
+                    eventData.gtuneAxis = axis;
+                    eventData.gtuneGyroAVG = AvgGyro[axis];
+                    eventData.gtuneNewP = newP;
+
+                    blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData);
+                }
+#endif
+
+                if (floatPID) pidProfile->P_f[axis] = (float)(newP / 10);     // new P value for float PID
+                else pidProfile->P8[axis] = newP;                             // new P value
             }
             OldError[axis] = error;
         }

From e7e297ad5377aaf90a5d2244978b3caa57819d0b Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Tue, 3 Mar 2015 08:26:37 +0100
Subject: [PATCH 23/41] Modified behavior of G-Tune switch and storing the
 tuned P values G-Tune documentation update G-Tune will only activated and
 deactivated when armed. G-Tune should deactivated while the copter is
 airborne. Tuned P values will only be stored when G-Tune is enabled while
 disarmed and G-Tune was used before.

---
 docs/Gtune.md         |  4 ++--
 src/main/flight/pid.c |  2 +-
 src/main/mw.c         | 14 +++++++-------
 3 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/docs/Gtune.md b/docs/Gtune.md
index a58100325..6851b1192 100644
--- a/docs/Gtune.md
+++ b/docs/Gtune.md
@@ -10,7 +10,7 @@ The G-Tune functionality for Cleanflight is ported from the Harakiri firmware.
 - Safety preamble: Use at your own risk -
 
 The implementation you have here is quiet different and just for adjusting the P values of ROLL/PITCH/YAW in Acro mode.
-When flying in Acro mode (yaw tune in other modes possible as well - see below) you can activate G-Tune with an AUX box (switch).
+When flying in Acro mode (yaw tune in other modes possible as well - see below) you can activate G-Tune with an AUX box (switch) while the copter is armed.
 It will start tuning the wanted / possible axes (see below) in a predefined range (see below).
 After activation you will probably notice nothing! That means G-Tune will not start shaking your copter, you will have to do it (or simply fly and let it work).
 The G-Tune is based on the gyro error so it is only active when you give no RC input (that would be an additional error). So if you just roll only pitch
@@ -19,7 +19,7 @@ The easiest way to tune all axes at once is to do some air-jumps with the copter
 You can set a too high P for the axes as default in the GUI, when the copter starts shaking the wobbles will be detected and P tuned down (be careful with the strength setting though - see below).
 Yaw tune is disabled in any copter with less than 4 motors (like tricopters).
 G-Tune in Horizon or Level mode will just affect Yaw axis (if more than 3 motors...)
-You will see the results in the GUI - the tuning results will only be saved if you disable G-Tune mode while the copter is disarmed or you save the configuration in an alternative way (like hitting save button in the GUI, casting an eepromwrite with trimming, acc calibration etc.)
+You will see the results in the GUI - the tuning results will only be saved if you enable G-Tune mode while the copter is disarmed and G-Tune was used before when armed. You also can save the configuration in an alternative way (like hitting save button in the GUI, casting an eepromwrite with trimming, acc calibration etc.)
 TPA and G-Tune: It is not tested and will most likely not result into something good. However G-Tune might be able to replace TPA for you.
 
 ## Parameters and their function:
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index a9fc9ff97..bb5026e71 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -712,7 +712,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
     Mwii3msTimescale = (int32_t)FLOATcycleTime & (int32_t)~3;                  // Filter last 2 bit jitter
     Mwii3msTimescale /= 3000.0f;
 
-    if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
+    if (pidProfile->pid5_oldyw) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw
         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(gyroADC[FD_YAW] * 0.25f);
         PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80;
diff --git a/src/main/mw.c b/src/main/mw.c
index 7553f7d69..e540a849c 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -168,18 +168,18 @@ void updateGtuneState(void)
     static bool GTuneWasUsed = false;
 
     if (IS_RC_MODE_ACTIVE(BOXGTUNE)) {
-        if (!FLIGHT_MODE(GTUNE_MODE)) {
-        	ENABLE_FLIGHT_MODE(GTUNE_MODE);
+        if (!FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+            ENABLE_FLIGHT_MODE(GTUNE_MODE);
             init_Gtune(&currentProfile->pidProfile);
             GTuneWasUsed = true;
         }
+        if (!FLIGHT_MODE(GTUNE_MODE) && !ARMING_FLAG(ARMED) && GTuneWasUsed) {
+            saveConfigAndNotify();
+            GTuneWasUsed = false;
+        }
     } else {
-        if (FLIGHT_MODE(GTUNE_MODE)) {
+        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
     	    DISABLE_FLIGHT_MODE(GTUNE_MODE);
-            if (!ARMING_FLAG(ARMED) && GTuneWasUsed) {
-                saveConfigAndNotify();
-                GTuneWasUsed = false;
-            }
         }
     }
 }

From fe2f2f30533712a92a2cd9e036a831ee65bdcc6c Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Sun, 12 Apr 2015 04:46:27 +0200
Subject: [PATCH 24/41] Fix some BlackBox isues, remove redundant code from PID
 controllers.

---
 src/main/blackbox/blackbox.c           |  4 ++--
 src/main/blackbox/blackbox_fielddefs.h |  4 ++--
 src/main/flight/gtune.c                |  3 ++-
 src/main/flight/pid.c                  | 15 ---------------
 4 files changed, 6 insertions(+), 20 deletions(-)

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index fb993d246..c98390f1c 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1164,8 +1164,8 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
             }
         case FLIGHT_LOG_EVENT_GTUNE_RESULT:
             blackboxWrite(data->gtuneCycleResult.gtuneAxis);
-            blackboxWrite(data->gtuneCycleResult.gtuneGyroAVG);
-            blackboxWrite(data->gtuneCycleResult.gtuneNewP);
+            blackboxWriteSignedVB(data->gtuneCycleResult.gtuneGyroAVG);
+            blackboxWriteS16(data->gtuneCycleResult.gtuneNewP);
         break;
         case FLIGHT_LOG_EVENT_LOGGING_RESUME:
             blackboxWriteUnsignedVB(data->loggingResume.logIteration);
diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h
index f74e0bb52..fcd087584 100644
--- a/src/main/blackbox/blackbox_fielddefs.h
+++ b/src/main/blackbox/blackbox_fielddefs.h
@@ -157,8 +157,8 @@ typedef struct flightLogEvent_loggingResume_t {
 =======
 typedef struct flightLogEvent_gtuneCycleResult_t {
     uint8_t gtuneAxis;
-    uint32_t gtuneGyroAVG;
-    uint8_t gtuneNewP;
+    int32_t gtuneGyroAVG;
+    int16_t gtuneNewP;
 } flightLogEvent_gtuneCycleResult_t;
 >>>>>>> 6f60c52 Add BlackBox recording for G-Tune
 
diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c
index d228c4aa8..bb3af9b3d 100644
--- a/src/main/flight/gtune.c
+++ b/src/main/flight/gtune.c
@@ -159,7 +159,8 @@ void calculate_Gtune(uint8_t axis)
 
                     eventData.gtuneAxis = axis;
                     eventData.gtuneGyroAVG = AvgGyro[axis];
-                    eventData.gtuneNewP = newP;
+                    if (floatPID) eventData.gtuneNewP = newP / 10;
+                    else eventData.gtuneNewP = newP;
 
                     blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData);
                 }
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index bb5026e71..b7f97c85e 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -456,11 +456,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
 
     axisPID[FD_YAW] =  PTerm + ITerm;
 
-    if (motorCount >= 4) {                                                     // prevent "yaw jump" during yaw correction
-        int16_t limit = ABS(rcCommand[FD_YAW]) + 100;
-        axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit);
-    }
-
 #ifdef GTUNE
     if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
         calculate_Gtune(FD_YAW);
@@ -594,11 +589,6 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
 
     axisPID[FD_YAW] =  PTerm + ITerm;
 
-    if (motorCount >= 4) {                                                     // prevent "yaw jump" during yaw correction
-        int16_t limit = ABS(rcCommand[FD_YAW]) + 100;
-        axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit);
-    }
-
 #ifdef GTUNE
     if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
         calculate_Gtune(FD_YAW);
@@ -749,11 +739,6 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
     axisPID[FD_YAW] = PTerm + ITerm;
     axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]);                                 // Round up result.
 
-    if (motorCount >= 4) {                                                     // prevent "yaw jump" during yaw correction
-        int16_t limit = ABS(rcCommand[FD_YAW]) + 100;
-        axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit);
-    }
-
 #ifdef GTUNE
     if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
         calculate_Gtune(FD_YAW);

From ef5887856d02ee2a1e49f3ce7af522e2b6ee5d64 Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Sun, 12 Apr 2015 21:56:11 +0200
Subject: [PATCH 25/41] Enable G-Tune on Sparky Fix code style

---
 src/main/flight/gtune.c         | 2 +-
 src/main/target/SPARKY/target.h | 1 +
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c
index bb3af9b3d..63c68afdb 100644
--- a/src/main/flight/gtune.c
+++ b/src/main/flight/gtune.c
@@ -155,7 +155,7 @@ void calculate_Gtune(uint8_t axis)
 
 #ifdef BLACKBOX
                 if (feature(FEATURE_BLACKBOX)) {
-                	flightLogEvent_gtuneCycleResult_t eventData;
+                    flightLogEvent_gtuneCycleResult_t eventData;
 
                     eventData.gtuneAxis = axis;
                     eventData.gtuneGyroAVG = AvgGyro[axis];
diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h
index 1c835d2b0..55e7febfc 100644
--- a/src/main/target/SPARKY/target.h
+++ b/src/main/target/SPARKY/target.h
@@ -108,6 +108,7 @@
 #define CURRENT_METER_ADC_CHANNEL   ADC_Channel_4
 
 #define AUTOTUNE
+#define GTUNE
 #define BLACKBOX
 #define TELEMETRY
 #define SERIAL_RX

From 53531224be0c6bb9972ff3297968eab22a04d850 Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Mon, 4 May 2015 14:06:09 +0200
Subject: [PATCH 26/41] Make G-Tune more configurable - add two new CLI
 paramaters "gtune_settle_time" and "gtune_average_cycles" - the settle time
 is not depending on looptime anymore - updated default setting to cover e
 wider range of copters - remove lower limit for P value for CLI (Zero P is
 now posible, but schould be used with care) - Documentation updates

---
 docs/Gtune.md            | 23 ++++++++++++-----------
 src/main/config/config.c | 14 ++++++++------
 src/main/flight/gtune.c  | 40 +++++++++++++++++++++++++---------------
 src/main/flight/pid.h    |  6 ++++--
 src/main/io/serial_cli.c |  4 +++-
 5 files changed, 52 insertions(+), 35 deletions(-)

diff --git a/docs/Gtune.md b/docs/Gtune.md
index 6851b1192..d51d7adfd 100644
--- a/docs/Gtune.md
+++ b/docs/Gtune.md
@@ -14,7 +14,7 @@ When flying in Acro mode (yaw tune in other modes possible as well - see below)
 It will start tuning the wanted / possible axes (see below) in a predefined range (see below).
 After activation you will probably notice nothing! That means G-Tune will not start shaking your copter, you will have to do it (or simply fly and let it work).
 The G-Tune is based on the gyro error so it is only active when you give no RC input (that would be an additional error). So if you just roll only pitch
-and yaw are tuned. If you stop rolling G-Tune will wait ca. 400ms to let the axis settle and then start tuning that axis again. All axes are treated independently.
+and yaw are tuned. If you stop rolling G-Tune will wait ca. 450ms to let the axis settle and then start tuning that axis again. All axes are treated independently.
 The easiest way to tune all axes at once is to do some air-jumps with the copter in Acro (RC centered and G-Tune activated... of course..).
 You can set a too high P for the axes as default in the GUI, when the copter starts shaking the wobbles will be detected and P tuned down (be careful with the strength setting though - see below).
 Yaw tune is disabled in any copter with less than 4 motors (like tricopters).
@@ -24,17 +24,18 @@ TPA and G-Tune: It is not tested and will most likely not result into something
 
 ## Parameters and their function:
 
-    gtune_loP_rll  = 20 [10..200] Lower limit of ROLL P during G-Tune.  Note "20" means "2.0" in the GUI.
-    gtune_loP_ptch = 20 [10..200] Lower limit of PITCH P during G-Tune. Note "20" means "2.0" in the GUI.
-    gtune_loP_yw   = 20 [10..200] Lower limit of YAW P during G-Tune.   Note "20" means "2.0" in the GUI.
-    gtune_hiP_rll  = 70 [0..200]  Higher limit of ROLL P during G-Tune. 0 Disables tuning for that axis.  Note "70" means "7.0" in the GUI.
-    gtune_hiP_ptch = 70 [0..200]  Higher limit of PITCH P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI.
-    gtune_hiP_yw   = 70 [0..200]  Higher limit of YAW P during G-Tune. 0 Disables tuning for that axis.   Note "70" means "7.0" in the GUI.
-    gtune_pwr      = 0  [0..10]   Strength of adjustment
+    gtune_loP_rll        = 10  [0..200] Lower limit of ROLL P during G-Tune.  Note "10" means "1.0" in the GUI.
+    gtune_loP_ptch       = 10  [0..200] Lower limit of PITCH P during G-Tune. Note "10" means "1.0" in the GUI.
+    gtune_loP_yw         = 10  [0..200] Lower limit of YAW P during G-Tune.   Note "10" means "1.0" in the GUI.
+    gtune_hiP_rll        = 100 [0..200] Higher limit of ROLL P during G-Tune. 0 Disables tuning for that axis.  Note "100" means "10.0" in the GUI.
+    gtune_hiP_ptch       = 100 [0..200] Higher limit of PITCH P during G-Tune. 0 Disables tuning for that axis. Note "100" means "10.0" in the GUI.
+    gtune_hiP_yw         = 100 [0..200] Higher limit of YAW P during G-Tune. 0 Disables tuning for that axis.   Note "100" means "10.0" in the GUI.
+    gtune_pwr            = 0   [0..10] Strength of adjustment
+    gtune_settle_time    = 450 [200..1000] Settle time in ms
+    gtune_average_cycles = 16  [8..128] Number of looptime cycles used for gyro average calcullation
 
-So you have lower and higher limits for each P for every axis. The preset range (GUI: 2.0 - 7.0) is quiet broad to represent most setups.
-If you want tighter ranges change them here. The gtune_loP_XXX can not be lower than "10" that means a P of "1.0" in the GUI. So you can not have "Zero P",
-you are in maybe sluggish initial control.
+So you have lower and higher limits for each P for every axis. The preset range (GUI: 1.0 - 10.0) is quiet broad to represent most setups.
+If you want tighter or more loose ranges change them here. gtune_loP_XXX can be configured lower than "10" that means a P of "1.0" in the GUI. So you can have "Zero P" but you may get sluggish initial control.
 If you want to exclude one axis from the tuning you must set gtune_hiP_XXX to zero. Let's say you want to disable yaw tuning write in CLI
 "set gtune_hiP_yw = 0". Note: The MultiWii Wiki advises you to trim the yaw axis on your transmitter. If you have done so (yaw not neutral on your RC)
 yaw tuning will be disabled.
diff --git a/src/main/config/config.c b/src/main/config/config.c
index 8dc232f34..1e612592f 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -192,13 +192,15 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->pid5_maincuthz = 12;
 
 #ifdef GTUNE
-    pidProfile->gtune_lolimP[ROLL] = 20;          // [10..200] Lower limit of ROLL P during G tune.
-    pidProfile->gtune_lolimP[PITCH] = 20;         // [10..200] Lower limit of PITCH P during G tune.
-    pidProfile->gtune_lolimP[YAW] = 20;           // [10..200] Lower limit of YAW P during G tune.
-    pidProfile->gtune_hilimP[ROLL] = 70;          // [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axis.
-    pidProfile->gtune_hilimP[PITCH] = 70;         // [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axis.
-    pidProfile->gtune_hilimP[YAW] = 70;           // [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axis.
+    pidProfile->gtune_lolimP[ROLL] = 10;          // [0..200] Lower limit of ROLL P during G tune.
+    pidProfile->gtune_lolimP[PITCH] = 10;         // [0..200] Lower limit of PITCH P during G tune.
+    pidProfile->gtune_lolimP[YAW] = 10;           // [0..200] Lower limit of YAW P during G tune.
+    pidProfile->gtune_hilimP[ROLL] = 100;         // [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axis.
+    pidProfile->gtune_hilimP[PITCH] = 100;        // [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axis.
+    pidProfile->gtune_hilimP[YAW] = 100;          // [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axis.
     pidProfile->gtune_pwr = 0;                    // [0..10] Strength of adjustment
+    pidProfile->gtune_settle_time = 450;          // [200..1000] Settle time in ms
+    pidProfile->gtune_average_cycles = 16;        // [8..128] Number of looptime cycles used for gyro average calculation
 #endif
 }
 
diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c
index 63c68afdb..bcb322340 100644
--- a/src/main/flight/gtune.c
+++ b/src/main/flight/gtune.c
@@ -44,6 +44,7 @@
 
 #include "config/runtime_config.h"
 
+extern uint16_t cycleTime;
 extern uint8_t motorCount;
 
 /*
@@ -77,21 +78,29 @@ extern uint8_t motorCount;
    Gyrosetting 2000DPS
    GyroScale = (1 / 16,4 ) * RADX(see board.h) = 0,001064225154 digit per rad/s
 
-    pidProfile->gtune_lolimP[ROLL]   = 20; [10..200] Lower limit of ROLL P during G tune.
-    pidProfile->gtune_lolimP[PITCH]  = 20; [10..200] Lower limit of PITCH P during G tune.
-    pidProfile->gtune_lolimP[YAW]    = 20; [10..200] Lower limit of YAW P during G tune.
-    pidProfile->gtune_hilimP[ROLL]   = 70; [0..200]  Higher limit of ROLL P during G tune. 0 Disables tuning for that axisis.
-    pidProfile->gtune_hilimP[PITCH]  = 70; [0..200]  Higher limit of PITCH P during G tune. 0 Disables tuning for that axisis.
-    pidProfile->gtune_hilimP[YAW]    = 70; [0..200]  Higher limit of YAW P during G tune. 0 Disables tuning for that axisis.
-    pidProfile->gtune_pwr            = 0;  [0..10] Strength of adjustment
+    pidProfile->gtune_lolimP[ROLL]   = 10;  [0..200] Lower limit of ROLL P during G tune.
+    pidProfile->gtune_lolimP[PITCH]  = 10;  [0..200] Lower limit of PITCH P during G tune.
+    pidProfile->gtune_lolimP[YAW]    = 10;  [0..200] Lower limit of YAW P during G tune.
+    pidProfile->gtune_hilimP[ROLL]   = 100; [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axisis.
+    pidProfile->gtune_hilimP[PITCH]  = 100; [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axisis.
+    pidProfile->gtune_hilimP[YAW]    = 100; [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axisis.
+    pidProfile->gtune_pwr            = 0;   [0..10] Strength of adjustment
+    pidProfile->gtune_settle_time    = 450; [200..1000] Settle time in ms
+    pidProfile->gtune_average_cycles = 16;  [8..128] Number of looptime cycles used for gyro average calculation
 */
 
 static pidProfile_t *pidProfile;
-static int8_t time_skip[3];
+static int16_t delay_cycles;
+static int16_t time_skip[3];
 static int16_t OldError[3], result_P64[3];
 static int32_t AvgGyro[3];
 static bool floatPID;
 
+void updateDelayCycles(void)
+{
+    delay_cycles = -(((int32_t)pidProfile->gtune_settle_time * 1000) / cycleTime);
+}
+
 void init_Gtune(pidProfile_t *pidProfileToTune)
 {
     uint8_t i;
@@ -99,9 +108,10 @@ void init_Gtune(pidProfile_t *pidProfileToTune)
     pidProfile = pidProfileToTune;
 	if (pidProfile->pidController == 2) floatPID = true;                        // LuxFloat is using float values for PID settings
 	else floatPID = false;
+	updateDelayCycles();
 	for (i = 0; i < 3; i++) {
         if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || // User config error disable axisis for tuning
-            (motorCount < 4 && i == FD_YAW)) pidProfile->gtune_hilimP[i] = 0;   // Disable Yawtuning for everything below a quadcopter
+            (motorCount < 4 && i == FD_YAW)) pidProfile->gtune_hilimP[i] = 0;   // Disable yawtuning for everything below a quadcopter
         if (floatPID) {
             if((pidProfile->P_f[i] * 10) < pidProfile->gtune_lolimP[i]) pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10);
             result_P64[i] = (int16_t)pidProfile->P_f[i] << 6;                   // 6 bit extra resolution for P.
@@ -110,7 +120,7 @@ void init_Gtune(pidProfile_t *pidProfileToTune)
             result_P64[i] = (int16_t)pidProfile->P8[i] << 6;                    // 6 bit extra resolution for P.
         }
         OldError[i] = 0;
-        time_skip[i] = -125;
+        time_skip[i] = delay_cycles;
     }
 }
 
@@ -118,9 +128,9 @@ void calculate_Gtune(uint8_t axis)
 {
     int16_t error, diff_G, threshP;
 
-    if(rcCommand[axis] || (axis != FD_YAW && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)))) {  // Block Tuning on stickinput. Always allow Gtune on YAW, Roll & Pitch only in acromode
+    if(rcCommand[axis] || (axis != FD_YAW && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)))) {  // Block tuning on stick input. Always allow G-Tune on YAW, Roll & Pitch only in acromode
         OldError[axis] = 0;
-        time_skip[axis] = -125;                                                 // Some settletime after stick center. (125 + 16)* 3ms clycle = 423ms (ca.)
+        time_skip[axis] = delay_cycles;                                         // Some settle time after stick center. default 450ms
     } else {
         if (!time_skip[axis]) AvgGyro[axis] = 0;
         time_skip[axis]++;
@@ -128,7 +138,7 @@ void calculate_Gtune(uint8_t axis)
             if (axis == FD_YAW) AvgGyro[axis] += 32 * ((int16_t)gyroData[axis] / 32); // Chop some jitter and average
             else AvgGyro[axis] += 128 * ((int16_t)gyroData[axis] / 128);        // Chop some jitter and average
         }
-        if (time_skip[axis] == 16) {                                            // ca 48 ms
+        if (time_skip[axis] == pidProfile->gtune_average_cycles) {              // Looptime cycles for gyro average calculation. default 16.
             AvgGyro[axis] /= time_skip[axis];                                   // AvgGyro[axis] has now very clean gyrodata
             time_skip[axis] = 0;
             if (axis == FD_YAW) {
@@ -166,8 +176,8 @@ void calculate_Gtune(uint8_t axis)
                 }
 #endif
 
-                if (floatPID) pidProfile->P_f[axis] = (float)(newP / 10);     // new P value for float PID
-                else pidProfile->P8[axis] = newP;                             // new P value
+                if (floatPID) pidProfile->P_f[axis] = (float)(newP / 10);       // new P value for float PID
+                else pidProfile->P8[axis] = newP;                               // new P value
             }
             OldError[axis] = error;
         }
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 21a6e048f..52c50eb8e 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -70,9 +70,11 @@ typedef struct pidProfile_s {
     uint8_t pid5_oldyw;                     // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw
 
 #ifdef GTUNE
-    uint8_t  gtune_lolimP[3];               // [10..200] Lower limit of P during G tune
+    uint8_t  gtune_lolimP[3];               // [0..200] Lower limit of P during G tune
     uint8_t  gtune_hilimP[3];               // [0..200] Higher limit of P during G tune. 0 Disables tuning for that axis.
-    int8_t   gtune_pwr;                     // [0..10] Strength of adjustment
+    uint8_t  gtune_pwr;                     // [0..10] Strength of adjustment
+    uint16_t gtune_settle_time;             // [200..1000] Settle time in ms
+    uint8_t  gtune_average_cycles;          // [8..128] Number of looptime cycles used for gyro average calculation
 #endif
 } pidProfile_t;
 
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 7ee8e4314..9822ef9f4 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -524,7 +524,9 @@ const clivalue_t valueTable[] = {
     { "gtune_hiP_rll",              VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_ROLL], 0, 200 },
     { "gtune_hiP_ptch",             VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_PITCH], 0, 200 },
     { "gtune_hiP_yw",               VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_YAW], 0, 200 },
-    { "gtune_pwr",                  VAR_INT8   | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_pwr, 0, 10 },
+    { "gtune_pwr",                  VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_pwr, 0, 10 },
+    { "gtune_settle_time",          VAR_UINT16 | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_settle_time, 200, 1000 },
+    { "gtune_average_cycles",       VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_average_cycles, 8, 128 },
 #endif
 
 #ifdef BLACKBOX

From a8aad05c5ae86dff5d13028a522bcb2a2e737220 Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Fri, 8 May 2015 20:19:32 +0200
Subject: [PATCH 27/41] Added additional yaw handling according to Spirre's
 PID2 testing. Flight tested with PID3 and PID5 (still functional without
 negative side effects) Additional code style updates of gtune.c

---
 src/main/flight/gtune.c | 62 +++++++++++++++++++++++++++++------------
 1 file changed, 44 insertions(+), 18 deletions(-)

diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c
index bcb322340..412f5cbfd 100644
--- a/src/main/flight/gtune.c
+++ b/src/main/flight/gtune.c
@@ -106,17 +106,25 @@ void init_Gtune(pidProfile_t *pidProfileToTune)
     uint8_t i;
 
     pidProfile = pidProfileToTune;
-	if (pidProfile->pidController == 2) floatPID = true;                        // LuxFloat is using float values for PID settings
-	else floatPID = false;
+	if (pidProfile->pidController == 2) {
+	    floatPID = true;                        // LuxFloat is using float values for PID settings
+	} else {
+	    floatPID = false;
+	}
 	updateDelayCycles();
 	for (i = 0; i < 3; i++) {
-        if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || // User config error disable axisis for tuning
-            (motorCount < 4 && i == FD_YAW)) pidProfile->gtune_hilimP[i] = 0;   // Disable yawtuning for everything below a quadcopter
+        if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || (motorCount < 4 && i == FD_YAW)) { // User config error disable axisis for tuning
+            pidProfile->gtune_hilimP[i] = 0;                                    // Disable yawtuning for everything below a quadcopter
+        }
         if (floatPID) {
-            if((pidProfile->P_f[i] * 10) < pidProfile->gtune_lolimP[i]) pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10);
+            if((pidProfile->P_f[i] * 10) < pidProfile->gtune_lolimP[i]) {
+                pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10);
+            }
             result_P64[i] = (int16_t)pidProfile->P_f[i] << 6;                   // 6 bit extra resolution for P.
         } else {
-            if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) pidProfile->P8[i] = pidProfile->gtune_lolimP[i];
+            if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) {
+                pidProfile->P8[i] = pidProfile->gtune_lolimP[i];
+            }
             result_P64[i] = (int16_t)pidProfile->P8[i] << 6;                    // 6 bit extra resolution for P.
         }
         OldError[i] = 0;
@@ -135,8 +143,11 @@ void calculate_Gtune(uint8_t axis)
         if (!time_skip[axis]) AvgGyro[axis] = 0;
         time_skip[axis]++;
         if (time_skip[axis] > 0) {
-            if (axis == FD_YAW) AvgGyro[axis] += 32 * ((int16_t)gyroData[axis] / 32); // Chop some jitter and average
-            else AvgGyro[axis] += 128 * ((int16_t)gyroData[axis] / 128);        // Chop some jitter and average
+            if (axis == FD_YAW) {
+                AvgGyro[axis] += 32 * ((int16_t)gyroData[axis] / 32);           // Chop some jitter and average
+            } else {
+                AvgGyro[axis] += 128 * ((int16_t)gyroData[axis] / 128);         // Chop some jitter and average
+            }
         }
         if (time_skip[axis] == pidProfile->gtune_average_cycles) {              // Looptime cycles for gyro average calculation. default 16.
             AvgGyro[axis] /= time_skip[axis];                                   // AvgGyro[axis] has now very clean gyrodata
@@ -151,15 +162,25 @@ void calculate_Gtune(uint8_t axis)
             if (pidProfile->gtune_hilimP[axis] && error && OldError[axis] && error != OldError[axis]) {  // Don't run when not needed or pointless to do so
                 diff_G = ABS(error) - ABS(OldError[axis]);
                 if ((error > 0 && OldError[axis] > 0) || (error < 0 && OldError[axis] < 0)) {
-                    if (diff_G > threshP) result_P64[axis] += 64 + pidProfile->gtune_pwr; // Shift balance a little on the plus side.
-                    else {
+                    if (diff_G > threshP) {
+                        if (axis == FD_YAW) {
+                            result_P64[axis] += 256 + pidProfile->gtune_pwr;    // YAW ends up at low limit on PID2, give it some more to work with.
+                        } else {
+                            result_P64[axis] += 64 + pidProfile->gtune_pwr;     // Shift balance a little on the plus side.
+                        }
+                    } else {
                         if (diff_G < -threshP) {
-                            if (axis == FD_YAW) result_P64[axis] -= 64 + pidProfile->gtune_pwr;
-                            else result_P64[axis] -= 32;
+                            if (axis == FD_YAW) {
+                                result_P64[axis] -= 64 + pidProfile->gtune_pwr;
+                            } else {
+                                result_P64[axis] -= 32;
+                            }
                         }
                     }
                 } else {
-                    if (ABS(diff_G) > threshP && axis != FD_YAW) result_P64[axis] -= 32; // Don't use antiwobble for YAW
+                    if (ABS(diff_G) > threshP && axis != FD_YAW) {
+                        result_P64[axis] -= 32;                                 // Don't use antiwobble for YAW
+                    }
                 }
                 int16_t newP = constrain((result_P64[axis] >> 6), (int16_t)pidProfile->gtune_lolimP[axis], (int16_t)pidProfile->gtune_hilimP[axis]);
 
@@ -169,15 +190,20 @@ void calculate_Gtune(uint8_t axis)
 
                     eventData.gtuneAxis = axis;
                     eventData.gtuneGyroAVG = AvgGyro[axis];
-                    if (floatPID) eventData.gtuneNewP = newP / 10;
-                    else eventData.gtuneNewP = newP;
-
+                    if (floatPID) {
+                        eventData.gtuneNewP = newP / 10;
+                    } else {
+                        eventData.gtuneNewP = newP;
+                    }
                     blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData);
                 }
 #endif
 
-                if (floatPID) pidProfile->P_f[axis] = (float)(newP / 10);       // new P value for float PID
-                else pidProfile->P8[axis] = newP;                               // new P value
+                if (floatPID) {
+                    pidProfile->P_f[axis] = (float)(newP / 10);                 // new P value for float PID
+                } else {
+                    pidProfile->P8[axis] = newP;                                // new P value
+                }
             }
             OldError[axis] = error;
         }

From afed9a5bba75bcce71dddb12b44ad0982ffd0912 Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Sat, 9 May 2015 16:41:26 +0200
Subject: [PATCH 28/41] G-Tune fix for PID controller 2 (LuxFloat)

---
 src/main/flight/gtune.c | 18 +++++++-----------
 1 file changed, 7 insertions(+), 11 deletions(-)

diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c
index 412f5cbfd..6fa2b1950 100644
--- a/src/main/flight/gtune.c
+++ b/src/main/flight/gtune.c
@@ -107,18 +107,18 @@ void init_Gtune(pidProfile_t *pidProfileToTune)
 
     pidProfile = pidProfileToTune;
 	if (pidProfile->pidController == 2) {
-	    floatPID = true;                        // LuxFloat is using float values for PID settings
+	    floatPID = true;                                                        // LuxFloat is using float values for PID settings
 	} else {
 	    floatPID = false;
 	}
 	updateDelayCycles();
 	for (i = 0; i < 3; i++) {
         if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || (motorCount < 4 && i == FD_YAW)) { // User config error disable axisis for tuning
-            pidProfile->gtune_hilimP[i] = 0;                                    // Disable yawtuning for everything below a quadcopter
+            pidProfile->gtune_hilimP[i] = 0;                                    // Disable YAW tuning for everything below a quadcopter
         }
         if (floatPID) {
-            if((pidProfile->P_f[i] * 10) < pidProfile->gtune_lolimP[i]) {
-                pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10);
+            if((pidProfile->P_f[i] * 10.0f) < pidProfile->gtune_lolimP[i]) {
+                pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10.0f);
             }
             result_P64[i] = (int16_t)pidProfile->P_f[i] << 6;                   // 6 bit extra resolution for P.
         } else {
@@ -164,7 +164,7 @@ void calculate_Gtune(uint8_t axis)
                 if ((error > 0 && OldError[axis] > 0) || (error < 0 && OldError[axis] < 0)) {
                     if (diff_G > threshP) {
                         if (axis == FD_YAW) {
-                            result_P64[axis] += 256 + pidProfile->gtune_pwr;    // YAW ends up at low limit on PID2, give it some more to work with.
+                            result_P64[axis] += 256 + pidProfile->gtune_pwr;    // YAW ends up at low limit on float PID, give it some more to work with.
                         } else {
                             result_P64[axis] += 64 + pidProfile->gtune_pwr;     // Shift balance a little on the plus side.
                         }
@@ -190,17 +190,13 @@ void calculate_Gtune(uint8_t axis)
 
                     eventData.gtuneAxis = axis;
                     eventData.gtuneGyroAVG = AvgGyro[axis];
-                    if (floatPID) {
-                        eventData.gtuneNewP = newP / 10;
-                    } else {
-                        eventData.gtuneNewP = newP;
-                    }
+                    eventData.gtuneNewP = newP;                                 // for float PID the logged P value is still mutiplyed by 10
                     blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData);
                 }
 #endif
 
                 if (floatPID) {
-                    pidProfile->P_f[axis] = (float)(newP / 10);                 // new P value for float PID
+                    pidProfile->P_f[axis] = (float)newP / 10.0f;                // new P value for float PID
                 } else {
                     pidProfile->P8[axis] = newP;                                // new P value
                 }

From 4bed8bc78a7725a2aef7c322874dc9c4b1c2592c Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Mon, 11 May 2015 06:39:37 +0200
Subject: [PATCH 29/41] Add G-Tune for NAZE32PRO target

---
 src/main/target/NAZE32PRO/target.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/target/NAZE32PRO/target.h b/src/main/target/NAZE32PRO/target.h
index 6b35128d3..608b90e0d 100644
--- a/src/main/target/NAZE32PRO/target.h
+++ b/src/main/target/NAZE32PRO/target.h
@@ -45,6 +45,7 @@
 #define TELEMETRY
 #define SERIAL_RX
 #define AUTOTUNE
+#define GTUNE
 #define USE_SERVOS
 #define USE_CLI
 

From 31dd2fc223f6a36c6ef866e8ccbc5c115711aad6 Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Tue, 12 May 2015 16:16:43 +0200
Subject: [PATCH 30/41] G-Tune documentation update

---
 docs/Modes.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/docs/Modes.md b/docs/Modes.md
index 115f273e0..8e74f1fde 100644
--- a/docs/Modes.md
+++ b/docs/Modes.md
@@ -28,6 +28,7 @@ auxillary receiver channels and other events such as failsafe detection.
 | 21      | 20     | AUTOTUNE   | Autotune Pitch/Roll PIDs                                             |
 | 22      | 21     | SONAR      | Altitude hold mode (sonar sensor only)                               |
 | 26      | 25     | BLACKBOX   | Enable BlackBox logging                                              |
+| 27      | 26     | GTUNE      | G-Tune - auto tuning of Pitch/Roll/Yaw P values                      |
 
 ## Mode details
 

From edf08648bce01bdb31f6880af8df230536fd53f1 Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Sat, 30 May 2015 02:04:46 +0200
Subject: [PATCH 31/41] Replace gyroData with gyroADC in gtune.c after rebase

---
 src/main/flight/gtune.c | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c
index 6fa2b1950..43fcf37f3 100644
--- a/src/main/flight/gtune.c
+++ b/src/main/flight/gtune.c
@@ -32,6 +32,7 @@
 #include "drivers/accgyro.h"
 
 #include "sensors/sensors.h"
+#include "sensors/gyro.h"
 #include "sensors/acceleration.h"
 
 #include "flight/pid.h"
@@ -144,9 +145,9 @@ void calculate_Gtune(uint8_t axis)
         time_skip[axis]++;
         if (time_skip[axis] > 0) {
             if (axis == FD_YAW) {
-                AvgGyro[axis] += 32 * ((int16_t)gyroData[axis] / 32);           // Chop some jitter and average
+                AvgGyro[axis] += 32 * ((int16_t)gyroADC[axis] / 32);           // Chop some jitter and average
             } else {
-                AvgGyro[axis] += 128 * ((int16_t)gyroData[axis] / 128);         // Chop some jitter and average
+                AvgGyro[axis] += 128 * ((int16_t)gyroADC[axis] / 128);         // Chop some jitter and average
             }
         }
         if (time_skip[axis] == pidProfile->gtune_average_cycles) {              // Looptime cycles for gyro average calculation. default 16.

From 72ff296850a58abe664ce6be5f8b5faa0f100536 Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Thu, 4 Jun 2015 18:25:40 +0200
Subject: [PATCH 32/41] Enable G-Tune for CC3D (but not included in the OPBL
 version)

---
 src/main/target/CC3D/target.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index a79dab592..136a7171a 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -113,6 +113,7 @@
 #define SERIAL_RX
 #define SONAR
 #define AUTOTUNE
+#define GTUNE
 #define USE_SERVOS
 #define USE_CLI
 
@@ -120,6 +121,7 @@
 // disabled some features for OPBL build due to code size.
 #undef AUTOTUNE
 #undef DISPLAY
+#undef GTUNE
 #undef SONAR
 #define SKIP_CLI_COMMAND_HELP
 #endif

From e42ed1ad7037d453b59c2454161e0f25cdf63ea7 Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Mon, 29 Jun 2015 22:34:38 +0200
Subject: [PATCH 33/41] Update to integrate with BorisB filters after rebase.
 Documentation update

---
 docs/PID tuning.md       | 3 ++-
 src/main/config/config.c | 1 -
 src/main/io/serial_cli.c | 1 -
 3 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/docs/PID tuning.md b/docs/PID tuning.md
index 972584c5e..a125739c1 100644
--- a/docs/PID tuning.md	
+++ b/docs/PID tuning.md	
@@ -152,7 +152,8 @@ PID Controller 5 is an port of the PID controller from the Harakiri firmware.
 
 The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don't need retuning of the PID values when looptime is changing. There are two additional settings which are configurable via the CLI in Harakiri:
 
-        set pid5_maincuthz = 12     [1-50Hz] Cut Off Frequency for D term of main Pid controller
+        set dterm_cut_hz = 0        [1-50Hz] Cut Off Frequency for D term of main PID controller
+                                    (default of 0 equals to 12Hz which was the hardcoded setting in previous Cleanflight versions)
         set pid5_oldyw = 0          [0/1] 0 = multiwii 2.3 yaw (default), 1 = older yaw
 
 The PID controller is flight tested and running well with the default PID settings. If you want do acrobatics start slowly.
diff --git a/src/main/config/config.c b/src/main/config/config.c
index 1e612592f..5eab90d1e 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -189,7 +189,6 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->H_sensitivity = 75;
 
     pidProfile->pid5_oldyw = 0;
-    pidProfile->pid5_maincuthz = 12;
 
 #ifdef GTUNE
     pidProfile->gtune_lolimP[ROLL] = 10;          // [0..200] Lower limit of ROLL P during G tune.
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 9822ef9f4..41200fdeb 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -515,7 +515,6 @@ const clivalue_t valueTable[] = {
 	{ "gyro_cut_hz",                VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 },
 
     { "pid5_oldyw",                 VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.pid5_oldyw, 0, 1 },
-    { "pid5_maincuthz",             VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.pid5_maincuthz, 1, 50 },
 
 #ifdef GTUNE
     { "gtune_loP_rll",              VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], 10, 200 },

From f15cedd057c347dec3678216b7af01b2d1c743ec Mon Sep 17 00:00:00 2001
From: Michael Jakob <michael.p.jakob@web.de>
Date: Tue, 4 Aug 2015 15:09:38 +0200
Subject: [PATCH 34/41] Fixes after rebase

---
 src/main/blackbox/blackbox_fielddefs.h | 7 +------
 1 file changed, 1 insertion(+), 6 deletions(-)

diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h
index fcd087584..47c128c1e 100644
--- a/src/main/blackbox/blackbox_fielddefs.h
+++ b/src/main/blackbox/blackbox_fielddefs.h
@@ -149,18 +149,16 @@ typedef struct flightLogEvent_inflightAdjustment_t {
     float newFloatValue;
 } flightLogEvent_inflightAdjustment_t;
 
-<<<<<<< Upstream, based on origin/master
 typedef struct flightLogEvent_loggingResume_t {
     uint32_t logIteration;
     uint32_t currentTime;
 } flightLogEvent_loggingResume_t;
-=======
+
 typedef struct flightLogEvent_gtuneCycleResult_t {
     uint8_t gtuneAxis;
     int32_t gtuneGyroAVG;
     int16_t gtuneNewP;
 } flightLogEvent_gtuneCycleResult_t;
->>>>>>> 6f60c52 Add BlackBox recording for G-Tune
 
 typedef union flightLogEventData_t
 {
@@ -169,11 +167,8 @@ typedef union flightLogEventData_t
     flightLogEvent_autotuneCycleResult_t autotuneCycleResult;
     flightLogEvent_autotuneTargets_t autotuneTargets;
     flightLogEvent_inflightAdjustment_t inflightAdjustment;
-<<<<<<< Upstream, based on origin/master
     flightLogEvent_loggingResume_t loggingResume;
-=======
     flightLogEvent_gtuneCycleResult_t gtuneCycleResult;
->>>>>>> 6f60c52 Add BlackBox recording for G-Tune
 } flightLogEventData_t;
 
 typedef struct flightLogEvent_t

From e5f3f1794fade3185bd7bba164072a3e1aed4257 Mon Sep 17 00:00:00 2001
From: Sean Vig <sean.v.775@gmail.com>
Date: Thu, 17 Sep 2015 00:27:25 -0500
Subject: [PATCH 35/41] Add tests for BMP280 barometer

Also get rid of the curved quotes for normal quotes (") and make
compensation computations static.
---
 src/main/drivers/barometer_bmp280.c   |  18 +--
 src/test/Makefile                     |  23 ++++
 src/test/unit/baro_bmp280_unittest.cc | 154 ++++++++++++++++++++++++++
 3 files changed, 186 insertions(+), 9 deletions(-)
 create mode 100644 src/test/unit/baro_bmp280_unittest.cc

diff --git a/src/main/drivers/barometer_bmp280.c b/src/main/drivers/barometer_bmp280.c
index 3edb55c5f..6591c72a1 100644
--- a/src/main/drivers/barometer_bmp280.c
+++ b/src/main/drivers/barometer_bmp280.c
@@ -90,16 +90,16 @@ typedef struct bmp280_calib_param_t {
 
 static uint8_t bmp280_chip_id = 0;
 static bool bmp280InitDone = false;
-static bmp280_calib_param_t bmp280_cal;
+STATIC_UNIT_TESTED bmp280_calib_param_t bmp280_cal;
 // uncompensated pressure and temperature
-static int32_t bmp280_up = 0;
-static int32_t bmp280_ut = 0;
+STATIC_UNIT_TESTED int32_t bmp280_up = 0;
+STATIC_UNIT_TESTED int32_t bmp280_ut = 0;
 
 static void bmp280_start_ut(void);
 static void bmp280_get_ut(void);
 static void bmp280_start_up(void);
 static void bmp280_get_up(void);
-static void bmp280_calculate(int32_t *pressure, int32_t *temperature);
+STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature);
 
 bool bmp280Detect(baro_t *baro)
 {
@@ -160,9 +160,9 @@ static void bmp280_get_up(void)
     bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4));
 }
 
-// Returns temperature in DegC, float precision. Output value of “51.23” equals 51.23 DegC.
+// Returns temperature in DegC, float precision. Output value of "51.23" equals 51.23 DegC.
 // t_fine carries fine temperature as global value
-float bmp280_compensate_T(int32_t adc_T)
+static float bmp280_compensate_T(int32_t adc_T)
 {
     float var1, var2, T;
 
@@ -174,8 +174,8 @@ float bmp280_compensate_T(int32_t adc_T)
     return T;
 }
 
-// Returns pressure in Pa as float. Output value of “96386.2” equals 96386.2 Pa = 963.862 hPa
-float bmp280_compensate_P(int32_t adc_P)
+// Returns pressure in Pa as float. Output value of "96386.2" equals 96386.2 Pa = 963.862 hPa
+static float bmp280_compensate_P(int32_t adc_P)
 {
     float var1, var2, p;
     var1 = ((float)bmp280_cal.t_fine / 2.0f) - 64000.0f;
@@ -196,7 +196,7 @@ float bmp280_compensate_P(int32_t adc_P)
     return p;
 }
 
-static void bmp280_calculate(int32_t *pressure, int32_t *temperature)
+STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature)
 {
     // calculate
     float t, p;
diff --git a/src/test/Makefile b/src/test/Makefile
index b48353663..5bbc50b7b 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -512,6 +512,29 @@ $(OBJECT_DIR)/baro_bmp085_unittest : \
 
 	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
 
+$(OBJECT_DIR)/drivers/barometer_bmp280.o : \
+    $(USER_DIR)/drivers/barometer_bmp280.c \
+    $(USER_DIR)/drivers/barometer_bmp280.h \
+    $(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/barometer_bmp280.c -o $@
+
+$(OBJECT_DIR)/baro_bmp280_unittest.o : \
+	$(TEST_DIR)/baro_bmp280_unittest.cc \
+	$(USER_DIR)/drivers/barometer_bmp280.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/baro_bmp280_unittest.cc -o $@
+
+$(OBJECT_DIR)/baro_bmp280_unittest : \
+	$(OBJECT_DIR)/drivers/barometer_bmp280.o \
+	$(OBJECT_DIR)/baro_bmp280_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
 $(OBJECT_DIR)/sensors/boardalignment.o : \
 	$(USER_DIR)/sensors/boardalignment.c \
 	$(USER_DIR)/sensors/boardalignment.h \
diff --git a/src/test/unit/baro_bmp280_unittest.cc b/src/test/unit/baro_bmp280_unittest.cc
new file mode 100644
index 000000000..50f1b5630
--- /dev/null
+++ b/src/test/unit/baro_bmp280_unittest.cc
@@ -0,0 +1,154 @@
+/*
+ * 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/>.
+ */
+#include <stdint.h>
+
+extern "C" {
+
+    void bmp280_calculate(int32_t *pressure, int32_t *temperature);
+    extern uint32_t bmp280_up;
+    extern uint32_t bmp280_ut;
+
+typedef struct bmp280_calib_param_t {
+    uint16_t dig_T1; /* calibration T1 data */
+    int16_t dig_T2; /* calibration T2 data */
+    int16_t dig_T3; /* calibration T3 data */
+    uint16_t dig_P1; /* calibration P1 data */
+    int16_t dig_P2; /* calibration P2 data */
+    int16_t dig_P3; /* calibration P3 data */
+    int16_t dig_P4; /* calibration P4 data */
+    int16_t dig_P5; /* calibration P5 data */
+    int16_t dig_P6; /* calibration P6 data */
+    int16_t dig_P7; /* calibration P7 data */
+    int16_t dig_P8; /* calibration P8 data */
+    int16_t dig_P9; /* calibration P9 data */
+    int32_t t_fine; /* calibration t_fine data */
+} bmp280_calib_param_t;
+
+    bmp280_calib_param_t bmp280_cal;
+}
+
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+
+TEST(baroBmp280Test, TestBmp280Calculate)
+{
+
+    // given
+    int32_t pressure, temperature;
+    bmp280_up = 415148; // Digital pressure value
+    bmp280_ut = 519888; // Digital temperature value
+
+    // and
+    bmp280_cal.dig_T1 = 27504;
+    bmp280_cal.dig_T2 = 26435;
+    bmp280_cal.dig_T3 = -1000;
+    bmp280_cal.dig_P1 = 36477;
+    bmp280_cal.dig_P2 = -10685;
+    bmp280_cal.dig_P3 = 3024;
+    bmp280_cal.dig_P4 = 2855;
+    bmp280_cal.dig_P5 = 140;
+    bmp280_cal.dig_P6 = -7;
+    bmp280_cal.dig_P7 = 15500;
+    bmp280_cal.dig_P8 = -14600;
+    bmp280_cal.dig_P9 = 6000;
+
+    // when
+    bmp280_calculate(&pressure, &temperature);
+
+    // then
+    EXPECT_EQ(100653, pressure); // 100653 Pa
+    EXPECT_EQ(2500, temperature); // 25.00 degC (data sheet says 25.08)
+
+}
+
+TEST(baroBmp280Test, TestBmp280CalculateHighP)
+{
+
+    // given
+    int32_t pressure, temperature;
+    bmp280_up = 215148; // Digital pressure value
+    bmp280_ut = 519888; // Digital temperature value
+
+    // and
+    bmp280_cal.dig_T1 = 27504;
+    bmp280_cal.dig_T2 = 26435;
+    bmp280_cal.dig_T3 = -1000;
+    bmp280_cal.dig_P1 = 36477;
+    bmp280_cal.dig_P2 = -10685;
+    bmp280_cal.dig_P3 = 3024;
+    bmp280_cal.dig_P4 = 2855;
+    bmp280_cal.dig_P5 = 140;
+    bmp280_cal.dig_P6 = -7;
+    bmp280_cal.dig_P7 = 15500;
+    bmp280_cal.dig_P8 = -14600;
+    bmp280_cal.dig_P9 = 6000;
+
+    // when
+    bmp280_calculate(&pressure, &temperature);
+
+    // then
+    EXPECT_EQ(135382, pressure); // 135385 Pa
+    EXPECT_EQ(2500, temperature); // 25.00 degC (data sheet says 25.08)
+
+}
+
+TEST(baroBmp280Test, TestBmp280CalculateZeroP)
+{
+
+    // given
+    int32_t pressure, temperature;
+    bmp280_up = 415148; // Digital pressure value
+    bmp280_ut = 519888; // Digital temperature value
+
+    // and
+    bmp280_cal.dig_T1 = 27504;
+    bmp280_cal.dig_T2 = 26435;
+    bmp280_cal.dig_T3 = -1000;
+    bmp280_cal.dig_P1 = 0;
+    bmp280_cal.dig_P2 = -10685;
+    bmp280_cal.dig_P3 = 3024;
+    bmp280_cal.dig_P4 = 2855;
+    bmp280_cal.dig_P5 = 140;
+    bmp280_cal.dig_P6 = -7;
+    bmp280_cal.dig_P7 = 15500;
+    bmp280_cal.dig_P8 = -14600;
+    bmp280_cal.dig_P9 = 6000;
+
+    // when
+    bmp280_calculate(&pressure, &temperature);
+
+    // then
+    EXPECT_EQ(0, pressure); // P1=0 trips pressure to 0 Pa, avoiding division by zero
+    EXPECT_EQ(2500, temperature); // 25.00 degC (data sheet says 25.08)
+
+}
+
+// STUBS
+
+extern "C" {
+
+    void delay(uint32_t) {}
+    bool i2cWrite(uint8_t, uint8_t, uint8_t) {
+        return 1;
+    }
+    bool i2cRead(uint8_t, uint8_t, uint8_t, uint8_t) {
+        return 1;
+    }
+
+}

From 456396b25a9597ec614d60a381478b55dea68756 Mon Sep 17 00:00:00 2001
From: Sean Vig <sean.v.775@gmail.com>
Date: Thu, 17 Sep 2015 01:54:43 -0500
Subject: [PATCH 36/41] Use fixed point calculation for BMP280 barometer

Substitute the floating point calculation for the comparably much faster
64-bit calculation recommended for a Cortex-M3 (floating point is
recommended only in PC applications with FPU) [1]. Even if we have an
FPU, we round the temperature to the nearest 1/100th of a degree and the
pressure to the nearest 1 Pa, both of which are within the limits of the
fixed-point algorithm's accuracy.

Conflicts:
	src/main/drivers/barometer_bmp280.c
---
 src/main/drivers/barometer_bmp280.c   | 58 +++++++++++++--------------
 src/test/unit/baro_bmp280_unittest.cc |  6 +--
 2 files changed, 32 insertions(+), 32 deletions(-)

diff --git a/src/main/drivers/barometer_bmp280.c b/src/main/drivers/barometer_bmp280.c
index 6591c72a1..b72531f3a 100644
--- a/src/main/drivers/barometer_bmp280.c
+++ b/src/main/drivers/barometer_bmp280.c
@@ -160,53 +160,53 @@ static void bmp280_get_up(void)
     bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4));
 }
 
-// Returns temperature in DegC, float precision. Output value of "51.23" equals 51.23 DegC.
+// Returns temperature in DegC, resolution is 0.01 DegC. Output value of "5123" equals 51.23 DegC
 // t_fine carries fine temperature as global value
-static float bmp280_compensate_T(int32_t adc_T)
+static int32_t bmp280_compensate_T(int32_t adc_T)
 {
-    float var1, var2, T;
+    int32_t var1, var2, T;
 
-    var1 = (((float)adc_T) / 16384.0f - ((float)bmp280_cal.dig_T1) / 1024.0f) * ((float)bmp280_cal.dig_T2);
-    var2 = ((((float)adc_T) / 131072.0f - ((float)bmp280_cal.dig_T1) / 8192.0f) * (((float)adc_T) / 131072.0f - ((float)bmp280_cal.dig_T1) / 8192.0f)) * ((float)bmp280_cal.dig_T3);
-    bmp280_cal.t_fine = (int32_t)(var1 + var2);
-    T = (var1 + var2) / 5120.0f;
+    var1 = ((((adc_T >> 3) - ((int32_t)bmp280_cal.dig_T1 << 1))) * ((int32_t)bmp280_cal.dig_T2)) >> 11;
+    var2  = (((((adc_T >> 4) - ((int32_t)bmp280_cal.dig_T1)) * ((adc_T >> 4) - ((int32_t)bmp280_cal.dig_T1))) >> 12) * ((int32_t)bmp280_cal.dig_T3)) >> 14;
+    bmp280_cal.t_fine = var1 + var2;
+    T = (bmp280_cal.t_fine * 5 + 128) >> 8;
 
     return T;
 }
 
-// Returns pressure in Pa as float. Output value of "96386.2" equals 96386.2 Pa = 963.862 hPa
-static float bmp280_compensate_P(int32_t adc_P)
+// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8 fractional bits).
+// Output value of "24674867" represents 24674867/256 = 96386.2 Pa = 963.862 hPa
+static uint32_t bmp280_compensate_P(int32_t adc_P)
 {
-    float var1, var2, p;
-    var1 = ((float)bmp280_cal.t_fine / 2.0f) - 64000.0f;
-    var2 = var1 * var1 * ((float)bmp280_cal.dig_P6) / 32768.0f;
-    var2 = var2 + var1 * ((float)bmp280_cal.dig_P5) * 2.0f;
-    var2 = (var2 / 4.0f) + (((float)bmp280_cal.dig_P4) * 65536.0f);
-    var1 = (((float)bmp280_cal.dig_P3) * var1 * var1 / 524288.0f + ((float)bmp280_cal.dig_P2) * var1) / 524288.0f;
-    var1 = (1.0f + var1 / 32768.0f) * ((float)bmp280_cal.dig_P1);
-    if (var1 == 0.0f)
-        return 0.0f; // avoid exception caused by division by zero
-
-    p = 1048576.0f - (float)adc_P;
-    p = (p - (var2 / 4096.0f)) * 6250.0f / var1;
-    var1 = ((float)bmp280_cal.dig_P9) * p * p / 2147483648.0f;
-    var2 = p * ((float)bmp280_cal.dig_P8) / 32768.0f;
-    p = p + (var1 + var2 + ((float)bmp280_cal.dig_P7)) / 16.0f;
-
-    return p;
+    int64_t var1, var2, p;
+    var1 = ((int64_t)bmp280_cal.t_fine) - 128000;
+    var2 = var1 * var1 * (int64_t)bmp280_cal.dig_P6;
+    var2 = var2 + ((var1*(int64_t)bmp280_cal.dig_P5) << 17);
+    var2 = var2 + (((int64_t)bmp280_cal.dig_P4) << 35);
+    var1 = ((var1 * var1 * (int64_t)bmp280_cal.dig_P3) >> 8) + ((var1 * (int64_t)bmp280_cal.dig_P2) << 12);
+    var1 = (((((int64_t)1) << 47) + var1)) * ((int64_t)bmp280_cal.dig_P1) >> 33;
+    if (var1 == 0)
+        return 0;
+    p = 1048576 - adc_P;
+    p = (((p << 31) - var2) * 3125) / var1;
+    var1 = (((int64_t)bmp280_cal.dig_P9) * (p >> 13) * (p >> 13)) >> 25;
+    var2 = (((int64_t)bmp280_cal.dig_P8) * p) >> 19;
+    p = ((p + var1 + var2) >> 8) + (((int64_t)bmp280_cal.dig_P7) << 4);
+    return (uint32_t)p;
 }
 
 STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature)
 {
     // calculate
-    float t, p;
+    int32_t t;
+    uint32_t p;
     t = bmp280_compensate_T(bmp280_ut);
     p = bmp280_compensate_P(bmp280_up);
 
     if (pressure)
-        *pressure = (int32_t)p;
+        *pressure = (int32_t)(p / 256);
     if (temperature)
-        *temperature = (int32_t)t * 100;
+        *temperature = t;
 }
 
 #endif
diff --git a/src/test/unit/baro_bmp280_unittest.cc b/src/test/unit/baro_bmp280_unittest.cc
index 50f1b5630..737f8278a 100644
--- a/src/test/unit/baro_bmp280_unittest.cc
+++ b/src/test/unit/baro_bmp280_unittest.cc
@@ -73,7 +73,7 @@ TEST(baroBmp280Test, TestBmp280Calculate)
 
     // then
     EXPECT_EQ(100653, pressure); // 100653 Pa
-    EXPECT_EQ(2500, temperature); // 25.00 degC (data sheet says 25.08)
+    EXPECT_EQ(2508, temperature); // 25.08 degC
 
 }
 
@@ -104,7 +104,7 @@ TEST(baroBmp280Test, TestBmp280CalculateHighP)
 
     // then
     EXPECT_EQ(135382, pressure); // 135385 Pa
-    EXPECT_EQ(2500, temperature); // 25.00 degC (data sheet says 25.08)
+    EXPECT_EQ(2508, temperature); // 25.08 degC
 
 }
 
@@ -135,7 +135,7 @@ TEST(baroBmp280Test, TestBmp280CalculateZeroP)
 
     // then
     EXPECT_EQ(0, pressure); // P1=0 trips pressure to 0 Pa, avoiding division by zero
-    EXPECT_EQ(2500, temperature); // 25.00 degC (data sheet says 25.08)
+    EXPECT_EQ(2508, temperature); // 25.08 degC
 
 }
 

From ba2e22ab894b5a3a02c0e1d4db2eae58455a39bd Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Tue, 6 Oct 2015 18:53:29 +0100
Subject: [PATCH 37/41] Add RMDO target.

---
 .travis.yml                             |   1 +
 Makefile                                |  24 +-
 docs/Board - RMDO.md                    |  12 +
 fake_travis_build.sh                    |   1 +
 src/main/target/RMDO/system_stm32f30x.c | 372 ++++++++++++++++++++++++
 src/main/target/RMDO/system_stm32f30x.h |  76 +++++
 src/main/target/RMDO/target.h           | 166 +++++++++++
 7 files changed, 649 insertions(+), 3 deletions(-)
 create mode 100644 docs/Board - RMDO.md
 create mode 100644 src/main/target/RMDO/system_stm32f30x.c
 create mode 100644 src/main/target/RMDO/system_stm32f30x.h
 create mode 100644 src/main/target/RMDO/target.h

diff --git a/.travis.yml b/.travis.yml
index 333cca912..754a436b8 100755
--- a/.travis.yml
+++ b/.travis.yml
@@ -12,6 +12,7 @@ env:
   - TARGET=NAZE
   - TARGET=NAZE32PRO
   - TARGET=OLIMEXINO
+  - TARGET=RMDO
   - TARGET=PORT103R
   - TARGET=SPARKY
   - TARGET=STM32F3DISCOVERY
diff --git a/Makefile b/Makefile
index 893b97fea..85244e39e 100755
--- a/Makefile
+++ b/Makefile
@@ -38,7 +38,7 @@ FLASH_SIZE ?=
 
 FORKNAME			 = cleanflight
 
-VALID_TARGETS	 = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 CC3D CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENWIIF1 ALIENWIIF3 COLIBRI_RACE
+VALID_TARGETS	 = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 CC3D CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENWIIF1 ALIENWIIF3 COLIBRI_RACE RMDO
 
 # Valid targets for OP BootLoader support
 OPBL_VALID_TARGETS = CC3D
@@ -47,7 +47,7 @@ OPBL_VALID_TARGETS = CC3D
 ifeq ($(FLASH_SIZE),)
 ifeq ($(TARGET),$(filter $(TARGET),CJMCU))
 FLASH_SIZE = 64
-else ifeq ($(TARGET),$(filter $(TARGET),NAZE CC3D ALIENWIIF1 OLIMEXINO))
+else ifeq ($(TARGET),$(filter $(TARGET),NAZE CC3D ALIENWIIF1 OLIMEXINO RMDO))
 FLASH_SIZE = 128
 else ifeq ($(TARGET),$(filter $(TARGET),EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY ALIENWIIF3 COLIBRI_RACE))
 FLASH_SIZE = 256
@@ -74,7 +74,7 @@ USBPERIPH_SRC = $(notdir $(wildcard $(USBFS_DIR)/src/*.c))
 
 CSOURCES        := $(shell find $(SRC_DIR) -name '*.c')
 
-ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY ALIENWIIF3 COLIBRI_RACE))
+ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY ALIENWIIF3 COLIBRI_RACE RMDO))
 
 STDPERIPH_DIR	= $(ROOT)/lib/main/STM32F30x_StdPeriph_Driver
 
@@ -120,6 +120,11 @@ ifeq ($(TARGET),CHEBUZZF3)
 TARGET_FLAGS := $(TARGET_FLAGS) -DSTM32F3DISCOVERY
 endif
 
+ifeq ($(TARGET),RMDO)
+# RMDO is a VARIANT of SPRACINGF3
+TARGET_FLAGS := $(TARGET_FLAGS) -DSPRACINGF3
+endif
+
 else ifeq ($(TARGET),$(filter $(TARGET),EUSTM32F103RC PORT103R))
 
 
@@ -547,6 +552,19 @@ SPARKY_SRC	 = \
 
 ALIENWIIF3_SRC	 = $(SPARKY_SRC)
 
+RMDO_SRC	 = \
+		   $(STM32F30x_COMMON_SRC) \
+		   drivers/accgyro_mpu.c \
+		   drivers/accgyro_mpu6050.c \
+		   drivers/barometer_bmp280.c \
+		   drivers/display_ug2864hsweg01.h \
+		   drivers/flash_m25p16.c \
+		   drivers/serial_softserial.c \
+		   drivers/sonar_hcsr04.c \
+		   io/flashfs.c \
+		   $(HIGHEND_SRC) \
+		   $(COMMON_SRC)
+
 SPRACINGF3_SRC	 = \
 		   $(STM32F30x_COMMON_SRC) \
 		   drivers/accgyro_mpu.c \
diff --git a/docs/Board - RMDO.md b/docs/Board - RMDO.md
new file mode 100644
index 000000000..abd4cf645
--- /dev/null
+++ b/docs/Board - RMDO.md	
@@ -0,0 +1,12 @@
+# Board - RMDO
+
+The DoDo board is a clone of the SPRacingF3 board in terms of CPU pin mappings.  See the SPRacingF3 documentation.
+
+Hardware differences compared to SPRacingF3 are as follows:
+
+* The CPU is the cheaper version of the F3 with only 128KB FLASH.
+* The external flash rom is the same size as found on the Naze32 (2MBit)
+* The barometer is the cheaper BMP280.
+* It does not have any compass sensor.
+* Onboard BEC.
+* Different physical connectors/pins/pads/ports.
diff --git a/fake_travis_build.sh b/fake_travis_build.sh
index 3d2f160f2..f6c510c21 100755
--- a/fake_travis_build.sh
+++ b/fake_travis_build.sh
@@ -13,6 +13,7 @@ targets=("PUBLISHMETA=True" \
     "TARGET=NAZE32PRO" \
     "TARGET=OLIMEXINO" \
     "TARGET=PORT103R" \
+    "TARGET=RMDO" \
     "TARGET=SPARKY" \
     "TARGET=STM32F3DISCOVERY" \
     "TARGET=ALIENWIIF1" \
diff --git a/src/main/target/RMDO/system_stm32f30x.c b/src/main/target/RMDO/system_stm32f30x.c
new file mode 100644
index 000000000..fca696982
--- /dev/null
+++ b/src/main/target/RMDO/system_stm32f30x.c
@@ -0,0 +1,372 @@
+/**
+  ******************************************************************************
+  * @file    system_stm32f30x.c
+  * @author  MCD Application Team
+  * @version V1.1.1
+  * @date    28-March-2014
+  * @brief   CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
+  *          This file contains the system clock configuration for STM32F30x devices,
+  *          and is generated by the clock configuration tool
+  *          stm32f30x_Clock_Configuration_V1.0.0.xls
+  *             
+  * 1.  This file provides two functions and one global variable to be called from 
+  *     user application:
+  *      - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
+  *                      and Divider factors, AHB/APBx prescalers and Flash settings),
+  *                      depending on the configuration made in the clock xls tool. 
+  *                      This function is called at startup just after reset and 
+  *                      before branch to main program. This call is made inside
+  *                      the "startup_stm32f30x.s" file.
+  *
+  *      - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
+  *                                  by the user application to setup the SysTick 
+  *                                  timer or configure other parameters.
+  *                                     
+  *      - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
+  *                                 be called whenever the core clock is changed
+  *                                 during program execution.
+  *
+  * 2. After each device reset the HSI (8 MHz) is used as system clock source.
+  *    Then SystemInit() function is called, in "startup_stm32f30x.s" file, to
+  *    configure the system clock before to branch to main program.
+  *
+  * 3. If the system clock source selected by user fails to startup, the SystemInit()
+  *    function will do nothing and HSI still used as system clock source. User can 
+  *    add some code to deal with this issue inside the SetSysClock() function.
+  *
+  * 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define
+  *    in "stm32f30x.h" file. When HSE is used as system clock source, directly or
+  *    through PLL, and you are using different crystal you have to adapt the HSE
+  *    value to your own configuration.
+  *
+  * 5. This file configures the system clock as follows:
+  *=============================================================================
+  *                         Supported STM32F30x device                          
+  *-----------------------------------------------------------------------------
+  *        System Clock source                    | PLL (HSE)
+  *-----------------------------------------------------------------------------
+  *        SYSCLK(Hz)                             | 72000000
+  *-----------------------------------------------------------------------------
+  *        HCLK(Hz)                               | 72000000
+  *-----------------------------------------------------------------------------
+  *        AHB Prescaler                          | 1
+  *-----------------------------------------------------------------------------
+  *        APB2 Prescaler                         | 2
+  *-----------------------------------------------------------------------------
+  *        APB1 Prescaler                         | 2
+  *-----------------------------------------------------------------------------
+  *        HSE Frequency(Hz)                      | 8000000
+  *----------------------------------------------------------------------------
+  *        PLLMUL                                 | 9
+  *-----------------------------------------------------------------------------
+  *        PREDIV                                 | 1
+  *-----------------------------------------------------------------------------
+  *        USB Clock                              | ENABLE
+  *-----------------------------------------------------------------------------
+  *        Flash Latency(WS)                      | 2
+  *-----------------------------------------------------------------------------
+  *        Prefetch Buffer                        | ON
+  *-----------------------------------------------------------------------------
+  *=============================================================================
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; COPYRIGHT 2014 STMicroelectronics</center></h2>
+  *
+  * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
+  * You may not use this file except in compliance with the License.
+  * You may obtain a copy of the License at:
+  *
+  *        http://www.st.com/software_license_agreement_liberty_v2
+  *
+  * Unless required by applicable law or agreed to in writing, software 
+  * distributed under the License is distributed on an "AS IS" BASIS, 
+  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  * See the License for the specific language governing permissions and
+  * limitations under the License.
+  *
+  ******************************************************************************
+  */
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32f30x_system
+  * @{
+  */  
+  
+/** @addtogroup STM32F30x_System_Private_Includes
+  * @{
+  */
+
+#include "stm32f30x.h"
+
+uint32_t hse_value = HSE_VALUE;
+
+/**
+  * @}
+  */
+
+/* Private typedef -----------------------------------------------------------*/
+
+/** @addtogroup STM32F30x_System_Private_Defines
+  * @{
+  */
+/*!< Uncomment the following line if you need to relocate your vector Table in
+     Internal SRAM. */ 
+/* #define VECT_TAB_SRAM */
+#define VECT_TAB_OFFSET  0x0 /*!< Vector Table base offset field. 
+                                  This value must be a multiple of 0x200. */  
+/**
+  * @}
+  */ 
+
+/* Private macro -------------------------------------------------------------*/
+
+/** @addtogroup STM32F30x_System_Private_Variables
+  * @{
+  */
+
+  uint32_t SystemCoreClock = 72000000;
+
+  __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F30x_System_Private_FunctionPrototypes
+  * @{
+  */
+
+void SetSysClock(void);
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F30x_System_Private_Functions
+  * @{
+  */
+
+/**
+  * @brief  Setup the microcontroller system
+  *         Initialize the Embedded Flash Interface, the PLL and update the 
+  *         SystemFrequency variable.
+  * @param  None
+  * @retval None
+  */
+void SystemInit(void)
+{
+  /* FPU settings ------------------------------------------------------------*/
+  #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+    SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2));  /* set CP10 and CP11 Full Access */
+  #endif
+
+  /* Reset the RCC clock configuration to the default reset state ------------*/
+  /* Set HSION bit */
+  RCC->CR |= (uint32_t)0x00000001;
+
+  /* Reset CFGR register */
+  RCC->CFGR &= 0xF87FC00C;
+
+  /* Reset HSEON, CSSON and PLLON bits */
+  RCC->CR &= (uint32_t)0xFEF6FFFF;
+
+  /* Reset HSEBYP bit */
+  RCC->CR &= (uint32_t)0xFFFBFFFF;
+
+  /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */
+  RCC->CFGR &= (uint32_t)0xFF80FFFF;
+
+  /* Reset PREDIV1[3:0] bits */
+  RCC->CFGR2 &= (uint32_t)0xFFFFFFF0;
+
+  /* Reset USARTSW[1:0], I2CSW and TIMs bits */
+  RCC->CFGR3 &= (uint32_t)0xFF00FCCC;
+  
+  /* Disable all interrupts */
+  RCC->CIR = 0x00000000;
+
+  /* Configure the System clock source, PLL Multiplier and Divider factors, 
+     AHB/APBx prescalers and Flash settings ----------------------------------*/
+  //SetSysClock(); // called from main()
+  
+#ifdef VECT_TAB_SRAM
+  SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */
+#else
+  SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */
+#endif  
+}
+
+/**
+   * @brief  Update SystemCoreClock variable according to Clock Register Values.
+  *         The SystemCoreClock variable contains the core clock (HCLK), it can
+  *         be used by the user application to setup the SysTick timer or configure
+  *         other parameters.
+  *           
+  * @note   Each time the core clock (HCLK) changes, this function must be called
+  *         to update SystemCoreClock variable value. Otherwise, any configuration
+  *         based on this variable will be incorrect.         
+  *     
+  * @note   - The system frequency computed by this function is not the real 
+  *           frequency in the chip. It is calculated based on the predefined 
+  *           constant and the selected clock source:
+  *             
+  *           - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
+  *                                              
+  *           - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
+  *                          
+  *           - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) 
+  *             or HSI_VALUE(*) multiplied/divided by the PLL factors.
+  *         
+  *         (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value
+  *             8 MHz) but the real value may vary depending on the variations
+  *             in voltage and temperature.   
+  *    
+  *         (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value
+  *              8 MHz), user has to ensure that HSE_VALUE is same as the real
+  *              frequency of the crystal used. Otherwise, this function may
+  *              have wrong result.
+  *                
+  *         - The result of this function could be not correct when using fractional
+  *           value for HSE crystal.
+  *     
+  * @param  None
+  * @retval None
+  */
+void SystemCoreClockUpdate (void)
+{
+  uint32_t tmp = 0, pllmull = 0, pllsource = 0, prediv1factor = 0;
+
+  /* Get SYSCLK source -------------------------------------------------------*/
+  tmp = RCC->CFGR & RCC_CFGR_SWS;
+  
+  switch (tmp)
+  {
+    case 0x00:  /* HSI used as system clock */
+      SystemCoreClock = HSI_VALUE;
+      break;
+    case 0x04:  /* HSE used as system clock */
+      SystemCoreClock = HSE_VALUE;
+      break;
+    case 0x08:  /* PLL used as system clock */
+      /* Get PLL clock source and multiplication factor ----------------------*/
+      pllmull = RCC->CFGR & RCC_CFGR_PLLMULL;
+      pllsource = RCC->CFGR & RCC_CFGR_PLLSRC;
+      pllmull = ( pllmull >> 18) + 2;
+      
+      if (pllsource == 0x00)
+      {
+        /* HSI oscillator clock divided by 2 selected as PLL clock entry */
+        SystemCoreClock = (HSI_VALUE >> 1) * pllmull;
+      }
+      else
+      {
+        prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1;
+        /* HSE oscillator clock selected as PREDIV1 clock entry */
+        SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; 
+      }      
+      break;
+    default: /* HSI used as system clock */
+      SystemCoreClock = HSI_VALUE;
+      break;
+  }
+  /* Compute HCLK clock frequency ----------------*/
+  /* Get HCLK prescaler */
+  tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
+  /* HCLK clock frequency */
+  SystemCoreClock >>= tmp;  
+}
+
+/**
+  * @brief  Configures the System clock source, PLL Multiplier and Divider factors,
+  *               AHB/APBx prescalers and Flash settings
+  * @note   This function should be called only once the RCC clock configuration  
+  *         is reset to the default reset state (done in SystemInit() function).             
+  * @param  None
+  * @retval None
+  */
+void SetSysClock(void)
+{
+  __IO uint32_t StartUpCounter = 0, HSEStatus = 0;
+
+/******************************************************************************/
+/*            PLL (clocked by HSE) used as System clock source                */
+/******************************************************************************/
+
+  /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/
+  /* Enable HSE */
+  RCC->CR |= ((uint32_t)RCC_CR_HSEON);
+ 
+  /* Wait till HSE is ready and if Time out is reached exit */
+  do
+  {
+    HSEStatus = RCC->CR & RCC_CR_HSERDY;
+    StartUpCounter++;
+  } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT));
+
+  if ((RCC->CR & RCC_CR_HSERDY) != RESET)
+  {
+    HSEStatus = (uint32_t)0x01;
+  }
+  else
+  {
+    HSEStatus = (uint32_t)0x00;
+  }
+
+  if (HSEStatus == (uint32_t)0x01)
+  {
+    /* Enable Prefetch Buffer and set Flash Latency */
+    FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1;
+ 
+     /* HCLK = SYSCLK / 1 */
+     RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1;
+       
+     /* PCLK2 = HCLK / 1 */
+     RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1;
+     
+     /* PCLK1 = HCLK / 2 */
+     RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2;
+
+    /* PLL configuration */
+    RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL));
+    RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9);
+
+    /* Enable PLL */
+    RCC->CR |= RCC_CR_PLLON;
+
+    /* Wait till PLL is ready */
+    while((RCC->CR & RCC_CR_PLLRDY) == 0)
+    {
+    }
+    
+    /* Select PLL as system clock source */
+    RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
+    RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL;
+
+    /* Wait till PLL is used as system clock source */
+    while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL)
+    {
+    }
+  }
+  else
+  { /* If HSE fails to start-up, the application will have wrong clock
+         configuration. User can add here some code to deal with this error */
+  }
+}
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
+
diff --git a/src/main/target/RMDO/system_stm32f30x.h b/src/main/target/RMDO/system_stm32f30x.h
new file mode 100644
index 000000000..4f999d305
--- /dev/null
+++ b/src/main/target/RMDO/system_stm32f30x.h
@@ -0,0 +1,76 @@
+/**
+  ******************************************************************************
+  * @file    system_stm32f30x.h
+  * @author  MCD Application Team
+  * @version V1.1.1
+  * @date    28-March-2014
+  * @brief   CMSIS Cortex-M4 Device System Source File for STM32F30x devices.  
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; COPYRIGHT 2014 STMicroelectronics</center></h2>
+  *
+  * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
+  * You may not use this file except in compliance with the License.
+  * You may obtain a copy of the License at:
+  *
+  *        http://www.st.com/software_license_agreement_liberty_v2
+  *
+  * Unless required by applicable law or agreed to in writing, software 
+  * distributed under the License is distributed on an "AS IS" BASIS, 
+  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  * See the License for the specific language governing permissions and
+  * limitations under the License.
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32f30x_system
+  * @{
+  */  
+  
+/**
+  * @brief Define to prevent recursive inclusion
+  */
+#ifndef __SYSTEM_STM32F30X_H
+#define __SYSTEM_STM32F30X_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif 
+
+/* Exported types ------------------------------------------------------------*/
+extern uint32_t SystemCoreClock;          /*!< System Clock Frequency (Core Clock) */
+/* Exported constants --------------------------------------------------------*/
+/* Exported macro ------------------------------------------------------------*/
+/* Exported functions ------------------------------------------------------- */
+
+/** @addtogroup STM32F30x_System_Exported_Functions
+  * @{
+  */
+  
+extern void SystemInit(void);
+extern void SystemCoreClockUpdate(void);
+
+/**
+  * @}
+  */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /*__SYSTEM_STM32F30X_H */
+
+/**
+  * @}
+  */
+  
+/**
+  * @}
+  */  
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h
new file mode 100644
index 000000000..2cefb62e6
--- /dev/null
+++ b/src/main/target/RMDO/target.h
@@ -0,0 +1,166 @@
+/*
+ * 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
+
+#define TARGET_BOARD_IDENTIFIER "RMDO" // Ready Made RC DoDo
+
+#define LED0_GPIO   GPIOB
+#define LED0_PIN    Pin_3
+#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB
+
+#define BEEP_GPIO   GPIOC
+#define BEEP_PIN    Pin_15
+#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC
+#define BEEPER_INVERTED
+
+#define USABLE_TIMER_CHANNEL_COUNT 17
+
+#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready
+
+#define USE_MPU_DATA_READY_SIGNAL
+#define ENSURE_MPU_DATA_READY_IS_LOW
+
+#define USE_MAG_DATA_READY_SIGNAL
+#define ENSURE_MAG_DATA_READY_IS_HIGH
+
+
+#define GYRO
+#define USE_GYRO_MPU6050
+#define GYRO_MPU6050_ALIGN CW270_DEG
+
+#define ACC
+#define USE_ACC_MPU6050
+#define ACC_MPU6050_ALIGN CW270_DEG
+
+#define BARO
+#define USE_BARO_BMP280
+
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+
+#define SONAR
+#define BEEPER
+#define LED0
+
+#define USE_USART1
+#define USE_USART2
+#define USE_USART3
+#define USE_SOFTSERIAL1
+#define USE_SOFTSERIAL2
+#define SERIAL_PORT_COUNT 5
+
+#ifndef UART1_GPIO
+#define UART1_TX_PIN        GPIO_Pin_9  // PA9
+#define UART1_RX_PIN        GPIO_Pin_10 // PA10
+#define UART1_GPIO          GPIOA
+#define UART1_GPIO_AF       GPIO_AF_7
+#define UART1_TX_PINSOURCE  GPIO_PinSource9
+#define UART1_RX_PINSOURCE  GPIO_PinSource10
+#endif
+
+#define UART2_TX_PIN        GPIO_Pin_14 // PA14 / SWCLK
+#define UART2_RX_PIN        GPIO_Pin_15 // PA15
+#define UART2_GPIO          GPIOA
+#define UART2_GPIO_AF       GPIO_AF_7
+#define UART2_TX_PINSOURCE  GPIO_PinSource14
+#define UART2_RX_PINSOURCE  GPIO_PinSource15
+
+#ifndef UART3_GPIO
+#define UART3_TX_PIN        GPIO_Pin_10 // PB10 (AF7)
+#define UART3_RX_PIN        GPIO_Pin_11 // PB11 (AF7)
+#define UART3_GPIO_AF       GPIO_AF_7
+#define UART3_GPIO          GPIOB
+#define UART3_TX_PINSOURCE  GPIO_PinSource10
+#define UART3_RX_PINSOURCE  GPIO_PinSource11
+#endif
+
+#define SOFTSERIAL_1_TIMER TIM3
+#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5
+#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6
+#define SOFTSERIAL_2_TIMER TIM3
+#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7
+#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8
+
+#define USE_I2C
+#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA
+
+#define USE_SPI
+#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5
+
+#define SPI2_GPIO               GPIOB
+#define SPI2_GPIO_PERIPHERAL    RCC_AHBPeriph_GPIOB
+#define SPI2_NSS_PIN            Pin_12
+#define SPI2_NSS_PIN_SOURCE     GPIO_PinSource12
+#define SPI2_SCK_PIN            Pin_13
+#define SPI2_SCK_PIN_SOURCE     GPIO_PinSource13
+#define SPI2_MISO_PIN           Pin_14
+#define SPI2_MISO_PIN_SOURCE    GPIO_PinSource14
+#define SPI2_MOSI_PIN           Pin_15
+#define SPI2_MOSI_PIN_SOURCE    GPIO_PinSource15
+
+#define M25P16_CS_GPIO          GPIOB
+#define M25P16_CS_PIN           GPIO_Pin_12
+#define M25P16_SPI_INSTANCE     SPI2
+
+#define USE_ADC
+#define BOARD_HAS_VOLTAGE_DIVIDER
+
+
+#define ADC_INSTANCE                ADC2
+#define ADC_DMA_CHANNEL             DMA2_Channel1
+#define ADC_AHB_PERIPHERAL          RCC_AHBPeriph_DMA2
+
+#define VBAT_ADC_GPIO               GPIOA
+#define VBAT_ADC_GPIO_PIN           GPIO_Pin_4
+#define VBAT_ADC_CHANNEL            ADC_Channel_1
+
+#define CURRENT_METER_ADC_GPIO      GPIOA
+#define CURRENT_METER_ADC_GPIO_PIN  GPIO_Pin_5
+#define CURRENT_METER_ADC_CHANNEL   ADC_Channel_2
+
+#define RSSI_ADC_GPIO               GPIOB
+#define RSSI_ADC_GPIO_PIN           GPIO_Pin_2
+#define RSSI_ADC_CHANNEL            ADC_Channel_12
+
+#define LED_STRIP
+#define LED_STRIP_TIMER TIM1
+
+#define USE_LED_STRIP_ON_DMA1_CHANNEL2
+#define WS2811_GPIO                     GPIOA
+#define WS2811_GPIO_AHB_PERIPHERAL      RCC_AHBPeriph_GPIOA
+#define WS2811_GPIO_AF                  GPIO_AF_6
+#define WS2811_PIN                      GPIO_Pin_8
+#define WS2811_PIN_SOURCE               GPIO_PinSource8
+#define WS2811_TIMER                    TIM1
+#define WS2811_TIMER_APB2_PERIPHERAL    RCC_APB2Periph_TIM1
+#define WS2811_DMA_CHANNEL              DMA1_Channel2
+#define WS2811_IRQ                      DMA1_Channel2_IRQn
+
+#define GPS
+#define BLACKBOX
+#define TELEMETRY
+#define SERIAL_RX
+#define AUTOTUNE
+#define DISPLAY
+#define USE_SERVOS
+#define USE_CLI
+
+#define SPEKTRUM_BIND
+// USART3,
+#define BIND_PORT  GPIOB
+#define BIND_PIN   Pin_11

From 6e59eb235d9d1b327461b2968d633da20b9445ec Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Tue, 6 Oct 2015 19:30:13 +0100
Subject: [PATCH 38/41] Bump version to 1.11.0.

---
 src/main/version.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/version.h b/src/main/version.h
index d221a2db4..93659d9f7 100644
--- a/src/main/version.h
+++ b/src/main/version.h
@@ -16,7 +16,7 @@
  */
 
 #define FC_VERSION_MAJOR            1  // increment when a major release is made (big new feature, etc)
-#define FC_VERSION_MINOR            10 // increment when a minor release is made (small new feature, change etc)
+#define FC_VERSION_MINOR            11 // increment when a minor release is made (small new feature, change etc)
 #define FC_VERSION_PATCH_LEVEL      0  // increment when a bug is fixed
 
 #define STR_HELPER(x) #x

From d36da111b22731706316fd80789483057f0744dc Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Tue, 6 Oct 2015 19:38:02 +0100
Subject: [PATCH 39/41] Delete autotune.

---
 Makefile                                  |   2 +-
 README.md                                 |   1 -
 docs/Autotune.md                          |  43 --
 docs/Board - CC3D.md                      |   1 -
 docs/Getting Started.md                   |   1 -
 docs/Modes.md                             |   1 -
 docs/PID tuning.md                        |   2 +-
 src/main/blackbox/blackbox.c              |  20 -
 src/main/blackbox/blackbox_fielddefs.h    |  34 +-
 src/main/config/runtime_config.h          |   2 +-
 src/main/flight/autotune.c                | 501 ----------------------
 src/main/flight/autotune.h                |  28 --
 src/main/flight/pid.c                     |  43 --
 src/main/io/rc_controls.h                 |   2 +-
 src/main/io/serial_msp.c                  |   7 +-
 src/main/mw.c                             |  47 --
 src/main/target/ALIENWIIF3/target.h       |   1 -
 src/main/target/CC3D/target.h             |   2 -
 src/main/target/CHEBUZZF3/target.h        |   1 -
 src/main/target/COLIBRI_RACE/target.h     |   1 -
 src/main/target/EUSTM32F103RC/target.h    |   1 -
 src/main/target/NAZE/target.h             |   1 -
 src/main/target/NAZE32PRO/target.h        |   1 -
 src/main/target/OLIMEXINO/target.h        |   1 -
 src/main/target/PORT103R/target.h         |   1 -
 src/main/target/RMDO/target.h             |   1 -
 src/main/target/SPARKY/target.h           |   1 -
 src/main/target/SPRACINGF3/target.h       |   1 -
 src/main/target/STM32F3DISCOVERY/target.h |   1 -
 src/main/telemetry/smartport.c            |   2 +-
 30 files changed, 8 insertions(+), 743 deletions(-)
 delete mode 100644 docs/Autotune.md
 delete mode 100644 src/main/flight/autotune.c
 delete mode 100644 src/main/flight/autotune.h

diff --git a/Makefile b/Makefile
index 85244e39e..bee0cbcba 100755
--- a/Makefile
+++ b/Makefile
@@ -265,7 +265,7 @@ COMMON_SRC	 = build_config.c \
 		   $(CMSIS_SRC) \
 		   $(DEVICE_STDPERIPH_SRC)
 
-HIGHEND_SRC  = flight/autotune.c \
+HIGHEND_SRC  = \
 		   flight/navigation.c \
 		   flight/gps_conversion.c \
 		   common/colorconversion.c \
diff --git a/README.md b/README.md
index 11286b7f2..bc0b04144 100644
--- a/README.md
+++ b/README.md
@@ -35,7 +35,6 @@ Cleanflight also has additional features not found in baseflight.
 * MSP Telemetry.
 * Smartport Telemetry.
 * RSSI via ADC - Uses ADC to read PWM RSSI signals, tested with FrSky D4R-II and X8R.
-* Autotune - ported from BradWii, experimental - feedback welcomed.
 * OLED Displays - Display information on: Battery voltage, profile, rate profile, version, sensors, RC, etc.
 * In-flight manual PID tuning and rate adjustment.
 * Rate profiles and in-flight selection of them.
diff --git a/docs/Autotune.md b/docs/Autotune.md
deleted file mode 100644
index 73aceee88..000000000
--- a/docs/Autotune.md
+++ /dev/null
@@ -1,43 +0,0 @@
-# Autotune
-
-Autotune helps to automatically tune your multirotor.
-
-WARNING: Autotune is an experimental feature.  Currently enough feedback has been gathered and we do not recommend that anyone uses it until this warning is removed.  Autotune may be replaced by G-Tune, Please see https://github.com/cleanflight/cleanflight/pull/568 for details.
-
-## Configuration.
-
-Autotune only works in HORIZON or ANGLE mode, before using auto-tune it's best you setup so there is as little drift as possible.
-Autotuning is best on a full battery in good flying conditions, i.e. no or minimal wind. Autotune does not support pid_controller 2  or higher (pid_controller 0 is the Cleanflight default, pid_controller 1 will work for autotune as well).
-
-Configure a two position switch on your transmitter to activate the AUTOTUNE mode. Autotune may be done in ether one of the both only, HORIZON or  ANGLE mode (will then apply on both modes). 
-
-
-## Using autotuning
-
-Turn off the autotune switch.  If the autotune switch is on while not armed the warning LED will flash and you cannot arm.
-
-1. Launch the multirotor.
-
-1. Phase 1: ROLL/YAW autotune. 
-Turn on/hold the autotune switch on your transmitter for approx 5 seconds.  You can observe roll left/right while a beep code sounds on the beeper, when turning off the autotune switch, PID settings will have been updated for ROLL and YAW.
-
-1. Stay in air and re-align your copter for the following PITCH/YAW autotune.
-
-1. Phase 2: PITCH/YAW autotune.
-Turn on/hold the switch again for approx 5 seconds.  You can observe pitch forwards/backwards while a beep code sounds on the beeper, when turning off the autotune switch, PID settings will have been updated for PITCH and YAW.
-
-1. Keep flying and see if it's better.  If it's worse then while still armed flip the switch again to restore previous PIDs that were present prior to arming.  You can do this while still flying or after landing.
-
-1. Land & disarm.  If desired you may verify results via an app while battery power still on. Cutting the power will lose the new unsaved PIDs.
-
-1. If you're happy with the PIDs then disarm (but leave the battery still on).
-
-1. Flip the autotune switch again (copter still under battery power) to save all settings. 
-A beeper will sound indicating the settings are saved.
-
-1. Then flip it back (so you can arm again).
-
-
-# References
-
-* Brad Quick for the initial Autotune algorithm in BradWii.
diff --git a/docs/Board - CC3D.md b/docs/Board - CC3D.md
index 9a93fd3d7..f313ea149 100644
--- a/docs/Board - CC3D.md	
+++ b/docs/Board - CC3D.md	
@@ -123,7 +123,6 @@ The OpenPilot bootloader code also allows the remaining section of flash to be r
 OpenPilot Ground Station (GCS) via USB without requiring a USB to uart adapter.
 
 The following features are not available:
- * Autotune
  * Display
  * Sonar
 
diff --git a/docs/Getting Started.md b/docs/Getting Started.md
index b7d4a452f..b0836255f 100644
--- a/docs/Getting Started.md	
+++ b/docs/Getting Started.md	
@@ -99,7 +99,6 @@ Some advanced configurations and features are documented in the following pages,
 * [Profiles](Profiles.md)
 * [PID tuning](PID tuning.md)
 * [In-flight Adjustments](Inflight Adjustments.md)
-* [Autotune](Autotune.md)
 * [Blackbox logging](Blackbox.md)
 * [Using a Sonar](Sonar.md)
 * [Spektrum Bind](Spektrum bind.md)
diff --git a/docs/Modes.md b/docs/Modes.md
index 115f273e0..75b8ca6d7 100644
--- a/docs/Modes.md
+++ b/docs/Modes.md
@@ -25,7 +25,6 @@ auxillary receiver channels and other events such as failsafe detection.
 | 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)                               |
 | 26      | 25     | BLACKBOX   | Enable BlackBox logging                                              |
 
diff --git a/docs/PID tuning.md b/docs/PID tuning.md
index 8c41f4ce8..63f8498cf 100644
--- a/docs/PID tuning.md	
+++ b/docs/PID tuning.md	
@@ -107,7 +107,7 @@ This controller has code that attempts to compensate for variations in the loopt
 don't have to be retuned when the looptime setting changes. 
 
 There were initially some problems with horizon mode, and sluggishness in acro mode, that were recently fixed by
-nebbian in v1.6.0. The autotune feature does not work on this controller, so don't try to autotune it.
+nebbian in v1.6.0.
 
 It is the first PID Controller designed for 32-bit processors and not derived from MultiWii.
 
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 2f7ac5174..bd7b482e8 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1134,26 +1134,6 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
         case FLIGHT_LOG_EVENT_SYNC_BEEP:
             blackboxWriteUnsignedVB(data->syncBeep.time);
         break;
-        case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START:
-            blackboxWrite(data->autotuneCycleStart.phase);
-            blackboxWrite(data->autotuneCycleStart.cycle | (data->autotuneCycleStart.rising ? 0x80 : 0));
-            blackboxWrite(data->autotuneCycleStart.p);
-            blackboxWrite(data->autotuneCycleStart.i);
-            blackboxWrite(data->autotuneCycleStart.d);
-        break;
-        case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT:
-            blackboxWrite(data->autotuneCycleResult.flags);
-            blackboxWrite(data->autotuneCycleStart.p);
-            blackboxWrite(data->autotuneCycleStart.i);
-            blackboxWrite(data->autotuneCycleStart.d);
-        break;
-        case FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS:
-            blackboxWriteS16(data->autotuneTargets.currentAngle);
-            blackboxWrite((uint8_t) data->autotuneTargets.targetAngle);
-            blackboxWrite((uint8_t) data->autotuneTargets.targetAngleAtPeak);
-            blackboxWriteS16(data->autotuneTargets.firstPeakAngle);
-            blackboxWriteS16(data->autotuneTargets.secondPeakAngle);
-        break;
         case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT:
             if (data->inflightAdjustment.floatFlag) {
                 blackboxWrite(data->inflightAdjustment.adjustmentFunction + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG);
diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h
index 72ffa8148..4da938e3d 100644
--- a/src/main/blackbox/blackbox_fielddefs.h
+++ b/src/main/blackbox/blackbox_fielddefs.h
@@ -103,9 +103,6 @@ typedef enum FlightLogFieldSign {
 
 typedef enum FlightLogEvent {
     FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
-    FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10,
-    FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11,
-    FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12,
     FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13,
     FLIGHT_LOG_EVENT_LOGGING_RESUME = 14,
     FLIGHT_LOG_EVENT_LOG_END = 255
@@ -115,32 +112,6 @@ typedef struct flightLogEvent_syncBeep_t {
     uint32_t time;
 } flightLogEvent_syncBeep_t;
 
-typedef struct flightLogEvent_autotuneCycleStart_t {
-    uint8_t phase;
-    uint8_t cycle;
-    uint8_t p;
-    uint8_t i;
-    uint8_t d;
-    uint8_t rising;
-} flightLogEvent_autotuneCycleStart_t;
-
-#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT 1
-#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT 2
-#define FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG 128
-
-typedef struct flightLogEvent_autotuneCycleResult_t {
-    uint8_t flags;
-    uint8_t p;
-    uint8_t i;
-    uint8_t d;
-} flightLogEvent_autotuneCycleResult_t;
-
-typedef struct flightLogEvent_autotuneTargets_t {
-    uint16_t currentAngle;
-    int8_t targetAngle, targetAngleAtPeak;
-    uint16_t firstPeakAngle, secondPeakAngle;
-} flightLogEvent_autotuneTargets_t;
-
 typedef struct flightLogEvent_inflightAdjustment_t {
     uint8_t adjustmentFunction;
     bool floatFlag;
@@ -153,12 +124,11 @@ typedef struct flightLogEvent_loggingResume_t {
     uint32_t currentTime;
 } flightLogEvent_loggingResume_t;
 
+#define FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG 128
+
 typedef union flightLogEventData_t
 {
     flightLogEvent_syncBeep_t syncBeep;
-    flightLogEvent_autotuneCycleStart_t autotuneCycleStart;
-    flightLogEvent_autotuneCycleResult_t autotuneCycleResult;
-    flightLogEvent_autotuneTargets_t autotuneTargets;
     flightLogEvent_inflightAdjustment_t inflightAdjustment;
     flightLogEvent_loggingResume_t loggingResume;
 } flightLogEventData_t;
diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h
index c5a1bff6a..857ebea59 100644
--- a/src/main/config/runtime_config.h
+++ b/src/main/config/runtime_config.h
@@ -38,7 +38,7 @@ typedef enum {
     GPS_HOME_MODE   = (1 << 4),
     GPS_HOLD_MODE   = (1 << 5),
     HEADFREE_MODE   = (1 << 6),
-    AUTOTUNE_MODE   = (1 << 7),
+    UNUSED_MODE     = (1 << 7), // old autotune
     PASSTHRU_MODE   = (1 << 8),
     SONAR_MODE      = (1 << 9),
     FAILSAFE_MODE   = (1 << 10),
diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c
deleted file mode 100644
index 17f3a21e4..000000000
--- a/src/main/flight/autotune.c
+++ /dev/null
@@ -1,501 +0,0 @@
-/*
- * 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/>.
- */
-
-#include <stdbool.h>
-#include <stdint.h>
-#include <string.h>
-#include <math.h>
-
-#include "platform.h"
-#include "build_config.h"
-#include "debug.h"
-
-#ifdef AUTOTUNE
-
-#include "common/axis.h"
-#include "common/maths.h"
-
-#include "drivers/system.h"
-#include "drivers/sensor.h"
-#include "drivers/accgyro.h"
-
-#include "sensors/sensors.h"
-#include "sensors/acceleration.h"
-
-#include "flight/pid.h"
-#include "flight/imu.h"
-
-#include "config/config.h"
-#include "blackbox/blackbox.h"
-
-/*
- * Authors
- * Brad Quick - initial implementation in BradWii
- * Dominic Clifton - baseflight port & cleanup.
- *
- * Autotune in BradWii thread here: http://www.rcgroups.com/forums/showthread.php?t=1922423
- *
- * We start with two input parameters. The first is our target angle. By default it's 20 degrees, so we will bank to 20 degrees,
- * see how the system reacts, then bank to -20 degrees and evaluate again. We repeat this over and over. The second input is
- * how much oscillation we can tolerate. This can range from 2 degrees to 5 or more degrees. This defaults to 4 degrees. The
- * higher this value is, the more agressive the result of the tuning will be.
- *
- * First, we turn the I gain down to zero so that we don't have to try to figure out how much overshoot is caused by the I term
- * vs. the P term.
- *
- * Then, we move to the target of 20 degrees and analyze the results. Our goal is to have no overshoot and to keep the bounce
- * back within the 4 degrees. By working to get zero overshoot, we can isolate the effects of the P and D terms. If we don't
- * overshoot, then the P term never works in the opposite direction, so we know that any bounce we get is caused by the D term.
- *
- * If we overshoot the target 20 degrees, then we know our P term is too high or our D term is too low. We can determine
- * which one to change by looking at how much bounce back (or the amplitude of the oscillation) we get. If it bounces back
- * more than the 4 degrees, then our D is already too high, so we can't increase it, so instead we decrease P.
- *
- * If we undershoot, then either our P is too low or our D is too high. Again, we can determine which to change by looking at
- * how much bounce we get.
- *
- * Once we have the P and D terms set, we then set the I term by repeating the same test above and measuring the overshoot.
- * If our maximum oscillation is set to 4 degrees, then we keep increasing the I until we get an overshoot of 2 degrees, so
- * that our oscillations are now centered around our target (in theory).
- *
- * In the BradWii software, it alternates between doing the P and D step and doing the I step so you can quit whenever you
- * want without having to tell it specifically to do the I term. The sequence is actually P&D, P&D, I, P&D, P&D, I...
- *
- * Note: The 4 degrees mentioned above is the value of AUTOTUNE_MAX_OSCILLATION_ANGLE.  In the BradWii code at the time of writing
- * the default value was 1.0f instead of 4.0f.
- *
- * To adjust how aggressive the tuning is, adjust the AUTOTUNE_MAX_OSCILLATION_ANGLE value.  A larger value will result in more
- * aggressive tuning. A lower value will result in softer tuning. It will rock back and forth between -AUTOTUNE_TARGET_ANGLE
- * and AUTOTUNE_TARGET_ANGLE degrees
- * AUTOTUNE_D_MULTIPLIER is a multiplier that puts in a little extra D when autotuning is done. This helps damp  the wobbles
- * after a quick angle change.
- * Always autotune on a full battery.
- */
-
-#define AUTOTUNE_MAX_OSCILLATION_ANGLE 2.0f
-#define AUTOTUNE_TARGET_ANGLE 20.0f
-#define AUTOTUNE_D_MULTIPLIER 1.2f
-#define AUTOTUNE_SETTLING_DELAY_MS 250  // 1/4 of a second.
-
-#define AUTOTUNE_INCREASE_MULTIPLIER 1.03f
-#define AUTOTUNE_DECREASE_MULTIPLIER 0.97f
-
-#define AUTOTUNE_MINIMUM_I_VALUE 0.001f
-
-#define YAW_GAIN_MULTIPLIER 2.0f
-
-
-typedef enum {
-    PHASE_IDLE = 0,
-    PHASE_TUNE_ROLL,
-    PHASE_TUNE_PITCH,
-    PHASE_SAVE_OR_RESTORE_PIDS,
-} autotunePhase_e;
-
-typedef enum {
-    CYCLE_TUNE_I = 0,
-    CYCLE_TUNE_PD,
-    CYCLE_TUNE_PD2
-} autotuneCycle_e;
-
-static const pidIndex_e angleIndexToPidIndexMap[] = {
-    PIDROLL,
-    PIDPITCH
-};
-
-#define AUTOTUNE_PHASE_MAX PHASE_SAVE_OR_RESTORE_PIDS
-#define AUTOTUNE_PHASE_COUNT (AUTOTUNE_PHASE_MAX + 1)
-
-#define FIRST_TUNE_PHASE PHASE_TUNE_ROLL
-
-static pidProfile_t *pidProfile;
-static pidProfile_t pidBackup;
-static uint8_t pidController;
-static uint8_t pidIndex;
-static bool rising;
-static autotuneCycle_e cycle;
-static uint32_t timeoutAt;
-static angle_index_t autoTuneAngleIndex;
-static autotunePhase_e phase = PHASE_IDLE;
-static autotunePhase_e nextPhase = FIRST_TUNE_PHASE;
-
-static float targetAngle = 0;
-static float targetAngleAtPeak;
-static float firstPeakAngle, secondPeakAngle; // in degrees
-
-typedef struct fp_pid {
-    float p;
-    float i;
-    float d;
-} fp_pid_t;
-
-static fp_pid_t pid;
-
-// These are used to convert between multiwii integer values to the float pid values used by the autotuner.
-#define MULTIWII_P_MULTIPLIER 10.0f    // e.g 0.4 * 10 = 40
-#define MULTIWII_I_MULTIPLIER 1000.0f  // e.g 0.030 * 1000 = 30
-// Note there is no D multiplier since D values are stored and used AS-IS
-
-bool isAutotuneIdle(void)
-{
-    return phase == PHASE_IDLE;
-}
-
-#ifdef BLACKBOX
-
-static void autotuneLogCycleStart()
-{
-    if (feature(FEATURE_BLACKBOX)) {
-        flightLogEvent_autotuneCycleStart_t eventData;
-
-        eventData.phase = phase;
-        eventData.cycle = cycle;
-        eventData.p = pid.p * MULTIWII_P_MULTIPLIER;
-        eventData.i = pid.i * MULTIWII_I_MULTIPLIER;
-        eventData.d = pid.d;
-        eventData.rising = rising ? 1 : 0;
-
-        blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START, (flightLogEventData_t*)&eventData);
-    }
-}
-
-static void autotuneLogAngleTargets(float currentAngle)
-{
-    if (feature(FEATURE_BLACKBOX)) {
-        flightLogEvent_autotuneTargets_t eventData;
-
-        // targetAngle is always just -AUTOTUNE_TARGET_ANGLE or +AUTOTUNE_TARGET_ANGLE so no need for float precision:
-        eventData.targetAngle = (int) targetAngle;
-        // and targetAngleAtPeak is set to targetAngle so it has the same small precision requirement:
-        eventData.targetAngleAtPeak = (int) targetAngleAtPeak;
-
-        // currentAngle is integer decidegrees divided by 10, so just reverse that process to get an integer again:
-        eventData.currentAngle = roundf(currentAngle * 10);
-        // the peak angles are only ever set to currentAngle, so they get the same treatment:
-        eventData.firstPeakAngle = roundf(firstPeakAngle * 10);
-        eventData.secondPeakAngle = roundf(secondPeakAngle * 10);
-
-        blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS, (flightLogEventData_t*)&eventData);
-    }
-}
-
-#endif
-
-static void startNewCycle(void)
-{
-    rising = !rising;
-    firstPeakAngle = secondPeakAngle = 0;
-
-#ifdef BLACKBOX
-    autotuneLogCycleStart();
-#endif
-}
-
-static void updatePidIndex(void)
-{
-    pidIndex = angleIndexToPidIndexMap[autoTuneAngleIndex];
-}
-
-static void updateTargetAngle(void)
-{
-    if (rising) {
-        targetAngle = AUTOTUNE_TARGET_ANGLE;
-    } else {
-        targetAngle = -AUTOTUNE_TARGET_ANGLE;
-    }
-
-#if 0
-    debug[2] = DEGREES_TO_DECIDEGREES(targetAngle);
-#endif
-}
-
-float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle)
-{
-    float currentAngle;
-    bool overshot;
-
-    if (!(phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) || autoTuneAngleIndex != angleIndex) {
-        return errorAngle;
-    }
-
-    if (IS_PID_CONTROLLER_FP_BASED(pidController)) {
-        // TODO support floating point based pid controllers
-        return errorAngle;
-    }
-
-
-#ifdef DEBUG_AUTOTUNE
-    debug[0] = 0;
-    debug[1] = inclination->rawAngles[angleIndex];
-#endif
-
-    updatePidIndex();
-
-    if (rising) {
-        currentAngle = DECIDEGREES_TO_DEGREES(inclination->raw[angleIndex]);
-    } else {
-        targetAngle = -targetAngle;
-        currentAngle = DECIDEGREES_TO_DEGREES(-inclination->raw[angleIndex]);
-    }
-
-#ifdef DEBUG_AUTOTUNE
-    debug[1] = DEGREES_TO_DECIDEGREES(currentAngle);
-    debug[2] = DEGREES_TO_DECIDEGREES(targetAngle);
-#endif
-
-#ifdef BLACKBOX
-    autotuneLogAngleTargets(currentAngle);
-#endif
-
-    if (secondPeakAngle == 0) {
-        // The peak will be when our angular velocity is negative.  To be sure we are in the right place,
-        // we also check to make sure our angle position is greater than zero.
-
-        if (currentAngle > firstPeakAngle) {
-            // we are still going up
-            firstPeakAngle = currentAngle;
-            targetAngleAtPeak = targetAngle;
-
-#ifdef DEBUG_AUTOTUNE
-            debug[3] = DEGREES_TO_DECIDEGREES(firstPeakAngle);
-#endif
-
-        } else if (firstPeakAngle > 0) {
-            switch (cycle) {
-                case CYCLE_TUNE_I:
-                    // when checking the I value, we would like to overshoot the target position by half of the max oscillation.
-                    overshot = currentAngle - targetAngle >= AUTOTUNE_MAX_OSCILLATION_ANGLE / 2;
-                    
-                    if (overshot) {
-                        pid.i *= AUTOTUNE_DECREASE_MULTIPLIER;
-                        if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) {
-                            pid.i = AUTOTUNE_MINIMUM_I_VALUE;
-                        }
-                    } else {
-                        pid.i *= AUTOTUNE_INCREASE_MULTIPLIER;
-                    }
-
-#ifdef BLACKBOX
-                    if (feature(FEATURE_BLACKBOX)) {
-                        flightLogEvent_autotuneCycleResult_t eventData;
-
-                        eventData.flags = overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT: 0;
-                        eventData.p = pidProfile->P8[pidIndex];
-                        eventData.i = pidProfile->I8[pidIndex];
-                        eventData.d = pidProfile->D8[pidIndex];
-
-                        blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT, (flightLogEventData_t*)&eventData);
-                    }
-#endif
-
-                    // go back to checking P and D
-                    cycle = CYCLE_TUNE_PD;
-                    pidProfile->I8[pidIndex] = 0;
-                    startNewCycle();
-                    break;
-
-                case CYCLE_TUNE_PD:
-                case CYCLE_TUNE_PD2:
-                    // we are checking P and D values
-                    // set up to look for the 2nd peak
-                    secondPeakAngle = currentAngle;
-                    timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS;
-                    break;
-            }
-        }
-    } else {
-        // We saw the first peak while tuning PD, looking for the second
-
-        if (currentAngle < secondPeakAngle) {
-            secondPeakAngle = currentAngle;
-#ifdef DEBUG_AUTOTUNE
-            debug[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle);
-#endif
-        }
-
-        float oscillationAmplitude = firstPeakAngle - secondPeakAngle;
-
-        uint32_t now = millis();
-        int32_t signedDiff = now - timeoutAt;
-        bool timedOut = signedDiff >= 0L;
-
-        // stop looking for the 2nd peak if we time out or if we change direction again after moving by more than half the maximum oscillation
-        if (timedOut || (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 && currentAngle > secondPeakAngle)) {
-            // analyze the data
-            // Our goal is to have zero overshoot and to have AUTOTUNE_MAX_OSCILLATION_ANGLE amplitude
-
-            overshot = firstPeakAngle > targetAngleAtPeak;
-            if (overshot) {
-#ifdef DEBUG_AUTOTUNE
-                debug[0] = 1;
-#endif
-
-#ifdef PREFER_HIGH_GAIN_SOLUTION
-                if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE) {
-                    // we have too much oscillation, so we can't increase D, so decrease P
-                    pid.p *= AUTOTUNE_DECREASE_MULTIPLIER;
-                } else {
-                    // we don't have too much oscillation, so we can increase D
-                    pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
-                }
-#else
-                pid.p *= AUTOTUNE_DECREASE_MULTIPLIER;
-                pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
-#endif
-            } else {
-#ifdef DEBUG_AUTOTUNE
-                debug[0] = 2;
-#endif
-                if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE) {
-                    // we have too much oscillation
-                    pid.d *= AUTOTUNE_DECREASE_MULTIPLIER;
-                } else {
-                    // we don't have too much oscillation
-                    pid.p *= AUTOTUNE_INCREASE_MULTIPLIER;
-                }
-            }
-
-            pidProfile->P8[pidIndex] = pid.p * MULTIWII_P_MULTIPLIER;
-            pidProfile->D8[pidIndex] = pid.d;
-
-#ifdef BLACKBOX
-            if (feature(FEATURE_BLACKBOX)) {
-                flightLogEvent_autotuneCycleResult_t eventData;
-
-                eventData.flags = (overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT : 0) | (timedOut ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT : 0);
-                eventData.p = pidProfile->P8[pidIndex];
-                eventData.i = pidProfile->I8[pidIndex];
-                eventData.d = pidProfile->D8[pidIndex];
-
-                blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT, (flightLogEventData_t*)&eventData);
-            }
-#endif
-
-            if (cycle == CYCLE_TUNE_PD2) {
-                // switch to testing I value
-                cycle = CYCLE_TUNE_I;
-                
-                pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER;
-            } else {
-                cycle = CYCLE_TUNE_PD2;
-            }
-
-            // switch to the other direction for the new cycle
-            startNewCycle();
-        }
-    }
-
-#ifdef DEBUG_AUTOTUNE
-    if (angleIndex == AI_ROLL) {
-        debug[0] += 100;
-    }
-#endif
-
-    updateTargetAngle();
-
-    return targetAngle - DECIDEGREES_TO_DEGREES(inclination->raw[angleIndex]);
-}
-
-void autotuneReset(void)
-{
-    targetAngle = 0;
-    phase = PHASE_IDLE;
-    nextPhase = FIRST_TUNE_PHASE;
-}
-
-void backupPids(pidProfile_t *pidProfileToTune)
-{
-    memcpy(&pidBackup, pidProfileToTune, sizeof(pidBackup));
-}
-
-void restorePids(pidProfile_t *pidProfileToTune)
-{
-    memcpy(pidProfileToTune, &pidBackup, sizeof(pidBackup));
-}
-
-void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune)
-{
-    phase = nextPhase;
-
-    if (phase == PHASE_SAVE_OR_RESTORE_PIDS) {
-        restorePids(pidProfileToTune);
-        return;
-    }
-
-    if (phase == FIRST_TUNE_PHASE) {
-        backupPids(pidProfileToTune);
-    }
-
-    if (phase == PHASE_TUNE_ROLL) {
-        autoTuneAngleIndex = AI_ROLL;
-    } if (phase == PHASE_TUNE_PITCH) {
-        autoTuneAngleIndex = AI_PITCH;
-    }
-
-    rising = true;
-    cycle = CYCLE_TUNE_PD;
-    firstPeakAngle = secondPeakAngle = 0;
-
-    pidProfile = pidProfileToTune;
-    pidController = pidProfile->pidController;
-
-    updatePidIndex();
-    updateTargetAngle();
-
-    pid.p = pidProfile->P8[pidIndex] / MULTIWII_P_MULTIPLIER;
-    pid.i = pidProfile->I8[pidIndex] / MULTIWII_I_MULTIPLIER;
-    // divide by D multiplier to get our working value.  We'll multiply by D multiplier when we are done.
-    pid.d = pidProfile->D8[pidIndex] * (1.0f / AUTOTUNE_D_MULTIPLIER);
-
-    pidProfile->D8[pidIndex] = pid.d;
-    pidProfile->I8[pidIndex] = 0;
-
-#ifdef BLACKBOX
-    autotuneLogCycleStart();
-#endif
-}
-
-void autotuneEndPhase(void)
-{
-    if (phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) {
-
-        // we leave P alone, just update I and D
-
-        pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER;
-
-        // multiply by D multiplier.  The best D is usually a little higher than what the algroithm produces.
-        pidProfile->D8[pidIndex] = (pid.d * AUTOTUNE_D_MULTIPLIER);
-
-        pidProfile->P8[PIDYAW] = pidProfile->P8[PIDROLL] * YAW_GAIN_MULTIPLIER;
-        pidProfile->I8[PIDYAW] = pidProfile->I8[PIDROLL];
-        pidProfile->D8[PIDYAW] = pidProfile->D8[PIDROLL];
-    }
-
-    if (phase == AUTOTUNE_PHASE_MAX) {
-        phase = PHASE_IDLE;
-        nextPhase = FIRST_TUNE_PHASE;
-    } else {
-        nextPhase++;
-    }
-}
-
-bool havePidsBeenUpdatedByAutotune(void)
-{
-    return targetAngle != 0;
-}
-
-#endif
diff --git a/src/main/flight/autotune.h b/src/main/flight/autotune.h
deleted file mode 100644
index 8d4296e3c..000000000
--- a/src/main/flight/autotune.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
- * 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
-
-void autotuneReset();
-void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune);
-float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle);
-void autotuneEndPhase();
-
-bool isAutotuneIdle(void);
-bool hasAutotunePhaseCompleted(void);
-bool havePidsBeenUpdatedByAutotune(void);
-
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 899015ce4..856c01db5 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -41,7 +41,6 @@
 #include "flight/pid.h"
 #include "flight/imu.h"
 #include "flight/navigation.h"
-#include "flight/autotune.h"
 #include "flight/filter.h"
 
 #include "config/runtime_config.h"
@@ -96,13 +95,6 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
 static filterStatePt1_t PTermState[3], DTermState[3];
 
-#ifdef AUTOTUNE
-bool shouldAutotune(void)
-{
-    return ARMING_FLAG(ARMED) && FLIGHT_MODE(AUTOTUNE_MODE);
-}
-#endif
-
 static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 {
@@ -158,12 +150,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
                     +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
 #endif
 
-#ifdef AUTOTUNE
-            if (shouldAutotune()) {
-                errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle);
-            }
-#endif
-
             if (FLIGHT_MODE(ANGLE_MODE)) {
                 // it's the ANGLE mode - control is angle based, so control loop is needed
                 AngleRate = errorAngle * pidProfile->A_level;
@@ -259,12 +245,6 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
                     +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
 #endif
 
-#ifdef AUTOTUNE
-            if (shouldAutotune()) {
-                errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
-            }
-#endif
-
             PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768   16 bits is ok for result
             PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
 
@@ -367,12 +347,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
                 +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
 #endif
 
-#ifdef AUTOTUNE
-            if (shouldAutotune()) {
-                errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
-            }
-#endif
-
             errorAngleI[axis]  = constrain(errorAngleI[axis] + errorAngle, -10000, +10000);                                                // WindUp     //16 bits is ok here
 
             PTermACC = ((int32_t)errorAngle * pidProfile->P8[PIDLEVEL]) >> 7;   // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768   16 bits is ok for result
@@ -473,12 +447,6 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
                     +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
 #endif
 
-#ifdef AUTOTUNE
-            if (shouldAutotune()) {
-                errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
-            }
-#endif
-
             PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768   16 bits is ok for result
             PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
 
@@ -597,11 +565,6 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
             error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
 #endif
 
-#ifdef AUTOTUNE
-            if (shouldAutotune()) {
-                error = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(error)));
-            }
-#endif
             PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f;
             float limitf = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
             PTermACC = constrain(PTermACC, -limitf, +limitf);
@@ -754,12 +717,6 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
                     +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here
 #endif
 
-#ifdef AUTOTUNE
-            if (shouldAutotune()) {
-                errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
-            }
-#endif
-
             if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
                 AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
                 if (FLIGHT_MODE(HORIZON_MODE)) {
diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index 8be258648..dbe552cbe 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -41,7 +41,7 @@ typedef enum {
     BOXGOV,
     BOXOSD,
     BOXTELEMETRY,
-    BOXAUTOTUNE,
+    // BOXAUTOTUNE,
     BOXSONAR,
     BOXSERVO1,
     BOXSERVO2,
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index f383acbf8..fa2a3c14f 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -340,7 +340,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXGOV, "GOVERNOR;", 18 },
     { BOXOSD, "OSD SW;", 19 },
     { BOXTELEMETRY, "TELEMETRY;", 20 },
-    { BOXAUTOTUNE, "AUTOTUNE;", 21 },
+    //{ BOXAUTOTUNE, "AUTOTUNE;", 21 },
     { BOXSONAR, "SONAR;", 22 },
     { BOXSERVO1, "SERVO1;", 23 },
     { BOXSERVO2, "SERVO2;", 24 },
@@ -677,10 +677,6 @@ void mspInit(serialConfig_t *serialConfig)
     if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch)
         activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
 
-#ifdef AUTOTUNE
-    activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
-#endif
-
     if (feature(FEATURE_SONAR)){
         activeBoxIds[activeBoxIdCount++] = BOXSONAR;
     }
@@ -821,7 +817,6 @@ static bool processOutCommand(uint8_t cmdMSP)
             IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
             IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
             IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
-            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) << BOXAUTOTUNE |
             IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
             IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
             IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
diff --git a/src/main/mw.c b/src/main/mw.c
index 4174d0e17..2ce13d722 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -70,7 +70,6 @@
 #include "flight/imu.h"
 #include "flight/altitudehold.h"
 #include "flight/failsafe.h"
-#include "flight/autotune.h"
 #include "flight/navigation.h"
 #include "flight/filter.h"
 
@@ -122,44 +121,6 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD
     saveConfigAndNotify();
 }
 
-#ifdef AUTOTUNE
-
-void updateAutotuneState(void)
-{
-    static bool landedAfterAutoTuning = false;
-    static bool autoTuneWasUsed = false;
-
-    if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
-        if (!FLIGHT_MODE(AUTOTUNE_MODE)) {
-            if (ARMING_FLAG(ARMED)) {
-                if (isAutotuneIdle() || landedAfterAutoTuning) {
-                    autotuneReset();
-                    landedAfterAutoTuning = false;
-                }
-                autotuneBeginNextPhase(&currentProfile->pidProfile);
-                ENABLE_FLIGHT_MODE(AUTOTUNE_MODE);
-                autoTuneWasUsed = true;
-            } else {
-                if (havePidsBeenUpdatedByAutotune()) {
-                    saveConfigAndNotify();
-                    autotuneReset();
-                }
-            }
-        }
-        return;
-    }
-
-    if (FLIGHT_MODE(AUTOTUNE_MODE)) {
-        autotuneEndPhase();
-        DISABLE_FLIGHT_MODE(AUTOTUNE_MODE);
-    }
-
-    if (!ARMING_FLAG(ARMED) && autoTuneWasUsed) {
-        landedAfterAutoTuning = true;
-    }
-}
-#endif
-
 bool isCalibrating()
 {
 #ifdef BARO
@@ -278,10 +239,6 @@ void annexCode(void)
             DISABLE_ARMING_FLAG(OK_TO_ARM);
         }
 
-        if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
-            DISABLE_ARMING_FLAG(OK_TO_ARM);
-        }
-
         if (isCalibrating()) {
             warningLedFlash();
             DISABLE_ARMING_FLAG(OK_TO_ARM);
@@ -796,10 +753,6 @@ void loop(void)
         haveProcessedAnnexCodeOnce = true;
 #endif
 
-#ifdef AUTOTUNE
-        updateAutotuneState();
-#endif
-
 #ifdef MAG
         if (sensors(SENSOR_MAG)) {
         	updateMagHold();
diff --git a/src/main/target/ALIENWIIF3/target.h b/src/main/target/ALIENWIIF3/target.h
index 70b3367ee..0011ea4a3 100644
--- a/src/main/target/ALIENWIIF3/target.h
+++ b/src/main/target/ALIENWIIF3/target.h
@@ -114,7 +114,6 @@
 #define SERIAL_RX
 //#define GPS
 //#define DISPLAY
-#define AUTOTUNE
 #define USE_SERVOS
 #define USE_CLI
 
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index 7f0043d7e..5744a5c1d 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -112,13 +112,11 @@
 #define TELEMETRY
 #define SERIAL_RX
 #define SONAR
-#define AUTOTUNE
 #define USE_SERVOS
 #define USE_CLI
 
 #if defined(OPBL)
 // disabled some features for OPBL build due to code size.
-#undef AUTOTUNE
 #undef DISPLAY
 #undef SONAR
 #define SKIP_CLI_COMMAND_HELP
diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h
index fdb41f8f0..f7bca39e1 100644
--- a/src/main/target/CHEBUZZF3/target.h
+++ b/src/main/target/CHEBUZZF3/target.h
@@ -121,6 +121,5 @@
 #define BLACKBOX
 #define TELEMETRY
 #define SERIAL_RX
-#define AUTOTUNE
 #define USE_SERVOS
 #define USE_CLI
diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h
index cb35d1074..924cdbf39 100755
--- a/src/main/target/COLIBRI_RACE/target.h
+++ b/src/main/target/COLIBRI_RACE/target.h
@@ -157,6 +157,5 @@
 
 #define TELEMETRY
 #define SERIAL_RX
-#define AUTOTUNE
 #define USE_SERVOS
 #define USE_CLI
diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h
index 4ff778d96..2786a9cb6 100644
--- a/src/main/target/EUSTM32F103RC/target.h
+++ b/src/main/target/EUSTM32F103RC/target.h
@@ -124,7 +124,6 @@
 #define BLACKBOX
 #define TELEMETRY
 #define SERIAL_RX
-#define AUTOTUNE
 #define USE_SERVOS
 #define USE_CLI
 
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index 52d9c5955..836ce7aab 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -177,7 +177,6 @@
 #define BLACKBOX
 #define TELEMETRY
 #define SERIAL_RX
-#define AUTOTUNE
 #define USE_SERVOS
 #define USE_CLI
 
diff --git a/src/main/target/NAZE32PRO/target.h b/src/main/target/NAZE32PRO/target.h
index 6b35128d3..1359fa140 100644
--- a/src/main/target/NAZE32PRO/target.h
+++ b/src/main/target/NAZE32PRO/target.h
@@ -44,7 +44,6 @@
 #define BLACKBOX
 #define TELEMETRY
 #define SERIAL_RX
-#define AUTOTUNE
 #define USE_SERVOS
 #define USE_CLI
 
diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h
index 3a2adb0c7..d1a74851c 100644
--- a/src/main/target/OLIMEXINO/target.h
+++ b/src/main/target/OLIMEXINO/target.h
@@ -109,7 +109,6 @@
 
 #define TELEMETRY
 #define SERIAL_RX
-#define AUTOTUNE
 #define BLACKBOX
 #define USE_SERVOS
 #define USE_CLI
diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h
index e1af6a738..d80b4360f 100644
--- a/src/main/target/PORT103R/target.h
+++ b/src/main/target/PORT103R/target.h
@@ -151,6 +151,5 @@
 #define BLACKBOX
 #define TELEMETRY
 #define SERIAL_RX
-#define AUTOTUNE
 #define USE_SERVOS
 #define USE_CLI
diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h
index 2cefb62e6..652a46e10 100644
--- a/src/main/target/RMDO/target.h
+++ b/src/main/target/RMDO/target.h
@@ -155,7 +155,6 @@
 #define BLACKBOX
 #define TELEMETRY
 #define SERIAL_RX
-#define AUTOTUNE
 #define DISPLAY
 #define USE_SERVOS
 #define USE_CLI
diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h
index 1c835d2b0..59c695f17 100644
--- a/src/main/target/SPARKY/target.h
+++ b/src/main/target/SPARKY/target.h
@@ -107,7 +107,6 @@
 #define CURRENT_METER_ADC_GPIO_PIN  GPIO_Pin_7
 #define CURRENT_METER_ADC_CHANNEL   ADC_Channel_4
 
-#define AUTOTUNE
 #define BLACKBOX
 #define TELEMETRY
 #define SERIAL_RX
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index 939316884..bb99a9479 100644
--- a/src/main/target/SPRACINGF3/target.h
+++ b/src/main/target/SPRACINGF3/target.h
@@ -160,7 +160,6 @@
 #define BLACKBOX
 #define TELEMETRY
 #define SERIAL_RX
-#define AUTOTUNE
 #define DISPLAY
 #define USE_SERVOS
 #define USE_CLI
diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h
index 886d33f27..5eb5f969e 100644
--- a/src/main/target/STM32F3DISCOVERY/target.h
+++ b/src/main/target/STM32F3DISCOVERY/target.h
@@ -95,6 +95,5 @@
 #define LED_STRIP_TIMER TIM16
 #define TELEMETRY
 #define SERIAL_RX
-#define AUTOTUNE
 #define USE_SERVOS
 #define USE_CLI
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index a8fd1678d..ece79e4f2 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -421,7 +421,7 @@ void handleSmartPortTelemetry(void)
                     tmpi += 10;
                 if (FLIGHT_MODE(HORIZON_MODE))
                     tmpi += 20;
-                if (FLIGHT_MODE(AUTOTUNE_MODE))
+                if (FLIGHT_MODE(UNUSED_MODE))
                     tmpi += 40;
                 if (FLIGHT_MODE(PASSTHRU_MODE))
                     tmpi += 40;

From 57e5794ae162d1b84aec619e7c2757467a57a60b Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Tue, 6 Oct 2015 21:25:40 +0100
Subject: [PATCH 40/41] Use the right mode (aka BOX) - replaces autotune mode. 
 This will help with migrations.

---
 src/main/io/rc_controls.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index 269cf51ca..d0f100113 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -41,14 +41,13 @@ typedef enum {
     BOXGOV,
     BOXOSD,
     BOXTELEMETRY,
-    // BOXAUTOTUNE,
+    BOXGTUNE,
     BOXSONAR,
     BOXSERVO1,
     BOXSERVO2,
     BOXSERVO3,
     BOXBLACKBOX,
     BOXFAILSAFE,
-    BOXGTUNE,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

From 233753009bc0154deba5ef5016c425b58fce4356 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Tue, 6 Oct 2015 21:25:53 +0100
Subject: [PATCH 41/41] Disable GTune on CC3D build (code size).

---
 src/main/target/CC3D/target.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index e30f41582..5744a5c1d 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -112,7 +112,6 @@
 #define TELEMETRY
 #define SERIAL_RX
 #define SONAR
-#define GTUNE
 #define USE_SERVOS
 #define USE_CLI