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

Smooth anti gravity

Browse source
This commit is contained in:
ctzsnooze 2018-07-09 11:35:26 +10:00 • committed by mikeller
parent 60a59334a5
commit 4c917efa50
17 changed files with 103 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -68,6 +68,12 @@ static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false;
static FAST_RAM_ZERO_INIT float dT;
static FAST_RAM_ZERO_INIT float pidFrequency;
static FAST_RAM_ZERO_INIT uint8_t antiGravityMode;
static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf;
static FAST_RAM_ZERO_INIT uint16_t itermAcceleratorGain;
static FAST_RAM float antiGravityOSDCutoff = 1.0f;
static FAST_RAM_ZERO_INIT bool antiGravityEnabled;
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
#ifdef STM32F10X
@ -96,6 +102,8 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
#define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint
#endif // USE_ACRO_TRAINER
#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 4);
void resetPidProfile(pidProfile_t *pidProfile)
@ -130,7 +138,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.dtermSetpointWeight = 60,
.yawRateAccelLimit = 100,
.rateAccelLimit = 0,
.itermThrottleThreshold = 350,
.itermThrottleThreshold = 250,
.itermAcceleratorGain = 5000,
.crash_time = 500, // ms
.crash_delay = 0, // ms
@ -158,6 +166,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.abs_control_gain = 0,
.abs_control_limit = 90,
.abs_control_error_limit = 20,
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
);
}
@ -182,9 +191,9 @@ void pidSetItermAccelerator(float newItermAccelerator)
itermAccelerator = newItermAccelerator;
}
float pidItermAccelerator(void)
bool pidOsdAntiGravityActive(void)
{
return itermAccelerator;
return (itermAccelerator > antiGravityOSDCutoff);
}
void pidStabilisationState(pidStabilisationState_e pidControllerState)
@ -222,6 +231,8 @@ static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
#endif // USE_RC_SMOOTHING_FILTER
static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
void pidInitFilters(const pidProfile_t *pidProfile)
{
BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
@ -306,6 +317,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
#endif
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
}
#ifdef USE_RC_SMOOTHING_FILTER
@ -402,6 +415,13 @@ static FAST_RAM_ZERO_INIT int acroTrainerAxisState[2]; // only need roll and pi
static FAST_RAM_ZERO_INIT float acroTrainerGain;
#endif // USE_ACRO_TRAINER
void pidUpdateAntiGravityThrottleFilter(float throttle)
{
if (antiGravityMode) {
antiGravityThrottleHpf = throttle - pt1FilterApply(&antiGravityThrottleLpf, throttle);
}
}
void pidInitConfig(const pidProfile_t *pidProfile)
{
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -426,6 +446,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
const float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
itermAcceleratorGain = pidProfile->itermAcceleratorGain;
crashTimeLimitUs = pidProfile->crash_time * 1000;
crashTimeDelayUs = pidProfile->crash_delay * 1000;
crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
@ -439,6 +460,18 @@ void pidInitConfig(const pidProfile_t *pidProfile)
throttleBoost = pidProfile->throttle_boost * 0.1f;
#endif
itermRotation = pidProfile->iterm_rotation;
antiGravityMode = pidProfile->antiGravityMode;
// Calculate the anti-gravity value that will trigger the OSD display.
// For classic AG it's either 1.0 for off and > 1.0 for on.
// For the new AG it's a continuous floating value so we want to trigger the OSD
// display when it exceeds 25% of its possible range. This gives a useful indication
// of AG activity without excessive display.
antiGravityOSDCutoff = 1.0f;
if (antiGravityMode == ANTI_GRAVITY_SMOOTH) {
antiGravityOSDCutoff += ((itermAcceleratorGain - 1000) / 1000.0f) * 0.25f;
}
#if defined(USE_SMART_FEEDFORWARD)
smartFeedforward = pidProfile->smart_feedforward;
#endif
@ -780,6 +813,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
// Dynamic i component,
// gradually scale back integration when above windup point
if ((antiGravityMode == ANTI_GRAVITY_SMOOTH) && antiGravityEnabled) {
itermAccelerator = 1 + fabsf(antiGravityThrottleHpf) * 0.01f * (itermAcceleratorGain - 1000);
DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(antiGravityThrottleHpf * 1000));
}
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000));
const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator;
// Dynamic d component, enable 2-DOF PID controller only for rate mode
@ -1028,3 +1068,17 @@ void pidSetAcroTrainerState(bool newState)
}
}
#endif // USE_ACRO_TRAINER
void pidSetAntiGravityState(bool newState)
{
if (newState != antiGravityEnabled) {
// reset the accelerator on state changes
itermAccelerator = 1.0f;
}
antiGravityEnabled = newState;
}
bool pidAntiGravityEnabled(void)
{
return antiGravityEnabled;
}