src/main/flight/feedforward.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <math.h>
  22 #include "platform.h"
  23
  24 #ifdef USE_FEEDFORWARD
  25
  26 #include "build/debug.h"
  27
  28 #include "common/maths.h"
  29
  30 #include "fc/rc.h"
  31
  32 #include "flight/pid.h"
  33
  34 #include "feedforward.h"
  35
  36 static float setpointDelta[XYZ_AXIS_COUNT];
  37 static float prevSetpoint[XYZ_AXIS_COUNT];
  38 static float prevSetpointSpeed[XYZ_AXIS_COUNT];
  39 static float prevAcceleration[XYZ_AXIS_COUNT];
  40 static uint8_t duplicateCount[XYZ_AXIS_COUNT];
  41 static uint8_t averagingCount;
  42 static float feedforwardMaxRateLimit[XYZ_AXIS_COUNT];
  43 static float feedforwardMaxRate[XYZ_AXIS_COUNT];
  44
  45 typedef struct laggedMovingAverageCombined_s {
  46      laggedMovingAverage_t filter;
  47      float buf[4];
  48 } laggedMovingAverageCombined_t;
  49 laggedMovingAverageCombined_t  setpointDeltaAvg[XYZ_AXIS_COUNT];
  50
  51 void feedforwardInit(const pidProfile_t *pidProfile) {
  52     const float feedforwardMaxRateScale = pidProfile->feedforward_max_rate_limit * 0.01f;
  53     averagingCount = pidProfile->feedforward_averaging + 1;
  54     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
  55         feedforwardMaxRate[i] = applyCurve(i, 1.0f);
  56         feedforwardMaxRateLimit[i] = feedforwardMaxRate[i] * feedforwardMaxRateScale;
  57         laggedMovingAverageInit(&setpointDeltaAvg[i].filter, averagingCount, (float *)&setpointDeltaAvg[i].buf[0]);
  58     }
  59 }
  60
  61 FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging) {
  62
  63     if (newRcFrame) {
  64
  65         const float feedforwardTransitionFactor = pidGetFeedforwardTransitionFactor();
  66         const float feedforwardSmoothFactor = pidGetFeedforwardSmoothFactor();
  67                     // good values : 25 for 111hz FrSky, 30 for 150hz, 50 for 250hz, 65 for 500hz links
  68         const float feedforwardJitterFactor = pidGetFeedforwardJitterFactor();
  69                     // 7 is default, 5 for faster links with smaller steps and for racing, 10-12 for 150hz freestyle
  70         const float feedforwardBoostFactor = pidGetFeedforwardBoostFactor();
  71
  72         const float rxInterval = getCurrentRxRefreshRate() * 1e-6f; // 0.0066 for 150hz RC Link.
  73         const float rxRate = 1.0f / rxInterval; // eg 150 for a 150Hz RC link
  74
  75         const float setpoint = getRawSetpoint(axis);
  76         const float absSetpointPercent = fabsf(setpoint) / feedforwardMaxRate[axis];
  77
  78         float rcCommandDelta = getRcCommandDelta(axis);
  79
  80         if (axis == FD_ROLL) {
  81             DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(rcCommandDelta * 100.0f));
  82             // rcCommand packet difference = value of 100 if 1000 RC steps
  83             DEBUG_SET(DEBUG_FEEDFORWARD, 0, lrintf(setpoint));
  84             // un-smoothed in blackbox
  85         }
  86
  87         // calculate setpoint speed
  88         float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
  89         float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention
  90         float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
  91
  92         float setpointAcceleration = 0.0f;
  93
  94         rcCommandDelta = fabsf(rcCommandDelta);
  95
  96         if (rcCommandDelta) {
  97             // we have movement and should calculate feedforward
  98
  99             // jitter attenuator falls below 1 when rcCommandDelta falls below jitter threshold
 100             float jitterAttenuator = 1.0f;
 101             if (feedforwardJitterFactor) {
 102                 if (rcCommandDelta < feedforwardJitterFactor) {
 103                     jitterAttenuator = MAX(1.0f - (rcCommandDelta / feedforwardJitterFactor), 0.0f);
 104                     jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator;
 105                 }
 106             }
 107
 108             // duplicateCount indicates number of prior duplicate/s, 1 means one only duplicate prior to this packet
 109             // reduce setpoint speed by half after a single duplicate or a third after two. Any more are forced to zero.
 110             // needed because while sticks are moving, the next valid step up will be proportionally bigger
 111             // and stops excessive feedforward where steps are at intervals, eg when the OpenTx ADC filter is active
 112             // downside is that for truly held sticks, the first feedforward step won't be as big as it should be
 113             if (duplicateCount[axis]) {
 114                 setpointSpeed /= duplicateCount[axis] + 1;
 115             }
 116
 117             // first order type smoothing for setpoint speed noise reduction
 118             setpointSpeed = prevSetpointSpeed[axis] + feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
 119
 120             // calculate acceleration from smoothed setpoint speed
 121             setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
 122
 123             // use rxRate to normalise acceleration to nominal RC packet interval of 100hz
 124             // without this, we would get less boost than we should at higher Rx rates
 125             // note rxRate updates with every new packet (though not every time data changes), hence
 126             // if no Rx packets are received for a period, boost amount is correctly attenuated in proportion to the delay
 127             setpointAcceleration *= rxRate * 0.01f;
 128
 129             // first order acceleration smoothing (with smoothed input this is effectively second order all up)
 130             setpointAcceleration = prevAcceleration[axis] + feedforwardSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
 131
 132             // jitter reduction to reduce acceleration spikes at low rcCommandDelta values
 133             // no effect for rcCommandDelta values above jitter threshold (zero delay)
 134             // does not attenuate the basic feedforward amount, but this is small anyway at centre due to expo
 135             setpointAcceleration *= jitterAttenuator;
 136
 137             if (absSetpointPercent > 0.95f && absSetpointSpeed < 3.0f * absPrevSetpointSpeed) {
 138                 // approaching max stick position so zero out feedforward to minimise overshoot
 139                 setpointSpeed = 0.0f;
 140                 setpointAcceleration = 0.0f;
 141             }
 142
 143             prevSetpointSpeed[axis] = setpointSpeed;
 144             prevAcceleration[axis] = setpointAcceleration;
 145
 146             setpointAcceleration *= feedforwardBoostFactor;
 147
 148             // add attenuated boost to base feedforward and apply jitter attenuation
 149             setpointDelta[axis] = (setpointSpeed + setpointAcceleration) * pidGetDT() * jitterAttenuator;
 150
 151             //reset counter
 152             duplicateCount[axis] = 0;
 153
 154         } else {
 155             // no movement
 156             if (duplicateCount[axis]) {
 157                 // increment duplicate count to max of 2
 158                 duplicateCount[axis] += (duplicateCount[axis] < 2) ? 1 : 0;
 159                 // second or subsequent duplicate, or duplicate when held at max stick or centre position.
 160                 // force feedforward to zero
 161                 setpointDelta[axis] = 0.0f;
 162                 // zero speed and acceleration for correct smoothing of next good packet
 163                 setpointSpeed = 0.0f;
 164                 prevSetpointSpeed[axis] = 0.0f;
 165                 prevAcceleration[axis] = 0.0f;
 166             } else {
 167                 // first duplicate; hold feedforward and previous static values, as if we just never got anything
 168                 duplicateCount[axis] = 1;
 169             }
 170         }
 171
 172
 173         if (axis == FD_ROLL) {
 174             DEBUG_SET(DEBUG_FEEDFORWARD, 1, lrintf(setpointSpeed * pidGetDT() * 100.0f)); // setpoint speed after smoothing
 175             DEBUG_SET(DEBUG_FEEDFORWARD, 2, lrintf(setpointAcceleration * pidGetDT() * 100.0f)); // boost amount after smoothing
 176             // debug 0 is interpolated setpoint, above
 177             // debug 3 is rcCommand delta, above
 178         }
 179
 180         prevSetpoint[axis] = setpoint;
 181
 182         // apply averaging, if enabled - include zero values in averaging
 183         if (feedforwardAveraging) {
 184             setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDelta[axis]);
 185         }
 186
 187         // apply feedforward transition
 188         setpointDelta[axis] *= feedforwardTransitionFactor > 0 ? MIN(1.0f, getRcDeflectionAbs(axis) * feedforwardTransitionFactor) : 1.0f;
 189
 190     }
 191     return setpointDelta[axis]; // the value used by the PID code
 192 }
 193
 194 FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint) {
 195     switch (axis) {
 196     case FD_ROLL:
 197         DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 0, value);
 198         break;
 199     case FD_PITCH:
 200         DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 1, value);
 201         break;
 202     }
 203
 204     if (value * currentPidSetpoint > 0.0f) {
 205         if (fabsf(currentPidSetpoint) <= feedforwardMaxRateLimit[axis]) {
 206             value = constrainf(value, (-feedforwardMaxRateLimit[axis] - currentPidSetpoint) * Kp, (feedforwardMaxRateLimit[axis] - currentPidSetpoint) * Kp);
 207         } else {
 208             value = 0;
 209         }
 210     }
 211
 212     if (axis == FD_ROLL) {
 213         DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 2, value);
 214     }
 215
 216     return value;
 217 }
 218
 219 bool shouldApplyFeedforwardLimits(int axis)
 220 {
 221     return axis < FD_YAW && feedforwardMaxRateLimit[axis] != 0.0f;
 222 }
 223 #endif