Commit e456feb5 authored by Paweł Spychalski's avatar Paweł Spychalski Committed by Konstantin Sharlaimov
Browse files

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
parent 1cafd0d9
...@@ -162,14 +162,14 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynam ...@@ -162,14 +162,14 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynam
void resetPidProfile(pidProfile_t *pidProfile) void resetPidProfile(pidProfile_t *pidProfile)
{ {
pidProfile->P8[ROLL] = 30; pidProfile->P8[ROLL] = 40;
pidProfile->I8[ROLL] = 20; pidProfile->I8[ROLL] = 30;
pidProfile->D8[ROLL] = 70; pidProfile->D8[ROLL] = 23;
pidProfile->P8[PITCH] = 30; pidProfile->P8[PITCH] = 40;
pidProfile->I8[PITCH] = 20; pidProfile->I8[PITCH] = 30;
pidProfile->D8[PITCH] = 70; pidProfile->D8[PITCH] = 23;
pidProfile->P8[YAW] = 100; // 2.5 * 40 pidProfile->P8[YAW] = 85;
pidProfile->I8[YAW] = 40; // 4.0 * 10 pidProfile->I8[YAW] = 45;
pidProfile->D8[YAW] = 0; // not used pidProfile->D8[YAW] = 0; // not used
pidProfile->P8[PIDALT] = 50; // NAV_POS_Z_P * 100 pidProfile->P8[PIDALT] = 50; // NAV_POS_Z_P * 100
pidProfile->I8[PIDALT] = 0; // not used pidProfile->I8[PIDALT] = 0; // not used
...@@ -183,7 +183,7 @@ void resetPidProfile(pidProfile_t *pidProfile) ...@@ -183,7 +183,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->P8[PIDNAVR] = 10; // FW_NAV_P * 100 pidProfile->P8[PIDNAVR] = 10; // FW_NAV_P * 100
pidProfile->I8[PIDNAVR] = 5; // FW_NAV_I * 100 pidProfile->I8[PIDNAVR] = 5; // FW_NAV_I * 100
pidProfile->D8[PIDNAVR] = 8; // FW_NAV_D * 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->I8[PIDLEVEL] = 15; // Self-leveing low-pass frequency (0 - disabled)
pidProfile->D8[PIDLEVEL] = 75; // 75% horizon strength pidProfile->D8[PIDLEVEL] = 75; // 75% horizon strength
pidProfile->P8[PIDMAG] = 60; pidProfile->P8[PIDMAG] = 60;
......
...@@ -125,10 +125,13 @@ float pidRcCommandToRate(int16_t stick, uint8_t rate) ...@@ -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; 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 FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
#define FP_PID_RATE_D_MULTIPLIER 4000.0f // betaflight - 1000.0 */
#define FP_PID_LEVEL_P_MULTIPLIER 40.0f // betaflight - 10.0 #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 FP_PID_YAWHOLD_P_MULTIPLIER 80.0f
#define KD_ATTENUATION_BREAK 0.25f #define KD_ATTENUATION_BREAK 0.25f
...@@ -214,7 +217,7 @@ static void pidLevel(const pidProfile_t *pidProfile, pidState_t *pidState, fligh ...@@ -214,7 +217,7 @@ static void pidLevel(const pidProfile_t *pidProfile, pidState_t *pidState, fligh
{ {
// This is ROLL/PITCH, run ANGLE/HORIZON controllers // This is ROLL/PITCH, run ANGLE/HORIZON controllers
const float angleTarget = pidRcCommandToAngle(rcCommand[axis]); 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) // P[LEVEL] defines self-leveling strength (both for ANGLE and HORIZON modes)
if (FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(HORIZON_MODE)) {
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment