diff --git a/src/cli.c b/src/cli.c
index fdd6fcb260..c12371af94 100644
--- a/src/cli.c
+++ b/src/cli.c
@@ -140,6 +140,7 @@ const clivalue_t valueTable[] = {
     { "deadband", VAR_UINT8, &cfg.deadband, 0, 32 },
     { "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 },
     { "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 },
+    { "throttle_angle_correction", VAR_UINT8, &cfg.throttle_angle_correction, 0, 100 },
     { "rc_rate", VAR_UINT8, &cfg.rcRate8, 0, 250 },
     { "rc_expo", VAR_UINT8, &cfg.rcExpo8, 0, 100 },
     { "thr_mid", VAR_UINT8, &cfg.thrMid8, 0, 100 },
@@ -215,6 +216,8 @@ const clivalue_t valueTable[] = {
 
 static void cliSetVar(const clivalue_t *var, const int32_t value);
 static void cliPrintVar(const clivalue_t *var, uint32_t full);
+static void cliPrint(const char *str);
+static void cliWrite(uint8_t ch);
 
 #ifndef HAVE_ITOA_FUNCTION
 
@@ -395,7 +398,7 @@ static char *ftoa(float x, char *floatString)
 
 static void cliPrompt(void)
 {
-    uartPrint("\r\n# ");
+    cliPrint("\r\n# ");
 }
 
 static int cliCompare(const void *a, const void *b)
@@ -440,7 +443,7 @@ static void cliCMix(char *cmdline)
     len = strlen(cmdline);
 
     if (len == 0) {
-        uartPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n");
+        cliPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n");
         for (i = 0; i < MAX_MOTORS; i++) {
             if (mcfg.customMixer[i].throttle == 0.0f)
                 break;
@@ -457,10 +460,10 @@ static void cliCMix(char *cmdline)
             mixsum[1] += mcfg.customMixer[i].pitch;
             mixsum[2] += mcfg.customMixer[i].yaw;
         }
-        uartPrint("Sanity check:\t");
+        cliPrint("Sanity check:\t");
         for (i = 0; i < 3; i++)
-            uartPrint(fabs(mixsum[i]) > 0.01f ? "NG\t" : "OK\t");
-        uartPrint("\r\n");
+            cliPrint(fabs(mixsum[i]) > 0.01f ? "NG\t" : "OK\t");
+        cliPrint("\r\n");
         return;
     } else if (strncasecmp(cmdline, "reset", 5) == 0) {
         // erase custom mixer
@@ -472,7 +475,7 @@ static void cliCMix(char *cmdline)
             len = strlen(++ptr);
             for (i = 0; ; i++) {
                 if (mixerNames[i] == NULL) {
-                    uartPrint("Invalid mixer type...\r\n");
+                    cliPrint("Invalid mixer type...\r\n");
                     break;
                 }
                 if (strncasecmp(ptr, mixerNames[i], len) == 0) {
@@ -508,7 +511,7 @@ static void cliCMix(char *cmdline)
                 check++;
             }
             if (check != 4) {
-                uartPrint("Wrong number of arguments, needs idx thr roll pitch yaw\r\n");
+                cliPrint("Wrong number of arguments, needs idx thr roll pitch yaw\r\n");
             } else {
                 cliCMix("");
             }
@@ -520,9 +523,9 @@ static void cliCMix(char *cmdline)
 
 static void cliDefaults(char *cmdline)
 {
-    uartPrint("Resetting to defaults...\r\n");
+    cliPrint("Resetting to defaults...\r\n");
     checkFirstTime(true);
-    uartPrint("Rebooting...");
+    cliPrint("Rebooting...");
     delay(10);
     systemReset(false);
 }
@@ -595,13 +598,13 @@ static void cliDump(char *cmdline)
         setval = &valueTable[i];
         printf("set %s = ", valueTable[i].name);
         cliPrintVar(setval, 0);
-        uartPrint("\r\n");
+        cliPrint("\r\n");
     }
 }
 
 static void cliExit(char *cmdline)
 {
-    uartPrint("\r\nLeaving CLI mode...\r\n");
+    cliPrint("\r\nLeaving CLI mode...\r\n");
     *cliBuffer = '\0';
     bufferIndex = 0;
     cliMode = 0;
@@ -619,22 +622,22 @@ static void cliFeature(char *cmdline)
     mask = featureMask();
 
     if (len == 0) {
-        uartPrint("Enabled features: ");
+        cliPrint("Enabled features: ");
         for (i = 0; ; i++) {
             if (featureNames[i] == NULL)
                 break;
             if (mask & (1 << i))
                 printf("%s ", featureNames[i]);
         }
-        uartPrint("\r\n");
+        cliPrint("\r\n");
     } else if (strncasecmp(cmdline, "list", len) == 0) {
-        uartPrint("Available features: ");
+        cliPrint("Available features: ");
         for (i = 0; ; i++) {
             if (featureNames[i] == NULL)
                 break;
             printf("%s ", featureNames[i]);
         }
-        uartPrint("\r\n");
+        cliPrint("\r\n");
         return;
     } else {
         bool remove = false;
@@ -647,16 +650,16 @@ static void cliFeature(char *cmdline)
 
         for (i = 0; ; i++) {
             if (featureNames[i] == NULL) {
-                uartPrint("Invalid feature name...\r\n");
+                cliPrint("Invalid feature name...\r\n");
                 break;
             }
             if (strncasecmp(cmdline, featureNames[i], len) == 0) {
                 if (remove) {
                     featureClear(1 << i);
-                    uartPrint("Disabled ");
+                    cliPrint("Disabled ");
                 } else {
                     featureSet(1 << i);
-                    uartPrint("Enabled ");
+                    cliPrint("Enabled ");
                 }
                 printf("%s\r\n", featureNames[i]);
                 break;
@@ -669,7 +672,7 @@ static void cliHelp(char *cmdline)
 {
     uint32_t i = 0;
 
-    uartPrint("Available commands:\r\n");    
+    cliPrint("Available commands:\r\n");    
     for (i = 0; i < CMD_COUNT; i++)
         printf("%s\t%s\r\n", cmdTable[i].name, cmdTable[i].param);
 }
@@ -689,12 +692,12 @@ static void cliMap(char *cmdline)
         for (i = 0; i < 8; i++) {
             if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i]))
                 continue;
-            uartPrint("Must be any order of AETR1234\r\n");
+            cliPrint("Must be any order of AETR1234\r\n");
             return;
         }
         parseRcChannels(cmdline);
     }
-    uartPrint("Current assignment: ");
+    cliPrint("Current assignment: ");
     for (i = 0; i < 8; i++)
         out[mcfg.rcmap[i]] = rcChannelLetters[i];
     out[i] = '\0';
@@ -712,19 +715,19 @@ static void cliMixer(char *cmdline)
         printf("Current mixer: %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]);
         return;
     } else if (strncasecmp(cmdline, "list", len) == 0) {
-        uartPrint("Available mixers: ");
+        cliPrint("Available mixers: ");
         for (i = 0; ; i++) {
             if (mixerNames[i] == NULL)
                 break;
             printf("%s ", mixerNames[i]);
         }
-        uartPrint("\r\n");
+        cliPrint("\r\n");
         return;
     }
 
     for (i = 0; ; i++) {
         if (mixerNames[i] == NULL) {
-            uartPrint("Invalid mixer type...\r\n");
+            cliPrint("Invalid mixer type...\r\n");
             break;
         }
         if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
@@ -756,13 +759,24 @@ static void cliProfile(char *cmdline)
 
 static void cliSave(char *cmdline)
 {
-    uartPrint("Saving...");
+    cliPrint("Saving...");
     writeEEPROM(0, true);
-    uartPrint("\r\nRebooting...");
+    cliPrint("\r\nRebooting...");
     delay(10);
     systemReset(false);
 }
 
+static void cliPrint(const char *str)
+{
+    while (*str)
+        uartWrite(core.mainport, *(str++));
+}
+
+static void cliWrite(uint8_t ch)
+{
+    uartWrite(core.mainport, ch);
+}
+
 static void cliPrintVar(const clivalue_t *var, uint32_t full)
 {
     int32_t value = 0;
@@ -837,12 +851,12 @@ static void cliSet(char *cmdline)
     len = strlen(cmdline);
 
     if (len == 0 || (len == 1 && cmdline[0] == '*')) {
-        uartPrint("Current settings: \r\n");
+        cliPrint("Current settings: \r\n");
         for (i = 0; i < VALUE_COUNT; i++) {
             val = &valueTable[i];
             printf("%s = ", valueTable[i].name);
             cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui
-            uartPrint("\r\n");
+            cliPrint("\r\n");
         }
     } else if ((eqptr = strstr(cmdline, "="))) {
         // has equal, set var
@@ -858,12 +872,12 @@ static void cliSet(char *cmdline)
                     printf("%s set to ", valueTable[i].name);
                     cliPrintVar(val, 0);
                 } else {
-                    uartPrint("ERR: Value assignment out of range\r\n");
+                    cliPrint("ERR: Value assignment out of range\r\n");
                 }
                 return;
             }
         }
-        uartPrint("ERR: Unknown variable name\r\n");
+        cliPrint("ERR: Unknown variable name\r\n");
     } else {
         // no equals, check for matching variables.
         for (i = 0; i < VALUE_COUNT; i++) {
@@ -898,26 +912,26 @@ static void cliStatus(char *cmdline)
         if (accHardware == ACC_MPU6050)
             printf(".%c", mcfg.mpu6050_scale ? 'o' : 'n');
     }
-    uartPrint("\r\n");
+    cliPrint("\r\n");
 
     printf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cGetErrorCounter(), sizeof(master_t));
 }
 
 static void cliVersion(char *cmdline)
 {
-    uartPrint("Afro32 CLI version 2.1 " __DATE__ " / " __TIME__);
+    cliPrint("Afro32 CLI version 2.2 " __DATE__ " / " __TIME__);
 }
 
 void cliProcess(void)
 {
     if (!cliMode) {
         cliMode = 1;
-        uartPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n");
+        cliPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n");
         cliPrompt();
     }
 
-    while (isUartAvailable()) {
-        uint8_t c = uartRead();
+    while (isUartAvailable(core.mainport)) {
+        uint8_t c = uartRead(core.mainport);
         if (c == '\t' || c == '?') {
             // do tab completion
             const clicmd_t *cmd, *pstart = NULL, *pend = NULL;
@@ -944,28 +958,28 @@ void cliProcess(void)
             }
             if (!bufferIndex || pstart != pend) {
                 /* Print list of ambiguous matches */
-                uartPrint("\r\033[K");
+                cliPrint("\r\033[K");
                 for (cmd = pstart; cmd <= pend; cmd++) {
-                    uartPrint(cmd->name);
-                    uartWrite('\t');
+                    cliPrint(cmd->name);
+                    cliWrite('\t');
                 }
                 cliPrompt();
                 i = 0;    /* Redraw prompt */
             }
             for (; i < bufferIndex; i++)
-                uartWrite(cliBuffer[i]);
+                cliWrite(cliBuffer[i]);
         } else if (!bufferIndex && c == 4) {
             cliExit(cliBuffer);
             return;
         } else if (c == 12) {
             // clear screen
-            uartPrint("\033[2J\033[1;1H");
+            cliPrint("\033[2J\033[1;1H");
             cliPrompt();
         } else if (bufferIndex && (c == '\n' || c == '\r')) {
             // enter pressed
             clicmd_t *cmd = NULL;
             clicmd_t target;
-            uartPrint("\r\n");
+            cliPrint("\r\n");
             cliBuffer[bufferIndex] = 0; // null terminate
 
             target.name = cliBuffer;
@@ -975,7 +989,7 @@ void cliProcess(void)
             if (cmd)
                 cmd->func(cliBuffer + strlen(cmd->name) + 1);
             else
-                uartPrint("ERR: Unknown command, try 'help'");
+                cliPrint("ERR: Unknown command, try 'help'");
 
             memset(cliBuffer, 0, sizeof(cliBuffer));
             bufferIndex = 0;
@@ -988,13 +1002,13 @@ void cliProcess(void)
             // backspace
             if (bufferIndex) {
                 cliBuffer[--bufferIndex] = 0;
-                uartPrint("\010 \010");
+                cliPrint("\010 \010");
             }
         } else if (bufferIndex < sizeof(cliBuffer) && c >= 32 && c <= 126) {
             if (!bufferIndex && c == 32)
                 continue;
             cliBuffer[bufferIndex++] = c;
-            uartWrite(c);
+            cliWrite(c);
         }
     }
 }
diff --git a/src/config.c b/src/config.c
index e79866ee9f..9e88f6f301 100755
--- a/src/config.c
+++ b/src/config.c
@@ -13,7 +13,7 @@ master_t mcfg;  // master config struct with data independent from profiles
 config_t cfg;   // profile config struct
 const char rcChannelLetters[] = "AERT1234";
 
-static uint8_t EEPROM_CONF_VERSION = 48;
+static const uint8_t EEPROM_CONF_VERSION = 48;
 static uint32_t enabledSensors = 0;
 static void resetConf(void);
 
@@ -260,6 +260,7 @@ static void resetConf(void)
     cfg.yawdeadband = 0;
     cfg.alt_hold_throttle_neutral = 40;
     cfg.alt_hold_fast_change = 1;
+    cfg.throttle_angle_correction = 0;      // could be 40
 
     // Failsafe Variables
     cfg.failsafe_delay = 10;                // 1sec
diff --git a/src/drv_gpio.h b/src/drv_gpio.h
index 082a09311d..bc3025f4f2 100644
--- a/src/drv_gpio.h
+++ b/src/drv_gpio.h
@@ -21,8 +21,8 @@ typedef enum
 
 typedef enum
 {
-    Pin_0 = 0x0000,
-    Pin_1 = 0x0001,
+    Pin_0 = 0x0001,
+    Pin_1 = 0x0002,
     Pin_2 = 0x0004,
     Pin_3 = 0x0008,
     Pin_4 = 0x0010,
diff --git a/src/drv_hcsr04.c b/src/drv_hcsr04.c
index a04a7cc9b3..1dc7821b48 100644
--- a/src/drv_hcsr04.c
+++ b/src/drv_hcsr04.c
@@ -19,7 +19,7 @@ static uint8_t exti_pin_source;
 static IRQn_Type exti_irqn;
 
 static uint32_t last_measurement;
-static volatile int16_t *distance_ptr;
+static volatile int32_t *distance_ptr;
 
 void ECHO_EXTI_IRQHandler(void)
 {
@@ -106,7 +106,7 @@ void hcsr04_init(sonar_config_t config)
 }
 
 // distance calculation is done asynchronously, using interrupt
-void hcsr04_get_distance(volatile int16_t *distance)
+void hcsr04_get_distance(volatile int32_t *distance)
 {
     uint32_t current_time = millis();
 
diff --git a/src/drv_hcsr04.h b/src/drv_hcsr04.h
index 2986f976b1..565b6c0f4b 100644
--- a/src/drv_hcsr04.h
+++ b/src/drv_hcsr04.h
@@ -6,4 +6,4 @@ typedef enum {
 } sonar_config_t;
 
 void hcsr04_init(sonar_config_t config);
-void hcsr04_get_distance(volatile int16_t* distance);
+void hcsr04_get_distance(volatile int32_t *distance);
diff --git a/src/drv_pwm.c b/src/drv_pwm.c
index 0929a92aed..4bf6e9a0d3 100755
--- a/src/drv_pwm.c
+++ b/src/drv_pwm.c
@@ -264,15 +264,12 @@ static void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
     TIM_ICInit(tim, &TIM_ICInitStructure);
 }
 
-static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, uint8_t input)
+static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
 {
     gpio_config_t cfg;
 
     cfg.pin = pin;
-    if (input)
-        cfg.mode = Mode_IPD;
-    else
-        cfg.mode = Mode_AF_PP;
+    cfg.mode = mode;
     cfg.speed = Speed_2MHz;
     gpioInit(gpio, &cfg);
 }
@@ -281,7 +278,7 @@ static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value
 {
     pwmPortData_t *p = &pwmPorts[port];
     pwmTimeBase(timerHardware[port].tim, period);
-    pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, 0);
+    pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_AF_PP);
     pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value);
     // Needed only on TIM1
     if (timerHardware[port].outputEnable)
