diff --git a/src/common/maths.c b/src/common/maths.c
index f04d2bbf0d..85b4cdcfcf 100644
--- a/src/common/maths.c
+++ b/src/common/maths.c
@@ -13,6 +13,16 @@ int constrain(int amt, int low, int high)
         return amt;
 }
 
+float constrainf(float amt, float low, float high)
+{
+    if (amt < low)
+        return low;
+    else if (amt > high)
+        return high;
+    else
+        return amt;
+}
+
 void devClear(stdev_t *dev)
 {
     dev->m_n = 0;
diff --git a/src/common/maths.h b/src/common/maths.h
index c7c6f63017..8caf22be3d 100644
--- a/src/common/maths.h
+++ b/src/common/maths.h
@@ -23,6 +23,7 @@ typedef struct stdev_t
 } stdev_t;
 
 int constrain(int amt, int low, int high);
+float constrainf(float amt, float low, float high);
 
 void devClear(stdev_t *dev);
 void devPush(stdev_t *dev, float x);
diff --git a/src/config.c b/src/config.c
index 9a27bc66bd..dede86b2ea 100755
--- a/src/config.c
+++ b/src/config.c
@@ -43,7 +43,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon
         escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse,
         airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfig, gimbalConfig_t *gimbalConfigToUse);
 
-#define FLASH_TO_RESERVE_FOR_CONFIG 0x400
+#define FLASH_TO_RESERVE_FOR_CONFIG 0x800
 
 #ifdef STM32F303xC
 #define FLASH_PAGE_COUNT 128
@@ -97,6 +97,18 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->P8[PIDVEL] = 120;
     pidProfile->I8[PIDVEL] = 45;
     pidProfile->D8[PIDVEL] = 1;
+
+    pidProfile->P_f[ROLL] = 2.5f;     // new PID with preliminary defaults test carefully
+    pidProfile->I_f[ROLL] = 0.3f;
+    pidProfile->D_f[ROLL] = 0.06f;
+    pidProfile->P_f[PITCH] = 2.5f;
+    pidProfile->I_f[PITCH] = 0.3f;
+    pidProfile->D_f[PITCH] = 0.06f;
+    pidProfile->P_f[YAW] = 8.0f;
+    pidProfile->I_f[YAW] = 0.5f;
+    pidProfile->D_f[YAW] = 0.05f;
+    pidProfile->A_level = 5.0f;
+    pidProfile->H_level = 3.0f;
 }
 
 void resetGpsProfile(gpsProfile_t *gpsProfile)
@@ -217,6 +229,7 @@ static void resetConf(void)
     resetFlight3DConfig(&masterConfig.flight3DConfig);
     masterConfig.motor_pwm_rate = 400;
     masterConfig.servo_pwm_rate = 50;
+
     // gps/nav stuff
     masterConfig.gps_provider = GPS_NMEA;
 
