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

Merge pull request #8736 from joelucid/ff2_1

Browse source 
various ff2_0 improvements
This commit is contained in:
Michael Keller 2019-09-05 02:14:23 +12:00 • committed by GitHub
commit 75447ef2b8
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
8 changed files with 96 additions and 77 deletions
Download patch file Download diff file Expand all files Collapse all files

11
src/main/cli/settings.c
View file

@ -473,6 +473,11 @@ static const char * const lookupTableDshotBitbang[] = {
"OFF", "ON", "AUTO"
};
static const char* const lookupTableInterpolatedSetpoint[] = {
"OFF", "ON", "AVERAGED"
};
#define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
const lookupTableEntry_t lookupTables[] = {
@ -588,6 +593,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTablePositionAltSource),
LOOKUP_TABLE_ENTRY(lookupTableDshotBitbang),
LOOKUP_TABLE_ENTRY(lookupTableInterpolatedSetpoint),
};
#undef LOOKUP_TABLE_ENTRY
@ -1073,10 +1079,9 @@ const clivalue_t valueTable[] = {
{ "transient_throttle_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, transient_throttle_limit) },
#endif
#ifdef USE_INTERPOLATED_SP
{ "ff_interpolate_sp", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_sp) },
{ "ff_spread", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 50}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_spread) },
{ "ff_interpolate_sp", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_INTERPOLATED_SP}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_sp) },
{ "ff_jerk_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 255}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_jerk_limit) },
{ "ff_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_max_rate_limit) },
{ "ff_lookahead_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 255}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_lookahead_limit) },
#endif
{ "ff_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) },

1
src/main/cli/settings.h
View file

@ -136,6 +136,7 @@ typedef enum {
TABLE_GYRO_FILTER_DEBUG,
TABLE_POSITION_ALT_SOURCE,
TABLE_DSHOT_BITBANG,
TABLE_INTERPOLATED_SP,
LOOKUP_TABLE_COUNT
} lookupTableIndex_e;

18
src/main/fc/rc.c
View file

@ -58,6 +58,8 @@ typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCom
static float rawSetpoint[XYZ_AXIS_COUNT];
// Stick deflection [-1.0, 1.0] before RC-Smoothing is applied
static float rawDeflection[XYZ_AXIS_COUNT];
static float oldRcCommand[XYZ_AXIS_COUNT];
static bool isDuplicateFrame;
#endif
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
static float throttlePIDAttenuation;
@ -124,6 +126,12 @@ float getRawDeflection(int axis)
{
return rawDeflection[axis];
}
bool rcIsDuplicateFrame()
{
return isDuplicateFrame;
}
#endif
#define THROTTLE_LOOKUP_LENGTH 12
@ -188,6 +196,11 @@ float applyCurve(int axis, float deflection)
return applyRates(axis, deflection, fabsf(deflection));
}
float getRcCurveSlope(int axis, float deflection)
{
return (applyCurve(axis, deflection + 0.01f) - applyCurve(axis, deflection)) * 100.0f;
}
static void calculateSetpointRate(int axis)
{
float angleRate;
@ -600,7 +613,12 @@ FAST_CODE void processRcCommand(void)
#ifdef USE_INTERPOLATED_SP
if (isRXDataNew) {
isDuplicateFrame = true;
for (int i = FD_ROLL; i <= FD_YAW; i++) {
if (rcCommand[i] != oldRcCommand[i]) {
isDuplicateFrame = false;
}
oldRcCommand[i] = rcCommand[i];
const float rcCommandf = rcCommand[i] / 500.0f;
const float rcCommandfAbs = fabsf(rcCommandf);
rawSetpoint[i] = applyRates(i, rcCommandf, rcCommandfAbs);

2
src/main/fc/rc.h
View file

@ -49,3 +49,5 @@ float getRawSetpoint(int axis);
float getRawDeflection(int axis);
float applyCurve(int axis, float deflection);
uint32_t getRcFrameNumber();
float getRcCurveSlope(int axis, float deflection);
bool rcIsDuplicateFrame();

100
src/main/flight/interpolated_setpoint.c
View file

@ -28,89 +28,76 @@
#include "fc/rc.h"
#include "flight/interpolated_setpoint.h"
static float projectedSetpoint[XYZ_AXIS_COUNT];
static float setpointDeltaImpl[XYZ_AXIS_COUNT];
static float prevSetpointDeltaImpl[XYZ_AXIS_COUNT];
static float setpointDelta[XYZ_AXIS_COUNT];
static float prevSetpointSpeed[XYZ_AXIS_COUNT];
static float prevRawSetpoint[XYZ_AXIS_COUNT];
static float prevRawDeflection[XYZ_AXIS_COUNT];
static uint16_t interpolationSteps[XYZ_AXIS_COUNT];
static float setpointChangePerIteration[XYZ_AXIS_COUNT];
static float deflectionChangePerIteration[XYZ_AXIS_COUNT];
static float setpointReservoir[XYZ_AXIS_COUNT];
static float deflectionReservoir[XYZ_AXIS_COUNT];
static float prevSetpointAcceleration[XYZ_AXIS_COUNT];
// Configuration
static float ffLookaheadLimit;
static float ffSpread;
static float ffMaxRateLimit[XYZ_AXIS_COUNT];
static float ffMaxRate[XYZ_AXIS_COUNT];
void interpolatedSpInit(const pidProfile_t *pidProfile) {
const float ffMaxRateScale = pidProfile->ff_max_rate_limit * 0.01f;
ffLookaheadLimit = pidProfile->ff_lookahead_limit * 0.0001f;
ffSpread = pidProfile->ff_spread;
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
ffMaxRate[i] = applyCurve(i, 1.0f);
ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale;
}
}
FAST_CODE_NOINLINE float interpolatedSpApply(int axis, float pidFrequency, bool newRcFrame) {
const float rawSetpoint = getRawSetpoint(axis);
const float rawDeflection = getRawDeflection(axis);
FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type) {
float pidSetpointDelta = 0.0f;
static int iterationsSinceLastUpdate[XYZ_AXIS_COUNT];
if (newRcFrame) {
float rawSetpoint = getRawSetpoint(axis);
setpointReservoir[axis] -= iterationsSinceLastUpdate[axis] * setpointChangePerIteration[axis];
deflectionReservoir[axis] -= iterationsSinceLastUpdate[axis] * deflectionChangePerIteration[axis];
iterationsSinceLastUpdate[axis] = 0;
const float rxInterval = currentRxRefreshRate * 1e-6f;
const float rxRate = 1.0f / rxInterval;
// get the number of interpolation steps either dynamically based on RX refresh rate
// or manually based on ffSpread configuration property
if (ffSpread) {
interpolationSteps[axis] = (uint16_t) ((ffSpread + 1.0f) * 0.001f * pidFrequency);
} else {
interpolationSteps[axis] = (uint16_t) ((currentRxRefreshRate + 1000) * pidFrequency * 1e-6f + 0.5f);
}
const float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
const float setpointAcceleration = (setpointSpeed - prevSetpointSpeed[axis]) * pidGetDT();
const float setpointJerk = setpointAcceleration - prevSetpointAcceleration[axis];
// interpolate stick deflection
deflectionReservoir[axis] += rawDeflection - prevRawDeflection[axis];
deflectionChangePerIteration[axis] = deflectionReservoir[axis] / interpolationSteps[axis];
const float projectedStickPos =
rawDeflection + deflectionChangePerIteration[axis] * pidFrequency * ffLookaheadLimit;
projectedSetpoint[axis] = applyCurve(axis, projectedStickPos);
prevRawDeflection[axis] = rawDeflection;
setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
// apply linear interpolation on setpoint
setpointReservoir[axis] += rawSetpoint - prevRawSetpoint[axis];
const float ffBoostFactor = pidGetFfBoostFactor();
float clip = 1.0f;
float boostAmount = 0.0f;
if (ffBoostFactor != 0.0f) {
const float speed = rawSetpoint - prevRawSetpoint[axis];
if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || fabsf(3.0f * speed) > fabsf(prevSetpointSpeed[axis])) {
const float setpointAcc = speed - prevSetpointSpeed[axis];
setpointReservoir[axis] += ffBoostFactor * setpointAcc;
}
prevSetpointSpeed[axis] = speed;
if (pidGetJerkLimitInverse()) {
clip = 1 / (1 + fabsf(setpointJerk * pidGetJerkLimitInverse()));
clip *= clip;
}
setpointChangePerIteration[axis] = setpointReservoir[axis] / interpolationSteps[axis];
// prevent kick-back spike at max deflection
if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) {
boostAmount = ffBoostFactor * setpointAcceleration;
}
}
prevSetpointSpeed[axis] = setpointSpeed;
prevSetpointAcceleration[axis] = setpointAcceleration;
prevRawSetpoint[axis] = rawSetpoint;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, rawDeflection * 100);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, projectedStickPos * 100);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, projectedSetpoint[axis]);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, setpointDeltaImpl[axis] * 1000);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, boostAmount * 1000);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, boostAmount * clip * 1000);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, setpointJerk * 1000);
}
setpointDeltaImpl[axis] += boostAmount * clip;
if (type == FF_INTERPOLATE_ON) {
setpointDelta[axis] = setpointDeltaImpl[axis];
} else {
setpointDelta[axis] = 0.5f * (setpointDeltaImpl[axis] + prevSetpointDeltaImpl[axis]);
prevSetpointDeltaImpl[axis] = setpointDeltaImpl[axis];
}
}
if (iterationsSinceLastUpdate[axis] < interpolationSteps[axis]) {
iterationsSinceLastUpdate[axis]++;
pidSetpointDelta = setpointChangePerIteration[axis];
}
return pidSetpointDelta;
return setpointDelta[axis];
}
FAST_CODE_NOINLINE float applyFfLimit(int axis, float value, float Kp, float currentPidSetpoint) {
@ -118,13 +105,6 @@ FAST_CODE_NOINLINE float applyFfLimit(int axis, float value, float Kp, float cur
DEBUG_SET(DEBUG_FF_LIMIT, 0, value);
}
if (ffLookaheadLimit) {
const float limit = fabsf((projectedSetpoint[axis] - prevRawSetpoint[axis]) * Kp);
value = constrainf(value, -limit, limit);
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, projectedSetpoint[axis]);
}
}
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_LIMIT, 1, value);
}
@ -144,7 +124,7 @@ FAST_CODE_NOINLINE float applyFfLimit(int axis, float value, float Kp, float cur
bool shouldApplyFfLimits(int axis)
{
return ffLookaheadLimit != 0.0f || ffMaxRateLimit[axis] != 0.0f;
return ffMaxRateLimit[axis] != 0.0f;
}

8
src/main/flight/interpolated_setpoint.h
View file

@ -25,7 +25,13 @@
#include "common/axis.h"
#include "flight/pid.h"
typedef enum ffInterpolationType_e {
FF_INTERPOLATE_OFF,
FF_INTERPOLATE_ON,
FF_INTERPOLATE_AVG
} ffInterpolationType_t;
void interpolatedSpInit(const pidProfile_t *pidProfile);
float interpolatedSpApply(int axis, float pidFrequency, bool newRcFrame);
float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type);
float applyFfLimit(int axis, float value, float Kp, float currentPidSetpoint);
bool shouldApplyFfLimits(int axis);

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

