diff --git a/docs/Autotune.md b/docs/Autotune.md
index 305e212096..3fc1b12376 100644
--- a/docs/Autotune.md
+++ b/docs/Autotune.md
@@ -4,31 +4,38 @@ Autotune helps to automatically tune your multirotor.
 
 ## Configuration.
 
-Autotune only works in HORIZON or ANGLE mode.
+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.
+
+Configure a two position switch on your transmitter to activate the AUTOTUNE and HORIZON or ANGLE modes using the auxilary configuration.
+You may find a momentary switch more suitable than a toggle switch.
 
-Configure a switch on your transmitter to activate the AUTOTUNE and HORIZON or ANGLE modes using the auxilary configuration.
 
 ## 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.
+
 Launch the multirotor.
 
-Flip the autotune switch on your transmitter.
+Turn on/hold the autotune switch on your transmitter.
 
-Observe roll/left/right a few times.
+Observe roll left/right.  A beep code will sound on the buzzer.
+
+Turn off/release the switch while still flying to stop this phase of tuning.
 
 PID settings will have been updated for ROLL/YAW.
 
-Disable the switch while still flying.
+Turn on/hold the switch again.
 
-Enable the switch again.
+Observe pitch forwards/backwards.  A beep code will sound on the buzzer.
 
-Observe pitch forwards/backwards a few times.
+Turn off/release the switch while still flying to stop this phase of tuning.
 
 PID settings will have been updated for PITCH/YAW.
 
-Disable the switch again.
+PIDS are updated, fly and see if it's better.
 
-PIDS are tunes, fly and see if it's better.
+If it's worse, flip the switch again to restore previous pids that were present prior to arming.
 
 Land.
 
@@ -36,7 +43,7 @@ Verify results via an app while power still applied if desired.
 
 Cutting the power will loose the unsaved pids.
 
-If you're happy with the pids then while disarmed flip the autotune switch again to save all settings.
+If you're happy with the pids then while disarmed flip the autotune switch again to save all settings then flip it back so you can arm again.
 
 A beeper will sound indicating the settings are saved.
 
diff --git a/src/flight_autotune.c b/src/flight_autotune.c
index f21e1ddb70..b6cd98d891 100644
--- a/src/flight_autotune.c
+++ b/src/flight_autotune.c
@@ -24,6 +24,14 @@ extern int16_t debug[4];
 #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,
@@ -31,6 +39,11 @@ typedef enum {
     PHASE_SAVE_OR_RESTORE_PIDS,
 } autotunePhase_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)
 
@@ -39,6 +52,7 @@ typedef enum {
 static pidProfile_t *pidProfile;
 static pidProfile_t pidBackup;
 static uint8_t pidController;
+static uint8_t pidIndex;
 static bool rising;
 static uint8_t cycleCount; // TODO can we replace this with an enum to improve readability.
 static uint32_t timeoutAt;
@@ -50,6 +64,19 @@ static float targetAngle = 0;
 static float targetAngleAtPeak;
 static float secondPeakAngle, firstPeakAngle; // deci dgrees, 180 deg = 1800
 
+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;
@@ -61,6 +88,11 @@ static void startNewCycle(void)
     secondPeakAngle = firstPeakAngle = 0;
 }
 