diff --git a/src/config_profile.h b/src/config_profile.h
index d5da111685..d47dfb2a7c 100644
--- a/src/config_profile.h
+++ b/src/config_profile.h
@@ -1,7 +1,7 @@
 #pragma once
 
 typedef struct profile_s {
-    uint8_t pidController;                  // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671
+    uint8_t pidController;                  // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid
 
     pidProfile_t pidProfile;
 
diff --git a/src/drivers/accgyro_l3g4200d.c b/src/drivers/accgyro_l3g4200d.c
index c585b9b1cc..5569c3fc9b 100644
--- a/src/drivers/accgyro_l3g4200d.c
+++ b/src/drivers/accgyro_l3g4200d.c
@@ -55,7 +55,7 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
     gyro->read = l3g4200dRead;
 
     // 14.2857dps/lsb scalefactor
-    gyro->scale = (((32767.0f / 14.2857f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
+    gyro->scale = 1.0f / 14.2857f;
 
     // default LPF is set to 32Hz
     switch (lpf) {
@@ -97,7 +97,8 @@ static void l3g4200dRead(int16_t *gyroData)
     uint8_t buf[6];
 
     i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf);
-    gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
-    gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
-    gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
+
+    gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
+    gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
+    gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
 }
diff --git a/src/drivers/accgyro_l3gd20.c b/src/drivers/accgyro_l3gd20.c
index 865dd9a66a..ee9b2831a7 100644
--- a/src/drivers/accgyro_l3gd20.c
+++ b/src/drivers/accgyro_l3gd20.c
@@ -216,9 +216,9 @@ static void l3gd20GyroRead(int16_t *gyroData)
 
     GPIO_SetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN);
 
-    gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 8;
-    gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 8;
-    gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 8;
+    gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
+    gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
+    gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
 
 #if 0
     debug[0] = (int16_t)((buf[1] << 8) | buf[0]);
@@ -227,14 +227,18 @@ static void l3gd20GyroRead(int16_t *gyroData)
 #endif
 }
 
+#define L3GD20_GYRO_SCALE_FACTOR  0.00030543f  // (17.5e-3) * pi/180  (17.5 mdps/bit)
+
 bool l3gd20Detect(gyro_t *gyro, uint16_t lpf)
 {
     gyro->init = l3gd20GyroInit;
     gyro->read = l3gd20GyroRead;
 
     // 16.4 dps/lsb scalefactor
-    gyro->scale = L3GD20_GYRO_SCALE_FACTOR;
-    gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
+    //gyro->scale = L3GD20_GYRO_SCALE_FACTOR;
+
+    // 14.2857dps/lsb scalefactor
+    gyro->scale = 1.0f / 14.2857f;
 
     return true;  // blindly assume it's present, for now.
 }
diff --git a/src/drivers/accgyro_l3gd20.h b/src/drivers/accgyro_l3gd20.h
index 8a1eb2e3c7..bd8c953bc5 100644
--- a/src/drivers/accgyro_l3gd20.h
+++ b/src/drivers/accgyro_l3gd20.h
@@ -21,6 +21,4 @@
 #define L3GD20_CS_PIN       GPIO_Pin_3
 #define L3GD20_CS_GPIO_CLK  RCC_AHBPeriph_GPIOE
 
-#define L3GD20_GYRO_SCALE_FACTOR  0.00030543f  // (17.5e-3) * pi/180  (17.5 mdps/bit)
-
 bool l3gd20Detect(gyro_t *gyro, uint16_t lpf);
diff --git a/src/drivers/accgyro_mpu3050.c b/src/drivers/accgyro_mpu3050.c
index ecbdf5d229..2a151017c3 100755
--- a/src/drivers/accgyro_mpu3050.c
+++ b/src/drivers/accgyro_mpu3050.c
@@ -56,8 +56,9 @@ bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
     gyro->init = mpu3050Init;
     gyro->read = mpu3050Read;
     gyro->temperature = mpu3050ReadTemp;
+
     // 16.4 dps/lsb scalefactor
-    gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
+    gyro->scale = 1.0f / 16.4f;
 
     // default lpf is 42Hz
     switch (lpf) {
@@ -107,9 +108,10 @@ static void mpu3050Read(int16_t *gyroData)
     uint8_t buf[6];
 
     i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
-    gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
-    gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
-    gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
+
+    gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
+    gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
+    gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
 }
 
 static void mpu3050ReadTemp(int16_t *tempData)
diff --git a/src/drivers/accgyro_mpu6050.c b/src/drivers/accgyro_mpu6050.c
index 6ac7563f76..a4292ce1a5 100644
--- a/src/drivers/accgyro_mpu6050.c
+++ b/src/drivers/accgyro_mpu6050.c
@@ -194,7 +194,7 @@ bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf)
     gyro->read = mpu6050GyroRead;
 
     // 16.4 dps/lsb scalefactor
-    gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
+    gyro->scale = 1.0f / 16.4f;
 
     // default lpf is 42Hz
     switch (lpf) {
@@ -279,7 +279,8 @@ static void mpu6050GyroRead(int16_t *gyroData)
     uint8_t buf[6];
 
     i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
-    gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
-    gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
-    gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
+
+    gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
+    gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
+    gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
 }
diff --git a/src/flight_common.c b/src/flight_common.c
index 408cba1a4c..8fc3618819 100644
--- a/src/flight_common.c
+++ b/src/flight_common.c
@@ -7,6 +7,9 @@
 
 #include "runtime_config.h"
 
