FP-PID rescaled to match LuxFloat and MWRewrite (#304)

* FP-PID rescaled to match LuxFloat and MWRewrite

* Change scaling of FP-PID LEVEL P to match LuxFloat/MwRewrite

src/main/config/config.c

@@ -162,14 +162,14 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynam
 
 void resetPidProfile(pidProfile_t *pidProfile)
 {
-    pidProfile->P8[ROLL] = 30;
-    pidProfile->I8[ROLL] = 20;
-    pidProfile->D8[ROLL] = 70;
-    pidProfile->P8[PITCH] = 30;
-    pidProfile->I8[PITCH] = 20;
-    pidProfile->D8[PITCH] = 70;
-    pidProfile->P8[YAW] = 100;      // 2.5 * 40
-    pidProfile->I8[YAW] = 40;       // 4.0 * 10
+    pidProfile->P8[ROLL] = 40;
+    pidProfile->I8[ROLL] = 30;
+    pidProfile->D8[ROLL] = 23;
+    pidProfile->P8[PITCH] = 40;
+    pidProfile->I8[PITCH] = 30;
+    pidProfile->D8[PITCH] = 23;
+    pidProfile->P8[YAW] = 85;
+    pidProfile->I8[YAW] = 45;
     pidProfile->D8[YAW] = 0;        // not used
     pidProfile->P8[PIDALT] = 50;    // NAV_POS_Z_P * 100
     pidProfile->I8[PIDALT] = 0;     // not used
@@ -183,7 +183,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->P8[PIDNAVR] = 10;   // FW_NAV_P * 100
     pidProfile->I8[PIDNAVR] = 5;    // FW_NAV_I * 100
     pidProfile->D8[PIDNAVR] = 8;    // FW_NAV_D * 100
-    pidProfile->P8[PIDLEVEL] = 120; // Self-level strength * 40 (4 * 40)
+    pidProfile->P8[PIDLEVEL] = 20;  // Self-level strength
     pidProfile->I8[PIDLEVEL] = 15;  // Self-leveing low-pass frequency (0 - disabled)
     pidProfile->D8[PIDLEVEL] = 75;  // 75% horizon strength
     pidProfile->P8[PIDMAG] = 60;

src/main/flight/pid.c

@@ -125,10 +125,13 @@ float pidRcCommandToRate(int16_t stick, uint8_t rate)
     return scaleRangef((float) stick, (float) -500, (float) 500, (float) -rate, (float) rate) * 10;
 }
 
-#define FP_PID_RATE_P_MULTIPLIER    40.0f       // betaflight - 40.0
-#define FP_PID_RATE_I_MULTIPLIER    10.0f       // betaflight - 10.0
-#define FP_PID_RATE_D_MULTIPLIER    4000.0f     // betaflight - 1000.0
-#define FP_PID_LEVEL_P_MULTIPLIER   40.0f       // betaflight - 10.0
+/*
+FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
+*/
+#define FP_PID_RATE_P_MULTIPLIER    31.0f
+#define FP_PID_RATE_I_MULTIPLIER    4.0f
+#define FP_PID_RATE_D_MULTIPLIER    1905.0f
+#define FP_PID_LEVEL_P_MULTIPLIER   65.6f
 #define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f
 
 #define KD_ATTENUATION_BREAK        0.25f
@@ -214,7 +217,7 @@ static void pidLevel(const pidProfile_t *pidProfile, pidState_t *pidState, fligh
 {
     // This is ROLL/PITCH, run ANGLE/HORIZON controllers
     const float angleTarget = pidRcCommandToAngle(rcCommand[axis]);
-    const float angleError = (constrain(angleTarget, -pidProfile->max_angle_inclination[axis], +pidProfile->max_angle_inclination[axis]) - attitude.raw[axis]) / 10.0f;
+    const float angleError = constrain(angleTarget, -pidProfile->max_angle_inclination[axis], +pidProfile->max_angle_inclination[axis]) - attitude.raw[axis];
 
     // P[LEVEL] defines self-leveling strength (both for ANGLE and HORIZON modes)
     if (FLIGHT_MODE(HORIZON_MODE)) {