mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 20:35:33 +03:00
214 lines
8.4 KiB
C
214 lines
8.4 KiB
C
/*
|
|
* This file is part of Cleanflight and Betaflight.
|
|
*
|
|
* Cleanflight and Betaflight are free software. You can redistribute
|
|
* this software and/or modify this software under the terms of the
|
|
* GNU General Public License as published by the Free Software
|
|
* Foundation, either version 3 of the License, or (at your option)
|
|
* any later version.
|
|
*
|
|
* Cleanflight and Betaflight are distributed in the hope that they
|
|
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
|
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
|
* See the GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this software.
|
|
*
|
|
* If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <math.h>
|
|
#include "platform.h"
|
|
|
|
#ifdef USE_INTERPOLATED_SP
|
|
|
|
#include "build/debug.h"
|
|
#include "common/maths.h"
|
|
#include "fc/rc.h"
|
|
#include "flight/interpolated_setpoint.h"
|
|
|
|
static float setpointDeltaImpl[XYZ_AXIS_COUNT];
|
|
static float setpointDelta[XYZ_AXIS_COUNT];
|
|
static uint8_t holdCount[XYZ_AXIS_COUNT];
|
|
|
|
typedef struct laggedMovingAverageCombined_s {
|
|
laggedMovingAverage_t filter;
|
|
float buf[4];
|
|
} laggedMovingAverageCombined_t;
|
|
|
|
laggedMovingAverageCombined_t setpointDeltaAvg[XYZ_AXIS_COUNT];
|
|
|
|
static float prevSetpointSpeed[XYZ_AXIS_COUNT];
|
|
static float prevAcceleration[XYZ_AXIS_COUNT];
|
|
static float prevRawSetpoint[XYZ_AXIS_COUNT];
|
|
static float prevDeltaImpl[XYZ_AXIS_COUNT];
|
|
static bool bigStep[XYZ_AXIS_COUNT];
|
|
|
|
// Configuration
|
|
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;
|
|
uint8_t j = pidProfile->ff_interpolate_sp;
|
|
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
|
ffMaxRate[i] = applyCurve(i, 1.0f);
|
|
ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale;
|
|
laggedMovingAverageInit(&setpointDeltaAvg[i].filter, j, (float *)&setpointDeltaAvg[i].buf[0]);
|
|
}
|
|
}
|
|
|
|
FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type) {
|
|
|
|
if (newRcFrame) {
|
|
float rawSetpoint = getRawSetpoint(axis);
|
|
|
|
const float rxInterval = currentRxRefreshRate * 1e-6f;
|
|
const float rxRate = 1.0f / rxInterval;
|
|
float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
|
|
float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
|
|
float setpointSpeedModified = setpointSpeed;
|
|
float setpointAccelerationModified = setpointAcceleration;
|
|
|
|
// Glitch reduction code for identical packets
|
|
if (fabsf(setpointAcceleration) > 3.0f * fabsf(prevAcceleration[axis])) {
|
|
bigStep[axis] = true;
|
|
} else {
|
|
bigStep[axis] = false;
|
|
}
|
|
|
|
if (setpointSpeed == 0 && fabsf(rawSetpoint) < 0.98f * ffMaxRate[axis]) {
|
|
// identical packet detected, not at full deflection.
|
|
// first packet on leaving full deflection always gets full FF
|
|
if (holdCount[axis] == 0) {
|
|
// previous packet had movement
|
|
if (bigStep[axis]) {
|
|
// type 1 = interpolate forward where acceleration change is large
|
|
setpointSpeedModified = prevSetpointSpeed[axis];
|
|
setpointAccelerationModified = prevAcceleration[axis];
|
|
holdCount[axis] = 1;
|
|
} else {
|
|
// type 2 = small change, no interpolation needed
|
|
setpointSpeedModified = prevSetpointSpeed[axis];
|
|
holdCount[axis] = 2;
|
|
}
|
|
} else {
|
|
// it is an unchanged packet after previous unchanged packet
|
|
// speed and acceleration will be zero, no need to change anything
|
|
holdCount[axis] = 3;
|
|
}
|
|
} else {
|
|
// we're moving, or sticks are at max
|
|
if (holdCount[axis] != 0) {
|
|
// previous step was a duplicate, handle each type differently
|
|
if (holdCount[axis] == 1) {
|
|
// interpolation was applied
|
|
// raw setpoint speed of next 'good' packet is twice what it should be
|
|
setpointSpeedModified = setpointSpeed / 2.0f;
|
|
// empirically this works best
|
|
setpointAccelerationModified = (prevAcceleration[axis] + setpointAcceleration) / 2.0f;
|
|
} else if (holdCount[axis] == 2) {
|
|
// interpolation was not applied
|
|
setpointSpeedModified = setpointSpeed / 2.0f;
|
|
setpointAccelerationModified = prevAcceleration[axis] / 2.0f;
|
|
} else if (holdCount[axis] == 3) {
|
|
// after persistent flat period or recent big step up, no boost
|
|
// reduces jitter from boost when flying smoothly
|
|
// WARNING: this means no boost if ADC is active on FrSky radios
|
|
setpointAccelerationModified = 0.0f;
|
|
}
|
|
holdCount[axis] = 0;
|
|
}
|
|
}
|
|
|
|
// smooth deadband type suppression of FF when sticks are centred.
|
|
const float rawSetpointCentred = fabsf(rawSetpoint) / 2.0f;
|
|
if (rawSetpointCentred < 1.0f) {
|
|
// force zero FF when sticks are centred for smoothness
|
|
setpointSpeedModified *= rawSetpointCentred;
|
|
setpointAccelerationModified *= rawSetpointCentred;
|
|
holdCount[axis] = 4;
|
|
}
|
|
|
|
setpointDeltaImpl[axis] = setpointSpeedModified * pidGetDT();
|
|
prevAcceleration[axis] = setpointAcceleration;
|
|
|
|
setpointAcceleration *= pidGetDT();
|
|
setpointAccelerationModified *= pidGetDT();
|
|
|
|
const float ffBoostFactor = pidGetFfBoostFactor();
|
|
float clip = 1.0f;
|
|
float boostAmount = 0.0f;
|
|
if (ffBoostFactor != 0.0f) {
|
|
//calculate clip factor to reduce boost on big spikes
|
|
if (pidGetSpikeLimitInverse()) {
|
|
clip = 1 / (1 + (setpointAcceleration * setpointAcceleration * pidGetSpikeLimitInverse()));
|
|
clip *= clip;
|
|
}
|
|
// don't clip first step inwards from max deflection
|
|
if (fabsf(prevRawSetpoint[axis]) > 0.95f * ffMaxRate[axis] && fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) {
|
|
clip = 1.0f;
|
|
}
|
|
// calculate boost and prevent kick-back spike at max deflection
|
|
if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) {
|
|
boostAmount = ffBoostFactor * setpointAccelerationModified;
|
|
}
|
|
}
|
|
|
|
prevSetpointSpeed[axis] = setpointSpeed;
|
|
prevRawSetpoint[axis] = rawSetpoint;
|
|
|
|
if (axis == FD_ROLL) {
|
|
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100));
|
|
DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(setpointAccelerationModified * 100));
|
|
DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(setpointAcceleration * 100));
|
|
DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, holdCount[axis]);
|
|
}
|
|
|
|
setpointDeltaImpl[axis] += boostAmount * clip;
|
|
|
|
// first order (kind of) smoothing of FF
|
|
const float ffSmoothFactor = pidGetFfSmoothFactor();
|
|
setpointDeltaImpl[axis] = prevDeltaImpl[axis] + ffSmoothFactor * (setpointDeltaImpl[axis] - prevDeltaImpl[axis]);
|
|
prevDeltaImpl[axis] = setpointDeltaImpl[axis];
|
|
|
|
if (type == FF_INTERPOLATE_ON) {
|
|
setpointDelta[axis] = setpointDeltaImpl[axis];
|
|
} else {
|
|
setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDeltaImpl[axis]);
|
|
}
|
|
}
|
|
return setpointDelta[axis];
|
|
}
|
|
|
|
FAST_CODE_NOINLINE float applyFfLimit(int axis, float value, float Kp, float currentPidSetpoint) {
|
|
switch (axis) {
|
|
case FD_ROLL:
|
|
DEBUG_SET(DEBUG_FF_LIMIT, 0, value);
|
|
|
|
break;
|
|
case FD_PITCH:
|
|
DEBUG_SET(DEBUG_FF_LIMIT, 1, value);
|
|
|
|
break;
|
|
}
|
|
|
|
if (fabsf(currentPidSetpoint) <= ffMaxRateLimit[axis]) {
|
|
value = constrainf(value, (-ffMaxRateLimit[axis] - currentPidSetpoint) * Kp, (ffMaxRateLimit[axis] - currentPidSetpoint) * Kp);
|
|
} else {
|
|
value = 0;
|
|
}
|
|
|
|
if (axis == FD_ROLL) {
|
|
DEBUG_SET(DEBUG_FF_LIMIT, 2, value);
|
|
}
|
|
|
|
return value;
|
|
}
|
|
|
|
bool shouldApplyFfLimits(int axis)
|
|
{
|
|
return ffMaxRateLimit[axis] != 0.0f && axis < FD_YAW;
|
|
}
|
|
#endif
|