@ -130,7 +130,7 @@ static FAST_RAM_ZERO_INIT float airmodeThrottleOffsetLimit;
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 12);
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 13);
void resetPidProfile(pidProfile_t *pidProfile)
{
@ -212,10 +212,9 @@ 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_interpolate_sp = FF_INTERPOLATE_AVG,
.ff_jerk_limit = 25,
.ff_max_rate_limit = 100,
.ff_boost = 15,
);
#ifndef USE_D_MIN
@ -315,6 +314,13 @@ static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2;
static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
static FAST_RAM_ZERO_INIT float ffBoostFactor;
static FAST_RAM_ZERO_INIT float ffJerkLimitInverse;
float pidGetJerkLimitInverse()
{
return ffJerkLimitInverse;
}
float pidGetFfBoostFactor()
{
@ -460,6 +466,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
ffBoostFactor = (float)pidProfile->ff_boost / 10.0f;
ffJerkLimitInverse = pidProfile->ff_jerk_limit ? 1.0f / ((float)pidProfile->ff_jerk_limit / 10.0f) : 0.0f;
}
#ifdef USE_RC_SMOOTHING_FILTER
@ -583,7 +590,7 @@ static FAST_RAM_ZERO_INIT float dMinSetpointGain;
#endif
#ifdef USE_INTERPOLATED_SP
static FAST_RAM_ZERO_INIT bool ffFromInterpolatedSetpoint;
static FAST_RAM_ZERO_INIT ffInterpolationType_t ffFromInterpolatedSetpoint;
#endif
void pidInitConfig(const pidProfile_t *pidProfile)
@ -1408,7 +1415,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
float pidSetpointDelta = 0;
#ifdef USE_INTERPOLATED_SP
if (ffFromInterpolatedSetpoint) {
pidSetpointDelta = interpolatedSpApply(axis, pidFrequency, newRcFrame);
pidSetpointDelta = interpolatedSpApply(axis, newRcFrame, ffFromInterpolatedSetpoint);
} else {
pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
}

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

@ -182,9 +182,8 @@ typedef struct pidProfile_s {
uint8_t idle_max_increase; // max integrated correction
uint8_t ff_interpolate_sp; // Calculate FF from interpolated setpoint
uint8_t ff_spread; // Spread ff out over at least min spread ms
uint8_t ff_max_rate_limit; // Maximum setpoint rate percentage for FF
uint8_t ff_lookahead_limit; // FF stick extrapolation lookahead period in ms
uint8_t ff_jerk_limit; // FF stick extrapolation lookahead period in ms
} pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@ -262,3 +261,4 @@ float pidGetPreviousSetpoint(int axis);
float pidGetDT();
float pidGetPidFrequency();
float pidGetFfBoostFactor();
float pidGetJerkLimitInverse();