@@ -309,7 +306,7 @@ static pwmPortData_t *pwmInConfig(uint8_t port, pwmCallbackPtr callback, uint8_t
 {
     pwmPortData_t *p = &pwmPorts[port];
     pwmTimeBase(timerHardware[port].tim, 0xFFFF);
-    pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, 1);
+    pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_IPD);
     pwmICConfig(timerHardware[port].tim, timerHardware[port].channel, TIM_ICPolarity_Rising);
     TIM_Cmd(timerHardware[port].tim, ENABLE);
     pwmNVICConfig(timerHardware[port].irq);
diff --git a/src/drv_system.c b/src/drv_system.c
index 8f21648f6c..db0fbc0896 100755
--- a/src/drv_system.c
+++ b/src/drv_system.c
@@ -88,8 +88,11 @@ void systemInit(void)
     LED1_OFF;
     BEEP_OFF;
 
-    for (i = 0; i < gpio_count; i++)
+    for (i = 0; i < gpio_count; i++) {
+        if (hse_value == 12000000 && gpio_setup[i].cfg.mode == Mode_Out_OD)
+            gpio_setup[i].cfg.mode = Mode_Out_PP;
         gpioInit(gpio_setup[i].gpio, &gpio_setup[i].cfg);
+    }
 
     // Init cycle counter
     cycleCounterInit();
