mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-25 01:05:27 +03:00
226 lines
9.5 KiB
C
226 lines
9.5 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_FEEDFORWARD
|
|
|
|
#include "build/debug.h"
|
|
|
|
#include "common/maths.h"
|
|
|
|
#include "fc/rc.h"
|
|
|
|
#include "flight/pid.h"
|
|
|
|
#include "feedforward.h"
|
|
|
|
static float setpointDelta[XYZ_AXIS_COUNT];
|
|
static float prevSetpoint[XYZ_AXIS_COUNT];
|
|
static float prevSetpointSpeed[XYZ_AXIS_COUNT];
|
|
static float prevAcceleration[XYZ_AXIS_COUNT];
|
|
static uint8_t duplicateCount[XYZ_AXIS_COUNT];
|
|
static uint8_t averagingCount;
|
|
static float feedforwardMaxRateLimit[XYZ_AXIS_COUNT];
|
|
static float feedforwardMaxRate[XYZ_AXIS_COUNT];
|
|
|
|
typedef struct laggedMovingAverageCombined_s {
|
|
laggedMovingAverage_t filter;
|
|
float buf[4];
|
|
} laggedMovingAverageCombined_t;
|
|
laggedMovingAverageCombined_t setpointDeltaAvg[XYZ_AXIS_COUNT];
|
|
|
|
void feedforwardInit(const pidProfile_t *pidProfile)
|
|
{
|
|
const float feedforwardMaxRateScale = pidProfile->feedforward_max_rate_limit * 0.01f;
|
|
averagingCount = pidProfile->feedforward_averaging + 1;
|
|
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
|
feedforwardMaxRate[i] = applyCurve(i, 1.0f);
|
|
feedforwardMaxRateLimit[i] = feedforwardMaxRate[i] * feedforwardMaxRateScale;
|
|
laggedMovingAverageInit(&setpointDeltaAvg[i].filter, averagingCount, (float *)&setpointDeltaAvg[i].buf[0]);
|
|
}
|
|
}
|
|
|
|
FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging)
|
|
{
|
|
|
|
if (newRcFrame) {
|
|
|
|
const float feedforwardTransitionFactor = pidGetFeedforwardTransitionFactor();
|
|
const float feedforwardSmoothFactor = pidGetFeedforwardSmoothFactor();
|
|
// good values : 25 for 111hz FrSky, 30 for 150hz, 50 for 250hz, 65 for 500hz links
|
|
const float feedforwardJitterFactor = pidGetFeedforwardJitterFactor();
|
|
// 7 is default, 5 for faster links with smaller steps and for racing, 10-12 for 150hz freestyle
|
|
const float feedforwardBoostFactor = pidGetFeedforwardBoostFactor();
|
|
|
|
const float rxInterval = getCurrentRxRefreshRate() * 1e-6f; // 0.0066 for 150hz RC Link.
|
|
const float rxRate = 1.0f / rxInterval; // eg 150 for a 150Hz RC link
|
|
|
|
const float setpoint = getRawSetpoint(axis);
|
|
const float absSetpointPercent = fabsf(setpoint) / feedforwardMaxRate[axis];
|
|
|
|
float rcCommandDelta = getRcCommandDelta(axis);
|
|
|
|
if (axis == FD_ROLL) {
|
|
DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(rcCommandDelta * 100.0f));
|
|
// rcCommand packet difference = value of 100 if 1000 RC steps
|
|
DEBUG_SET(DEBUG_FEEDFORWARD, 0, lrintf(setpoint));
|
|
// un-smoothed in blackbox
|
|
}
|
|
|
|
// calculate setpoint speed
|
|
float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
|
|
float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention
|
|
float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
|
|
|
|
float setpointAcceleration = 0.0f;
|
|
|
|
rcCommandDelta = fabsf(rcCommandDelta);
|
|
|
|
if (rcCommandDelta) {
|
|
// we have movement and should calculate feedforward
|
|
|
|
// jitter attenuator falls below 1 when rcCommandDelta falls below jitter threshold
|
|
float jitterAttenuator = 1.0f;
|
|
if (feedforwardJitterFactor) {
|
|
if (rcCommandDelta < feedforwardJitterFactor) {
|
|
jitterAttenuator = MAX(1.0f - (rcCommandDelta / feedforwardJitterFactor), 0.0f);
|
|
jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator;
|
|
}
|
|
}
|
|
|
|
// duplicateCount indicates number of prior duplicate/s, 1 means one only duplicate prior to this packet
|
|
// reduce setpoint speed by half after a single duplicate or a third after two. Any more are forced to zero.
|
|
// needed because while sticks are moving, the next valid step up will be proportionally bigger
|
|
// and stops excessive feedforward where steps are at intervals, eg when the OpenTx ADC filter is active
|
|
// downside is that for truly held sticks, the first feedforward step won't be as big as it should be
|
|
if (duplicateCount[axis]) {
|
|
setpointSpeed /= duplicateCount[axis] + 1;
|
|
}
|
|
|
|
// first order type smoothing for setpoint speed noise reduction
|
|
setpointSpeed = prevSetpointSpeed[axis] + feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
|
|
|
|
// calculate acceleration from smoothed setpoint speed
|
|
setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
|
|
|
|
// use rxRate to normalise acceleration to nominal RC packet interval of 100hz
|
|
// without this, we would get less boost than we should at higher Rx rates
|
|
// note rxRate updates with every new packet (though not every time data changes), hence
|
|
// if no Rx packets are received for a period, boost amount is correctly attenuated in proportion to the delay
|
|
setpointAcceleration *= rxRate * 0.01f;
|
|
|
|
// first order acceleration smoothing (with smoothed input this is effectively second order all up)
|
|
setpointAcceleration = prevAcceleration[axis] + feedforwardSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
|
|
|
|
// jitter reduction to reduce acceleration spikes at low rcCommandDelta values
|
|
// no effect for rcCommandDelta values above jitter threshold (zero delay)
|
|
// does not attenuate the basic feedforward amount, but this is small anyway at centre due to expo
|
|
setpointAcceleration *= jitterAttenuator;
|
|
|
|
if (absSetpointPercent > 0.95f && absSetpointSpeed < 3.0f * absPrevSetpointSpeed) {
|
|
// approaching max stick position so zero out feedforward to minimise overshoot
|
|
setpointSpeed = 0.0f;
|
|
setpointAcceleration = 0.0f;
|
|
}
|
|
|
|
prevSetpointSpeed[axis] = setpointSpeed;
|
|
prevAcceleration[axis] = setpointAcceleration;
|
|
|
|
setpointAcceleration *= feedforwardBoostFactor;
|
|
|
|
// add attenuated boost to base feedforward and apply jitter attenuation
|
|
setpointDelta[axis] = (setpointSpeed + setpointAcceleration) * pidGetDT() * jitterAttenuator;
|
|
|
|
//reset counter
|
|
duplicateCount[axis] = 0;
|
|
|
|
} else {
|
|
// no movement
|
|
if (duplicateCount[axis]) {
|
|
// increment duplicate count to max of 2
|
|
duplicateCount[axis] += (duplicateCount[axis] < 2) ? 1 : 0;
|
|
// second or subsequent duplicate, or duplicate when held at max stick or centre position.
|
|
// force feedforward to zero
|
|
setpointDelta[axis] = 0.0f;
|
|
// zero speed and acceleration for correct smoothing of next good packet
|
|
setpointSpeed = 0.0f;
|
|
prevSetpointSpeed[axis] = 0.0f;
|
|
prevAcceleration[axis] = 0.0f;
|
|
} else {
|
|
// first duplicate; hold feedforward and previous static values, as if we just never got anything
|
|
duplicateCount[axis] = 1;
|
|
}
|
|
}
|
|
|
|
|
|
if (axis == FD_ROLL) {
|
|
DEBUG_SET(DEBUG_FEEDFORWARD, 1, lrintf(setpointSpeed * pidGetDT() * 100.0f)); // setpoint speed after smoothing
|
|
DEBUG_SET(DEBUG_FEEDFORWARD, 2, lrintf(setpointAcceleration * pidGetDT() * 100.0f)); // boost amount after smoothing
|
|
// debug 0 is interpolated setpoint, above
|
|
// debug 3 is rcCommand delta, above
|
|
}
|
|
|
|
prevSetpoint[axis] = setpoint;
|
|
|
|
// apply averaging, if enabled - include zero values in averaging
|
|
if (feedforwardAveraging) {
|
|
setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDelta[axis]);
|
|
}
|
|
|
|
// apply feedforward transition
|
|
setpointDelta[axis] *= feedforwardTransitionFactor > 0 ? MIN(1.0f, getRcDeflectionAbs(axis) * feedforwardTransitionFactor) : 1.0f;
|
|
|
|
}
|
|
return setpointDelta[axis]; // the value used by the PID code
|
|
}
|
|
|
|
FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint)
|
|
{
|
|
switch (axis) {
|
|
case FD_ROLL:
|
|
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 0, value);
|
|
break;
|
|
case FD_PITCH:
|
|
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 1, value);
|
|
break;
|
|
}
|
|
|
|
if (value * currentPidSetpoint > 0.0f) {
|
|
if (fabsf(currentPidSetpoint) <= feedforwardMaxRateLimit[axis]) {
|
|
value = constrainf(value, (-feedforwardMaxRateLimit[axis] - currentPidSetpoint) * Kp, (feedforwardMaxRateLimit[axis] - currentPidSetpoint) * Kp);
|
|
} else {
|
|
value = 0;
|
|
}
|
|
}
|
|
|
|
if (axis == FD_ROLL) {
|
|
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 2, value);
|
|
}
|
|
|
|
return value;
|
|
}
|
|
|
|
bool shouldApplyFeedforwardLimits(int axis)
|
|
{
|
|
return axis < FD_YAW && feedforwardMaxRateLimit[axis] != 0.0f;
|
|
}
|
|
#endif
|