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

ff from interpolated setpoint

Browse source
This commit is contained in:
Thorsten Laux 2019-02-22 08:23:45 +01:00
parent 3ba5f7e819
commit 91ad2498ff
13 changed files with 319 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -49,6 +49,7 @@
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/rpm_filter.h"
#include "flight/interpolated_setpoint.h"
#include "io/gps.h"
@ -211,6 +212,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
.idle_p = 50,
.idle_pid_limit = 200,
.idle_max_increase = 150,
.ff_interpolate_sp = 0,
.ff_spread = 0,
.ff_max_rate_limit = 0,
.ff_lookahead_limit = 0,
.ff_boost = 15,
);
#ifndef USE_D_MIN
pidProfile->pid[PID_ROLL].D = 30;
@ -285,6 +291,7 @@ static FAST_RAM_ZERO_INIT float acLimit;
static FAST_RAM_ZERO_INIT float acErrorLimit;
static FAST_RAM_ZERO_INIT float acCutoff;
static FAST_RAM_ZERO_INIT pt1Filter_t acLpf[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float oldSetpointCorrection[XYZ_AXIS_COUNT];
#endif
#if defined(USE_D_MIN)
@ -307,6 +314,14 @@ static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2;
static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
static FAST_RAM_ZERO_INIT float ffBoostFactor;
float pidGetFfBoostFactor()
{
return ffBoostFactor;
}
void pidInitFilters(const pidProfile_t *pidProfile)
{
STATIC_ASSERT(FD_YAW == 2, FD_YAW_incorrect); // ensure yaw axis is 2
@ -443,6 +458,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
#endif
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
ffBoostFactor = (float)pidProfile->ff_boost / 10.0f;
}
#ifdef USE_RC_SMOOTHING_FILTER
@ -565,6 +582,10 @@ static FAST_RAM_ZERO_INIT float dMinGyroGain;
static FAST_RAM_ZERO_INIT float dMinSetpointGain;
#endif
#ifdef USE_INTERPOLATED_SP
static FAST_RAM_ZERO_INIT bool ffFromInterpolatedSetpoint;
#endif
void pidInitConfig(const pidProfile_t *pidProfile)
{
if (pidProfile->feedForwardTransition == 0) {
@ -708,6 +729,10 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#if defined(USE_AIRMODE_LPF)
airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
#endif
#ifdef USE_INTERPOLATED_SP
ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp;
interpolatedSpInit(pidProfile);
#endif
}
void pidInit(const pidProfile_t *pidProfile)
@ -1212,6 +1237,9 @@ static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
{
static float previousGyroRateDterm[XYZ_AXIS_COUNT];
#ifdef USE_INTERPOLATED_SP
static FAST_RAM_ZERO_INIT uint32_t lastFrameNumber;
#endif
#if defined(USE_ACC)
static timeUs_t levelModeStartTimeUs = 0;
@ -1286,6 +1314,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
rpmFilterUpdate();
#endif
#ifdef USE_INTERPOLATED_SP
bool newRcFrame = false;
if (lastFrameNumber != getRcFrameNumber()) {
lastFrameNumber = getRcFrameNumber();
newRcFrame = true;
}
#endif
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
@ -1336,6 +1371,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
const float previousIterm = pidData[axis].I;
float itermErrorRate = errorRate;
float uncorrectedSetpoint = currentPidSetpoint;
#if defined(USE_ITERM_RELAX)
if (!launchControlActive && !inCrashRecoveryMode) {
@ -1343,6 +1379,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
errorRate = currentPidSetpoint - gyroRate;
}
#endif
#ifdef USE_ABSOLUTE_CONTROL
float setpointCorrection = currentPidSetpoint - uncorrectedSetpoint;
#endif
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
// 2-DOF PID controller with optional filter on derivative term.
@ -1365,9 +1404,18 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// -----calculate pidSetpointDelta
float pidSetpointDelta = 0;
#ifdef USE_INTERPOLATED_SP
if (ffFromInterpolatedSetpoint) {
pidSetpointDelta = interpolatedSpApply(axis, pidFrequency, newRcFrame);
} else {
pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
}
#else
pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
#endif
previousPidSetpoint[axis] = currentPidSetpoint;
#ifdef USE_RC_SMOOTHING_FILTER
pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
#endif // USE_RC_SMOOTHING_FILTER
@ -1416,12 +1464,25 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
previousGyroRateDterm[axis] = gyroRateDterm[axis];
// -----calculate feedforward component
#ifdef USE_ABSOLUTE_CONTROL
// include abs control correction in FF
pidSetpointDelta += setpointCorrection - oldSetpointCorrection[axis];
oldSetpointCorrection[axis] = setpointCorrection;
#endif
// Only enable feedforward for rate mode and if launch control is inactive
const float feedforwardGain = (flightModeFlags || launchControlActive) ? 0.0f : pidCoefficient[axis].Kf;
if (feedforwardGain > 0) {
// no transition if feedForwardTransition == 0
float transition = feedForwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * feedForwardTransition) : 1;
pidData[axis].F = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
float feedForward = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
#ifdef USE_INTERPOLATED_SP
pidData[axis].F = shouldApplyFFLimits(axis) ?
applyFFLimit(axis, feedForward, pidCoefficient[axis].Kp, currentPidSetpoint) : feedForward;
#else
pidData[axis].F = feedForward;
#endif
} else {
pidData[axis].F = 0;
}