diff --git a/src/drv_uart.c b/src/drv_uart.c
index e65265c71d..e2e30e7fd8 100755
--- a/src/drv_uart.c
+++ b/src/drv_uart.c
@@ -1,60 +1,41 @@
 #include "board.h"
-
 /*
     DMA UART routines idea lifted from AutoQuad
     Copyright © 2011  Bill Nesbitt
 */
-#define UART_BUFFER_SIZE    256
 
-// Receive buffer, circular DMA
-volatile uint8_t rxBuffer[UART_BUFFER_SIZE];
-volatile uint32_t rxDMAPos = 0;
-volatile uint8_t txBuffer[UART_BUFFER_SIZE];
-volatile uint32_t txBufferTail = 0;
-volatile uint32_t txBufferHead = 0;
-volatile bool txDMAEmpty = false;
+static serialPort_t serialPort1;
+static serialPort_t serialPort2;
 
-static void uartTxDMA(void)
-{
-    DMA1_Channel4->CMAR = (uint32_t)&txBuffer[txBufferTail];
-    if (txBufferHead > txBufferTail) {
-        DMA1_Channel4->CNDTR = txBufferHead - txBufferTail;
-        txBufferTail = txBufferHead;
-    } else {
-        DMA1_Channel4->CNDTR = UART_BUFFER_SIZE - txBufferTail;
-        txBufferTail = 0;
-    }
-    txDMAEmpty = false;
-    DMA_Cmd(DMA1_Channel4, ENABLE);
-}
-
-void DMA1_Channel4_IRQHandler(void)
-{
-    DMA_ClearITPendingBit(DMA1_IT_TC4);
-    DMA_Cmd(DMA1_Channel4, DISABLE);
-
-    if (txBufferHead != txBufferTail)
-        uartTxDMA();
-    else
-        txDMAEmpty = true;
-}
-
-void uartInit(uint32_t speed)
+// USART1 - Telemetry (RX/TX by DMA)
+serialPort_t *serialUSART1(uint32_t baudRate, portmode_t mode)
 {
+    serialPort_t *s;
+    static volatile uint8_t rx1Buffer[UART1_RX_BUFFER_SIZE];
+    static volatile uint8_t tx1Buffer[UART1_TX_BUFFER_SIZE];
     gpio_config_t gpio;
-    USART_InitTypeDef USART_InitStructure;
-    DMA_InitTypeDef DMA_InitStructure;
     NVIC_InitTypeDef NVIC_InitStructure;
 
+    s = &serialPort1;
+    s->rxBufferSize = UART1_RX_BUFFER_SIZE;
+    s->txBufferSize = UART1_TX_BUFFER_SIZE;
+    s->rxBuffer = rx1Buffer;
+    s->txBuffer = tx1Buffer;
+    s->rxDMAChannel = DMA1_Channel5;
+    s->txDMAChannel = DMA1_Channel4;
+
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
     // USART1_TX    PA9
     // USART1_RX    PA10
-    gpio.pin = Pin_9;
     gpio.speed = Speed_2MHz;
+    gpio.pin = Pin_9;
     gpio.mode = Mode_AF_PP;
-    gpioInit(GPIOA, &gpio);
+    if (mode & MODE_TX)
+        gpioInit(GPIOA, &gpio);
     gpio.pin = Pin_10;
     gpio.mode = Mode_IPU;
-    gpioInit(GPIOA, &gpio);
+    if (mode & MODE_RX)
+        gpioInit(GPIOA, &gpio);
 
     // DMA TX Interrupt
     NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel4_IRQn;
@@ -63,191 +44,255 @@ void uartInit(uint32_t speed)
     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
     NVIC_Init(&NVIC_InitStructure);
 
-    USART_InitStructure.USART_BaudRate = speed;
-    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
-    USART_InitStructure.USART_StopBits = USART_StopBits_1;
-    USART_InitStructure.USART_Parity = USART_Parity_No;
-    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
-    USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
-    USART_Init(USART1, &USART_InitStructure);
-
-    // Receive DMA into a circular buffer
-    DMA_DeInit(DMA1_Channel5);
-    DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
-    DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
-    DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&USART1->DR;
-    DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)rxBuffer;
-    DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
-    DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
-    DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
-    DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
-    DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
-    DMA_InitStructure.DMA_BufferSize = UART_BUFFER_SIZE;
-    DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
-    DMA_Init(DMA1_Channel5, &DMA_InitStructure);
-
-    DMA_Cmd(DMA1_Channel5, ENABLE);
-    USART_DMACmd(USART1, USART_DMAReq_Rx, ENABLE);
-    rxDMAPos = DMA_GetCurrDataCounter(DMA1_Channel5);
-
-    // Transmit DMA
-    DMA_DeInit(DMA1_Channel4);
-    DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&USART1->DR;
-    DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
-    DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
-    DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
-    DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
-    DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
-    DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
-    DMA_Init(DMA1_Channel4, &DMA_InitStructure);
-    DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
-    DMA1_Channel4->CNDTR = 0;
-    USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);
-
-    USART_Cmd(USART1, ENABLE);
+    return s;
 }
 