+static void updatePidIndex(void)
+{
+    pidIndex = angleIndexToPidIndexMap[autoTuneAngleIndex];
+}
+
 static void updateTargetAngle(void)
 {
     if (rising) {
@@ -75,19 +107,26 @@ static void updateTargetAngle(void)
 #endif
 }
 
-float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination, float errorAngle)
+float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle)
 {
+    float currentAngle;
+
     if (!(phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) || autoTuneAngleIndex != angleIndex) {
         return errorAngle;
     }
 
+    if (pidController == 2) {
+        // TODO support new baseflight pid controller
+        return errorAngle;
+    }
+
     debug[0] = 0;
 
 #if 0
     debug[1] = inclination->rawAngles[angleIndex];
 #endif
 
-    float currentAngle;
+    updatePidIndex();
 
     if (rising) {
         currentAngle = DECIDEGREES_TO_DEGREES(inclination->rawAngles[angleIndex]);
@@ -115,16 +154,18 @@ float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination,
         } else if (secondPeakAngle > 0) {
             if (cycleCount == 0) {
                 // when checking the I value, we would like to overshoot the target position by half of the max oscillation.
-//                if (currentangle-targetangle<(FPAUTOTUNEMAXOSCILLATION>>1)) {
-//                    currentivalueshifted=lib_fp_multiply(currentivalueshifted,AUTOTUNEINCREASEMULTIPLIER);
-//                } else {
-//                    currentivalueshifted=lib_fp_multiply(currentivalueshifted,AUTOTUNEDECREASEMULTIPLIER);
-//                    if (currentivalueshifted<AUTOTUNEMINIMUMIVALUE) currentivalueshifted=AUTOTUNEMINIMUMIVALUE;
-//                }
+                if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION / 2) {
+                    pid.i *= AUTOTUNE_INCREASE_MULTIPLIER;
+                } else {
+                    pid.i *= AUTOTUNE_DECREASE_MULTIPLIER;
+                    if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) {
+                        pid.i = AUTOTUNE_MINIMUM_I_VALUE;
+                    }
+                }
 
                 // go back to checking P and D
                 cycleCount = 1;
-//                usersettings.pid_igain[autotuneindex]=0;
+                pidProfile->I8[pidIndex] = 0;
                 startNewCycle();
             } else {
                 // we are checking P and D values
@@ -152,7 +193,6 @@ float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination,
             // analyze the data
             // Our goal is to have zero overshoot and to have AUTOTUNEMAXOSCILLATION amplitude
 
-
             if (secondPeakAngle > targetAngleAtPeak) {
                 // overshot
                 debug[0] = 1;
@@ -161,16 +201,12 @@ float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination,
                 if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION) {
                     // we have too much oscillation, so we can't increase D, so decrease P
 #endif
-                // decrease P
-                //currentpvalueshifted=lib_fp_multiply(currentpvalueshifted, AUTOTUNEDECREASEMULTIPLIER); // TODO
-
+                    pid.p *= AUTOTUNE_DECREASE_MULTIPLIER;
 #ifdef PREFER_HIGH_GAIN_SOLUTION
                 } else {
-                    // we don't have too much oscillation, so we can increase D"
+                    // we don't have too much oscillation, so we can increase D
 #endif
-
-                // increase D
-                //currentdvalueshifted=lib_fp_multiply(currentdvalueshifted, AUTOTUNEINCREASEMULTIPLIER); // TODO
+                    pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
 #ifdef PREFER_HIGH_GAIN_SOLUTION
                 }
 #endif
@@ -179,16 +215,16 @@ float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination,
                 debug[0] = 2;
 
                 if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION) {
-                    // we have too much oscillation, so we should lower D
-                    //currentdvalueshifted=lib_fp_multiply(currentdvalueshifted, AUTOTUNEDECREASEMULTIPLIER); // TODO
+                    // we have too much oscillation
+                    pid.d *= AUTOTUNE_DECREASE_MULTIPLIER;
                 } else {
-                    // we don't have too much oscillation, so we increase P
-                    //currentpvalueshifted=lib_fp_multiply(currentpvalueshifted, AUTOTUNEINCREASEMULTIPLIER); // TODO
+                    // we don't have too much oscillation
+                    pid.p *= AUTOTUNE_INCREASE_MULTIPLIER;
                 }
             }
 
-            //usersettings.pid_pgain[autotuneindex]=currentpvalueshifted>>AUTOTUNESHIFT; // TODO
-            //usersettings.pid_dgain[autotuneindex]=currentdvalueshifted>>AUTOTUNESHIFT; // TODO
+            pidProfile->P8[pidIndex] = pid.p * MULTIWII_P_MULTIPLIER;
+            pidProfile->D8[pidIndex] = pid.d;
 
             // switch to the other direction and start a new cycle
             startNewCycle();
@@ -197,7 +233,7 @@ float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination,
                 // switch to testing I value
                 cycleCount = 0;
 
-                //usersettings.pid_igain[autotuneindex]=currentivalueshifted>>AUTOTUNESHIFT; // TODO
+                pidProfile->I8[pidIndex] = pid.i  * MULTIWII_I_MULTIPLIER;
             }
         }
     }
@@ -254,30 +290,33 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle
     pidProfile = pidProfileToTune;
     pidController = pidControllerInUse;
 
+    updatePidIndex();
     updateTargetAngle();
 
