src/main/fc/rc.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 <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24 #include <math.h>
  25
  26 #include "platform.h"
  27
  28 #include "build/debug.h"
  29
  30 #include "common/axis.h"
  31 #include "common/utils.h"
  32
  33 #include "config/config.h"
  34 #include "config/feature.h"
  35
  36 #include "fc/controlrate_profile.h"
  37 #include "fc/core.h"
  38 #include "fc/rc.h"
  39 #include "fc/rc_controls.h"
  40 #include "fc/rc_modes.h"
  41 #include "fc/runtime_config.h"
  42
  43 #include "flight/failsafe.h"
  44 #include "flight/imu.h"
  45 #include "flight/gps_rescue.h"
  46 #include "flight/pid.h"
  47 #include "flight/pid_init.h"
  48
  49 #include "pg/rx.h"
  50
  51 #include "rx/rx.h"
  52
  53 #include "sensors/battery.h"
  54 #include "sensors/gyro.h"
  55
  56 #include "rc.h"
  57
  58
  59 typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs);
  60 // note that rcCommand[] is an external float
  61
  62 static float rawSetpoint[XYZ_AXIS_COUNT];
  63
  64 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; // deflection range -1 to 1
  65 static float maxRcDeflectionAbs;
  66
  67 static bool reverseMotors = false;
  68 static applyRatesFn *applyRates;
  69
  70 static uint16_t currentRxIntervalUs; // packet interval in microseconds
  71 static float currentRxRateHz; // packet rate in hertz
  72
  73 static bool isRxDataNew = false;
  74 static bool isRxIntervalValid = false;
  75 static float rcCommandDivider = 500.0f;
  76 static float rcCommandYawDivider = 500.0f;
  77
  78 enum {
  79     ROLL_FLAG = 1 << ROLL,
  80     PITCH_FLAG = 1 << PITCH,
  81     YAW_FLAG = 1 << YAW,
  82     THROTTLE_FLAG = 1 << THROTTLE,
  83 };
  84
  85 #ifdef USE_FEEDFORWARD
  86 static float feedforwardSmoothed[3];
  87 static float feedforwardRaw[3];
  88 typedef struct laggedMovingAverageCombined_s {
  89     laggedMovingAverage_t filter;
  90     float buf[4];
  91 } laggedMovingAverageCombined_t;
  92 laggedMovingAverageCombined_t  feedforwardDeltaAvg[XYZ_AXIS_COUNT];
  93
  94 float getFeedforward(int axis)
  95 {
  96 #ifdef USE_RC_SMOOTHING_FILTER
  97     return feedforwardSmoothed[axis];
  98 #else
  99     return feedforwardRaw[axis];
 100 #endif
 101 }
 102 #endif // USE_FEEDFORWARD
 103
 104 #ifdef USE_RC_SMOOTHING_FILTER
 105 static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
 106 static float rcDeflectionSmoothed[3];
 107 #endif // USE_RC_SMOOTHING_FILTER
 108
 109 #define RX_INTERVAL_MIN_US     950 // 0.950ms to fit 1kHz without an issue
 110 #define RX_INTERVAL_MAX_US   65500 // 65.5ms or 15.26hz
 111
 112 float getSetpointRate(int axis)
 113 {
 114 #ifdef USE_RC_SMOOTHING_FILTER
 115     return setpointRate[axis];
 116 #else
 117     return rawSetpoint[axis];
 118 #endif
 119 }
 120
 121 static float maxRcRate[3];
 122 float getMaxRcRate(int axis)
 123 {
 124     return maxRcRate[axis];
 125 }
 126
 127 float getRcDeflection(int axis)
 128 {
 129 #ifdef USE_RC_SMOOTHING_FILTER
 130     return rcDeflectionSmoothed[axis];
 131 #else
 132     return rcDeflection[axis];
 133 #endif
 134 }
 135
 136 float getRcDeflectionRaw(int axis)
 137 {
 138     return rcDeflection[axis];
 139 }
 140
 141 float getRcDeflectionAbs(int axis)
 142 {
 143     return rcDeflectionAbs[axis];
 144 }
 145
 146 float getMaxRcDeflectionAbs(void)
 147 {
 148     return maxRcDeflectionAbs;
 149 }
 150
 151 #define THROTTLE_LOOKUP_LENGTH 12
 152 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH];    // lookup table for expo & mid THROTTLE
 153
 154 static int16_t rcLookupThrottle(int32_t tmp)
 155 {
 156     const int32_t tmp2 = tmp / 100;
 157     // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
 158     return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;
 159 }
 160
 161 #define SETPOINT_RATE_LIMIT_MIN -1998.0f
 162 #define SETPOINT_RATE_LIMIT_MAX 1998.0f
 163 STATIC_ASSERT(CONTROL_RATE_CONFIG_RATE_LIMIT_MAX <= (uint16_t)SETPOINT_RATE_LIMIT_MAX, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX_too_large);
 164
 165 #define RC_RATE_INCREMENTAL 14.54f
 166
 167 float applyBetaflightRates(const int axis, float rcCommandf, const float rcCommandfAbs)
 168 {
 169     if (currentControlRateProfile->rcExpo[axis]) {
 170         const float expof = currentControlRateProfile->rcExpo[axis] / 100.0f;
 171         rcCommandf = rcCommandf * power3(rcCommandfAbs) * expof + rcCommandf * (1 - expof);
 172     }
 173
 174     float rcRate = currentControlRateProfile->rcRates[axis] / 100.0f;
 175     if (rcRate > 2.0f) {
 176         rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f);
 177     }
 178     float angleRate = 200.0f * rcRate * rcCommandf;
 179     if (currentControlRateProfile->rates[axis]) {
 180         const float rcSuperfactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
 181         angleRate *= rcSuperfactor;
 182     }
 183
 184     return angleRate;
 185 }
 186
 187 float applyRaceFlightRates(const int axis, float rcCommandf, const float rcCommandfAbs)
 188 {
 189     // -1.0 to 1.0 ranged and curved
 190     rcCommandf = ((1.0f + 0.01f * currentControlRateProfile->rcExpo[axis] * (rcCommandf * rcCommandf - 1.0f)) * rcCommandf);
 191     // convert to -2000 to 2000 range using acro+ modifier
 192     float angleRate = 10.0f * currentControlRateProfile->rcRates[axis] * rcCommandf;
 193     angleRate = angleRate * (1 + rcCommandfAbs * (float)currentControlRateProfile->rates[axis] * 0.01f);
 194
 195     return angleRate;
 196 }
 197
 198 float applyKissRates(const int axis, float rcCommandf, const float rcCommandfAbs)
 199 {
 200     const float rcCurvef = currentControlRateProfile->rcExpo[axis] / 100.0f;
 201
 202     float kissRpyUseRates = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
 203     float kissRcCommandf = (power3(rcCommandf) * rcCurvef + rcCommandf * (1 - rcCurvef)) * (currentControlRateProfile->rcRates[axis] / 1000.0f);
 204     float kissAngle = constrainf(((2000.0f * kissRpyUseRates) * kissRcCommandf), SETPOINT_RATE_LIMIT_MIN, SETPOINT_RATE_LIMIT_MAX);
 205
 206     return kissAngle;
 207 }
 208
 209 float applyActualRates(const int axis, float rcCommandf, const float rcCommandfAbs)
 210 {
 211     float expof = currentControlRateProfile->rcExpo[axis] / 100.0f;
 212     expof = rcCommandfAbs * (power5(rcCommandf) * expof + rcCommandf * (1 - expof));
 213
 214     const float centerSensitivity = currentControlRateProfile->rcRates[axis] * 10.0f;
 215     const float stickMovement = MAX(0, currentControlRateProfile->rates[axis] * 10.0f - centerSensitivity);
 216     const float angleRate = rcCommandf * centerSensitivity + stickMovement * expof;
 217
 218     return angleRate;
 219 }
 220
 221 float applyQuickRates(const int axis, float rcCommandf, const float rcCommandfAbs)
 222 {
 223     const uint16_t rcRate = currentControlRateProfile->rcRates[axis] * 2;
 224     const uint16_t maxDPS = MAX(currentControlRateProfile->rates[axis] * 10, rcRate);
 225     const float expof = currentControlRateProfile->rcExpo[axis] / 100.0f;
 226     const float superFactorConfig = ((float)maxDPS / rcRate - 1) / ((float)maxDPS / rcRate);
 227
 228     float curve;
 229     float superFactor;
 230     float angleRate;
 231
 232     if (currentControlRateProfile->quickRatesRcExpo) {
 233         curve = power3(rcCommandf) * expof + rcCommandf * (1 - expof);
 234         superFactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * superFactorConfig), 0.01f, 1.00f));
 235         angleRate = constrainf(curve * rcRate * superFactor, SETPOINT_RATE_LIMIT_MIN, SETPOINT_RATE_LIMIT_MAX);
 236     } else {
 237         curve = power3(rcCommandfAbs) * expof + rcCommandfAbs * (1 - expof);
 238         superFactor = 1.0f / (constrainf(1.0f - (curve * superFactorConfig), 0.01f, 1.00f));
 239         angleRate = constrainf(rcCommandf * rcRate * superFactor, SETPOINT_RATE_LIMIT_MIN, SETPOINT_RATE_LIMIT_MAX);
 240     }
 241
 242     return angleRate;
 243 }
 244
 245 static void scaleRawSetpointToFpvCamAngle(void)
 246 {
 247     //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
 248     static uint8_t lastFpvCamAngleDegrees = 0;
 249     static float cosFactor = 1.0;
 250     static float sinFactor = 0.0;
 251
 252     if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees) {
 253         lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
 254         cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD);
 255         sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
 256     }
 257
 258     float roll = rawSetpoint[ROLL];
 259     float yaw = rawSetpoint[YAW];
 260     rawSetpoint[ROLL] = constrainf(roll * cosFactor -  yaw * sinFactor, SETPOINT_RATE_LIMIT_MIN, SETPOINT_RATE_LIMIT_MAX);
 261     rawSetpoint[YAW]  = constrainf(yaw  * cosFactor + roll * sinFactor, SETPOINT_RATE_LIMIT_MIN, SETPOINT_RATE_LIMIT_MAX);
 262 }
 263
 264 void updateRcRefreshRate(timeUs_t currentTimeUs)
 265 {
 266     static timeUs_t lastRxTimeUs;
 267
 268     timeDelta_t frameAgeUs;
 269     timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs);
 270
 271     if (!frameDeltaUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
 272         frameDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs);
 273     }
 274
 275     DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX));
 276     DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX));
 277
 278     lastRxTimeUs = currentTimeUs;
 279     currentRxIntervalUs = constrain(frameDeltaUs, RX_INTERVAL_MIN_US, RX_INTERVAL_MAX_US);
 280     isRxIntervalValid = frameDeltaUs == currentRxIntervalUs;
 281
 282     currentRxRateHz = 1e6f / currentRxIntervalUs; // cannot be zero due to preceding constraint
 283     DEBUG_SET(DEBUG_RX_TIMING, 2, isRxIntervalValid);
 284     DEBUG_SET(DEBUG_RX_TIMING, 3, MIN(currentRxIntervalUs / 10, INT16_MAX));
 285 }
 286
 287 uint16_t getCurrentRxIntervalUs(void)
 288 {
 289     return currentRxIntervalUs;
 290 }
 291
 292 #ifdef USE_RC_SMOOTHING_FILTER
 293
 294 // Initialize or update the filters base on either the manually selected cutoff, or
 295 // the auto-calculated cutoff frequency based on detected rx frame rate.
 296 FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
 297 {
 298     // in auto mode, calculate the RC smoothing cutoff from the smoothed Rx link frequency
 299     const uint16_t oldSetpointCutoff = smoothingData->setpointCutoffFrequency;
 300     const uint16_t oldFeedforwardCutoff = smoothingData->feedforwardCutoffFrequency;
 301     const uint16_t minCutoffHz = 15; // don't let any RC smoothing filter cutoff go below 15Hz
 302     if (smoothingData->setpointCutoffSetting == 0) {
 303         smoothingData->setpointCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint));
 304     }
 305     if (smoothingData->throttleCutoffSetting == 0) {
 306         smoothingData->throttleCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorThrottle));
 307     }
 308
 309     if (smoothingData->feedforwardCutoffSetting == 0) {
 310         smoothingData->feedforwardCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorFeedforward));
 311     }
 312
 313     const float dT = targetPidLooptime * 1e-6f;
 314     if ((smoothingData->setpointCutoffFrequency != oldSetpointCutoff) || !smoothingData->filterInitialized) {
 315         // note that cutoff frequencies are integers, filter cutoffs won't re-calculate until there is > 1hz variation from previous cutoff
 316         // initialize or update the setpoint cutoff based filters
 317         const float setpointCutoffFrequency = smoothingData->setpointCutoffFrequency;
 318         for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
 319             if (i < THROTTLE) {
 320                 if (!smoothingData->filterInitialized) {
 321                     pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT));
 322                 } else {
 323                     pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT));
 324                 }
 325             } else {
 326                 const float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency;
 327                 if (!smoothingData->filterInitialized) {
 328                     pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(throttleCutoffFrequency, dT));
 329                 } else {
 330                     pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3FilterGain(throttleCutoffFrequency, dT));
 331                 }
 332             }
 333         }
 334         // initialize or update the RC Deflection filter
 335         for (int i = FD_ROLL; i < FD_YAW; i++) {
 336             if (!smoothingData->filterInitialized) {
 337                 pt3FilterInit(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT));
 338             } else {
 339                 pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT));
 340             }
 341         }
 342     }
 343     // initialize or update the Feedforward filter
 344     if ((smoothingData->feedforwardCutoffFrequency != oldFeedforwardCutoff) || !smoothingData->filterInitialized) {
 345        for (int i = FD_ROLL; i <= FD_YAW; i++) {
 346             const float feedforwardCutoffFrequency = smoothingData->feedforwardCutoffFrequency;
 347             if (!smoothingData->filterInitialized) {
 348                 pt3FilterInit(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT));
 349             } else {
 350                 pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT));
 351             }
 352         }
 353     }
 354
 355     DEBUG_SET(DEBUG_RC_SMOOTHING, 1, smoothingData->setpointCutoffFrequency);
 356     DEBUG_SET(DEBUG_RC_SMOOTHING, 2, smoothingData->feedforwardCutoffFrequency);
 357 }
 358
 359 // Determine if we need to caclulate filter cutoffs. If not then we can avoid
 360 // examining the rx frame times completely
 361 FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
 362 {
 363     // if any rc smoothing cutoff is 0 (auto) then we need to calculate cutoffs
 364     if ((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.feedforwardCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) {
 365         return true;
 366     }
 367     return false;
 368 }
 369
 370 static FAST_CODE void processRcSmoothingFilter(void)
 371 {
 372     static FAST_DATA_ZERO_INIT float rxDataToSmooth[4];
 373     static FAST_DATA_ZERO_INIT bool initialized;
 374     static FAST_DATA_ZERO_INIT bool calculateCutoffs;
 375
 376     // first call initialization
 377     if (!initialized) {
 378         initialized = true;
 379         rcSmoothingData.filterInitialized = false;
 380         rcSmoothingData.smoothedRxRateHz = 0.0f;
 381         rcSmoothingData.sampleCount = 0;
 382         rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
 383
 384         rcSmoothingData.autoSmoothnessFactorSetpoint = 1.5f / (1.0f + (rxConfig()->rc_smoothing_auto_factor_rpy / 10.0f));
 385         rcSmoothingData.autoSmoothnessFactorFeedforward = 1.5f / (1.0f + (rxConfig()->rc_smoothing_auto_factor_rpy / 10.0f));
 386         rcSmoothingData.autoSmoothnessFactorThrottle = 1.5f / (1.0f + (rxConfig()->rc_smoothing_auto_factor_throttle / 10.0f));
 387
 388         rcSmoothingData.setpointCutoffSetting = rxConfig()->rc_smoothing_setpoint_cutoff;
 389         rcSmoothingData.throttleCutoffSetting = rxConfig()->rc_smoothing_throttle_cutoff;
 390         rcSmoothingData.feedforwardCutoffSetting = rxConfig()->rc_smoothing_feedforward_cutoff;
 391
 392         rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting;
 393         rcSmoothingData.feedforwardCutoffFrequency = rcSmoothingData.feedforwardCutoffSetting;
 394         rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting;
 395
 396         if (rxConfig()->rc_smoothing_mode) {
 397             calculateCutoffs = rcSmoothingAutoCalculate();
 398             // if we don't need to calculate cutoffs dynamically then the filters can be initialized now
 399             if (!calculateCutoffs) {
 400                 rcSmoothingSetFilterCutoffs(&rcSmoothingData);
 401                 rcSmoothingData.filterInitialized = true;
 402             }
 403         }
 404     }
 405
 406     if (isRxDataNew) {
 407         if (calculateCutoffs) {
 408             // for auto calculated filters, calculate the link interval and update the RC smoothing filters at regular intervals
 409             // this is more efficient than monitoring for significant changes and making comparisons to decide whether to update the filter
 410             const timeMs_t currentTimeMs = millis();
 411             int sampleState = 0;
 412             const bool ready = (currentTimeMs > 1000) && (targetPidLooptime > 0);
 413             if (ready) { // skip during FC initialization
 414                 // Wait 1000ms after power to let the PID loop stabilize before starting average frame rate calculation
 415                 if (rxIsReceivingSignal() && isRxIntervalValid) {
 416
 417                     static uint16_t previousRxIntervalUs;
 418                     if (abs(currentRxIntervalUs - previousRxIntervalUs) < (previousRxIntervalUs - (previousRxIntervalUs / 8))) {
 419                         // exclude large steps, eg after dropouts or telemetry
 420                         // by using interval here, we catch a dropout/telemetry where the inteval increases by 100%, but accept
 421                         // the return to normal value, which is only 50% different from the 100% interval of a single drop, and 66% of a return after a double drop.
 422                         static float prevRxRateHz;
 423                         // smooth the current Rx link frequency estimates
 424                         const float kF = 0.1f; // first order lowpass smoothing filter coefficient
 425                         const float smoothedRxRateHz = prevRxRateHz + kF * (currentRxRateHz - prevRxRateHz);
 426                         prevRxRateHz = smoothedRxRateHz;
 427
 428                         // recalculate cutoffs every 3 acceptable samples
 429                         if (rcSmoothingData.sampleCount) {
 430                             rcSmoothingData.sampleCount --;
 431                             sampleState = 1;
 432                         } else {
 433                             rcSmoothingData.smoothedRxRateHz = smoothedRxRateHz;
 434                             rcSmoothingSetFilterCutoffs(&rcSmoothingData);
 435                             rcSmoothingData.filterInitialized = true;
 436                             rcSmoothingData.sampleCount = 3;
 437                             sampleState = 2;
 438                         }
 439                     }
 440                     previousRxIntervalUs = currentRxIntervalUs;
 441                 } else {
 442                     // either we stopped receiving rx samples (failsafe?) or the sample interval is unreasonable
 443                     // require a full re-evaluation period after signal is restored
 444                     rcSmoothingData.sampleCount = 0;
 445                     sampleState = 4;
 446                 }
 447             }
 448             DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxIntervalUs / 10);
 449             DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.sampleCount);
 450             DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.smoothedRxRateHz); // value used by filters
 451             DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); // guard time = 1, guard time expired = 2
 452         }
 453         // Get new values to be smoothed
 454         for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
 455             rxDataToSmooth[i] = i == THROTTLE ? rcCommand[i] : rawSetpoint[i];
 456             if (i < THROTTLE) {
 457                 DEBUG_SET(DEBUG_RC_INTERPOLATION, i, lrintf(rxDataToSmooth[i]));
 458             } else {
 459                 DEBUG_SET(DEBUG_RC_INTERPOLATION, i, ((lrintf(rxDataToSmooth[i])) - 1000));
 460             }
 461         }
 462     }
 463
 464     DEBUG_SET(DEBUG_RC_SMOOTHING, 0, rcSmoothingData.smoothedRxRateHz);
 465     DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.sampleCount);
 466
 467     // each pid loop, apply the last received channel value to the filter, if initialised - thanks @klutvott
 468     for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
 469         float *dst = i == THROTTLE ? &rcCommand[i] : &setpointRate[i];
 470         if (rcSmoothingData.filterInitialized) {
 471             *dst = pt3FilterApply(&rcSmoothingData.filterSetpoint[i], rxDataToSmooth[i]);
 472         } else {
 473             // If filter isn't initialized yet, as in smoothing off, use the actual unsmoothed rx channel data
 474             *dst = rxDataToSmooth[i];
 475         }
 476     }
 477
 478     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 479         // Feedforward smoothing
 480         feedforwardSmoothed[axis] = pt3FilterApply(&rcSmoothingData.filterFeedforward[axis], feedforwardRaw[axis]);
 481         // Horizon mode smoothing of rcDeflection on pitch and roll to provide a smooth angle element
 482         const bool smoothRcDeflection = FLIGHT_MODE(HORIZON_MODE) && rcSmoothingData.filterInitialized;
 483         if (smoothRcDeflection && axis < FD_YAW) {
 484             rcDeflectionSmoothed[axis] = pt3FilterApply(&rcSmoothingData.filterRcDeflection[axis], rcDeflection[axis]);
 485         } else {
 486             rcDeflectionSmoothed[axis] = rcDeflection[axis];
 487         }
 488     }
 489 }
 490 #endif // USE_RC_SMOOTHING_FILTER
 491
 492 NOINLINE void initAveraging(uint16_t feedforwardAveraging)
 493 {
 494     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 495         laggedMovingAverageInit(&feedforwardDeltaAvg[i].filter, feedforwardAveraging + 1, (float *)&feedforwardDeltaAvg[i].buf[0]);
 496     }
 497 }
 498
 499 FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, int axis)
 500 {
 501     const float rxInterval = currentRxIntervalUs * 1e-6f; // seconds
 502     float rxRate = currentRxRateHz;
 503     static float prevRxInterval;
 504
 505     static float prevRcCommand[3];
 506     static float prevRcCommandDeltaAbs[3];          // for duplicate interpolation
 507     static float prevSetpoint[3];                   // equals raw unless interpolated
 508     static float prevSetpointSpeed[3];
 509     static float prevAcceleration[3];               // for duplicate interpolation
 510     static bool prevDuplicatePacket[3];             // to identify multiple identical packets
 511     static uint16_t feedforwardAveraging = 0;
 512
 513     if (feedforwardAveraging != pid->feedforwardAveraging) {
 514         feedforwardAveraging = pid->feedforwardAveraging;
 515         initAveraging(feedforwardAveraging);
 516     }
 517
 518     const float rcCommandDeltaAbs = fabsf(rcCommand[axis] - prevRcCommand[axis]);
 519     prevRcCommand[axis] = rcCommand[axis];
 520
 521     float setpoint = rawSetpoint[axis];
 522     float setpointSpeed = (setpoint - prevSetpoint[axis]);
 523     prevSetpoint[axis] = setpoint;
 524     float setpointAcceleration = 0.0f;
 525
 526
 527     if (axis == FD_ROLL) {
 528         DEBUG_SET(DEBUG_FEEDFORWARD, 0, lrintf(setpoint * 10.0f)); // un-smoothed final feedforward
 529     }
 530
 531     // attenuators
 532     float zeroTheAcceleration = 1.0f;
 533     float jitterAttenuator = 1.0f;
 534     if (pid->feedforwardJitterFactor) {
 535         if (rcCommandDeltaAbs < pid->feedforwardJitterFactor) {
 536             jitterAttenuator = MAX(1.0f - (rcCommandDeltaAbs + prevRcCommandDeltaAbs[axis]) * pid->feedforwardJitterFactorInv, 0.0f);
 537             // note that feedforwardJitterFactorInv includes a divide by 2 to average the two previous rcCommandDeltaAbs values
 538             jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator;
 539         }
 540     }
 541     prevRcCommandDeltaAbs[axis] = rcCommandDeltaAbs;
 542
 543     // interpolate setpoint if necessary
 544     if (rcCommandDeltaAbs) {
 545         // movement!
 546         if (prevDuplicatePacket[axis] == true) {
 547             rxRate = 1.0f / (rxInterval + prevRxInterval);
 548             zeroTheAcceleration = 0.0f;
 549             // don't add acceleration, empirically seems better on FrSky
 550         }
 551         setpointSpeed *= rxRate;
 552         prevDuplicatePacket[axis] = false;
 553     } else {
 554         // no movement!
 555         if (prevDuplicatePacket[axis] == false) {
 556             // first duplicate after movement
 557             setpointSpeed = prevSetpointSpeed[axis];
 558             if (fabsf(setpoint) < 0.95f * maxRcRate[axis]) {
 559                 setpointSpeed += prevAcceleration[axis];
 560             }
 561             zeroTheAcceleration = 0.0f; // force acceleration to zero
 562         } else {
 563             // second and subsequent duplicates after movement should be zeroed
 564             setpointSpeed = 0.0f;
 565             prevSetpointSpeed[axis] = 0.0f;
 566             zeroTheAcceleration = 0.0f; // force acceleration to zero
 567         }
 568         prevDuplicatePacket[axis] = true;
 569     }
 570     prevRxInterval = rxInterval;
 571
 572     // smooth the setpointSpeed value
 573     setpointSpeed = prevSetpointSpeed[axis] + pid->feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
 574
 575     // calculate acceleration and attenuate
 576     setpointAcceleration = (setpointSpeed - prevSetpointSpeed[axis]) * rxRate * 0.01f;
 577     prevSetpointSpeed[axis] = setpointSpeed;
 578
 579     // smooth the acceleration element (effectively a second order filter) and apply jitter reduction
 580     setpointAcceleration = prevAcceleration[axis] + pid->feedforwardSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
 581     prevAcceleration[axis] = setpointAcceleration * zeroTheAcceleration;
 582     setpointAcceleration = setpointAcceleration * pid->feedforwardBoostFactor * jitterAttenuator * zeroTheAcceleration;
 583
 584     if (axis == FD_ROLL) {
 585         DEBUG_SET(DEBUG_FEEDFORWARD, 1, lrintf(setpointSpeed * 0.1f)); // base feedforward without acceleration
 586     }
 587
 588     float feedforward = setpointSpeed + setpointAcceleration;
 589
 590     if (axis == FD_ROLL) {
 591         DEBUG_SET(DEBUG_FEEDFORWARD, 2, lrintf(feedforward * 0.1f));
 592         // un-smoothed feedforward including acceleration but before limiting, transition, averaging, and jitter reduction
 593     }
 594
 595     // apply feedforward transition
 596     const bool useTransition = (pid->feedforwardTransition != 0.0f) && (rcDeflectionAbs[axis] < pid->feedforwardTransition);
 597     if (useTransition) {
 598         feedforward *= rcDeflectionAbs[axis] * pid->feedforwardTransitionInv;
 599     }
 600
 601     // apply averaging
 602     if (feedforwardAveraging) {
 603         feedforward = laggedMovingAverageUpdate(&feedforwardDeltaAvg[axis].filter, feedforward);
 604     }
 605
 606     // apply jitter reduction
 607      feedforward *= jitterAttenuator;
 608
 609     // apply max rate limiting
 610     if (pid->feedforwardMaxRateLimit && axis < FD_YAW) {
 611         if (feedforward * setpoint > 0.0f) { // in same direction
 612             const float limit = (maxRcRate[axis] - fabsf(setpoint)) * pid->feedforwardMaxRateLimit;
 613             feedforward = (limit > 0.0f) ? constrainf(feedforward, -limit, limit) : 0.0f;
 614         }
 615     }
 616
 617     feedforwardRaw[axis] = feedforward;
 618
 619     if (axis == FD_ROLL) {
 620         DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(feedforwardRaw[axis] * 0.1f)); // un-smoothed final feedforward
 621         DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 0, lrintf(jitterAttenuator * 100.0f)); // un-smoothed final feedforward
 622         DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 1, lrintf(maxRcRate[axis])); // un-smoothed final feedforward
 623         DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 2, lrintf(setpoint)); // un-smoothed final feedforward
 624         DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 3, lrintf(feedforwardRaw[axis])); // un-smoothed final feedforward
 625     }
 626 }
 627
 628 FAST_CODE void processRcCommand(void)
 629 {
 630     if (isRxDataNew) {
 631         maxRcDeflectionAbs = 0.0f;
 632         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 633
 634             float angleRate;
 635
 636 #ifdef USE_GPS_RESCUE
 637             if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
 638                 // If GPS Rescue is active then override the setpointRate used in the
 639                 // pid controller with the value calculated from the desired heading logic.
 640                 angleRate = gpsRescueGetYawRate();
 641                 // Treat the stick input as centered to avoid any stick deflection base modifications (like acceleration limit)
 642                 rcDeflection[axis] = 0;
 643                 rcDeflectionAbs[axis] = 0;
 644             } else
 645 #endif
 646             {
 647                 // scale rcCommandf to range [-1.0, 1.0]
 648                 float rcCommandf;
 649                 if (axis == FD_YAW) {
 650                     rcCommandf = rcCommand[axis] / rcCommandYawDivider;
 651                 } else {
 652                     rcCommandf = rcCommand[axis] / rcCommandDivider;
 653                 }
 654
 655                 rcDeflection[axis] = rcCommandf;
 656                 const float rcCommandfAbs = fabsf(rcCommandf);
 657                 rcDeflectionAbs[axis] = rcCommandfAbs;
 658                 maxRcDeflectionAbs = fmaxf(maxRcDeflectionAbs, rcCommandfAbs);
 659
 660                 angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
 661             }
 662
 663             rawSetpoint[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]);
 664             DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
 665
 666 #ifdef USE_FEEDFORWARD
 667         calculateFeedforward(&pidRuntime, axis);
 668 #endif // USE_FEEDFORWARD
 669
 670         }
 671         // adjust unfiltered setpoint steps to camera angle (mixing Roll and Yaw)
 672         if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
 673             scaleRawSetpointToFpvCamAngle();
 674         }
 675     }
 676
 677 #ifdef USE_RC_SMOOTHING_FILTER
 678     processRcSmoothingFilter();
 679 #endif
 680
 681     isRxDataNew = false;
 682 }
 683
 684 FAST_CODE_NOINLINE void updateRcCommands(void)
 685 {
 686     isRxDataNew = true;
 687
 688     for (int axis = 0; axis < 3; axis++) {
 689         // non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
 690
 691         float tmp = MIN(fabsf(rcData[axis] - rxConfig()->midrc), 500.0f);
 692         if (axis == ROLL || axis == PITCH) {
 693             if (tmp > rcControlsConfig()->deadband) {
 694                 tmp -= rcControlsConfig()->deadband;
 695             } else {
 696                 tmp = 0;
 697             }
 698             rcCommand[axis] = tmp;
 699         } else {
 700             if (tmp > rcControlsConfig()->yaw_deadband) {
 701                 tmp -= rcControlsConfig()->yaw_deadband;
 702             } else {
 703                 tmp = 0;
 704             }
 705             rcCommand[axis] = tmp * -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
 706         }
 707         if (rcData[axis] < rxConfig()->midrc) {
 708             rcCommand[axis] = -rcCommand[axis];
 709         }
 710     }
 711
 712     int32_t tmp;
 713     if (featureIsEnabled(FEATURE_3D)) {
 714         tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
 715         tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
 716     } else {
 717         tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
 718         tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);
 719     }
 720
 721     if (getLowVoltageCutoff()->enabled) {
 722         tmp = tmp * getLowVoltageCutoff()->percentage / 100;
 723     }
 724
 725     rcCommand[THROTTLE] = rcLookupThrottle(tmp);
 726
 727     if (featureIsEnabled(FEATURE_3D) && !failsafeIsActive()) {
 728         if (!flight3DConfig()->switched_mode3d) {
 729             if (IS_RC_MODE_ACTIVE(BOX3D)) {
 730                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 731                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
 732             }
 733         } else {
 734             if (IS_RC_MODE_ACTIVE(BOX3D)) {
 735                 reverseMotors = true;
 736                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 737                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MIN - rxConfig()->midrc);
 738             } else {
 739                 reverseMotors = false;
 740                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 741                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
 742             }
 743         }
 744     }
 745     if (FLIGHT_MODE(HEADFREE_MODE)) {
 746         static t_fp_vector_def  rcCommandBuff;
 747
 748         rcCommandBuff.X = rcCommand[ROLL];
 749         rcCommandBuff.Y = rcCommand[PITCH];
 750         if ((!FLIGHT_MODE(ANGLE_MODE) && (!FLIGHT_MODE(HORIZON_MODE)) && (!FLIGHT_MODE(GPS_RESCUE_MODE)))) {
 751             rcCommandBuff.Z = rcCommand[YAW];
 752         } else {
 753             rcCommandBuff.Z = 0;
 754         }
 755         imuQuaternionHeadfreeTransformVectorEarthToBody(&rcCommandBuff);
 756         rcCommand[ROLL] = rcCommandBuff.X;
 757         rcCommand[PITCH] = rcCommandBuff.Y;
 758         if ((!FLIGHT_MODE(ANGLE_MODE)&&(!FLIGHT_MODE(HORIZON_MODE)) && (!FLIGHT_MODE(GPS_RESCUE_MODE)))) {
 759             rcCommand[YAW] = rcCommandBuff.Z;
 760         }
 761     }
 762 }
 763
 764 void resetYawAxis(void)
 765 {
 766     rcCommand[YAW] = 0;
 767     setpointRate[YAW] = 0;
 768 }
 769
 770 bool isMotorsReversed(void)
 771 {
 772     return reverseMotors;
 773 }
 774
 775 void initRcProcessing(void)
 776 {
 777     rcCommandDivider = 500.0f - rcControlsConfig()->deadband;
 778     rcCommandYawDivider = 500.0f - rcControlsConfig()->yaw_deadband;
 779
 780     for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
 781         const int16_t tmp = 10 * i - currentControlRateProfile->thrMid8;
 782         uint8_t y = 1;
 783         if (tmp > 0)
 784             y = 100 - currentControlRateProfile->thrMid8;
 785         if (tmp < 0)
 786             y = currentControlRateProfile->thrMid8;
 787         lookupThrottleRC[i] = 10 * currentControlRateProfile->thrMid8 + tmp * (100 - currentControlRateProfile->thrExpo8 + (int32_t) currentControlRateProfile->thrExpo8 * (tmp * tmp) / (y * y)) / 10;
 788         lookupThrottleRC[i] = PWM_RANGE_MIN + PWM_RANGE * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
 789     }
 790
 791     switch (currentControlRateProfile->rates_type) {
 792     case RATES_TYPE_BETAFLIGHT:
 793     default:
 794         applyRates = applyBetaflightRates;
 795         break;
 796     case RATES_TYPE_RACEFLIGHT:
 797         applyRates = applyRaceFlightRates;
 798         break;
 799     case RATES_TYPE_KISS:
 800         applyRates = applyKissRates;
 801         break;
 802     case RATES_TYPE_ACTUAL:
 803         applyRates = applyActualRates;
 804         break;
 805     case RATES_TYPE_QUICK:
 806         applyRates = applyQuickRates;
 807         break;
 808     }
 809
 810     for (int i = 0; i < 3; i++) {
 811         maxRcRate[i] = applyRates(i, 1.0f, 1.0f);
 812 #ifdef USE_FEEDFORWARD
 813         feedforwardSmoothed[i] = 0.0f;
 814         feedforwardRaw[i] = 0.0f;
 815 #endif // USE_FEEDFORWARD
 816     }
 817
 818 #ifdef USE_YAW_SPIN_RECOVERY
 819     const int maxYawRate = (int)maxRcRate[FD_YAW];
 820     initYawSpinRecovery(maxYawRate);
 821 #endif
 822 }
 823
 824 // send rc smoothing details to blackbox
 825 #ifdef USE_RC_SMOOTHING_FILTER
 826 rcSmoothingFilter_t *getRcSmoothingData(void)
 827 {
 828     return &rcSmoothingData;
 829 }
 830
 831 bool rcSmoothingInitializationComplete(void)
 832 {
 833     return rcSmoothingData.filterInitialized;
 834 }
 835 #endif // USE_RC_SMOOTHING_FILTER