-bool isUartAvailable(void)
+// USART2 - GPS or Spektrum or ?? (RX + TX by IRQ)
+serialPort_t *serialUSART2(uint32_t baudRate, portmode_t mode)
 {
-    return (DMA_GetCurrDataCounter(DMA1_Channel5) != rxDMAPos) ? true : false;
-}
-
-bool isUartTransmitDMAEmpty(void)
-{
-    return txDMAEmpty;
-}
-
-bool isUartTransmitEmpty(void)
-{
-    return (txBufferTail == txBufferHead);
-}
-
-uint8_t uartRead(void)
-{
-    uint8_t ch;
-
-    ch = rxBuffer[UART_BUFFER_SIZE - rxDMAPos];
-    // go back around the buffer
-    if (--rxDMAPos == 0)
-        rxDMAPos = UART_BUFFER_SIZE;
-
-    return ch;
-}
-
-uint8_t uartReadPoll(void)
-{
-    while (!isUartAvailable()); // wait for some bytes
-    return uartRead();
-}
-
-void uartWrite(uint8_t ch)
-{
-    txBuffer[txBufferHead] = ch;
-    txBufferHead = (txBufferHead + 1) % UART_BUFFER_SIZE;
-
-    // if DMA wasn't enabled, fire it up
-    if (!(DMA1_Channel4->CCR & 1))
-        uartTxDMA();
-}
-
-void uartPrint(char *str)
-{
-    while (*str)
-        uartWrite(*(str++));
-}
-
-/* -------------------------- UART2 (Spektrum, GPS) ----------------------------- */
-uartReceiveCallbackPtr uart2Callback = NULL;
-#define UART2_BUFFER_SIZE    128
-
-volatile uint8_t tx2Buffer[UART2_BUFFER_SIZE];
-uint32_t tx2BufferTail = 0;
-uint32_t tx2BufferHead = 0;
-bool uart2RxOnly = false;
-
-static void uart2Open(uint32_t speed)
-{
-    USART_InitTypeDef USART_InitStructure;
-
-    USART_StructInit(&USART_InitStructure);
-    USART_InitStructure.USART_BaudRate = speed;
-    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
-    USART_InitStructure.USART_StopBits = USART_StopBits_1;
-    USART_InitStructure.USART_Parity = USART_Parity_No;
-    USART_InitStructure.USART_Mode = USART_Mode_Rx | (uart2RxOnly ? 0 : USART_Mode_Tx);
-    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
-    USART_Init(USART2, &USART_InitStructure);
-    USART_Cmd(USART2, ENABLE);
-}
-
-void uart2Init(uint32_t speed, uartReceiveCallbackPtr func, bool rxOnly)
-{
-    NVIC_InitTypeDef NVIC_InitStructure;
+    serialPort_t *s;
+    static volatile uint8_t rx2Buffer[UART2_RX_BUFFER_SIZE];
+    static volatile uint8_t tx2Buffer[UART2_TX_BUFFER_SIZE];
     gpio_config_t gpio;
+    NVIC_InitTypeDef NVIC_InitStructure;
+
+    s = &serialPort2;
+    s->baudRate = baudRate;
+    s->rxBufferSize = UART2_RX_BUFFER_SIZE;
+    s->txBufferSize = UART2_TX_BUFFER_SIZE;
+    s->rxBuffer = rx2Buffer;
+    s->txBuffer = tx2Buffer;
+    s->USARTx = USART2;
 
     RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
+    // USART2_TX    PA2
+    // USART2_RX    PA3
+    gpio.speed = Speed_2MHz;
+    gpio.pin = GPIO_Pin_2;
+    gpio.mode = Mode_AF_PP;
+    if (mode & MODE_TX)
+        gpioInit(GPIOA, &gpio);
+    gpio.pin = Pin_3;
+    gpio.mode = Mode_IPU;
+    if (mode & MODE_RX)
+        gpioInit(GPIOA, &gpio);
 
-    uart2RxOnly = rxOnly;
-
+    // RX/TX Interrupt
     NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
     NVIC_Init(&NVIC_InitStructure);
 
-    // USART2_TX    PA2
-    // USART2_RX    PA3
-    gpio.pin = GPIO_Pin_2;
-    gpio.speed = Speed_2MHz;
-    gpio.mode = Mode_AF_PP;
-    if (!rxOnly)
-        gpioInit(GPIOA, &gpio);
-    gpio.pin = Pin_3;
-    gpio.mode = Mode_IPU;
-    gpioInit(GPIOA, &gpio);
-
-    uart2Open(speed);
-    USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
-    if (!rxOnly)
-        USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
-    uart2Callback = func;
+    return s;
 }
 
-void uart2ChangeBaud(uint32_t speed)
+serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode)
 {
-    uart2Open(speed);
-}
+    DMA_InitTypeDef DMA_InitStructure;
+    USART_InitTypeDef USART_InitStructure;
+    serialPort_t *s = NULL;
 
-void uart2Write(uint8_t ch)
-{
-    if (uart2RxOnly)
-        return;
+    if (USARTx == USART1)
+        s = serialUSART1(baudRate, mode);
+    if (USARTx == USART2)
+        s = serialUSART2(baudRate, mode);
 
-    tx2Buffer[tx2BufferHead] = ch;
-    tx2BufferHead = (tx2BufferHead + 1) % UART2_BUFFER_SIZE;
+    s->USARTx = USARTx;
+    s->rxBufferHead = s->rxBufferTail = 0;
+    s->txBufferHead = s->txBufferTail = 0;
+    // callback for IRQ-based RX ONLY
+    s->callback = callback;
+    s->mode = mode;
 
-    USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
-}
+    USART_InitStructure.USART_BaudRate = baudRate;
+    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
+    USART_InitStructure.USART_StopBits = USART_StopBits_1;
+    USART_InitStructure.USART_Parity = USART_Parity_No;
+    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
+    USART_InitStructure.USART_Mode = 0;
+    if (mode & MODE_RX)
+        USART_InitStructure.USART_Mode |= USART_Mode_Rx;
+    if (mode & MODE_TX)
+        USART_InitStructure.USART_Mode |= USART_Mode_Tx;
+    USART_Init(USARTx, &USART_InitStructure);
+    USART_Cmd(USARTx, ENABLE);
 
-bool isUart2TransmitEmpty(void)
-{
-    return tx2BufferTail == tx2BufferHead;
-}
+    DMA_StructInit(&DMA_InitStructure);
+    DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&USARTx->DR;
+    DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
+    DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
+    DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+    DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+    DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+    DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
 
