diff --git a/src/drv_uart.c b/src/drv_uart.c
index 978fdb642..13cf425cc 100755
--- a/src/drv_uart.c
+++ b/src/drv_uart.c
@@ -8,10 +8,11 @@
 
 // Receive buffer, circular DMA
 volatile uint8_t rxBuffer[UART_BUFFER_SIZE];
-uint32_t rxDMAPos = 0;
+volatile uint32_t rxDMAPos = 0;
 volatile uint8_t txBuffer[UART_BUFFER_SIZE];
-uint32_t txBufferTail = 0;
-uint32_t txBufferHead = 0;
+volatile uint32_t txBufferTail = 0;
+volatile uint32_t txBufferHead = 0;
+volatile bool txDMAEmpty = false;
 
 static void uartTxDMA(void)
 {
@@ -23,7 +24,7 @@ static void uartTxDMA(void)
         DMA1_Channel4->CNDTR = UART_BUFFER_SIZE - txBufferTail;
         txBufferTail = 0;
     }
-
+    txDMAEmpty = false;
     DMA_Cmd(DMA1_Channel4, ENABLE);
 }
 
@@ -34,6 +35,8 @@ void DMA1_Channel4_IRQHandler(void)
 
     if (txBufferHead != txBufferTail)
         uartTxDMA();
+    else
+        txDMAEmpty = true;
 }
 
 void uartInit(uint32_t speed)
@@ -109,6 +112,11 @@ uint16_t uartAvailable(void)
     return (DMA_GetCurrDataCounter(DMA1_Channel5) != rxDMAPos) ? true : false;
 }
 
+bool uartTransmitDMAEmpty(void)
+{
+    return txDMAEmpty;
+}
+
 bool uartTransmitEmpty(void)
 {
     return (txBufferTail == txBufferHead);
diff --git a/src/drv_uart.h b/src/drv_uart.h
index f029b0ec2..27674740d 100755
--- a/src/drv_uart.h
+++ b/src/drv_uart.h
@@ -4,6 +4,7 @@
 void uartInit(uint32_t speed);
 uint16_t uartAvailable(void);
 bool uartTransmitEmpty(void);
+bool uartTransmitDMAEmpty(void);
 uint8_t uartRead(void);
 uint8_t uartReadPoll(void);
 void uartWrite(uint8_t ch);
diff --git a/src/main.c b/src/main.c
index f35d9db9f..6de4a93c4 100755
--- a/src/main.c
+++ b/src/main.c
@@ -18,6 +18,8 @@ static void _putc(void *p, char c)
 // 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 (!uartTransmitDMAEmpty());
     uartWrite(c);
     return c;
 }
diff --git a/src/mw.c b/src/mw.c
index ebb4cef91..e12a0c0e0 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -342,13 +342,13 @@ static void pidMultiWii(void)
 
 static void pidRewrite(void)
 {
-    int16_t errorAngle;
+    int32_t errorAngle;
     int axis;
-    int16_t delta, deltaSum;
-    static int16_t delta1[3], delta2[3];
-    int16_t PTerm, ITerm, DTerm;
-    static int16_t lastError[3] = { 0, 0, 0 };
-    int16_t AngleRateTmp, RateError;
+    int32_t delta, deltaSum;
+    static int32_t delta1[3], delta2[3];
+    int32_t PTerm, ITerm, DTerm;
+    static int32_t lastError[3] = { 0, 0, 0 };
+    int32_t AngleRateTmp, RateError;
 
     // ----------PID controller----------
     for (axis = 0; axis < 3; axis++) {
@@ -358,16 +358,16 @@ static void pidRewrite(void)
             errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]; // 16 bits is ok here
         }
         if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
-            AngleRateTmp = (((int32_t) (cfg.yawRate + 27) * rcCommand[2]) >> 5);
+            AngleRateTmp = (((int32_t)(cfg.yawRate + 27) * rcCommand[2]) >> 5);
          } else {
             if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
                 AngleRateTmp = ((int32_t) (cfg.rollPitchRate + 27) * rcCommand[axis]) >> 4;
                 if (f.HORIZON_MODE) {
                     // mix up angle error to desired AngleRateTmp to add a little auto-level feel
-                    AngleRateTmp += ((int32_t) errorAngle * cfg.I8[PIDLEVEL]) >> 8;
+                    AngleRateTmp += (errorAngle * cfg.I8[PIDLEVEL]) >> 8;
                 }
             } else { // it's the ANGLE mode - control is angle based, so control loop is needed
-                AngleRateTmp = ((int32_t) errorAngle * cfg.P8[PIDLEVEL]) >> 4;
+                AngleRateTmp = (errorAngle * cfg.P8[PIDLEVEL]) >> 4;
             }
         }
 
@@ -378,13 +378,13 @@ static void pidRewrite(void)
         RateError = AngleRateTmp - gyroData[axis];
 
         // -----calculate P component
-        PTerm = ((int32_t)RateError * cfg.P8[axis]) >> 7;
+        PTerm = (RateError * cfg.P8[axis]) >> 7;
         // -----calculate I component
         // there should be no division before accumulating the error to integrator, because the precision would be reduced.
         // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
         // Time correction (to avoid different I scaling for different builds based on average cycle time)
         // is normalized to cycle time = 2048.
-        errorGyroI[axis] = errorGyroI[axis] + (((int32_t)RateError * cycleTime) >> 11) * cfg.I8[axis];
+        errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * cfg.I8[axis];
 
         // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
         // I coefficient (I8) moved before integration to make limiting independent from PID settings
@@ -397,7 +397,7 @@ static void pidRewrite(void)
 
         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
         // would be scaled by different dt each time. Division by dT fixes that.
-        delta = ((int32_t) delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6;
+        delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6;
         // add moving average here to reduce noise
         deltaSum = delta1[axis] + delta2[axis] + delta;
         delta2[axis] = delta1[axis];