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
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;
......
......@@ -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)) {
......
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