-void USART2_IRQHandler(void)
-{
-    uint16_t SR = USART2->SR;
-
-    if (SR & USART_IT_RXNE) {
-        if (uart2Callback)
-            uart2Callback(USART_ReceiveData(USART2));
-    }
-    if (SR & USART_FLAG_TXE) {
-        if (tx2BufferTail != tx2BufferHead) {
-            USART2->DR = tx2Buffer[tx2BufferTail];
-            tx2BufferTail = (tx2BufferTail + 1) % UART2_BUFFER_SIZE;
+    // Receive DMA or IRQ
+    if (mode & MODE_RX) {
+        if (s->rxDMAChannel) {
+            DMA_InitStructure.DMA_BufferSize = s->rxBufferSize;
+            DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
+            DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
+            DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)s->rxBuffer;
+            DMA_DeInit(s->rxDMAChannel);
+            DMA_Init(s->rxDMAChannel, &DMA_InitStructure);
+            DMA_Cmd(s->rxDMAChannel, ENABLE);
+            USART_DMACmd(USARTx, USART_DMAReq_Rx, ENABLE);
+            s->rxDMAPos = DMA_GetCurrDataCounter(s->rxDMAChannel);
         } else {
-            USART_ITConfig(USART2, USART_IT_TXE, DISABLE);
+            USART_ITConfig(USARTx, USART_IT_RXNE, ENABLE);
+        }
+    }
+
+    // Transmit DMA or IRQ
+    if (mode & MODE_TX) {
+        if (s->txDMAChannel) {
+            DMA_InitStructure.DMA_BufferSize = s->txBufferSize;
+            DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
+            DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
+            DMA_DeInit(s->txDMAChannel);
+            DMA_Init(s->txDMAChannel, &DMA_InitStructure);
+            DMA_ITConfig(s->txDMAChannel, DMA_IT_TC, ENABLE);
+            DMA_SetCurrDataCounter(s->txDMAChannel, 0);
+            s->txDMAChannel->CNDTR = 0;
+            USART_DMACmd(USARTx, USART_DMAReq_Tx, ENABLE);
+        } else {
+            USART_ITConfig(USARTx, USART_IT_TXE, ENABLE);
+        }
+    }
+
+    return s;
+}
+
+void uartChangeBaud(serialPort_t *s, uint32_t baudRate)
+{
+    USART_InitTypeDef USART_InitStructure;
+
+    USART_InitStructure.USART_BaudRate = baudRate;
+    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
+    USART_InitStructure.USART_StopBits = USART_StopBits_1;
+    USART_InitStructure.USART_Parity = USART_Parity_No;
+    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
+    USART_InitStructure.USART_Mode = 0;
+    if (s->mode & MODE_RX)
+        USART_InitStructure.USART_Mode |= USART_Mode_Rx;
+    if (s->mode & MODE_TX)
+        USART_InitStructure.USART_Mode |= USART_Mode_Tx;
+    USART_Init(s->USARTx, &USART_InitStructure);
+}
+
+static void uartStartTxDMA(serialPort_t *s)
+{
+    s->txDMAChannel->CMAR = (uint32_t)&s->txBuffer[s->txBufferTail];
+    if (s->txBufferHead > s->txBufferTail) {
+        s->txDMAChannel->CNDTR = s->txBufferHead - s->txBufferTail;
+        s->txBufferTail = s->txBufferHead;
+    } else {
+        s->txDMAChannel->CNDTR = s->txBufferSize - s->txBufferTail;
+        s->txBufferTail = 0;
+    }
+    s->txDMAEmpty = false;
+    DMA_Cmd(s->txDMAChannel, ENABLE);
+}
+
+bool isUartAvailable(serialPort_t *s)
+{
+    if (s->rxDMAChannel)
+        return s->rxDMAChannel->CNDTR != s->rxDMAPos;
+    else
+        return s->rxBufferTail != s->rxBufferHead;
+}
+
+// BUGBUG TODO TODO FIXME
+bool isUartTransmitEmpty(serialPort_t *s)
+{
+    if (s->txDMAChannel)
+        return s->txDMAEmpty;
+    else
+        return s->txBufferTail == s->txBufferHead;
+}
+
+uint8_t uartRead(serialPort_t *s)
+{
+    uint8_t ch;
+
+    if (s->rxDMAChannel) {
+        ch = s->rxBuffer[s->rxBufferSize - s->rxDMAPos];
+        if (--s->rxDMAPos == 0)
+            s->rxDMAPos = s->rxBufferSize;
+    } else {
+        ch = s->rxBuffer[s->rxBufferTail];
+        s->rxBufferTail = (s->rxBufferTail + 1) % s->rxBufferSize;
+    }
+
+    return ch;
+}
+
+void uartWrite(serialPort_t *s, uint8_t ch)
+{
+    s->txBuffer[s->txBufferHead] = ch;
+    s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
+
+    if (s->txDMAChannel) {
+        if (!(s->txDMAChannel->CCR & 1))
+            uartStartTxDMA(s);
+    } else {
+        USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE);
+    }
+}
+
+// Handlers
+
+// USART1 Tx DMA Handler
+void DMA1_Channel4_IRQHandler(void)
+{
+    serialPort_t *s = &serialPort1;
+    DMA_ClearITPendingBit(DMA1_IT_TC4);
+    DMA_Cmd(s->txDMAChannel, DISABLE);
+
+    if (s->txBufferHead != s->txBufferTail)
+        uartStartTxDMA(s);
+    else
+        s->txDMAEmpty = true;
+}
+
+// USART1 Tx IRQ Handler
+void USART1_IRQHandler(void)
+{
+    serialPort_t *s = &serialPort1;
+    uint16_t SR = s->USARTx->SR;
+
+    if (SR & USART_FLAG_TXE) {
+        if (s->txBufferTail != s->txBufferHead) {
+            s->USARTx->DR = s->txBuffer[s->txBufferTail];
+            s->txBufferTail = (s->txBufferTail + 1) % s->txBufferSize;
+        } else {
+            USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
+        }
+    }
+}
+
+// USART2 Rx/Tx IRQ Handler
+void USART2_IRQHandler(void)
+{
+    serialPort_t *s = &serialPort2;
+    uint16_t SR = s->USARTx->SR;
+
+    if (SR & USART_FLAG_RXNE) {
+        // If we registered a callback, pass crap there
+        if (s->callback) {
+            s->callback(s->USARTx->DR);
+        } else {
+            s->rxBuffer[s->rxBufferHead] = s->USARTx->DR;
+            s->rxBufferHead = (s->rxBufferHead + 1) % s->rxBufferSize;
+        }
+    }
+    if (SR & USART_FLAG_TXE) {
+        if (s->txBufferTail != s->txBufferHead) {
+            s->USARTx->DR = s->txBuffer[s->txBufferTail];
+            s->txBufferTail = (s->txBufferTail + 1) % s->txBufferSize;
+        } else {
+            USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
         }
     }
 }
diff --git a/src/drv_uart.h b/src/drv_uart.h
index 780dc92afe..df6369a116 100755
--- a/src/drv_uart.h
+++ b/src/drv_uart.h
@@ -1,17 +1,47 @@
 #pragma once
 
-// USART1
-void uartInit(uint32_t speed);
-bool isUartAvailable(void);
-bool isUartTransmitEmpty(void);
-bool isUartTransmitDMAEmpty(void);
-uint8_t uartRead(void);
-uint8_t uartReadPoll(void);
-void uartWrite(uint8_t ch);
-void uartPrint(char *str);
+#define UART_BUFFER_SIZE    64
 
