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 setpointDeltaImpl[XYZ_AXIS_COUNT];
  37 static float setpointDelta[XYZ_AXIS_COUNT];
  38
  39 typedef struct laggedMovingAverageCombined_s {
  40      laggedMovingAverage_t filter;
  41      float buf[4];
  42 } laggedMovingAverageCombined_t;
  43
  44 laggedMovingAverageCombined_t  setpointDeltaAvg[XYZ_AXIS_COUNT];
  45
  46 static float prevSetpoint[XYZ_AXIS_COUNT]; // equals raw unless interpolated
  47 static float prevSetpointSpeed[XYZ_AXIS_COUNT]; // equals raw unless interpolated
  48 static float prevAcceleration[XYZ_AXIS_COUNT]; // for accurate duplicate interpolation
  49 static float prevRcCommandDelta[XYZ_AXIS_COUNT]; // for accurate duplicate interpolation
  50
  51 static bool prevDuplicatePacket[XYZ_AXIS_COUNT]; // to identify multiple identical packets
  52 static uint8_t averagingCount;
  53
  54 static float ffMaxRateLimit[XYZ_AXIS_COUNT];
  55 static float ffMaxRate[XYZ_AXIS_COUNT];
  56
  57 void feedforwardInit(const pidProfile_t *pidProfile) {
  58     const float ffMaxRateScale = pidProfile->feedforward_max_rate_limit * 0.01f;
  59     averagingCount = pidProfile->feedforward_averaging + 1;
  60     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
  61         ffMaxRate[i] = applyCurve(i, 1.0f);
  62         ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale;
  63         laggedMovingAverageInit(&setpointDeltaAvg[i].filter, averagingCount, (float *)&setpointDeltaAvg[i].buf[0]);
  64     }
  65 }
  66
  67 FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging) {
  68
  69     if (newRcFrame) {
  70         float rcCommandDelta = getRcCommandDelta(axis);
  71         float setpoint = getRawSetpoint(axis);
  72         const float rxInterval = getCurrentRxRefreshRate() * 1e-6f;
  73         const float rxRate = 1.0f / rxInterval;
  74         float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
  75         float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
  76         float setpointAcceleration = 0.0f;
  77         const float ffSmoothFactor = pidGetFfSmoothFactor();
  78         const float ffJitterFactor = pidGetFfJitterFactor();
  79
  80         // calculate an attenuator from average of two most recent rcCommand deltas vs jitter threshold
  81         float ffAttenuator = 1.0f;
  82         if (ffJitterFactor) {
  83             if (rcCommandDelta < ffJitterFactor) {
  84                 ffAttenuator = MAX(1.0f - ((rcCommandDelta + prevRcCommandDelta[axis]) / 2.0f) / ffJitterFactor, 0.0f);
  85                 ffAttenuator = 1.0f - ffAttenuator * ffAttenuator;
  86             }
  87         }
  88
  89         // interpolate setpoint if necessary
  90         if (rcCommandDelta == 0.0f) {
  91             if (prevDuplicatePacket[axis] == false && fabsf(setpoint) < 0.98f * ffMaxRate[axis]) {
  92                 // first duplicate after movement
  93                 // interpolate rawSetpoint by adding (speed + acceleration) * attenuator to previous setpoint
  94                 setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis]) * ffAttenuator * rxInterval;
  95                 // recalculate setpointSpeed and (later) acceleration from this new setpoint value
  96                 setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
  97             }
  98             prevDuplicatePacket[axis] = true;
  99         } else {
 100             // movement!
 101             if (prevDuplicatePacket[axis] == true) {
 102                 // don't boost the packet after a duplicate, the feedforward alone is enough, usually
 103                 // in part because after a duplicate, the raw up-step is large, so the jitter attenuator is less active
 104                 ffAttenuator = 0.0f;
 105             }
 106             prevDuplicatePacket[axis] = false;
 107         }
 108         prevSetpoint[axis] = setpoint;
 109
 110         if (axis == FD_ROLL) {
 111             DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(setpoint)); // setpoint after interpolations
 112         }
 113
 114         float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention
 115
 116         // calculate acceleration, smooth and attenuate it
 117         setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
 118         setpointAcceleration = prevAcceleration[axis] + ffSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
 119         setpointAcceleration *= ffAttenuator;
 120
 121         // smooth setpointSpeed but don't attenuate
 122         setpointSpeed = prevSetpointSpeed[axis] + ffSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
 123
 124         prevSetpointSpeed[axis] = setpointSpeed;
 125         prevAcceleration[axis] = setpointAcceleration;
 126         prevRcCommandDelta[axis] = rcCommandDelta;
 127
 128         setpointAcceleration *= pidGetDT();
 129         setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
 130
 131         // calculate boost and prevent kick-back spike at max deflection
 132         const float ffBoostFactor = pidGetFfBoostFactor();
 133         float boostAmount = 0.0f;
 134         if (ffBoostFactor) {
 135             if (fabsf(setpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) {
 136                 boostAmount = ffBoostFactor * setpointAcceleration;
 137             }
 138         }
 139
 140         if (axis == FD_ROLL) {
 141             DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100.0f)); // base feedforward
 142             DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(boostAmount * 100.0f)); // boost amount
 143             // debug 2 is interpolated setpoint, above
 144             DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, lrintf(rcCommandDelta * 100.0f)); // rcCommand packet difference
 145         }
 146
 147         // add boost to base feedforward
 148         setpointDeltaImpl[axis] += boostAmount;
 149
 150         // apply averaging
 151         if (feedforwardAveraging) {
 152             setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDeltaImpl[axis]);
 153         } else {
 154             setpointDelta[axis] = setpointDeltaImpl[axis];
 155         }
 156     }
 157     return setpointDelta[axis];
 158 }
 159
 160 FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint) {
 161     switch (axis) {
 162     case FD_ROLL:
 163         DEBUG_SET(DEBUG_FF_LIMIT, 0, value);
 164
 165         break;
 166     case FD_PITCH:
 167         DEBUG_SET(DEBUG_FF_LIMIT, 1, value);
 168
 169         break;
 170     }
 171
 172     if (fabsf(currentPidSetpoint) <= ffMaxRateLimit[axis]) {
 173         value = constrainf(value, (-ffMaxRateLimit[axis] - currentPidSetpoint) * Kp, (ffMaxRateLimit[axis] - currentPidSetpoint) * Kp);
 174     } else {
 175         value = 0;
 176     }
 177
 178     if (axis == FD_ROLL) {
 179         DEBUG_SET(DEBUG_FF_LIMIT, 2, value);
 180     }
 181
 182     return value;
 183 }
 184
 185 bool shouldApplyFeedforwardLimits(int axis)
 186 {
 187     return ffMaxRateLimit[axis] != 0.0f && axis < FD_YAW;
 188 }
 189 #endif