+#include "drivers/accgyro_common.h"
+#include "sensors_common.h"
+#include "sensors_gyro.h"
 #include "rc_controls.h"
 #include "flight_common.h"
 #include "gps_common.h"
@@ -18,6 +21,7 @@ int16_t axisPID[3];
 uint8_t dynP8[3], dynI8[3], dynD8[3];
 
 static int32_t errorGyroI[3] = { 0, 0, 0 };
+static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
 static int32_t errorAngleI[2] = { 0, 0 };
 
 static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
@@ -45,8 +49,80 @@ void resetErrorGyro(void)
     errorGyroI[ROLL] = 0;
     errorGyroI[PITCH] = 0;
     errorGyroI[YAW] = 0;
+
+    errorGyroIf[ROLL] = 0;
+    errorGyroIf[PITCH] = 0;
+    errorGyroIf[YAW] = 0;
 }
 
+
+static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
+        uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
+{
+    float RateError, errorAngle, AngleRate, gyroRate;
+    float ITerm,PTerm,DTerm;
+    static float lastGyroRate[3];
+    static float delta1[3], delta2[3];
+    float delta, deltaSum;
+    float dT;
+    int axis;
+
+    dT = (float)cycleTime * 0.000001f;
+
+    // ----------PID controller----------
+    for (axis = 0; axis < 3; axis++) {
+        // -----Get the desired angle rate depending on flight mode
+        if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
+            AngleRate = (float)((controlRateConfig->yawRate + 10) * rcCommand[YAW]) / 50.0f;
+         } else {
+            // calculate error and limit the angle to 50 degrees max inclination
+            errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -500, +500) - inclination.rawAngles[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
+            if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
+                AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate
+                if (f.HORIZON_MODE) {
+                    // mix up angle error to desired AngleRateTmp to add a little auto-level feel
+                    AngleRate += errorAngle * pidProfile->H_level;
+                }
+            } else { // it's the ANGLE mode - control is angle based, so control loop is needed
+                AngleRate = errorAngle * pidProfile->A_level;
+            }
+        }
+
+        gyroRate = gyroData[axis] * gyro.scale; // gyro output scaled to dps
+
+        // --------low-level gyro-based PID. ----------
+        // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
+        // -----calculate scaled error.AngleRates
+        // multiplication of rcCommand corresponds to changing the sticks scaling here
+        RateError = AngleRate - gyroRate;
+
+        // -----calculate P component
+        PTerm = RateError * pidProfile->P_f[axis];
+        // -----calculate I component
+        errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis], -250.0f, 250.0f);
+
+        // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
+        // I coefficient (I8) moved before integration to make limiting independent from PID settings
+        ITerm = errorGyroIf[axis];
+
+        //-----calculate D-term
+        delta = gyroRate - lastGyroRate[axis];  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
+        lastGyroRate[axis] = gyroRate;
+
+        // 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 *= (1.0f / dT);
+        // add moving average here to reduce noise
+        deltaSum = delta1[axis] + delta2[axis] + delta;
+        delta2[axis] = delta1[axis];
+        delta1[axis] = delta;
+        DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis], -300.0f, 300.0f);
+
+        // -----calculate total PID output
+        axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000);
+    }
+
+}
 static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
 {
@@ -73,14 +149,14 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         }
         if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
             error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
-            error -= gyroData[axis];
+            error -= gyroData[axis] / 4;
 
             PTermGYRO = rcCommand[axis];
 
             errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
-            if (abs(gyroData[axis]) > 640)
+            if (abs(gyroData[axis]) > (640 * 4))
                 errorGyroI[axis] = 0;
-            ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) >> 6;
+            ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64;
         }
         if (f.HORIZON_MODE && (axis == FD_ROLL || axis == FD_PITCH)) {
             PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
@@ -95,8 +171,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
             }
         }
 