-// USART2 (GPS, Spektrum)
-void uart2Init(uint32_t speed, uartReceiveCallbackPtr func, bool rxOnly);
-void uart2ChangeBaud(uint32_t speed);
-bool isUart2TransmitEmpty(void);
-void uart2Write(uint8_t ch);
+#define UART1_RX_BUFFER_SIZE    256
+#define UART1_TX_BUFFER_SIZE    256
+#define UART2_RX_BUFFER_SIZE    128
+#define UART2_TX_BUFFER_SIZE    64
+#define MAX_SERIAL_PORTS        2
+
+// This is a bitmask
+typedef enum portmode_t {
+    MODE_RX = 1,
+    MODE_TX = 2,
+    MODE_RXTX = 3
+} portmode_t;
+
+typedef struct {
+    uint32_t baudRate;
+    uint32_t rxBufferSize;
+    uint32_t txBufferSize;
+    volatile uint8_t *rxBuffer;
+    volatile uint8_t *txBuffer;
+    uint32_t rxDMAPos;
+    uint32_t rxBufferHead;
+    uint32_t rxBufferTail;
+    uint32_t txBufferHead;
+    uint32_t txBufferTail;
+
+    DMA_Channel_TypeDef *rxDMAChannel;
+    DMA_Channel_TypeDef *txDMAChannel;
+    uint32_t rxDMAIrq;
+    uint32_t txDMAIrq;
+    bool txDMAEmpty;
+    USART_TypeDef *USARTx;
+
+    uartReceiveCallbackPtr callback;
+    portmode_t mode;
+} serialPort_t;
+
+serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode);
+void uartChangeBaud(serialPort_t *s, uint32_t baudRate);
+bool isUartAvailable(serialPort_t *s);
+bool isUartTransmitEmpty(serialPort_t *s);
+uint8_t uartRead(serialPort_t *s);
+void uartWrite(serialPort_t *s, uint8_t ch);
+void uartPrint(serialPort_t *s, const char *str);
diff --git a/src/gps.c b/src/gps.c
index 7e559bad65..28dae1d9c2 100644
--- a/src/gps.c
+++ b/src/gps.c
@@ -43,7 +43,7 @@ void gpsInit(uint32_t baudrate)
     int offset = 0;
 
     GPS_set_pids();
-    uart2Init(baudrate, GPS_NewData, false);
+    core.gpsport = uartOpen(USART2, GPS_NewData, baudrate, MODE_RXTX);
 
     if (mcfg.gps_type == GPS_UBLOX)
         offset = 0;
@@ -52,7 +52,7 @@ void gpsInit(uint32_t baudrate)
 
     if (mcfg.gps_type != GPS_NMEA) {
         for (i = 0; i < 5; i++) {
-            uart2ChangeBaud(init_speed[i]);
+            uartChangeBaud(core.gpsport, init_speed[i]);
             switch (baudrate) {
                 case 19200:
                     gpsPrint(gpsInitStrings[offset]);
@@ -71,10 +71,10 @@ void gpsInit(uint32_t baudrate)
         }
     }
 
-    uart2ChangeBaud(baudrate);
+    uartChangeBaud(core.gpsport, baudrate);
     if (mcfg.gps_type == GPS_UBLOX) {
         for (i = 0; i < sizeof(ubloxInit); i++) {
-            uart2Write(ubloxInit[i]); // send ubx init binary
+            uartWrite(core.gpsport, ubloxInit[i]); // send ubx init binary
             delay(4);
         }
     } else if (mcfg.gps_type == GPS_MTK) {
@@ -91,13 +91,13 @@ void gpsInit(uint32_t baudrate)
 static void gpsPrint(const char *str)
 {
     while (*str) {
-        uart2Write(*str);
+        uartWrite(core.gpsport, *str);
         if (mcfg.gps_type == GPS_UBLOX)
             delay(4);
         str++;
     }
     // wait to send all
-    while (!isUart2TransmitEmpty());
+    while (!isUartTransmitEmpty(core.gpsport));
     delay(30);
 }
 
@@ -1112,7 +1112,6 @@ static bool UBLOX_parse_gps(void)
             f.GPS_FIX = false;
         GPS_numSat = _buffer.solution.satellites;
         // GPS_hdop                        = _buffer.solution.position_DOP;
-        // debug[3] = GPS_hdop;
         break;
     case MSG_VELNED:
         // speed_3d                        = _buffer.velned.speed_3d;  // cm/s
diff --git a/src/imu.c b/src/imu.c
index 5a2a4167d8..8470b4f26d 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -7,13 +7,14 @@ int32_t baroPressure = 0;
 int32_t baroTemperature = 0;
 int32_t baroPressureSum = 0;
 int32_t BaroAlt = 0;
-int16_t sonarAlt;              // to think about the unit
+int32_t sonarAlt;              // to think about the unit
 int32_t EstAlt;                // in cm
-int16_t BaroPID = 0;
+int32_t BaroPID = 0;
 int32_t AltHold;
-int16_t errorAltitudeI = 0;
-int16_t vario = 0;              // variometer in cm/s
-float magneticDeclination = 0.0f; // calculated at startup from config
+int32_t errorAltitudeI = 0;
+int32_t vario = 0;                      // variometer in cm/s
+int16_t throttleAngleCorrection = 0;    // correction of throttle in lateral wind,
+float magneticDeclination = 0.0f;       // calculated at startup from config
 float accVelScale;
 
 // **************
@@ -85,7 +86,7 @@ void computeIMU(void)
             Smoothing[YAW] = (mcfg.gyro_smoothing_factor) & 0xff;
         }
         for (axis = 0; axis < 3; axis++) {
-            gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1 ) / Smoothing[axis]);
+            gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1) / Smoothing[axis]);
             gyroSmooth[axis] = gyroData[axis];
         }
     } else if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
@@ -115,15 +116,6 @@ void computeIMU(void)
 // code size deduction and tmp vector intermediate step for vector rotation computation: October 2011 by Alex
 // **************************************************
 
-//******  advanced users settings *******************
-
-/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
-/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
-/* Default WMC value: n/a*/
-#define GYR_CMPFM_FACTOR 200.0f
-
-//****** end of advanced users settings *************
-
 #define INV_GYR_CMPF_FACTOR   (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))
 #define INV_GYR_CMPFM_FACTOR  (1.0f / ((float)mcfg.gyro_cmpfm_factor + 1.0f))
 
@@ -235,7 +227,7 @@ static void getEstimatedAttitude(void)
     // Apply complimentary filter (Gyro drift correction)
     // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
     // To do that, we just skip filter, as EstV already rotated by Gyro
-    if (72 < accMag && accMag < 133) {
+    if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
         for (axis = 0; axis < 3; axis++)
             EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
     }
@@ -291,13 +283,19 @@ static void getEstimatedAttitude(void)
             heading = heading + 360;
     }
 #endif
+
+    if (cfg.throttle_angle_correction) {
+        int cosZ = EstG.V.Z / acc_1G * 100.0f;
+        throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8;
+        
+    }
 }
 
 #ifdef BARO
 #define UPDATE_INTERVAL 25000   // 40hz update rate (20hz LPF on acc)
 #define INIT_DELAY      4000000 // 4 sec initialization delay
 
