diff --git a/src/main/fc/config.c b/src/main/fc/config.c index a2837d98a..b4cad26f4 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -176,15 +176,13 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dterm_notch_cutoff = 160; pidProfile->vbatPidCompensation = 0; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; - pidProfile->max_angle_inclination = 70.0f; // 70 degrees - - // Betaflight PID controller parameters + pidProfile->levelAngleLimit = 70.0f; // 70 degrees pidProfile->setpointRelaxRatio = 30; pidProfile->dtermSetpointWeight = 200; pidProfile->yawRateAccelLimit = 20.0f; pidProfile->rateAccelLimit = 0.0f; pidProfile->itermThrottleThreshold = 350; - pidProfile->levelSensitivity = 2.0f; + pidProfile->levelSensitivity = 100.0f; } void resetProfile(profile_t *profile) diff --git a/src/main/fc/fc_main.c b/src/main/fc/fc_main.c index 5aca65f7d..e1bfee8b5 100644 --- a/src/main/fc/fc_main.c +++ b/src/main/fc/fc_main.c @@ -97,8 +97,7 @@ static float throttlePIDAttenuation; uint16_t filteredCycleTime; bool isRXDataNew; static bool armingCalibrationWasInitialised; -static float setpointRate[3]; -static float rcDeflection[3]; +static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; float getThrottlePIDAttenuation(void) { return throttlePIDAttenuation; @@ -112,6 +111,10 @@ float getRcDeflection(int axis) { return rcDeflection[axis]; } +float getRcDeflectionAbs(int axis) { + return rcDeflectionAbs[axis]; +} + void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) { accelerometerConfig()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; @@ -149,7 +152,8 @@ void calculateSetpointRate(int axis, int16_t rc) { if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); rcCommandf = rc / 500.0f; - rcDeflection[axis] = ABS(rcCommandf); + rcDeflection[axis] = rcCommandf; + rcDeflectionAbs[axis] = ABS(rcCommandf); if (rcExpo) { float expof = rcExpo / 100.0f; diff --git a/src/main/fc/fc_main.h b/src/main/fc/fc_main.h index c44300b85..1471e15a8 100644 --- a/src/main/fc/fc_main.h +++ b/src/main/fc/fc_main.h @@ -37,3 +37,4 @@ void taskMainPidLoop(timeUs_t currentTimeUs); float getThrottlePIDAttenuation(void); float getSetpointRate(int axis); float getRcDeflection(int axis); +float getRcDeflectionAbs(int axis); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3e7dcc987..4931847d9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -162,7 +162,7 @@ float calcHorizonLevelStrength(const pidProfile_t *pidProfile) { if(pidProfile->D8[PIDLEVEL] == 0){ horizonLevelStrength = 0; } else { - const float mostDeflectedPos = MAX(getRcDeflection(FD_ROLL), getRcDeflection(FD_PITCH)); + const float mostDeflectedPos = MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH)); // Progressively turn off the horizon self level strength as the stick is banged over horizonLevelStrength = (1.0f - mostDeflectedPos); // 1 at centre stick, 0 = max stick deflection horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * horizonTransition) + 1, 0, 1); @@ -172,11 +172,11 @@ float calcHorizonLevelStrength(const pidProfile_t *pidProfile) { float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) { // calculate error angle and limit the angle to the max inclination - float errorAngle = pidProfile->levelSensitivity * rcCommand[axis]; + float errorAngle = pidProfile->levelSensitivity * getRcDeflection(axis); #ifdef GPS errorAngle += GPS_angle[axis]; #endif - errorAngle = constrainf(errorAngle, -pidProfile->max_angle_inclination, pidProfile->max_angle_inclination); + errorAngle = constrainf(errorAngle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit); errorAngle = (errorAngle - ((attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f)); if(FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed @@ -252,7 +252,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an if (axis != FD_YAW) { float dynC = c[axis]; if (pidProfile->setpointRelaxRatio < 100) { - const float rcDeflection = getRcDeflection(axis); + const float rcDeflection = getRcDeflectionAbs(axis); dynC = c[axis]; if (currentPidSetpoint > 0) { if ((currentPidSetpoint - previousSetpoint[axis]) < previousSetpoint[axis]) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 171c261a7..b2e82c220 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -72,7 +72,7 @@ typedef struct pidProfile_s { uint8_t dterm_average_count; // Configurable delta count for dterm uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. - float max_angle_inclination; + float levelAngleLimit; // Betaflight PID controller parameters uint16_t itermThrottleThreshold; // max allowed throttle delta before errorGyroReset in ms diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 8e5cd7a0b..32f9a05d5 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -833,7 +833,6 @@ const clivalue_t valueTable[] = { { "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &mixerConfig()->yaw_motor_direction, .config.minmax = { -1, 1 } }, { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } }, { "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } }, - { "max_angle_inclination", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.max_angle_inclination, .config.minmax = { 10.0f, 120.0f } }, #ifdef USE_SERVOS { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, @@ -925,7 +924,8 @@ const clivalue_t valueTable[] = { { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } }, - { "level_sensitivity", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 0.1, 3.0 } }, + { "level_stick_sensitivity", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 10.0f, 200.0f } }, + { "level_angle_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelAngleLimit, .config.minmax = { 10.0f, 120.0f } }, #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &blackboxConfig()->rate_num, .config.minmax = { 1, 32 } },