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 <math.h>
  24
  25 #include "platform.h"
  26
  27 #include "build/debug.h"
  28
  29 #include "common/axis.h"
  30 #include "common/utils.h"
  31
  32 #include "config/config.h"
  33 #include "config/feature.h"
  34
  35 #include "fc/controlrate_profile.h"
  36 #include "fc/core.h"
  37 #include "fc/rc.h"
  38 #include "fc/rc_controls.h"
  39 #include "fc/rc_modes.h"
  40 #include "fc/runtime_config.h"
  41
  42 #include "flight/failsafe.h"
  43 #include "flight/imu.h"
  44 #include "flight/feedforward.h"
  45 #include "flight/gps_rescue.h"
  46 #include "flight/pid_init.h"
  47
  48 #include "pg/rx.h"
  49
  50 #include "rx/rx.h"
  51
  52 #include "sensors/battery.h"
  53 #include "sensors/gyro.h"
  54
  55 #include "rc.h"
  56
  57
  58 typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs);
  59
  60 #ifdef USE_FEEDFORWARD
  61 static float oldRcCommand[XYZ_AXIS_COUNT];
  62 static bool isDuplicate[XYZ_AXIS_COUNT];
  63 float rcCommandDelta[XYZ_AXIS_COUNT];
  64 #endif
  65 static float rawSetpoint[XYZ_AXIS_COUNT];
  66 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
  67 static float throttlePIDAttenuation;
  68 static bool reverseMotors = false;
  69 static applyRatesFn *applyRates;
  70 static uint16_t currentRxRefreshRate;
  71 static bool isRxDataNew = false;
  72 static float rcCommandDivider = 500.0f;
  73 static float rcCommandYawDivider = 500.0f;
  74
  75 static FAST_DATA_ZERO_INIT bool newRxDataForFF;
  76
  77 enum {
  78     ROLL_FLAG = 1 << ROLL,
  79     PITCH_FLAG = 1 << PITCH,
  80     YAW_FLAG = 1 << YAW,
  81     THROTTLE_FLAG = 1 << THROTTLE,
  82 };
  83
  84 #ifdef USE_RC_SMOOTHING_FILTER
  85 #define RC_SMOOTHING_FILTER_STARTUP_DELAY_MS    5000  // Time to wait after power to let the PID loop stabilize before starting average frame rate calculation
  86 #define RC_SMOOTHING_FILTER_TRAINING_SAMPLES    50    // Number of rx frame rate samples to average during initial training
  87 #define RC_SMOOTHING_FILTER_RETRAINING_SAMPLES  20    // Number of rx frame rate samples to average during frame rate changes
  88 #define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS   1000  // Additional time to wait after receiving first valid rx frame before initial training starts
  89 #define RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS 2000  // Guard time to wait after retraining to prevent retraining again too quickly
  90 #define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT     20    // Look for samples varying this much from the current detected frame rate to initiate retraining
  91 #define RC_SMOOTHING_RX_RATE_MIN_US             1000  // 1ms
  92 #define RC_SMOOTHING_RX_RATE_MAX_US             50000 // 50ms or 20hz
  93 #define RC_SMOOTHING_FEEDFORWARD_INITIAL_HZ     100 // The value to use for "auto" when interpolated feedforward is enabled
  94
  95 static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
  96 #endif // USE_RC_SMOOTHING_FILTER
  97
  98 bool getShouldUpdateFeedforward()
  99 // only used in pid.c, when feedforward is enabled, to initiate a new FF value
 100 {
 101     const bool updateFf = newRxDataForFF;
 102     if (newRxDataForFF == true){
 103         newRxDataForFF = false;
 104     }
 105     return updateFf;
 106 }
 107
 108 float getSetpointRate(int axis)
 109 // only used in pid.c to provide setpointRate for the crash recovery function
 110 {
 111     return setpointRate[axis];
 112 }
 113
 114 float getRcDeflection(int axis)
 115 {
 116     return rcDeflection[axis];
 117 }
 118
 119 float getRcDeflectionAbs(int axis)
 120 {
 121     return rcDeflectionAbs[axis];
 122 }
 123
 124 float getThrottlePIDAttenuation(void)
 125 {
 126     return throttlePIDAttenuation;
 127 }
 128
 129 #ifdef USE_FEEDFORWARD
 130 float getRawSetpoint(int axis)
 131 {
 132     return rawSetpoint[axis];
 133 }
 134
 135 float getRcCommandDelta(int axis)
 136 {
 137     return rcCommandDelta[axis];
 138 }
 139 #endif
 140
 141 #define THROTTLE_LOOKUP_LENGTH 12
 142 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH];    // lookup table for expo & mid THROTTLE
 143
 144 static int16_t rcLookupThrottle(int32_t tmp)
 145 {
 146     const int32_t tmp2 = tmp / 100;
 147     // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
 148     return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;
 149 }
 150
 151 #define SETPOINT_RATE_LIMIT 1998
 152 STATIC_ASSERT(CONTROL_RATE_CONFIG_RATE_LIMIT_MAX <= SETPOINT_RATE_LIMIT, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX_too_large);
 153
 154 #define RC_RATE_INCREMENTAL 14.54f
 155
 156 float applyBetaflightRates(const int axis, float rcCommandf, const float rcCommandfAbs)
 157 {
 158     if (currentControlRateProfile->rcExpo[axis]) {
 159         const float expof = currentControlRateProfile->rcExpo[axis] / 100.0f;
 160         rcCommandf = rcCommandf * power3(rcCommandfAbs) * expof + rcCommandf * (1 - expof);
 161     }
 162
 163     float rcRate = currentControlRateProfile->rcRates[axis] / 100.0f;
 164     if (rcRate > 2.0f) {
 165         rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f);
 166     }
 167     float angleRate = 200.0f * rcRate * rcCommandf;
 168     if (currentControlRateProfile->rates[axis]) {
 169         const float rcSuperfactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
 170         angleRate *= rcSuperfactor;
 171     }
 172
 173     return angleRate;
 174 }
 175
 176 float applyRaceFlightRates(const int axis, float rcCommandf, const float rcCommandfAbs)
 177 {
 178     // -1.0 to 1.0 ranged and curved
 179     rcCommandf = ((1.0f + 0.01f * currentControlRateProfile->rcExpo[axis] * (rcCommandf * rcCommandf - 1.0f)) * rcCommandf);
 180     // convert to -2000 to 2000 range using acro+ modifier
 181     float angleRate = 10.0f * currentControlRateProfile->rcRates[axis] * rcCommandf;
 182     angleRate = angleRate * (1 + rcCommandfAbs * (float)currentControlRateProfile->rates[axis] * 0.01f);
 183
 184     return angleRate;
 185 }
 186
 187 float applyKissRates(const int axis, float rcCommandf, const float rcCommandfAbs)
 188 {
 189     const float rcCurvef = currentControlRateProfile->rcExpo[axis] / 100.0f;
 190
 191     float kissRpyUseRates = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
 192     float kissRcCommandf = (power3(rcCommandf) * rcCurvef + rcCommandf * (1 - rcCurvef)) * (currentControlRateProfile->rcRates[axis] / 1000.0f);
 193     float kissAngle = constrainf(((2000.0f * kissRpyUseRates) * kissRcCommandf), -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
 194
 195     return kissAngle;
 196 }
 197
 198 float applyActualRates(const int axis, float rcCommandf, const float rcCommandfAbs)
 199 {
 200     float expof = currentControlRateProfile->rcExpo[axis] / 100.0f;
 201     expof = rcCommandfAbs * (powf(rcCommandf, 5) * expof + rcCommandf * (1 - expof));
 202
 203     const float centerSensitivity = currentControlRateProfile->rcRates[axis] * 10.0f;
 204     const float stickMovement = MAX(0, currentControlRateProfile->rates[axis] * 10.0f - centerSensitivity);
 205     const float angleRate = rcCommandf * centerSensitivity + stickMovement * expof;
 206
 207     return angleRate;
 208 }
 209
 210 float applyQuickRates(const int axis, float rcCommandf, const float rcCommandfAbs)
 211 {
 212     const uint16_t rcRate = currentControlRateProfile->rcRates[axis] * 2;
 213     const uint16_t maxDPS = MAX(currentControlRateProfile->rates[axis] * 10, rcRate);
 214     const float expof = currentControlRateProfile->rcExpo[axis] / 100.0f;
 215     const float superFactorConfig = ((float)maxDPS / rcRate - 1) / ((float)maxDPS / rcRate);
 216
 217     float curve;
 218     float superFactor;
 219     float angleRate;
 220
 221     if (currentControlRateProfile->quickRatesRcExpo) {
 222         curve = power3(rcCommandf) * expof + rcCommandf * (1 - expof);
 223         superFactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * superFactorConfig), 0.01f, 1.00f));
 224         angleRate = constrainf(curve * rcRate * superFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
 225     } else {
 226         curve = power3(rcCommandfAbs) * expof + rcCommandfAbs * (1 - expof);
 227         superFactor = 1.0f / (constrainf(1.0f - (curve * superFactorConfig), 0.01f, 1.00f));
 228         angleRate = constrainf(rcCommandf * rcRate * superFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
 229     }
 230
 231     return angleRate;
 232 }
 233
 234 float applyCurve(int axis, float deflection)
 235 {
 236     return applyRates(axis, deflection, fabsf(deflection));
 237 }
 238
 239 static void scaleSetpointToFpvCamAngle(void)
 240 {
 241     //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
 242     static uint8_t lastFpvCamAngleDegrees = 0;
 243     static float cosFactor = 1.0;
 244     static float sinFactor = 0.0;
 245
 246     if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees) {
 247         lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
 248         cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD);
 249         sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
 250     }
 251
 252     float roll = setpointRate[ROLL];
 253     float yaw = setpointRate[YAW];
 254     setpointRate[ROLL] = constrainf(roll * cosFactor -  yaw * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f);
 255     setpointRate[YAW]  = constrainf(yaw  * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f);
 256 }
 257
 258 #define THROTTLE_BUFFER_MAX 20
 259 #define THROTTLE_DELTA_MS 100
 260
 261 static void checkForThrottleErrorResetState(uint16_t rxRefreshRate)
 262 {
 263     static int index;
 264     static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX];
 265
 266     const int rxRefreshRateMs = rxRefreshRate / 1000;
 267     const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX);
 268     const int16_t throttleVelocityThreshold = (featureIsEnabled(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold;
 269
 270     rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE];
 271     if (index >= indexMax) {
 272         index = 0;
 273     }
 274
 275     const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
 276
 277     if (currentPidProfile->antiGravityMode == ANTI_GRAVITY_STEP) {
 278         if (ABS(rcCommandSpeed) > throttleVelocityThreshold) {
 279             pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
 280         } else {
 281             pidSetItermAccelerator(0.0f);
 282         }
 283     }
 284 }
 285
 286 void updateRcRefreshRate(timeUs_t currentTimeUs)
 287 {
 288     static timeUs_t lastRxTimeUs;
 289
 290     timeDelta_t frameAgeUs;
 291     timeDelta_t refreshRateUs = rxGetFrameDelta(&frameAgeUs);
 292     if (!refreshRateUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
 293         refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
 294     }
 295     lastRxTimeUs = currentTimeUs;
 296     currentRxRefreshRate = constrain(refreshRateUs, 1000, 30000);
 297 }
 298
 299 uint16_t getCurrentRxRefreshRate(void)
 300 {
 301     return currentRxRefreshRate;
 302 }
 303
 304 #ifdef USE_RC_SMOOTHING_FILTER
 305 // Determine a cutoff frequency based on smoothness factor and calculated average rx frame time
 306 FAST_CODE_NOINLINE int calcAutoSmoothingCutoff(int avgRxFrameTimeUs, uint8_t autoSmoothnessFactor)
 307 {
 308     if (avgRxFrameTimeUs > 0) {
 309         const float cutoffFactor = 1.5f / (1.0f + (autoSmoothnessFactor / 10.0f));
 310         float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f));  // link frequency
 311         cutoff = cutoff * cutoffFactor;
 312         return lrintf(cutoff);
 313     } else {
 314         return 0;
 315     }
 316 }
 317
 318 // Preforms a reasonableness check on the rx frame time to avoid bad data
 319 // skewing the average.
 320 static FAST_CODE bool rcSmoothingRxRateValid(int currentRxRefreshRate)
 321 {
 322     return (currentRxRefreshRate >= RC_SMOOTHING_RX_RATE_MIN_US && currentRxRefreshRate <= RC_SMOOTHING_RX_RATE_MAX_US);
 323 }
 324
 325 // Initialize or update the filters base on either the manually selected cutoff, or
 326 // the auto-calculated cutoff frequency based on detected rx frame rate.
 327 FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
 328 {
 329     const float dT = targetPidLooptime * 1e-6f;
 330     uint16_t oldCutoff = smoothingData->setpointCutoffFrequency;
 331
 332     if (smoothingData->setpointCutoffSetting == 0) {
 333         smoothingData->setpointCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint);
 334     }
 335     if (smoothingData->throttleCutoffSetting == 0) {
 336         smoothingData->throttleCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle);
 337     }
 338
 339
 340     // initialize or update the Setpoint filter
 341     if ((smoothingData->setpointCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
 342         for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
 343             if (i < THROTTLE) { // Throttle handled by smoothing rcCommand
 344                 if (!smoothingData->filterInitialized) {
 345                     pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
 346                 } else {
 347                     pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
 348                 }
 349             } else {
 350                 if (!smoothingData->filterInitialized) {
 351                     pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
 352                 } else {
 353                     pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
 354                 }
 355             }
 356         }
 357     }
 358
 359     // update or initialize the FF filter
 360     oldCutoff = smoothingData->feedforwardCutoffFrequency;
 361     if (rcSmoothingData.ffCutoffSetting == 0) {
 362         smoothingData->feedforwardCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint);
 363     }
 364     if (!smoothingData->filterInitialized) {
 365         pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis);
 366     } else if (smoothingData->feedforwardCutoffFrequency != oldCutoff) {
 367         pidUpdateFeedforwardLpf(smoothingData->feedforwardCutoffFrequency);
 368     }
 369 }
 370
 371 FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(rcSmoothingFilter_t *smoothingData)
 372 {
 373     smoothingData->training.sum = 0;
 374     smoothingData->training.count = 0;
 375     smoothingData->training.min = UINT16_MAX;
 376     smoothingData->training.max = 0;
 377 }
 378
 379 // Accumulate the rx frame time samples. Once we've collected enough samples calculate the
 380 // average and return true.
 381 static FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothingData, int rxFrameTimeUs)
 382 {
 383     smoothingData->training.sum += rxFrameTimeUs;
 384     smoothingData->training.count++;
 385     smoothingData->training.max = MAX(smoothingData->training.max, rxFrameTimeUs);
 386     smoothingData->training.min = MIN(smoothingData->training.min, rxFrameTimeUs);
 387
 388     // if we've collected enough samples then calculate the average and reset the accumulation
 389     const int sampleLimit = (rcSmoothingData.filterInitialized) ? RC_SMOOTHING_FILTER_RETRAINING_SAMPLES : RC_SMOOTHING_FILTER_TRAINING_SAMPLES;
 390     if (smoothingData->training.count >= sampleLimit) {
 391         smoothingData->training.sum = smoothingData->training.sum - smoothingData->training.min - smoothingData->training.max; // Throw out high and low samples
 392         smoothingData->averageFrameTimeUs = lrintf(smoothingData->training.sum / (smoothingData->training.count - 2));
 393         rcSmoothingResetAccumulation(smoothingData);
 394         return true;
 395     }
 396     return false;
 397 }
 398
 399 // Determine if we need to caclulate filter cutoffs. If not then we can avoid
 400 // examining the rx frame times completely
 401 FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
 402 {
 403     // if any rc smoothing cutoff is 0 (auto) then we need to calculate cutoffs
 404     if ((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.ffCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) {
 405         return true;
 406     }
 407     return false;
 408 }
 409
 410 static FAST_CODE void processRcSmoothingFilter(void)
 411 {
 412     static FAST_DATA_ZERO_INIT float rxDataToSmooth[4];
 413     static FAST_DATA_ZERO_INIT bool initialized;
 414     static FAST_DATA_ZERO_INIT timeMs_t validRxFrameTimeMs;
 415     static FAST_DATA_ZERO_INIT bool calculateCutoffs;
 416
 417     // first call initialization
 418     if (!initialized) {
 419         initialized = true;
 420         rcSmoothingData.filterInitialized = false;
 421         rcSmoothingData.averageFrameTimeUs = 0;
 422         rcSmoothingData.autoSmoothnessFactorSetpoint = rxConfig()->rc_smoothing_auto_factor_rpy;
 423         rcSmoothingData.autoSmoothnessFactorThrottle = rxConfig()->rc_smoothing_auto_factor_throttle;
 424         rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
 425         rcSmoothingData.setpointCutoffSetting = rxConfig()->rc_smoothing_setpoint_cutoff;
 426         rcSmoothingData.throttleCutoffSetting = rxConfig()->rc_smoothing_throttle_cutoff;
 427         rcSmoothingData.ffCutoffSetting = rxConfig()->rc_smoothing_feedforward_cutoff;
 428         rcSmoothingResetAccumulation(&rcSmoothingData);
 429         rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting;
 430         rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting;
 431         if (rcSmoothingData.ffCutoffSetting == 0) {
 432             // calculate and use an initial derivative cutoff until the RC interval is known
 433             const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactorSetpoint / 10.0f));
 434             float ffCutoff = RC_SMOOTHING_FEEDFORWARD_INITIAL_HZ * cutoffFactor;
 435             rcSmoothingData.feedforwardCutoffFrequency = lrintf(ffCutoff);
 436         } else {
 437             rcSmoothingData.feedforwardCutoffFrequency = rcSmoothingData.ffCutoffSetting;
 438         }
 439
 440         if (rxConfig()->rc_smoothing_mode) {
 441             calculateCutoffs = rcSmoothingAutoCalculate();
 442
 443             // if we don't need to calculate cutoffs dynamically then the filters can be initialized now
 444             if (!calculateCutoffs) {
 445                 rcSmoothingSetFilterCutoffs(&rcSmoothingData);
 446                 rcSmoothingData.filterInitialized = true;
 447             }
 448         }
 449     }
 450
 451     if (isRxDataNew) {
 452         // for auto calculated filters we need to examine each rx frame interval
 453         if (calculateCutoffs) {
 454             const timeMs_t currentTimeMs = millis();
 455             int sampleState = 0;
 456
 457             // If the filter cutoffs in auto mode, and we have good rx data, then determine the average rx frame rate
 458             // and use that to calculate the filter cutoff frequencies
 459             if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization
 460                 if (rxIsReceivingSignal()  && rcSmoothingRxRateValid(currentRxRefreshRate)) {
 461
 462                     // set the guard time expiration if it's not set
 463                     if (validRxFrameTimeMs == 0) {
 464                         validRxFrameTimeMs = currentTimeMs + (rcSmoothingData.filterInitialized ? RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS : RC_SMOOTHING_FILTER_TRAINING_DELAY_MS);
 465                     } else {
 466                         sampleState = 1;
 467                     }
 468
 469                     // if the guard time has expired then process the rx frame time
 470                     if (currentTimeMs > validRxFrameTimeMs) {
 471                         sampleState = 2;
 472                         bool accumulateSample = true;
 473
 474                         // During initial training process all samples.
 475                         // During retraining check samples to determine if they vary by more than the limit percentage.
 476                         if (rcSmoothingData.filterInitialized) {
 477                             const float percentChange = (ABS(currentRxRefreshRate - rcSmoothingData.averageFrameTimeUs) / (float)rcSmoothingData.averageFrameTimeUs) * 100;
 478                             if (percentChange < RC_SMOOTHING_RX_RATE_CHANGE_PERCENT) {
 479                                 // We received a sample that wasn't more than the limit percent so reset the accumulation
 480                                 // During retraining we need a contiguous block of samples that are all significantly different than the current average
 481                                 rcSmoothingResetAccumulation(&rcSmoothingData);
 482                                 accumulateSample = false;
 483                             }
 484                         }
 485
 486                         // accumlate the sample into the average
 487                         if (accumulateSample) {
 488                             if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) {
 489                                 // the required number of samples were collected so set the filter cutoffs, but only if smoothing is active
 490                                 if (rxConfig()->rc_smoothing_mode) {
 491                                     rcSmoothingSetFilterCutoffs(&rcSmoothingData);
 492                                     rcSmoothingData.filterInitialized = true;
 493                                 }
 494                                 validRxFrameTimeMs = 0;
 495                             }
 496                         }
 497
 498                     }
 499                 } else {
 500                     // we have either stopped receiving rx samples (failsafe?) or the sample time is unreasonable so reset the accumulation
 501                     rcSmoothingResetAccumulation(&rcSmoothingData);
 502                 }
 503             }
 504
 505             // rx frame rate training blackbox debugging
 506             if (debugMode == DEBUG_RC_SMOOTHING_RATE) {
 507                 DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxRefreshRate);              // log each rx frame interval
 508                 DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.training.count);    // log the training step count
 509                 DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.averageFrameTimeUs);// the current calculated average
 510                 DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState);                       // indicates whether guard time is active
 511             }
 512         }
 513         // Get new values to be smoothed
 514         for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
 515             rxDataToSmooth[i] = i == THROTTLE ? rcCommand[i] : rawSetpoint[i];
 516             if (i < THROTTLE) {
 517                 DEBUG_SET(DEBUG_RC_INTERPOLATION, i, lrintf(rxDataToSmooth[i]));
 518             } else {
 519                 DEBUG_SET(DEBUG_RC_INTERPOLATION, i, ((lrintf(rxDataToSmooth[i])) - 1000));
 520             }
 521         }
 522     }
 523
 524     if (rcSmoothingData.filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) {
 525         // after training has completed then log the raw rc channel and the calculated
 526         // average rx frame rate that was used to calculate the automatic filter cutoffs
 527         DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs);
 528     }
 529
 530     // each pid loop, apply the last received channel value to the filter, if initialised - thanks @klutvott
 531     for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
 532         float *dst = i == THROTTLE ? &rcCommand[i] : &setpointRate[i];
 533         if (rcSmoothingData.filterInitialized) {
 534             *dst = pt3FilterApply(&rcSmoothingData.filter[i], rxDataToSmooth[i]);
 535         } else {
 536             // If filter isn't initialized yet, as in smoothing off, use the actual unsmoothed rx channel data
 537             *dst = rxDataToSmooth[i];
 538         }
 539     }
 540 }
 541 #endif // USE_RC_SMOOTHING_FILTER
 542
 543 FAST_CODE void processRcCommand(void)
 544 {
 545     if (isRxDataNew) {
 546         newRxDataForFF = true;
 547     }
 548
 549     if (isRxDataNew && pidAntiGravityEnabled()) {
 550         checkForThrottleErrorResetState(currentRxRefreshRate);
 551     }
 552
 553     if (isRxDataNew) {
 554         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 555
 556             isDuplicate[axis] = (oldRcCommand[axis] == rcCommand[axis]);
 557             rcCommandDelta[axis] = fabsf(rcCommand[axis] - oldRcCommand[axis]);
 558             oldRcCommand[axis] = rcCommand[axis];
 559
 560             float angleRate;
 561
 562 #ifdef USE_GPS_RESCUE
 563             if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
 564                 // If GPS Rescue is active then override the setpointRate used in the
 565                 // pid controller with the value calculated from the desired heading logic.
 566                 angleRate = gpsRescueGetYawRate();
 567                 // Treat the stick input as centered to avoid any stick deflection base modifications (like acceleration limit)
 568                 rcDeflection[axis] = 0;
 569                 rcDeflectionAbs[axis] = 0;
 570             } else
 571 #endif
 572             {
 573                 // scale rcCommandf to range [-1.0, 1.0]
 574                 float rcCommandf;
 575                 if (axis == FD_YAW) {
 576                     rcCommandf = rcCommand[axis] / rcCommandYawDivider;
 577                 } else {
 578                     rcCommandf = rcCommand[axis] / rcCommandDivider;
 579                 }
 580
 581                 rcDeflection[axis] = rcCommandf;
 582                 const float rcCommandfAbs = fabsf(rcCommandf);
 583                 rcDeflectionAbs[axis] = rcCommandfAbs;
 584
 585                 angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
 586
 587             }
 588             rawSetpoint[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]);
 589             DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
 590         }
 591
 592         // adjust un-filtered setpoint steps to camera angle (mixing Roll and Yaw)
 593         if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
 594             scaleSetpointToFpvCamAngle();
 595         }
 596     }
 597
 598 #ifdef USE_RC_SMOOTHING_FILTER
 599     processRcSmoothingFilter();
 600 #endif
 601
 602     isRxDataNew = false;
 603 }
 604
 605 FAST_CODE_NOINLINE void updateRcCommands(void)
 606 {
 607     isRxDataNew = true;
 608
 609     // PITCH & ROLL only dynamic PID adjustment,  depending on throttle value
 610     int32_t prop;
 611     if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
 612         prop = 100;
 613         throttlePIDAttenuation = 1.0f;
 614     } else {
 615         if (rcData[THROTTLE] < 2000) {
 616             prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
 617         } else {
 618             prop = 100 - currentControlRateProfile->dynThrPID;
 619         }
 620         throttlePIDAttenuation = prop / 100.0f;
 621     }
 622
 623     for (int axis = 0; axis < 3; axis++) {
 624         // non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
 625
 626         float tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
 627         if (axis == ROLL || axis == PITCH) {
 628             if (tmp > rcControlsConfig()->deadband) {
 629                 tmp -= rcControlsConfig()->deadband;
 630             } else {
 631                 tmp = 0;
 632             }
 633             rcCommand[axis] = tmp;
 634         } else {
 635             if (tmp > rcControlsConfig()->yaw_deadband) {
 636                 tmp -= rcControlsConfig()->yaw_deadband;
 637             } else {
 638                 tmp = 0;
 639             }
 640             rcCommand[axis] = tmp * -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
 641         }
 642         if (rcData[axis] < rxConfig()->midrc) {
 643             rcCommand[axis] = -rcCommand[axis];
 644         }
 645     }
 646
 647     int32_t tmp;
 648     if (featureIsEnabled(FEATURE_3D)) {
 649         tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
 650         tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
 651     } else {
 652         tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
 653         tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);
 654     }
 655
 656     if (getLowVoltageCutoff()->enabled) {
 657         tmp = tmp * getLowVoltageCutoff()->percentage / 100;
 658     }
 659
 660     rcCommand[THROTTLE] = rcLookupThrottle(tmp);
 661
 662     if (featureIsEnabled(FEATURE_3D) && !failsafeIsActive()) {
 663         if (!flight3DConfig()->switched_mode3d) {
 664             if (IS_RC_MODE_ACTIVE(BOX3D)) {
 665                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 666                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
 667             }
 668         } else {
 669             if (IS_RC_MODE_ACTIVE(BOX3D)) {
 670                 reverseMotors = true;
 671                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 672                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MIN - rxConfig()->midrc);
 673             } else {
 674                 reverseMotors = false;
 675                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 676                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
 677             }
 678         }
 679     }
 680     if (FLIGHT_MODE(HEADFREE_MODE)) {
 681         static t_fp_vector_def  rcCommandBuff;
 682
 683         rcCommandBuff.X = rcCommand[ROLL];
 684         rcCommandBuff.Y = rcCommand[PITCH];
 685         if ((!FLIGHT_MODE(ANGLE_MODE) && (!FLIGHT_MODE(HORIZON_MODE)) && (!FLIGHT_MODE(GPS_RESCUE_MODE)))) {
 686             rcCommandBuff.Z = rcCommand[YAW];
 687         } else {
 688             rcCommandBuff.Z = 0;
 689         }
 690         imuQuaternionHeadfreeTransformVectorEarthToBody(&rcCommandBuff);
 691         rcCommand[ROLL] = rcCommandBuff.X;
 692         rcCommand[PITCH] = rcCommandBuff.Y;
 693         if ((!FLIGHT_MODE(ANGLE_MODE)&&(!FLIGHT_MODE(HORIZON_MODE)) && (!FLIGHT_MODE(GPS_RESCUE_MODE)))) {
 694             rcCommand[YAW] = rcCommandBuff.Z;
 695         }
 696     }
 697 }
 698
 699 void resetYawAxis(void)
 700 {
 701     rcCommand[YAW] = 0;
 702     setpointRate[YAW] = 0;
 703 }
 704
 705 bool isMotorsReversed(void)
 706 {
 707     return reverseMotors;
 708 }
 709
 710 void initRcProcessing(void)
 711 {
 712     rcCommandDivider = 500.0f - rcControlsConfig()->deadband;
 713     rcCommandYawDivider = 500.0f - rcControlsConfig()->yaw_deadband;
 714
 715     for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
 716         const int16_t tmp = 10 * i - currentControlRateProfile->thrMid8;
 717         uint8_t y = 1;
 718         if (tmp > 0)
 719             y = 100 - currentControlRateProfile->thrMid8;
 720         if (tmp < 0)
 721             y = currentControlRateProfile->thrMid8;
 722         lookupThrottleRC[i] = 10 * currentControlRateProfile->thrMid8 + tmp * (100 - currentControlRateProfile->thrExpo8 + (int32_t) currentControlRateProfile->thrExpo8 * (tmp * tmp) / (y * y)) / 10;
 723         lookupThrottleRC[i] = PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
 724     }
 725
 726     switch (currentControlRateProfile->rates_type) {
 727     case RATES_TYPE_BETAFLIGHT:
 728     default:
 729         applyRates = applyBetaflightRates;
 730
 731         break;
 732     case RATES_TYPE_RACEFLIGHT:
 733         applyRates = applyRaceFlightRates;
 734
 735         break;
 736     case RATES_TYPE_KISS:
 737         applyRates = applyKissRates;
 738
 739         break;
 740     case RATES_TYPE_ACTUAL:
 741         applyRates = applyActualRates;
 742
 743         break;
 744     case RATES_TYPE_QUICK:
 745         applyRates = applyQuickRates;
 746
 747         break;
 748     }
 749
 750 #ifdef USE_YAW_SPIN_RECOVERY
 751     const int maxYawRate = (int)applyRates(FD_YAW, 1.0f, 1.0f);
 752     initYawSpinRecovery(maxYawRate);
 753 #endif
 754 }
 755
 756 // send rc smoothing details to blackbox
 757 #ifdef USE_RC_SMOOTHING_FILTER
 758 rcSmoothingFilter_t *getRcSmoothingData(void)
 759 {
 760     return &rcSmoothingData;
 761 }
 762
 763 bool rcSmoothingInitializationComplete(void) {
 764     return rcSmoothingData.filterInitialized;
 765 }
 766 #endif // USE_RC_SMOOTHING_FILTER