-int16_t applyDeadband(int16_t value, int16_t deadband)
+int16_t applyDeadband(int32_t value, int32_t deadband)
 {
     if (abs(value) < deadband) {
         value = 0;
@@ -315,13 +313,13 @@ int getEstimatedAltitude(void)
     static uint32_t previousT;
     uint32_t currentT = micros();
     uint32_t dTime;
-    int16_t error16;
-    int16_t baroVel;
-    int16_t accZ;
-    static int16_t accZoffset = 0;
+    int32_t error;
+    int32_t baroVel;
+    int32_t accZ;
+    int32_t vel_tmp;
+    static int32_t accZoffset = 0;
     static float vel = 0.0f;
     static int32_t lastBaroAlt;
-    int16_t vel_tmp;
 
     dTime = currentT - previousT;
     if (dTime < UPDATE_INTERVAL)
@@ -337,27 +335,27 @@ int getEstimatedAltitude(void)
     // baroGroundPressure is not supposed to be 0 here
     // see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
     BaroAlt = logf(baroGroundPressure * (cfg.baro_tab_size - 1) / (float)baroPressureSum) * (baroTemperature + 27315) * 29.271267f; // in cemtimeter 
-    EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3;   // additional LPF to reduce baro noise
+    EstAlt = (EstAlt * 6 + BaroAlt * 2) / 8;   // additional LPF to reduce baro noise
 
     //P
-    error16 = constrain(AltHold - EstAlt, -300, 300);
-    error16 = applyDeadband(error16, 10); // remove small P parametr to reduce noise near zero position
-    BaroPID = constrain((cfg.P8[PIDALT] * error16 >> 7), -150, +150);
+    error = constrain(AltHold - EstAlt, -300, 300);
+    error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
+    BaroPID = constrain(((cfg.P8[PIDALT] * error) / 128), -150, +150);
 
     //I
-    errorAltitudeI += cfg.I8[PIDALT] * error16 >> 6;
+    errorAltitudeI += (cfg.I8[PIDALT] * error) / 64;
     errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
-    BaroPID += errorAltitudeI >> 9;     // I in range +/-60
+    BaroPID += errorAltitudeI / 512;     // I in range +/-60
 
     // projection of ACC vector to global Z, with 1G subtructed
     // Math: accZ = A * G / |G| - 1G (invG is calculated in getEstimatedAttitude)
     accZ = (accSmooth[ROLL] * EstG.V.X + accSmooth[PITCH] * EstG.V.Y + accSmooth[YAW] * EstG.V.Z) * invG;
 
     if (!f.ARMED) {
-        accZoffset -= accZoffset >> 3;
+        accZoffset -= accZoffset / 8;
         accZoffset += accZ;
     }
-    accZ -= accZoffset >> 3;
+    accZ -= accZoffset / 8;
     accZ = applyDeadband(accZ, cfg.accz_deadband);
 
     // Integrator - velocity, cm/sec
@@ -377,7 +375,7 @@ int getEstimatedAltitude(void)
     vel_tmp = vel;
     vel_tmp = applyDeadband(vel_tmp, 5);
     vario = vel_tmp;
-    BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp >> 4, -150, 150);
+    BaroPID -= constrain((cfg.D8[PIDALT] * vel_tmp) / 16, -150, 150);
 
     return 1;
 }
diff --git a/src/main.c b/src/main.c
index bf033cd0e9..a2eb82fab2 100755
--- a/src/main.c
+++ b/src/main.c
@@ -1,7 +1,8 @@
 #include "board.h"
 #include "mw.h"
+
+core_t core;
 
-extern uint8_t useServo;
 extern rcReadRawDataPtr rcReadRawFunc;
 
 // two receiver read functions
@@ -12,15 +13,15 @@ extern uint16_t spektrumReadRawRC(uint8_t chan);
 // gcc/GNU version
 static void _putc(void *p, char c)
 {
-    uartWrite(c);
+    uartWrite(core.mainport, c);
 }
 #else
 // keil/armcc version
 int fputc(int c, FILE *f)
 {
     // let DMA catch up a bit when using set or dump, we're too fast.
-    while (!isUartTransmitDMAEmpty());
-    uartWrite(c);
+    while (!isUartTransmitEmpty(core.mainport));
+    uartWrite(core.mainport, c);
     return c;
 }
 #endif
@@ -31,27 +32,6 @@ int main(void)
     drv_pwm_config_t pwm_params;
     drv_adc_config_t adc_params;
 
-#if 0
-    // PC12, PA15
-    // using this to write asm for bootloader :)
-    RCC->APB2ENR |= RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO; // GPIOA/C+AFIO only
-    AFIO->MAPR &= 0xF0FFFFFF;
-    AFIO->MAPR = 0x02000000;
-    GPIOA->CRH = 0x34444444; // PIN 15 Output 50MHz
-    GPIOA->BRR = 0x8000; // set low 15
-    GPIOC->CRH = 0x44434444; // PIN 12 Output 50MHz
-    GPIOC->BRR = 0x1000; // set low 12
-#endif
-
-#if 0
-    // using this to write asm for bootloader :)
-    RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO
-    AFIO->MAPR &= 0xF0FFFFFF;
-    AFIO->MAPR = 0x02000000;
-    GPIOB->BRR = 0x18; // set low 4 & 3
-    GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz
-#endif
-
     systemInit();
 #ifdef USE_LAME_PRINTF
     init_printf(NULL, _putc);
@@ -73,7 +53,7 @@ int main(void)
     // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
     sensorsSet(SENSORS_SET);
 
-    mixerInit(); // this will set useServo var depending on mixer type
+    mixerInit(); // this will set core.useServo var depending on mixer type
     // when using airplane/wing mixer, servo/motor outputs are remapped
     if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
         pwm_params.airplane = true;
@@ -82,7 +62,7 @@ int main(void)
     pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SPEKTRUM); // spektrum support uses UART too
     pwm_params.usePPM = feature(FEATURE_PPM);
     pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
-    pwm_params.useServos = useServo;
+    pwm_params.useServos = core.useServo;
     pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
     pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
     pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
diff --git a/src/mixer.c b/src/mixer.c
index d640d8ee23..28f13cdfd8 100755
--- a/src/mixer.c
+++ b/src/mixer.c
@@ -2,7 +2,6 @@
 #include "mw.h"
 
 static uint8_t numberMotor = 0;
-uint8_t useServo = 0;
 int16_t motor[MAX_MOTORS];
 int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
 
@@ -144,10 +143,10 @@ void mixerInit(void)
     int i;
 
     // enable servos for mixes that require them. note, this shifts motor counts.
-    useServo = mixers[mcfg.mixerConfiguration].useServo;
+    core.useServo = mixers[mcfg.mixerConfiguration].useServo;
     // if we want camstab/trig, that also enables servos, even if mixer doesn't
     if (feature(FEATURE_SERVO_TILT))
