diff --git a/docs/PID tuning.md b/docs/PID tuning.md
index 8380bd1939..4ae7fb154a 100644
--- a/docs/PID tuning.md	
+++ b/docs/PID tuning.md	
@@ -31,13 +31,13 @@ strength of the correction to be backed off in order to avoid overshooting the t
 
 ## PID controllers
 
-Cleanflight has three built-in PID controllers which each have different flight behavior. Each controller requires
+Cleanflight has 6 built-in PID controllers which each have different flight behavior. Each controller requires
 different PID settings for best performance, so if you tune your craft using one PID controller, those settings will
 likely not work well on any of the other controllers.
 
-You can change between PID controllers by running `set pid_controller = X` on the CLI tab of the Cleanflight
-Configurator, where X is the number of the controller you want to use. Please read these notes first before trying one
-out!
+You can change between PID controllers by running `set pid_controller=n` on the CLI tab of the Cleanflight
+Configurator, where `n` is the number of the controller you want to use. Please read these notes first before trying one
+out.
 
 ### PID controller 0, "MultiWii" (default)
 
@@ -71,7 +71,7 @@ need to be increased in order to perform like PID controller 0.
 
 The LEVEL "D" setting is not used by this controller.
 
-### PID controller 2, "Baseflight"
+### PID controller 2, "LuxFloat"
 
 PID Controller 2 is Lux's new floating point PID controller. Both controller 0 and 1 use integer arithmetic, which was
 faster in the days of the slower 8-bit MultiWii controllers, but is less precise.
@@ -82,10 +82,7 @@ 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.
 
-Even though PC2 is called "pidBaseflight" in the code, it was never in Baseflight or MultiWii. A better name might have
-been pidFloatingPoint, or pidCleanflight. It is the first PID Controller designed for 32-bit processors and not derived
-from MultiWii. I believe it was named pidBaseflight because it was to be the first true 32-bit processor native PID
-controller, and thus the native Baseflight PC, but Timecop never accepted the code into Baseflight.
+It is the first PID Controller designed for 32-bit processors and not derived from MultiWii.
 
 The strength of the auto-leveling correction applied during Angle mode is set by the parameter "level_angle" which
 is labeled "LEVEL Integral" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to
@@ -110,7 +107,7 @@ stick, and no self-leveling will be applied at 75% stick and onwards.
 
 PID Controller 3 is an direct port of the PID controller from MultiWii 2.3 and later.
 
-The algorithm is handling roll and pitch differently to yaw. users with problems on yaw authority should try this one.
+The algorithm is handling roll and pitch differently to yaw. Users with problems on yaw authority should try this one.
 
 For the ALIENWII32 targets the gyroscale is removed for even more yaw authority. This will provide best performance on very small multicopters with brushed motors.
 
@@ -118,6 +115,22 @@ For the ALIENWII32 targets the gyroscale is removed for even more yaw authority.
 
 PID Controller 4 is an hybrid version of two MultiWii PID controllers. Roll and pitch is using the MultiWii 2.2 algorithm and yaw is using the 2.3 algorithm. 
 
-This PID controller was initialy implementd for testing purposes but is also performing quite well.
+This PID controller was initialy implemented for testing purposes but is also performing quite well.
 
 For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This will provide best performance on very small multicopters with brushed motors.
+
+### PID controller 5, "Harakiri"
+
+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:
+
+        OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw.
+        MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
+
+The PID controller is flight tested and running well with the default PID settings. If you want do acrobatics start slowly.
+
+Yaw authority is also quite good.
+
+
+
diff --git a/src/main/config/config.c b/src/main/config/config.c
index b0e56548be..d7e93ce178 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -720,6 +720,12 @@ void validateAndFixConfig(void)
     }
 #endif
 
+#if defined(CC3D) && defined(DISPLAY) && defined(USE_USART3)
+    if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DISPLAY)) {
+        featureClear(FEATURE_DISPLAY);
+    }
+#endif
+
     useRxConfig(&masterConfig.rxConfig);
 
     serialConfig_t *serialConfig = &masterConfig.serialConfig;
diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c
index e0b72a6ba0..5d057a050b 100644
--- a/src/main/drivers/pwm_rx.c
+++ b/src/main/drivers/pwm_rx.c
@@ -234,9 +234,7 @@ static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t ca
     pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, overflowCb);
 
     if (++pwmInputPort->missedEvents > MAX_MISSED_PWM_EVENTS) {
-        if (pwmInputPort->state == 0) {
-            captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT;
-        }
+        captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT;
         pwmInputPort->missedEvents = 0;
     }
 }
diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c
index 8cc1519df1..5369206dc3 100644
--- a/src/main/drivers/serial_uart_stm32f10x.c
+++ b/src/main/drivers/serial_uart_stm32f10x.c
@@ -47,13 +47,20 @@ static uartPort_t uartPort2;
 static uartPort_t uartPort3;
 #endif
 
+// Using RX DMA disables the use of receive callbacks
+#define USE_USART1_RX_DMA
+
+#if defined(CC3D) // FIXME move board specific code to target.h files.
+#undef USE_USART1_RX_DMA
+#endif
+
 void uartStartTxDMA(uartPort_t *s);
 
 void usartIrqCallback(uartPort_t *s)
 {
     uint16_t SR = s->USARTx->SR;
 
-    if (SR & USART_FLAG_RXNE) {
+    if (SR & USART_FLAG_RXNE && !s->rxDMAChannel) {
         // If we registered a callback, pass crap there
         if (s->port.callback) {
             s->port.callback(s->USARTx->DR);
@@ -98,11 +105,13 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
     
     s->USARTx = USART1;
 
-    s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
-    s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
 
+#ifdef USE_USART1_RX_DMA
     s->rxDMAChannel = DMA1_Channel5;
+    s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
+#endif
     s->txDMAChannel = DMA1_Channel4;
+    s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
 
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
     RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
@@ -129,6 +138,15 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
     NVIC_Init(&NVIC_InitStructure);
 
+#ifndef USE_USART1_RX_DMA
+    // RX/TX Interrupt
+    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
+    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART1);
+    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART1);
+    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+    NVIC_Init(&NVIC_InitStructure);
+#endif
+
     return s;
 }
 
@@ -146,20 +164,11 @@ void DMA1_Channel4_IRQHandler(void)
         s->txDMAEmpty = true;
 }
 
-// USART1 Tx IRQ Handler
+// USART1 Rx/Tx IRQ Handler
 void USART1_IRQHandler(void)
 {
     uartPort_t *s = &uartPort1;
-    uint16_t SR = s->USARTx->SR;
-
-    if (SR & USART_FLAG_TXE) {
-        if (s->port.txBufferTail != s->port.txBufferHead) {
-            s->USARTx->DR = s->port.txBuffer[s->port.txBufferTail];
-            s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
-        } else {
-            USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
-        }
-    }
+    usartIrqCallback(s);
 }
 
 #endif
diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c
index 5fa40d624b..bfd7be91db 100644
--- a/src/main/flight/altitudehold.c
+++ b/src/main/flight/altitudehold.c
@@ -53,7 +53,6 @@ uint8_t velocityControl = 0;
 int32_t errorVelocityI = 0;
 int32_t altHoldThrottleAdjustment = 0;
 int32_t AltHold;
-int32_t EstAlt;                // in cm
 int32_t vario = 0;                      // variometer in cm/s
 
 
@@ -78,7 +77,7 @@ void configureAltitudeHold(
 #if defined(BARO) || defined(SONAR)
 
 static int16_t initialThrottleHold;
-
+static int32_t EstAlt;                // in cm
 
 // 40hz update rate (20hz LPF on acc)
 #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
@@ -230,6 +229,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     float vel_acc;
     int32_t vel_tmp;
     float accZ_tmp;
+    int32_t sonarAlt = -1;
     static float accZ_old = 0.0f;
     static float vel = 0.0f;
     static float accAlt = 0.0f;
@@ -242,8 +242,6 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     int16_t tiltAngle;
 #endif
 
-
-
     dTime = currentTime - previousTime;
     if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
         return;
@@ -329,5 +327,11 @@ void calculateEstimatedAltitude(uint32_t currentTime)
 
     accZ_old = accZ_tmp;
 }