-    // TODO
-//    currentpvalueshifted=usersettings.pid_pgain[autotuneindex]<<AUTOTUNESHIFT;
-//    currentivalueshifted=usersettings.pid_igain[autotuneindex]<<AUTOTUNESHIFT;
-//    // divide by D multiplier to get our working value.  We'll multiply by D multiplier when we are done.
-//    usersettings.pid_dgain[autotuneindex]=lib_fp_multiply(usersettings.pid_dgain[autotuneindex],FPONEOVERAUTOTUNE_D_MULTIPLIER);
-//    currentdvalueshifted=usersettings.pid_dgain[autotuneindex]<<AUTOTUNESHIFT;
-//
-//    usersettings.pid_igain[autotuneindex]=0;
+    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;
 }
 
 void autotuneEndPhase(void)
 {
-    // TODO
-//    usersettings.pid_igain[autotuneindex]=currentivalueshifted>>AUTOTUNESHIFT;
-//
-//    // multiply by D multiplier.  The best D is usually a little higher than what the algroithm produces.
-//    usersettings.pid_dgain[autotuneindex]=lib_fp_multiply(currentdvalueshifted,FPAUTOTUNE_D_MULTIPLIER)>>AUTOTUNESHIFT;
-//
-//    usersettings.pid_igain[YAWINDEX]=usersettings.pid_igain[ROLLINDEX];
-//    usersettings.pid_dgain[YAWINDEX]=usersettings.pid_dgain[ROLLINDEX];
-//    usersettings.pid_pgain[YAWINDEX]=lib_fp_multiply(usersettings.pid_pgain[ROLLINDEX],YAWGAINMULTIPLIER);
+    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;
diff --git a/src/flight_autotune.h b/src/flight_autotune.h
index 69cc01f7d2..1f2b53ccf7 100644
--- a/src/flight_autotune.h
+++ b/src/flight_autotune.h
@@ -2,7 +2,7 @@
 
 void autotuneReset();
 void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse);
-float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination, float errorAngle);
+float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle);
 void autotuneEndPhase();
 
 bool isAutotuneIdle(void);
diff --git a/src/flight_common.c b/src/flight_common.c
index a2e53abf9a..2d4e60579d 100644
--- a/src/flight_common.c
+++ b/src/flight_common.c
@@ -56,7 +56,7 @@ void resetErrorGyro(void)
     errorGyroIf[YAW] = 0;
 }
 
-angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
+const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
 bool shouldAutotune(void)
 {
diff --git a/src/flight_common.h b/src/flight_common.h
index 3d50aff4d0..083b86c1e6 100644
--- a/src/flight_common.h
+++ b/src/flight_common.h
@@ -1,7 +1,7 @@
 #pragma once
 
 
-enum {
+typedef enum {
     PIDROLL,
     PIDPITCH,
     PIDYAW,
@@ -13,7 +13,7 @@ enum {
     PIDMAG,
     PIDVEL,
     PID_ITEM_COUNT
-};
+} pidIndex_e;
 
 typedef struct pidProfile_s {
     uint8_t P8[PID_ITEM_COUNT];
diff --git a/src/serial_common.c b/src/serial_common.c
index 222043fc8a..6896303dce 100644
--- a/src/serial_common.c
+++ b/src/serial_common.c
@@ -78,12 +78,12 @@ typedef struct serialPortSearchResult_s {
     uint8_t startSerialPortFunctionIndex; // used by findNextSerialPort
 } serialPortSearchResult_t;
 
-static serialPortFunctionList_t serialPortFunctionList = {
+static const serialPortFunctionList_t serialPortFunctionList = {
         4,
         serialPortFunctions
 };
 
-serialPortFunctionList_t *getSerialPortFunctionList(void)
+const serialPortFunctionList_t *getSerialPortFunctionList(void)
 {
     return &serialPortFunctionList;
 }
diff --git a/src/serial_common.h b/src/serial_common.h
index c698871223..24a4e0afb7 100644
--- a/src/serial_common.h
+++ b/src/serial_common.h
@@ -113,7 +113,7 @@ bool isSerialConfigValid(serialConfig_t *serialConfig);
 bool doesConfigurationUsePort(serialConfig_t *serialConfig, serialPortIdentifier_e portIdentifier);
 bool isSerialPortFunctionShared(serialPortFunction_e functionToUse, uint16_t functionMask);
 
-serialPortFunctionList_t *getSerialPortFunctionList(void);
+const serialPortFunctionList_t *getSerialPortFunctionList(void);
 
 void evaluateOtherData(uint8_t sr);
 void handleSerial(void);
diff --git a/src/serial_msp.c b/src/serial_msp.c
index f19b289627..04dc0af44c 100755
--- a/src/serial_msp.c
+++ b/src/serial_msp.c
@@ -316,7 +316,7 @@ static void openAllMSPSerialPorts(serialConfig_t *serialConfig)
     } while (port);
 
     // XXX this function might help with adding support for MSP on more than one port, if not delete it.
-    serialPortFunctionList_t *serialPortFunctionList = getSerialPortFunctionList();
+    const serialPortFunctionList_t *serialPortFunctionList = getSerialPortFunctionList();
     UNUSED(serialPortFunctionList);
 }