-        PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
-        delta = gyroData[axis] - lastGyro[axis];
+        PTerm -= ((int32_t)gyroData[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
+        delta = (gyroData[axis] - lastGyro[axis]) / 4;
         lastGyro[axis] = gyroData[axis];
         deltaSum = delta1[axis] + delta2[axis] + delta;
         delta2[axis] = delta1[axis];
@@ -144,7 +220,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
         // -----calculate scaled error.AngleRates
         // multiplication of rcCommand corresponds to changing the sticks scaling here
-        RateError = AngleRateTmp - gyroData[axis];
+        RateError = AngleRateTmp - (gyroData[axis] / 4);
 
         // -----calculate P component
         PTerm = (RateError * pidProfile->P8[axis]) >> 7;
@@ -188,6 +264,8 @@ void setPIDController(int type)
         case 1:
             pid_controller = pidRewrite;
             break;
+        case 2:
+            pid_controller = pidBaseflight;
     }
 }
 
diff --git a/src/flight_common.h b/src/flight_common.h
index 61e481b180..31b2576cd0 100644
--- a/src/flight_common.h
+++ b/src/flight_common.h
@@ -19,6 +19,12 @@ typedef struct pidProfile_s {
     uint8_t P8[PID_ITEM_COUNT];
     uint8_t I8[PID_ITEM_COUNT];
     uint8_t D8[PID_ITEM_COUNT];
+
+    float P_f[3];                           // float p i and d factors for the new baseflight pid
+    float I_f[3];
+    float D_f[3];
+    float A_level;
+    float H_level;
 } pidProfile_t;
 
 enum {
diff --git a/src/flight_imu.c b/src/flight_imu.c
old mode 100644
new mode 100755
index 5d3aa00714..bc3422bb16
--- a/src/flight_imu.c
+++ b/src/flight_imu.c
@@ -62,7 +62,7 @@ float throttleAngleScale;
 int32_t BaroPID = 0;
 
 float magneticDeclination = 0.0f;       // calculated at startup from config
-
+float gyroScaleRad;
 
 // **************
 // gyro+acc IMU
@@ -80,6 +80,7 @@ void imuInit(void)
     smallAngle = lrintf(acc_1G * cosf(RAD * 25.0f));
     accVelScale = 9.80665f / acc_1G / 10000.0f;
     throttleAngleScale = (1800.0f / M_PI) * (900.0f / currentProfile.throttle_correction_angle);
+    gyroScaleRad = gyro.scale * (M_PI / 180.0f) * 0.000001f;
 
 #ifdef MAG
     // if mag sensor is enabled, use it
@@ -278,7 +279,7 @@ static void getEstimatedAttitude(void)
     float scale;
     fp_angles_t deltaGyroAngle;
     deltaT = currentT - previousT;
-    scale = deltaT * gyro.scale;
+    scale = deltaT * gyroScaleRad;
     previousT = currentT;
 
     // Initialization
diff --git a/src/mw.c b/src/mw.c
index 620951ef6d..0d5f359a8a 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -275,7 +275,7 @@ static void mwVario(void)
 {
 
 }
-
+
 void loop(void)
 {
     static uint8_t rcDelayCommand;      // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
diff --git a/src/serial_cli.c b/src/serial_cli.c
index 5126994a98..9ae0c72b6a 100644
--- a/src/serial_cli.c
+++ b/src/serial_cli.c
@@ -270,7 +270,8 @@ const clivalue_t valueTable[] = {
     { "nav_speed_max", VAR_UINT16, &currentProfile.gpsProfile.nav_speed_max, 10, 2000 },
     { "nav_slew_rate", VAR_UINT8, &currentProfile.gpsProfile.nav_slew_rate, 0, 100 },
 
-    { "pid_controller", VAR_UINT8, &currentProfile.pidController, 0, 1 },
+    { "pid_controller", VAR_UINT8, &currentProfile.pidController, 0, 2 },
+
     { "p_pitch", VAR_UINT8, &currentProfile.pidProfile.P8[PITCH], 0, 200 },
     { "i_pitch", VAR_UINT8, &currentProfile.pidProfile.I8[PITCH], 0, 200 },
     { "d_pitch", VAR_UINT8, &currentProfile.pidProfile.D8[PITCH], 0, 200 },
@@ -280,12 +281,28 @@ const clivalue_t valueTable[] = {
     { "p_yaw", VAR_UINT8, &currentProfile.pidProfile.P8[YAW], 0, 200 },
     { "i_yaw", VAR_UINT8, &currentProfile.pidProfile.I8[YAW], 0, 200 },
     { "d_yaw", VAR_UINT8, &currentProfile.pidProfile.D8[YAW], 0, 200 },
+
+    { "Ppitchf", VAR_FLOAT, &currentProfile.pidProfile.P_f[PITCH], 0, 100 },
+    { "Ipitchf", VAR_FLOAT, &currentProfile.pidProfile.I_f[PITCH], 0, 100 },
+    { "Dpitchf", VAR_FLOAT, &currentProfile.pidProfile.D_f[PITCH], 0, 100 },
+    { "Prollf", VAR_FLOAT, &currentProfile.pidProfile.P_f[ROLL], 0, 100 },
+    { "Irollf", VAR_FLOAT, &currentProfile.pidProfile.I_f[ROLL], 0, 100 },
+    { "Drollf", VAR_FLOAT, &currentProfile.pidProfile.D_f[ROLL], 0, 100 },
+    { "Pyawf", VAR_FLOAT, &currentProfile.pidProfile.P_f[YAW], 0, 100 },
+    { "Iyawf", VAR_FLOAT, &currentProfile.pidProfile.I_f[YAW], 0, 100 },
+    { "Dyawf", VAR_FLOAT, &currentProfile.pidProfile.D_f[YAW], 0, 100 },
+
+    { "level_horizon", VAR_FLOAT, &currentProfile.pidProfile.H_level, 0, 10 },
+    { "level_angle", VAR_FLOAT, &currentProfile.pidProfile.A_level, 0, 10 },
+
     { "p_alt", VAR_UINT8, &currentProfile.pidProfile.P8[PIDALT], 0, 200 },
     { "i_alt", VAR_UINT8, &currentProfile.pidProfile.I8[PIDALT], 0, 200 },
     { "d_alt", VAR_UINT8, &currentProfile.pidProfile.D8[PIDALT], 0, 200 },
+
     { "p_level", VAR_UINT8, &currentProfile.pidProfile.P8[PIDLEVEL], 0, 200 },
     { "i_level", VAR_UINT8, &currentProfile.pidProfile.I8[PIDLEVEL], 0, 200 },
     { "d_level", VAR_UINT8, &currentProfile.pidProfile.D8[PIDLEVEL], 0, 200 },
+
     { "p_vel", VAR_UINT8, &currentProfile.pidProfile.P8[PIDVEL], 0, 200 },
     { "i_vel", VAR_UINT8, &currentProfile.pidProfile.I8[PIDVEL], 0, 200 },
     { "d_vel", VAR_UINT8, &currentProfile.pidProfile.D8[PIDVEL], 0, 200 },
diff --git a/src/serial_msp.c b/src/serial_msp.c
index 115e30cc8e..847bab7ab9 100755
--- a/src/serial_msp.c
+++ b/src/serial_msp.c
@@ -1,11 +1,13 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
+#include <math.h>
 
 #include "build_config.h"
 
 #include "platform.h"
 
+#include "common/maths.h"
 #include "common/axis.h"
 
 #include "drivers/system_common.h"
@@ -326,34 +328,45 @@ void mspInit(serialConfig_t *serialConfig)
 
     idx = 0;
     availableBoxes[idx++] = BOXARM;
+
     if (sensors(SENSOR_ACC)) {
         availableBoxes[idx++] = BOXANGLE;
         availableBoxes[idx++] = BOXHORIZON;
     }
+
     if (sensors(SENSOR_BARO)) {
         availableBoxes[idx++] = BOXBARO;
         if (feature(FEATURE_VARIO))
             availableBoxes[idx++] = BOXVARIO;
     }
+
     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
         availableBoxes[idx++] = BOXMAG;
         availableBoxes[idx++] = BOXHEADFREE;
         availableBoxes[idx++] = BOXHEADADJ;
     }
+
     if (feature(FEATURE_SERVO_TILT))
         availableBoxes[idx++] = BOXCAMSTAB;
+
     if (feature(FEATURE_GPS)) {
         availableBoxes[idx++] = BOXGPSHOME;
         availableBoxes[idx++] = BOXGPSHOLD;
     }
+
     if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE)
         availableBoxes[idx++] = BOXPASSTHRU;
+
     availableBoxes[idx++] = BOXBEEPERON;
+
     if (feature(FEATURE_INFLIGHT_ACC_CAL))
         availableBoxes[idx++] = BOXCALIB;
+
     availableBoxes[idx++] = BOXOSD;
+
     if (feature(FEATURE_TELEMETRY && masterConfig.telemetryConfig.telemetry_switch))
         availableBoxes[idx++] = BOXTELEMETRY;
+
     numberBoxItems = idx;
 
     openAllMSPSerialPorts(serialConfig);
@@ -389,10 +402,29 @@ static void evaluateCommand(void)
         headSerialReply(0);
         break;
     case MSP_SET_PID:
-        for (i = 0; i < PID_ITEM_COUNT; i++) {
-            currentProfile.pidProfile.P8[i] = read8();
-            currentProfile.pidProfile.I8[i] = read8();
-            currentProfile.pidProfile.D8[i] = read8();
+        if (currentProfile.pidController == 2) {
+            for (i = 0; i < 3; i++) {
+                currentProfile.pidProfile.P_f[i] = (float)read8() / 10.0f;
+                currentProfile.pidProfile.I_f[i] = (float)read8() / 100.0f;
+                currentProfile.pidProfile.D_f[i] = (float)read8() / 1000.0f;
+            }
+            for (i = 3; i < PID_ITEM_COUNT; i++) {
+                if (i == PIDLEVEL) {
+                    currentProfile.pidProfile.A_level = (float)read8() / 10.0f;
+                    currentProfile.pidProfile.H_level = (float)read8() / 10.0f;
+                    read8();
+                } else {
+                    currentProfile.pidProfile.P8[i] = read8();
+                    currentProfile.pidProfile.I8[i] = read8();
+                    currentProfile.pidProfile.D8[i] = read8();
+                }
+            }
+        } else {
+            for (i = 0; i < PID_ITEM_COUNT; i++) {
+                currentProfile.pidProfile.P8[i] = read8();
+                currentProfile.pidProfile.I8[i] = read8();
+                currentProfile.pidProfile.D8[i] = read8();
+            }
         }
         headSerialReply(0);
         break;
@@ -599,10 +631,29 @@ static void evaluateCommand(void)
         break;
     case MSP_PID:
         headSerialReply(3 * PID_ITEM_COUNT);
-        for (i = 0; i < PID_ITEM_COUNT; i++) {
-            serialize8(currentProfile.pidProfile.P8[i]);
-            serialize8(currentProfile.pidProfile.I8[i]);
-            serialize8(currentProfile.pidProfile.D8[i]);
+        if (currentProfile.pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
+            for (i = 0; i < 3; i++) {
+                serialize8(constrain(lrintf(currentProfile.pidProfile.P_f[i] * 10.0f), 0, 250));
+                serialize8(constrain(lrintf(currentProfile.pidProfile.I_f[i] * 100.0f), 0, 250));
+                serialize8(constrain(lrintf(currentProfile.pidProfile.D_f[i] * 1000.0f), 0, 100));
+            }
+            for (i = 3; i < PID_ITEM_COUNT; i++) {
+                if (i == PIDLEVEL) {
+                    serialize8(constrain(lrintf(currentProfile.pidProfile.A_level * 10.0f), 0, 250));
+                    serialize8(constrain(lrintf(currentProfile.pidProfile.H_level * 10.0f), 0, 250));
+                    serialize8(0);
+                } else {
+                    serialize8(currentProfile.pidProfile.P8[i]);
+                    serialize8(currentProfile.pidProfile.I8[i]);
+                    serialize8(currentProfile.pidProfile.D8[i]);
+                }
+            }
+        } else {
+            for (i = 0; i < PID_ITEM_COUNT; i++) {
+                serialize8(currentProfile.pidProfile.P8[i]);
+                serialize8(currentProfile.pidProfile.I8[i]);
+                serialize8(currentProfile.pidProfile.D8[i]);
+            }
         }
         break;
     case MSP_PIDNAMES: