Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-22 07:45:29 +03:00
Code Issues Wiki Activity

Simplify anti gravity

Browse source
This commit is contained in:
borisbstyle 2017-02-07 10:26:21 +01:00
parent 2a77107376
commit 89527df273
4 changed files with 2 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/fc/cli.c
View file

@ -702,7 +702,6 @@ const clivalue_t valueTable[] = {
{ "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } },
{ "anti_gravity_thresh", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } }, { "anti_gravity_thresh", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } },
{ "anti_gravity_gain", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorGain, .config.minmax = {1, 30 } }, { "anti_gravity_gain", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorGain, .config.minmax = {1, 30 } },
{ "anti_gravity_rate_max", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorRateLimit, .config.minmax = {0, 2000 } },
{ "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } }, { "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } },
{ "d_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "d_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } },
{ "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } }, { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } },

1
src/main/fc/config.c
View file

@ -180,7 +180,6 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->rateAccelLimit = 0.0f; pidProfile->rateAccelLimit = 0.0f;
pidProfile->itermThrottleThreshold = 350; pidProfile->itermThrottleThreshold = 350;
pidProfile->itermAcceleratorGain = 1.0f; pidProfile->itermAcceleratorGain = 1.0f;
pidProfile->itermAcceleratorRateLimit = 80;
} }
void resetProfile(profile_t *profile) void resetProfile(profile_t *profile)

10
src/main/flight/pid.c
View file

@ -152,7 +152,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
static float Kp[3], Ki[3], Kd[3], maxVelocity[3]; static float Kp[3], Ki[3], Kd[3], maxVelocity[3];
static float relaxFactor; static float relaxFactor;
static float dtermSetpointWeight; static float dtermSetpointWeight;
static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv, itermAcceleratorRateLimit; static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv;
void pidInitConfig(const pidProfile_t *pidProfile) { void pidInitConfig(const pidProfile_t *pidProfile) {
for(int axis = FD_ROLL; axis <= FD_YAW; axis++) { for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -169,7 +169,6 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
itermAcceleratorRateLimit = (float)pidProfile->itermAcceleratorRateLimit;
} }
static float calcHorizonLevelStrength(void) { static float calcHorizonLevelStrength(void) {
@ -255,12 +254,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
float ITerm = previousGyroIf[axis]; float ITerm = previousGyroIf[axis];
if (motorMixRange < 1.0f) { if (motorMixRange < 1.0f) {
// Only increase ITerm if motor output is not saturated // Only increase ITerm if motor output is not saturated
float ITermDelta = Ki[axis] * errorRate * dT * dynKi; ITerm += Ki[axis] * errorRate * dT * dynKi * itermAccelerator;
if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit) {
// ITerm will only be accelerated below steady rate threshold
ITermDelta *= itermAccelerator;
}
ITerm += ITermDelta;
previousGyroIf[axis] = ITerm; previousGyroIf[axis] = ITerm;
} }

1
src/main/flight/pid.h
View file

@ -79,7 +79,6 @@ typedef struct pidProfile_s {
// Betaflight PID controller parameters // Betaflight PID controller parameters
uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms
float itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit float itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit
uint16_t itermAcceleratorRateLimit; // Setpointrate limit for iterm accelerator to operate within
uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect
uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
float yawRateAccelLimit; // yaw accel limiter for deg/sec/ms float yawRateAccelLimit; // yaw accel limiter for deg/sec/ms