+
+int32_t altitudeHoldGetEstimatedAltitude(void)
+{
+    return EstAlt;
+}
+
 #endif
 
diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h
index fefbb75824..1e42c65efc 100644
--- a/src/main/flight/altitudehold.h
+++ b/src/main/flight/altitudehold.h
@@ -15,14 +15,16 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-//extern int32_t errorVelocityI;
-//extern int32_t altHoldThrottleAdjustment;
-//extern uint8_t velocityControl;
-//extern int32_t setVelocity;
+#include "flight/flight.h"
 
+#include "io/escservo.h"
+#include "io/rc_controls.h"
+
+#include "sensors/barometer.h"
 
 void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig);
 void applyAltHold(airplaneConfig_t *airplaneConfig);
 void updateAltHoldState(void);
 void updateSonarAltHoldState(void);
 
+int32_t altitudeHoldGetEstimatedAltitude(void);
diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c
index cf2e66c3ed..26e18f618d 100644
--- a/src/main/flight/flight.c
+++ b/src/main/flight/flight.c
@@ -97,7 +97,7 @@ bool shouldAutotune(void)
 }
 #endif
 
-static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
+static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 {
     float RateError, errorAngle, AngleRate, gyroRate;
@@ -506,6 +506,136 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
 #endif
 }
 
+#define RCconstPI   0.159154943092f // 0.5f / M_PI;
+#define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
+#define OLD_YAW	0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw.
+
+static void pidHarakiri(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
+rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
+{
+    UNUSED(rxConfig);
+
+    float delta, RCfactor, rcCommandAxis, MainDptCut;
+    float PTerm = 0, ITerm = 0, DTerm = 0, PTermACC = 0, ITermACC = 0, ITermGYRO = 0, error = 0, prop = 0;
+    static float lastGyro[2] = {0, 0}, lastDTerm[2] = {0, 0};
+    float tmp0flt;
+    int32_t tmp0;
+    uint8_t axis;
+    float ACCDeltaTimeINS = 0;
+    float FLOATcycleTime = 0;
+
+//    MainDptCut = RCconstPI / (float)cfg.maincuthz;                           // Initialize Cut off frequencies for mainpid D
+    MainDptCut = RCconstPI / MAIN_CUT_HZ;                                      // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now
+    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
+
+    if (FLIGHT_MODE(HORIZON_MODE)) {
+        prop = (float)MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 450) / 450.0f;
+    }
+
+    for (axis = 0; axis < 2; axis++) {
+        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];
+#else
+            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;
+            tmp0flt = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
+            PTermACC = constrain(PTermACC, -tmp0flt, +tmp0flt);
+            errorAngleI[axis] = constrain(errorAngleI[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
+            ITermACC = errorAngleI[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
+        }
+
+        if (!FLIGHT_MODE(ANGLE_MODE)) {
+            if (ABS((int16_t)gyroData[axis]) > 2560) {
+                errorGyroI[axis] = 0.0f;
+            } else {
+                error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis];
+                errorGyroI[axis] = constrain(errorGyroI[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
+            }
+
+            ITermGYRO = errorGyroI[axis] * (float)pidProfile->I8[axis] * 0.01f;
+
+            if (FLIGHT_MODE(HORIZON_MODE)) {
+                PTerm = PTermACC + prop * (rcCommandAxis - PTermACC);
+                ITerm = ITermACC + prop * (ITermGYRO - ITermACC);
+            } else {
+                PTerm = rcCommandAxis;
+                ITerm = ITermGYRO;
+            }
+        } else {
+            PTerm = PTermACC;
+            ITerm = ITermACC;
+        }
+
+        PTerm -= gyroData[axis] * dynP8[axis] * 0.003f;
+        delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS;
+
+        lastGyro[axis] = gyroData[axis];
+        lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]);
+        DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f;
+
+        axisPID[axis] = lrintf(PTerm + ITerm - DTerm);                         // Round up result.
+
+#ifdef BLACKBOX
+        axisPID_P[axis] = PTerm;
+        axisPID_I[axis] = ITerm;
+        axisPID_D[axis] = -DTerm;
+#endif
+    }
+
+    tmp0flt = (int32_t)FLOATcycleTime & (int32_t)~3;                          // Filter last 2 bit jitter
+    tmp0flt /= 3000.0f;
+
+    if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
+        PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
+        tmp0 = lrintf(gyroData[FD_YAW] * 0.25f);
+        PTerm = rcCommand[FD_YAW] - tmp0 * PTerm / 80;
+        if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
+            errorGyroI[FD_YAW] = 0;
+        } else {
+            error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0;
+            errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp
+            ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6;
+        }
+    } else {
+        tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5;
+        error = tmp0 - lrintf(gyroData[FD_YAW] * 0.25f);                       // Less Gyrojitter works actually better
+
+        if (ABS(tmp0) > 50) {
+            errorGyroI[FD_YAW] = 0;
+        } else {
+            errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454);
+        }
+
+        ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX);
+        PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6;
+
+        if (motorCount >= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
+            tmp0 = 300;
+            if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW];
+            PTerm = constrain(PTerm, -tmp0, tmp0);
+        }
+    }
+    axisPID[FD_YAW] = PTerm + ITerm;
+    axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]);                                 // Round up result.
+
+#ifdef BLACKBOX
+    axisPID_P[FD_YAW] = PTerm;
+    axisPID_I[FD_YAW] = ITerm;
+    axisPID_D[FD_YAW] = 0;
+#endif
+}
+
 static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
         rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 {
@@ -606,13 +736,16 @@ void setPIDController(int type)
             pid_controller = pidRewrite;
             break;
         case 2:
-            pid_controller = pidBaseflight;
+            pid_controller = pidLuxFloat;
             break;
         case 3:
             pid_controller = pidMultiWii23;
             break;
         case 4:
             pid_controller = pidMultiWiiHybrid;
+            break;
+        case 5:
+            pid_controller = pidHarakiri;
     }
 }
 
diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h
index 95877badf2..51fd74a0a7 100644
--- a/src/main/flight/flight.h
+++ b/src/main/flight/flight.h
@@ -111,7 +111,6 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 extern int16_t heading, magHold;
 
 extern int32_t AltHold;
-extern int32_t EstAlt;
 extern int32_t vario;
 
 void setPIDController(int type);
diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index 8441ea90f1..a1c2a10a50 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "rx/rx.h"
+
 typedef enum {
     BOXARM = 0,
     BOXANGLE,
diff --git a/src/main/io/serial.c b/src/main/io/serial.c
index 397d4ee744..4c9b5c4d75 100644
--- a/src/main/io/serial.c
+++ b/src/main/io/serial.c
@@ -44,7 +44,6 @@
 
 void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintToUpdate);
 
-void mspInit(serialConfig_t *serialConfig);
 void cliInit(serialConfig_t *serialConfig);
 
 // this exists so the user can reference scenarios by a number in the CLI instead of an unuser-friendly bitmask.
@@ -92,13 +91,13 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
     {SERIAL_PORT_USB_VCP,       9600,   115200,   SPF_NONE },
 #endif
 #ifdef USE_USART1
-    {SERIAL_PORT_USART1,        9600,   115200,   SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART1,        9600,   250000,   SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 #endif
 #ifdef USE_USART2
-    {SERIAL_PORT_USART2,        9600,   115200,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART2,        9600,   250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 #endif
 #ifdef USE_USART3
-    {SERIAL_PORT_USART3,        9600,   115200,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART3,        9600,   250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 #endif
 };
 
@@ -118,8 +117,8 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
 #ifdef USE_VCP
     {SERIAL_PORT_USB_VCP,       9600,   115200,   SPF_NONE },
 #endif
-    {SERIAL_PORT_USART1,        9600, 115200,   SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
-    {SERIAL_PORT_USART3,        9600, 115200,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART1,        9600, 250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART3,        9600, 250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
     {SERIAL_PORT_SOFTSERIAL1,   9600, 19200,    SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}
 };
 #else
@@ -134,7 +133,7 @@ static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
 };
 
 const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
-    {SERIAL_PORT_USART1,        9600, 250000,   SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
+    {SERIAL_PORT_USART1,        9600, 250000,   SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
     {SERIAL_PORT_USART2,        9600, 250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 #if (SERIAL_PORT_COUNT > 2)
     {SERIAL_PORT_SOFTSERIAL1,   9600, 19200,    SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE},
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 9a20e7e050..a64e416ed5 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -384,7 +384,7 @@ const clivalue_t valueTable[] = {
     { "mag_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.mag_hardware, 0, MAG_NONE },
     { "mag_declination",            VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 },
 
-    { "pid_controller",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 4 },
+    { "pid_controller",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 5 },
 
     { "p_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 },
     { "i_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 },
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 3a6ff18e7d..0c4481f0b3 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -45,6 +45,8 @@
 #include "flight/mixer.h"
 #include "flight/failsafe.h"
 #include "flight/navigation.h"
+#include "flight/altitudehold.h"
+
 #include "rx/rx.h"
 #include "rx/msp.h"
 #include "io/escservo.h"
@@ -55,9 +57,11 @@
 #include "io/ledstrip.h"
 #include "io/flashfs.h"
 #include "telemetry/telemetry.h"
+
 #include "sensors/boardalignment.h"
 #include "sensors/sensors.h"
 #include "sensors/battery.h"
+#include "sensors/sonar.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
 #include "sensors/compass.h"
@@ -204,6 +208,10 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER;
 #define MSP_VOLTAGE_METER_CONFIG        56
 #define MSP_SET_VOLTAGE_METER_CONFIG    57
 
+#define MSP_SONAR_ALTITUDE              58 //out message get sonar altitude [cm]
+
+#define MSP_PID_CONTROLLER              59
+#define MSP_SET_PID_CONTROLLER          60
 //
 // Baseflight MSP commands (if enabled they exist in Cleanflight)
 //
@@ -373,7 +381,7 @@ static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
 
 static mspPort_t *currentPort;
 
-void serialize32(uint32_t a)
+static void serialize32(uint32_t a)
 {
     static uint8_t t;
     t = a;
@@ -390,7 +398,7 @@ void serialize32(uint32_t a)
     currentPort->checksum ^= t;
 }
 
-void serialize16(int16_t a)
+static void serialize16(int16_t a)
 {
     static uint8_t t;
     t = a;
@@ -401,32 +409,32 @@ void serialize16(int16_t a)
     currentPort->checksum ^= t;
 }
 
-void serialize8(uint8_t a)
+static void serialize8(uint8_t a)
 {
     serialWrite(mspSerialPort, a);
     currentPort->checksum ^= a;
 }
 
-uint8_t read8(void)
+static uint8_t read8(void)
 {
     return currentPort->inBuf[currentPort->indRX++] & 0xff;
 }
 
-uint16_t read16(void)
+static uint16_t read16(void)
 {
     uint16_t t = read8();
     t += (uint16_t)read8() << 8;
     return t;
 }
 
-uint32_t read32(void)
+static uint32_t read32(void)
 {
     uint32_t t = read16();
     t += (uint32_t)read16() << 16;
     return t;
 }
 
-void headSerialResponse(uint8_t err, uint8_t responseBodySize)
+static void headSerialResponse(uint8_t err, uint8_t responseBodySize)
 {
     serialize8('$');
     serialize8('M');
@@ -436,36 +444,36 @@ void headSerialResponse(uint8_t err, uint8_t responseBodySize)
     serialize8(currentPort->cmdMSP);
 }
 
-void headSerialReply(uint8_t responseBodySize)
+static void headSerialReply(uint8_t responseBodySize)
 {
     headSerialResponse(0, responseBodySize);
 }
 
-void headSerialError(uint8_t responseBodySize)
+static void headSerialError(uint8_t responseBodySize)
 {
     headSerialResponse(1, responseBodySize);
 }
 
-void tailSerialReply(void)
+static void tailSerialReply(void)
 {
     serialize8(currentPort->checksum);
 }
 
-void s_struct(uint8_t *cb, uint8_t siz)
+static void s_struct(uint8_t *cb, uint8_t siz)
 {
     headSerialReply(siz);
     while (siz--)
         serialize8(*cb++);
 }
 
-void serializeNames(const char *s)
+static void serializeNames(const char *s)
 {
     const char *c;
     for (c = s; *c; c++)
         serialize8(*c);
 }
 
-const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
+static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
 {
     uint8_t boxIndex;
     const box_t *candidate;
@@ -478,7 +486,7 @@ const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
     return NULL;
 }
 
-const box_t *findBoxByPermenantId(uint8_t permenantId)
+static const box_t *findBoxByPermenantId(uint8_t permenantId)
 {
     uint8_t boxIndex;
     const box_t *candidate;
@@ -491,7 +499,7 @@ const box_t *findBoxByPermenantId(uint8_t permenantId)
     return NULL;
 }
 
-void serializeBoxNamesReply(void)
+static void serializeBoxNamesReply(void)
 {
     int i, activeBoxId, j, flag = 1, count = 0, len;
     const box_t *box;
@@ -523,7 +531,7 @@ reset:
     }
 }
 
-void serializeDataflashSummaryReply(void)
+static void serializeDataflashSummaryReply(void)
 {
 #ifdef FLASHFS
     const flashGeometry_t *geometry = flashfsGetGeometry();
@@ -538,7 +546,7 @@ void serializeDataflashSummaryReply(void)
 }
 
 #ifdef FLASHFS
-void serializeDataflashReadReply(uint32_t address, uint8_t size)
+static void serializeDataflashReadReply(uint32_t address, uint8_t size)
 {
     enum { DATAFLASH_READ_REPLY_CHUNK_SIZE = 128 };
 
@@ -668,7 +676,7 @@ void mspInit(serialConfig_t *serialConfig)
         activeBoxIds[activeBoxIdCount++] = BOXLEDLOW;
     }
 #endif
-    
+
     if (feature(FEATURE_INFLIGHT_ACC_CAL))
         activeBoxIds[activeBoxIdCount++] = BOXCALIB;
 
@@ -695,7 +703,6 @@ static bool processOutCommand(uint8_t cmdMSP)
 {
     uint32_t i, tmp, junk;
 
-
 #ifdef GPS
     uint8_t wp_no;
     int32_t lat = 0, lon = 0;
@@ -864,18 +871,30 @@ static bool processOutCommand(uint8_t cmdMSP)
         break;
     case MSP_ALTITUDE:
         headSerialReply(6);
-        serialize32(EstAlt);
+#if defined(BARO) || defined(SONAR)
+        serialize32(altitudeHoldGetEstimatedAltitude());
+#else
+        serialize32(0);
+#endif
         serialize16(vario);
         break;
+    case MSP_SONAR_ALTITUDE:
+        headSerialReply(4);
+#if defined(SONAR)
+        serialize32(sonarGetLatestAltitude());
+#else
+        serialize32(0);
+#endif
+        break;
     case MSP_ANALOG:
         headSerialReply(7);
         serialize8((uint8_t)constrain(vbat, 0, 255));
         serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamphours drawn from battery
         serialize16(rssi);
         if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) {
-            serialize16((uint16_t)constrain((ABS(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps
+            serialize16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
         } else
-            serialize16((uint16_t)constrain(ABS(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps
+            serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
         break;
     case MSP_RC_TUNING:
         headSerialReply(7);
@@ -918,6 +937,10 @@ static bool processOutCommand(uint8_t cmdMSP)
         headSerialReply(sizeof(pidnames) - 1);
         serializeNames(pidnames);
         break;
+    case MSP_PID_CONTROLLER:
+        headSerialReply(1);
+        serialize8(currentProfile->pidController);
+        break;
     case MSP_MODE_RANGES:
         headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT);
         for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
@@ -1240,6 +1263,10 @@ static bool processInCommand(void)
         currentProfile->accelerometerTrims.values.pitch = read16();
         currentProfile->accelerometerTrims.values.roll  = read16();
         break;
+    case MSP_SET_PID_CONTROLLER:
+        currentProfile->pidController = read8();
+        setPIDController(currentProfile->pidController);
+        break;
     case MSP_SET_PID:
         if (currentProfile->pidController == 2) {
             for (i = 0; i < 3; i++) {
diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h
index 80abd02132..fd26e9981e 100644
--- a/src/main/io/serial_msp.h
+++ b/src/main/io/serial_msp.h
@@ -17,9 +17,14 @@
 
 #pragma once
 
+#include "io/serial.h"
+#include "drivers/serial.h"
+
 // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
 #define MAX_MSP_PORT_COUNT 2
 
+void mspInit(serialConfig_t *serialConfig);
+
 void mspProcess(void);
 void sendMspTelemetry(void);
 void mspSetTelemetryPort(serialPort_t *mspTelemetryPort);
diff --git a/src/main/main.c b/src/main/main.c
index e8092aa598..ecea01c39c 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -333,7 +333,7 @@ void init(void)
 
 #ifdef SONAR
     if (feature(FEATURE_SONAR)) {
-        Sonar_init();
+        sonarInit();
     }
 #endif
 
diff --git a/src/main/mw.c b/src/main/mw.c
index 12425863a8..0d542311c5 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -469,7 +469,7 @@ void executePeriodicTasks(void)
 #ifdef SONAR
     case UPDATE_SONAR_TASK:
         if (sensors(SENSOR_SONAR)) {
-            Sonar_update();
+            sonarUpdate();
         }
         break;
 #endif
diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c
index 518cb56052..842df64045 100644
--- a/src/main/sensors/sonar.c
+++ b/src/main/sensors/sonar.c
@@ -33,11 +33,13 @@
 #include "sensors/sensors.h"
 #include "sensors/sonar.h"
 
-int32_t sonarAlt = -1;                  // in cm , -1 indicate sonar is not in range - inclination adjusted by imu
+// in cm , -1 indicate sonar is not in range - inclination adjusted by imu
 
 #ifdef SONAR
 
-void Sonar_init(void)
+static int32_t calculatedAltitude;
+
+void sonarInit(void)
 {
 #if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R)
     static const sonarHardware_t const sonarPWM56 = {
@@ -74,25 +76,33 @@ void Sonar_init(void)
 #endif
 
     sensorsSet(SENSOR_SONAR);
-    sonarAlt = 0;
+    calculatedAltitude = -1;
 }
 
-void Sonar_update(void)
+void sonarUpdate(void)
 {
     hcsr04_start_reading();
 }
 
+int32_t sonarRead(void)
+{
+    return hcsr04_get_distance();
+}
+
 int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
 {
     // calculate sonar altitude only if the sonar is facing downwards(<25deg)
     if (tiltAngle > 250)
-        return -1;
+        calculatedAltitude = -1;
+    else
+        calculatedAltitude = sonarAlt * (900.0f - tiltAngle) / 900.0f;
 
-    return sonarAlt * (900.0f - tiltAngle) / 900.0f;
+    return calculatedAltitude;
 }
 
-int32_t sonarRead(void) {
-    return hcsr04_get_distance();
+int32_t sonarGetLatestAltitude(void)
+{
+    return calculatedAltitude;
 }
 
 #endif
diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h
index a6a2a8b94d..3b8674e424 100644
--- a/src/main/sensors/sonar.h
+++ b/src/main/sensors/sonar.h
@@ -17,10 +17,9 @@
 
 #pragma once
 
-extern int32_t sonarAlt;
-
-void Sonar_init(void);
-void Sonar_update(void);
+void sonarInit(void);
+void sonarUpdate(void);
 
 int32_t sonarRead(void);
 int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);
+int32_t sonarGetLatestAltitude(void);
diff --git a/src/main/version.h b/src/main/version.h
index a31fe051fc..cb3706b0c3 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            6  // increment when a minor release is made (small new feature, change etc)
+#define FC_VERSION_MINOR            7  // 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 MW_VERSION              231