-        useServo = 1;
+        core.useServo = 1;
 
     if (mcfg.mixerConfiguration == MULTITYPE_CUSTOM) {
         // load custom mixer into currentMixer
@@ -198,7 +197,7 @@ void mixerLoadMix(int index)
 
 void writeServos(void)
 {
-    if (!useServo)
+    if (!core.useServo)
         return;
 
     switch (mcfg.mixerConfiguration) {
diff --git a/src/mw.c b/src/mw.c
index 6a9f6600bc..dffb084ded 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -762,7 +762,6 @@ void loop(void)
 #ifdef SONAR
             if (sensors(SENSOR_SONAR)) {
                 Sonar_update();
-                debug[2] = sonarAlt;
             }
 #endif
             if (feature(FEATURE_VARIO) && f.VARIO_MODE)
@@ -838,6 +837,10 @@ void loop(void)
         }
 #endif
 
+        if (cfg.throttle_angle_correction && (f.ANGLE_MODE || f.HORIZON_MODE)) {
+            rcCommand[THROTTLE]+= throttleAngleCorrection;
+        }
+
         if (sensors(SENSOR_GPS)) {
             if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) {
                 float sin_yaw_y = sinf(heading * 0.0174532925f);
diff --git a/src/mw.h b/src/mw.h
index 081416e387..6d912b36cb 100755
--- a/src/mw.h
+++ b/src/mw.h
@@ -176,6 +176,7 @@ typedef struct config_t {
     uint8_t yawdeadband;                    // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
     uint8_t alt_hold_throttle_neutral;      // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
     uint8_t alt_hold_fast_change;           // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement
+    uint8_t throttle_angle_correction;      // 
 
     // Failsafe related configuration
     uint8_t failsafe_delay;                 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
@@ -285,6 +286,16 @@ typedef struct master_t {
     uint8_t chk;                            // XOR checksum
 } master_t;
 
+// Core runtime settings
+typedef struct core_t {
+    serialPort_t *mainport;
+    serialPort_t *gpsport;
+    serialPort_t *telemport;
+    serialPort_t *rcvrport;
+    bool useServo;
+
+} core_t;
+
 typedef struct flags_t {
     uint8_t OK_TO_ARM;
     uint8_t ARMED;
@@ -327,14 +338,14 @@ extern int32_t baroPressure;
 extern int32_t baroTemperature;
 extern int32_t baroPressureSum;
 extern int32_t BaroAlt;
-extern int16_t sonarAlt;
+extern int32_t sonarAlt;
 extern int32_t EstAlt;
 extern int32_t AltHold;
-extern int16_t errorAltitudeI;
-extern int16_t BaroPID;
-extern int16_t vario;
+extern int32_t errorAltitudeI;
+extern int32_t BaroPID;
+extern int32_t vario;
+extern int16_t throttleAngleCorrection;
 extern int16_t headFreeModeHold;
-extern int16_t zVelocity;
 extern int16_t heading, magHold;
 extern int16_t motor[MAX_MOTORS];
 extern int16_t servo[8];
@@ -367,7 +378,8 @@ extern uint8_t  GPS_svinfo_chn[16];                          // Channel number
 extern uint8_t  GPS_svinfo_svid[16];                         // Satellite ID
 extern uint8_t  GPS_svinfo_quality[16];                      // Bitfield Qualtity
 extern uint8_t  GPS_svinfo_cno[16];                          // Carrier to Noise Ratio (Signal Strength)
-
+
+extern core_t core;
 extern master_t mcfg;
 extern config_t cfg;
 extern flags_t f;
diff --git a/src/printf.c b/src/printf.c
index 7c75c9effc..d3cf616d5f 100644
--- a/src/printf.c
+++ b/src/printf.c
@@ -30,6 +30,7 @@
  */
 
 #include "board.h"
+#include "mw.h"
 #ifdef USE_LAME_PRINTF
 #define PRINTF_LONG_SUPPORT
 
@@ -227,7 +228,7 @@ void tfp_printf(char *fmt, ...)
     va_start(va, fmt);
     tfp_format(stdout_putp, stdout_putf, fmt, va);
     va_end(va);
-    while (!isUartTransmitEmpty());
+    while (!isUartTransmitEmpty(core.mainport));
 }
 
 static void putcp(void *p, char c)
@@ -235,8 +236,6 @@ static void putcp(void *p, char c)
     *(*((char **) p))++ = c;
 }
 
-
-
 void tfp_sprintf(char *s, char *fmt, ...)
 {
     va_list va;
diff --git a/src/serial.c b/src/serial.c
index ddf9857550..bf56a3c4b6 100755
--- a/src/serial.c
+++ b/src/serial.c
@@ -157,16 +157,16 @@ void serialize32(uint32_t a)
 {
     static uint8_t t;
     t = a;
-    uartWrite(t);
+    uartWrite(core.mainport, t);
     checksum ^= t;
     t = a >> 8;
-    uartWrite(t);
+    uartWrite(core.mainport, t);
     checksum ^= t;
     t = a >> 16;
-    uartWrite(t);
+    uartWrite(core.mainport, t);
     checksum ^= t;
     t = a >> 24;
-    uartWrite(t);
+    uartWrite(core.mainport, t);
     checksum ^= t;
 }
 
@@ -174,16 +174,16 @@ void serialize16(int16_t a)
 {
     static uint8_t t;
     t = a;
-    uartWrite(t);
+    uartWrite(core.mainport, t);
     checksum ^= t;
     t = a >> 8 & 0xff;
-    uartWrite(t);
+    uartWrite(core.mainport, t);
     checksum ^= t;
 }
 
 void serialize8(uint8_t a)
 {
-    uartWrite(a);
+    uartWrite(core.mainport, a);
     checksum ^= a;
 }
 
@@ -261,7 +261,9 @@ void serialInit(uint32_t baudrate)
 {
     int idx;
 
-    uartInit(baudrate);
+    core.mainport = uartOpen(USART1, NULL, baudrate, MODE_RXTX);
+    // TODO fix/hax
+    core.telemport = core.mainport;
     // calculate used boxes based on features and fill availableBoxes[] array
     memset(availableBoxes, 0xFF, sizeof(availableBoxes));
 
@@ -602,6 +604,8 @@ static void evaluateCommand(void)
         break;
     case MSP_DEBUG:
         headSerialReply(8);
+        // make use of this crap, output some useful QA statistics
+        debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
         for (i = 0; i < 4; i++)
             serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
         break;
@@ -668,8 +672,8 @@ void serialCom(void)
         return;
     }
 
-    while (isUartAvailable()) {
-        c = uartRead();
+    while (isUartAvailable(core.mainport)) {
+        c = uartRead(core.mainport);
 
         if (c_state == IDLE) {
             c_state = (c == '$') ? HEADER_START : IDLE;
@@ -705,7 +709,7 @@ void serialCom(void)
             c_state = IDLE;
         }
     }
-    if (!cliMode && !isUartAvailable() && feature(FEATURE_TELEMETRY) && f.ARMED) { // The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream
+    if (!cliMode && !isUartAvailable(core.telemport) && feature(FEATURE_TELEMETRY) && f.ARMED) { // The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream
         sendTelemetry();
         return;
     }
diff --git a/src/spektrum.c b/src/spektrum.c
index 215cccab95..c8d4351ac0 100644
--- a/src/spektrum.c
+++ b/src/spektrum.c
@@ -27,10 +27,10 @@ void spektrumInit(void)
         spek_chan_mask = 0x03;
     }
 
-    uart2Init(115200, spektrumDataReceive, true);
+    core.rcvrport = uartOpen(USART2, spektrumDataReceive, 115200, MODE_RX);
 }
 
-// UART2 Receive ISR callback
+// Receive ISR callback
 static void spektrumDataReceive(uint16_t c)
 {
     uint32_t spekTime;
diff --git a/src/telemetry.c b/src/telemetry.c
index 582ca6c890..39b42560d3 100644
--- a/src/telemetry.c
+++ b/src/telemetry.c
@@ -48,29 +48,28 @@
 // from sensors.c
 extern uint8_t batteryCellCount;
 
- 
 static void sendDataHead(uint8_t id)
 {
-    uartWrite(PROTOCOL_HEADER);
-    uartWrite(id);
+    uartWrite(core.telemport, PROTOCOL_HEADER);
+    uartWrite(core.telemport, id);
 }
 
 static void sendTelemetryTail(void)
 {
-    uartWrite(PROTOCOL_TAIL);
+    uartWrite(core.telemport, PROTOCOL_TAIL);
 }
 
 static void serializeFrsky(uint8_t data)
 {
     // take care of byte stuffing
     if (data == 0x5e) {
-        uartWrite(0x5d);
-        uartWrite(0x3e);
+        uartWrite(core.telemport, 0x5d);
+        uartWrite(core.telemport, 0x3e);
     } else if (data == 0x5d) {
-        uartWrite(0x5d);
-        uartWrite(0x3d);
+        uartWrite(core.telemport, 0x5d);
+        uartWrite(core.telemport, 0x3d);
     } else
-        uartWrite(data);
+        uartWrite(core.telemport, data);
 }
 
 static void serialize16(int16_t a)
@@ -183,7 +182,7 @@ static void sendVoltage(void)
 /*
  * Send voltage with ID_VOLTAGE_AMP
  */
-static void sendVoltageAmp()
+static void sendVoltageAmp(void)
 {
     uint16_t voltage = (vbat * 110) / 21;