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