src/main/fc/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/maths.h"
  31 #include "common/utils.h"
  32
  33 #include "config/feature.h"
  34
  35 #include "fc/config.h"
  36 #include "fc/controlrate_profile.h"
  37 #include "drivers/time.h"
  38 #include "fc/fc_core.h"
  39 #include "fc/fc_rc.h"
  40 #include "fc/rc_controls.h"
  41 #include "fc/rc_modes.h"
  42 #include "fc/runtime_config.h"
  43
  44 #include "flight/failsafe.h"
  45 #include "flight/imu.h"
  46 #include "flight/pid.h"
  47 #include "pg/rx.h"
  48 #include "rx/rx.h"
  49
  50
  51 #include "sensors/battery.h"
  52
  53 typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs);
  54
  55 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
  56 static float throttlePIDAttenuation;
  57 static bool reverseMotors = false;
  58 static applyRatesFn *applyRates;
  59 uint16_t currentRxRefreshRate;
  60
  61 FAST_RAM_ZERO_INIT uint8_t interpolationChannels;
  62
  63 enum {
  64     ROLL_FLAG = 1 << ROLL,
  65     PITCH_FLAG = 1 << PITCH,
  66     YAW_FLAG = 1 << YAW,
  67     THROTTLE_FLAG = 1 << THROTTLE,
  68 };
  69
  70 #ifdef USE_RC_SMOOTHING_FILTER
  71 #define RC_SMOOTHING_IDENTITY_FREQUENCY         80    // Used in the formula to convert a BIQUAD cutoff frequency to PT1
  72 #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
  73 #define RC_SMOOTHING_FILTER_TRAINING_SAMPLES    50    // Number of rx frame rate samples to average
  74 #define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS   1000  // Additional time to wait after receiving first valid rx frame before initial training starts
  75 #define RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS 2000  // Guard time to wait after retraining to prevent retraining again too quickly
  76 #define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT     20    // Look for samples varying this much from the current detected frame rate to initiate retraining
  77 #define RC_SMOOTHING_RX_RATE_MIN_US             5000  // 5ms or 200hz
  78 #define RC_SMOOTHING_RX_RATE_MAX_US             50000 // 50ms or 20hz
  79
  80 static FAST_RAM rcSmoothingFilter_t rcSmoothingData;
  81 #endif // USE_RC_SMOOTHING_FILTER
  82
  83 float getSetpointRate(int axis)
  84 {
  85     return setpointRate[axis];
  86 }
  87
  88 float getRcDeflection(int axis)
  89 {
  90     return rcDeflection[axis];
  91 }
  92
  93 float getRcDeflectionAbs(int axis)
  94 {
  95     return rcDeflectionAbs[axis];
  96 }
  97
  98 float getThrottlePIDAttenuation(void)
  99 {
 100     return throttlePIDAttenuation;
 101 }
 102
 103 #define THROTTLE_LOOKUP_LENGTH 12
 104 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH];    // lookup table for expo & mid THROTTLE
 105
 106 static int16_t rcLookupThrottle(int32_t tmp)
 107 {
 108     const int32_t tmp2 = tmp / 100;
 109     // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
 110     return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;
 111 }
 112
 113 #define SETPOINT_RATE_LIMIT 1998.0f
 114 #define RC_RATE_INCREMENTAL 14.54f
 115
 116 float applyBetaflightRates(const int axis, float rcCommandf, const float rcCommandfAbs)
 117 {
 118     if (currentControlRateProfile->rcExpo[axis]) {
 119         const float expof = currentControlRateProfile->rcExpo[axis] / 100.0f;
 120         rcCommandf = rcCommandf * power3(rcCommandfAbs) * expof + rcCommandf * (1 - expof);
 121     }
 122
 123     float rcRate = currentControlRateProfile->rcRates[axis] / 100.0f;
 124     if (rcRate > 2.0f) {
 125         rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f);
 126     }
 127     float angleRate = 200.0f * rcRate * rcCommandf;
 128     if (currentControlRateProfile->rates[axis]) {
 129         const float rcSuperfactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
 130         angleRate *= rcSuperfactor;
 131     }
 132
 133     return angleRate;
 134 }
 135
 136 float applyRaceFlightRates(const int axis, float rcCommandf, const float rcCommandfAbs)
 137 {
 138     // -1.0 to 1.0 ranged and curved
 139     rcCommandf = ((1.0f + 0.01f * currentControlRateProfile->rcExpo[axis] * (rcCommandf * rcCommandf - 1.0f)) * rcCommandf);
 140     // convert to -2000 to 2000 range using acro+ modifier
 141     float angleRate = 10.0f * currentControlRateProfile->rcRates[axis] * rcCommandf;
 142     angleRate = angleRate * (1 + rcCommandfAbs * (float)currentControlRateProfile->rates[axis] * 0.01f);
 143
 144     return angleRate;
 145 }
 146
 147 static void calculateSetpointRate(int axis)
 148 {
 149     // scale rcCommandf to range [-1.0, 1.0]
 150     float rcCommandf = rcCommand[axis] / 500.0f;
 151     rcDeflection[axis] = rcCommandf;
 152     const float rcCommandfAbs = ABS(rcCommandf);
 153     rcDeflectionAbs[axis] = rcCommandfAbs;
 154
 155     float angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
 156
 157     DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
 158
 159     setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec)
 160 }
 161
 162 static void scaleRcCommandToFpvCamAngle(void)
 163 {
 164     //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
 165     static uint8_t lastFpvCamAngleDegrees = 0;
 166     static float cosFactor = 1.0;
 167     static float sinFactor = 0.0;
 168
 169     if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees) {
 170         lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
 171         cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD);
 172         sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
 173     }
 174
 175     float roll = setpointRate[ROLL];
 176     float yaw = setpointRate[YAW];
 177     setpointRate[ROLL] = constrainf(roll * cosFactor -  yaw * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
 178     setpointRate[YAW]  = constrainf(yaw  * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
 179 }
 180
 181 #define THROTTLE_BUFFER_MAX 20
 182 #define THROTTLE_DELTA_MS 100
 183
 184 static void checkForThrottleErrorResetState(uint16_t rxRefreshRate)
 185 {
 186     static int index;
 187     static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX];
 188
 189     const int rxRefreshRateMs = rxRefreshRate / 1000;
 190     const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX);
 191     const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold;
 192
 193     rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE];
 194     if (index >= indexMax) {
 195         index = 0;
 196     }
 197
 198     const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
 199
 200     if (ABS(rcCommandSpeed) > throttleVelocityThreshold) {
 201         pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
 202     } else {
 203         pidSetItermAccelerator(1.0f);
 204     }
 205 }
 206
 207 FAST_CODE uint8_t processRcInterpolation(void)
 208 {
 209     static FAST_RAM_ZERO_INIT float rcCommandInterp[4];
 210     static FAST_RAM_ZERO_INIT float rcStepSize[4];
 211     static FAST_RAM_ZERO_INIT int16_t rcInterpolationStepCount;
 212
 213     uint16_t rxRefreshRate;
 214     uint8_t updatedChannel = 0;
 215
 216     if (rxConfig()->rcInterpolation) {
 217          // Set RC refresh rate for sampling and channels to filter
 218         switch (rxConfig()->rcInterpolation) {
 219         case RC_SMOOTHING_AUTO:
 220             rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps
 221             break;
 222         case RC_SMOOTHING_MANUAL:
 223             rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval;
 224             break;
 225         case RC_SMOOTHING_OFF:
 226         case RC_SMOOTHING_DEFAULT:
 227         default:
 228             rxRefreshRate = rxGetRefreshRate();
 229         }
 230
 231         if (isRXDataNew && rxRefreshRate > 0) {
 232             rcInterpolationStepCount = rxRefreshRate / targetPidLooptime;
 233
 234             for (int channel = 0; channel < PRIMARY_CHANNEL_COUNT; channel++) {
 235                 if ((1 << channel) & interpolationChannels) {
 236                     rcStepSize[channel] = (rcCommand[channel] - rcCommandInterp[channel]) / (float)rcInterpolationStepCount;
 237                 }
 238             }
 239
 240            DEBUG_SET(DEBUG_RC_INTERPOLATION, 0, lrintf(rcCommand[0]));
 241            DEBUG_SET(DEBUG_RC_INTERPOLATION, 1, lrintf(currentRxRefreshRate / 1000));
 242         } else {
 243             rcInterpolationStepCount--;
 244         }
 245
 246         // Interpolate steps of rcCommand
 247         if (rcInterpolationStepCount > 0) {
 248             for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
 249                 if ((1 << updatedChannel) & interpolationChannels) {
 250                     rcCommandInterp[updatedChannel] += rcStepSize[updatedChannel];
 251                     rcCommand[updatedChannel] = rcCommandInterp[updatedChannel];
 252                 }
 253             }
 254         }
 255     } else {
 256         rcInterpolationStepCount = 0; // reset factor in case of level modes flip flopping
 257     }
 258
 259     DEBUG_SET(DEBUG_RC_INTERPOLATION, 2, rcInterpolationStepCount);
 260
 261     return updatedChannel;
 262
 263 }
 264
 265 #ifdef USE_RC_SMOOTHING_FILTER
 266 FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1)
 267 {
 268     if (avgRxFrameTimeUs > 0) {
 269         float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)) / 2;  // calculate the nyquist frequency
 270         cutoff = cutoff * 0.90f;  // Use 90% of the calculated nyquist frequency
 271         if (pt1) {
 272             cutoff = sq(cutoff) / RC_SMOOTHING_IDENTITY_FREQUENCY; // convert to a cutoff for pt1 that has similar characteristics
 273         }
 274         return lrintf(cutoff);
 275     } else {
 276         return 0;
 277     }
 278 }
 279
 280 FAST_CODE bool rcSmoothingRxRateValid(int currentRxRefreshRate)
 281 {
 282     return (currentRxRefreshRate >= RC_SMOOTHING_RX_RATE_MIN_US && currentRxRefreshRate <= RC_SMOOTHING_RX_RATE_MAX_US);
 283 }
 284
 285 FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *rcSmoothingData)
 286 {
 287     const float dT = targetPidLooptime * 1e-6f;
 288     uint16_t oldCutoff = rcSmoothingData->inputCutoffFrequency;
 289     if (rxConfig()->rc_smoothing_input_cutoff == 0) {
 290         rcSmoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(rcSmoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1));
 291     }
 292     if ((rcSmoothingData->inputCutoffFrequency != oldCutoff) || !rcSmoothingData->filterInitialized) {
 293         for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
 294             if ((1 << i) & interpolationChannels) {
 295                 switch (rxConfig()->rc_smoothing_input_type) {
 296                     case RC_SMOOTHING_INPUT_PT1:
 297                         if (!rcSmoothingData->filterInitialized) {
 298                             pt1FilterInit(&rcSmoothingData->filterPt1[i], pt1FilterGain(rcSmoothingData->inputCutoffFrequency, dT));
 299                         } else {
 300                             pt1FilterUpdateCutoff(&rcSmoothingData->filterPt1[i], pt1FilterGain(rcSmoothingData->inputCutoffFrequency, dT));
 301                         }
 302                         break;
 303                     case RC_SMOOTHING_INPUT_BIQUAD:
 304                     default:
 305                         if (!rcSmoothingData->filterInitialized) {
 306                             biquadFilterInitLPF(&rcSmoothingData->filterBiquad[i], rcSmoothingData->inputCutoffFrequency, targetPidLooptime);
 307                         } else {
 308                             biquadFilterUpdateLPF(&rcSmoothingData->filterBiquad[i], rcSmoothingData->inputCutoffFrequency, targetPidLooptime);
 309                         }
 310                         break;
 311                 }
 312             }
 313         }
 314     }
 315     oldCutoff = rcSmoothingData->derivativeCutoffFrequency;
 316     if ((rxConfig()->rc_smoothing_derivative_cutoff == 0) && (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF)) {
 317         rcSmoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(rcSmoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1));
 318     }
 319     if ((rcSmoothingData->derivativeCutoffFrequency != oldCutoff) || !rcSmoothingData->filterInitialized) {
 320         if (!rcSmoothingData->filterInitialized) {
 321             pidInitSetpointDerivativeLpf(rcSmoothingData->derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type);
 322         } else {
 323             pidUpdateSetpointDerivativeLpf(rcSmoothingData->derivativeCutoffFrequency);
 324         }
 325     }
 326 }
 327
 328 FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(rcSmoothingFilter_t *rcSmoothingData)
 329 {
 330     rcSmoothingData->training.sum = 0;
 331     rcSmoothingData->training.count = 0;
 332     rcSmoothingData->training.min = UINT16_MAX;
 333     rcSmoothingData->training.max = 0;
 334 }
 335
 336 FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *rcSmoothingData, int rxFrameTimeUs)
 337 {
 338     rcSmoothingData->training.sum += rxFrameTimeUs;
 339     rcSmoothingData->training.count++;
 340     rcSmoothingData->training.max = MAX(rcSmoothingData->training.max, rxFrameTimeUs);
 341     rcSmoothingData->training.min = MIN(rcSmoothingData->training.min, rxFrameTimeUs);
 342     if (rcSmoothingData->training.count >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) {
 343         rcSmoothingData->training.sum = rcSmoothingData->training.sum - rcSmoothingData->training.min - rcSmoothingData->training.max; // Throw out high and low samples
 344         rcSmoothingData->averageFrameTimeUs = lrintf(rcSmoothingData->training.sum / (rcSmoothingData->training.count - 2));
 345         rcSmoothingResetAccumulation(rcSmoothingData);
 346         return true;
 347     }
 348     return false;
 349 }
 350
 351 FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
 352 {
 353     bool ret = false;
 354     if (rxConfig()->rc_smoothing_input_cutoff == 0) {
 355         ret = true;
 356     }
 357     if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) {
 358         if (rxConfig()->rc_smoothing_derivative_cutoff == 0) {
 359             ret = true;
 360         }
 361     }
 362     return ret;
 363 }
 364
 365 FAST_CODE uint8_t processRcSmoothingFilter(void)
 366 {
 367     uint8_t updatedChannel = 0;
 368     static FAST_RAM_ZERO_INIT float lastRxData[4];
 369     static FAST_RAM_ZERO_INIT bool initialized;
 370     static FAST_RAM_ZERO_INIT timeMs_t validRxFrameTimeMs;
 371     static FAST_RAM_ZERO_INIT bool calculateCutoffs;
 372
 373     if (!initialized) {
 374         initialized = true;
 375         rcSmoothingData.filterInitialized = false;
 376         rcSmoothingData.averageFrameTimeUs = 0;
 377         rcSmoothingResetAccumulation(&rcSmoothingData);
 378         rcSmoothingData.inputCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff;
 379         if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) {
 380             rcSmoothingData.derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff;
 381         }
 382         calculateCutoffs = rcSmoothingAutoCalculate();
 383         if (!calculateCutoffs) {
 384             rcSmoothingSetFilterCutoffs(&rcSmoothingData);
 385             rcSmoothingData.filterInitialized = true;
 386         }
 387     }
 388
 389     if (isRXDataNew) {
 390         for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
 391             if ((1 << i) & interpolationChannels) {
 392                 lastRxData[i] = rcCommand[i];
 393             }
 394         }
 395         if (calculateCutoffs) {
 396             const timeMs_t currentTimeMs = millis();
 397             int sampleState = 0;
 398
 399             // If the filter cutoffs are set to auto and we have good rx data, then determine the average rx frame rate
 400             // and use that to calculate the filter cutoff frequencies
 401             if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization
 402                 if (rxIsReceivingSignal()  && rcSmoothingRxRateValid(currentRxRefreshRate)) {
 403                     if (validRxFrameTimeMs == 0) {
 404                         validRxFrameTimeMs = currentTimeMs + (rcSmoothingData.filterInitialized ? RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS : RC_SMOOTHING_FILTER_TRAINING_DELAY_MS);
 405                     } else {
 406                         sampleState = 1;
 407                     }
 408                     if (currentTimeMs > validRxFrameTimeMs) {
 409                         sampleState = 2;
 410                         bool accumulateSample = true;
 411                         if (rcSmoothingData.filterInitialized) {
 412                             const float percentChange = (ABS(currentRxRefreshRate - rcSmoothingData.averageFrameTimeUs) / (float)rcSmoothingData.averageFrameTimeUs) * 100;
 413                             if (percentChange < RC_SMOOTHING_RX_RATE_CHANGE_PERCENT) {
 414                                 rcSmoothingResetAccumulation(&rcSmoothingData);
 415                                 accumulateSample = false;
 416                             }
 417                         }
 418                         if (accumulateSample) {
 419                             if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) {
 420                                 rcSmoothingSetFilterCutoffs(&rcSmoothingData);
 421                                 rcSmoothingData.filterInitialized = true;
 422                                 validRxFrameTimeMs = 0;
 423                             }
 424                         }
 425                     }
 426                 } else {
 427                     validRxFrameTimeMs = 0;
 428                     rcSmoothingResetAccumulation(&rcSmoothingData);
 429                 }
 430             }
 431             if (debugMode == DEBUG_RC_SMOOTHING_RATE) {
 432                 DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxRefreshRate);     // log each rx frame interval
 433                 DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.training.count);    // log the training step count
 434                 DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.averageFrameTimeUs);
 435                 DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState);
 436             }
 437         }
 438     }
 439
 440     if (rcSmoothingData.filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) {
 441         // after training has completed then log the raw rc channel and the calculated
 442         // average rx frame rate that was used to calculate the automatic filter cutoffs
 443         DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rxConfig()->rc_smoothing_debug_axis]));
 444         DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs);
 445     }
 446
 447     for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
 448         if ((1 << updatedChannel) & interpolationChannels) {
 449             if (rcSmoothingData.filterInitialized) {
 450                 switch (rxConfig()->rc_smoothing_input_type) {
 451                     case RC_SMOOTHING_INPUT_PT1:
 452                         rcCommand[updatedChannel] = pt1FilterApply(&rcSmoothingData.filterPt1[updatedChannel], lastRxData[updatedChannel]);
 453                         break;
 454                     case RC_SMOOTHING_INPUT_BIQUAD:
 455                     default:
 456                         rcCommand[updatedChannel] = biquadFilterApplyDF1(&rcSmoothingData.filterBiquad[updatedChannel], lastRxData[updatedChannel]);
 457                         break;
 458                 }
 459             } else {
 460                 // If filter isn't initialized yet then use the actual unsmoothed rx channel data
 461                 rcCommand[updatedChannel] = lastRxData[updatedChannel];
 462             }
 463         }
 464     }
 465
 466     return interpolationChannels;
 467 }
 468 #endif // USE_RC_SMOOTHING_FILTER
 469
 470 FAST_CODE void processRcCommand(void)
 471 {
 472     uint8_t updatedChannel;
 473
 474     if (isRXDataNew && isAntiGravityModeActive()) {
 475         checkForThrottleErrorResetState(currentRxRefreshRate);
 476     }
 477
 478     switch (rxConfig()->rc_smoothing_type) {
 479 #ifdef USE_RC_SMOOTHING_FILTER
 480     case RC_SMOOTHING_TYPE_FILTER:
 481         updatedChannel = processRcSmoothingFilter();
 482         break;
 483 #endif // USE_RC_SMOOTHING_FILTER
 484     case RC_SMOOTHING_TYPE_INTERPOLATION:
 485     default:
 486         updatedChannel = processRcInterpolation();
 487         break;
 488     }
 489
 490     if (isRXDataNew || updatedChannel) {
 491         const uint8_t maxUpdatedAxis = isRXDataNew ? FD_YAW : MIN(updatedChannel, FD_YAW); // throttle channel doesn't require rate calculation
 492 #if defined(SIMULATOR_BUILD)
 493 #pragma GCC diagnostic push
 494 #pragma GCC diagnostic ignored "-Wunsafe-loop-optimizations"
 495 #endif
 496         for (int axis = FD_ROLL; axis <= maxUpdatedAxis; axis++) {
 497 #if defined(SIMULATOR_BUILD)
 498 #pragma GCC diagnostic pop
 499 #endif
 500             calculateSetpointRate(axis);
 501         }
 502
 503         DEBUG_SET(DEBUG_RC_INTERPOLATION, 3, setpointRate[0]);
 504
 505         // Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
 506         if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
 507             scaleRcCommandToFpvCamAngle();
 508         }
 509     }
 510
 511     if (isRXDataNew) {
 512         isRXDataNew = false;
 513     }
 514 }
 515
 516 FAST_CODE FAST_CODE_NOINLINE void updateRcCommands(void)
 517 {
 518     // PITCH & ROLL only dynamic PID adjustment,  depending on throttle value
 519     int32_t prop;
 520     if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
 521         prop = 100;
 522         throttlePIDAttenuation = 1.0f;
 523     } else {
 524         if (rcData[THROTTLE] < 2000) {
 525             prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
 526         } else {
 527             prop = 100 - currentControlRateProfile->dynThrPID;
 528         }
 529         throttlePIDAttenuation = prop / 100.0f;
 530     }
 531
 532     for (int axis = 0; axis < 3; axis++) {
 533         // non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
 534
 535         int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
 536         if (axis == ROLL || axis == PITCH) {
 537             if (tmp > rcControlsConfig()->deadband) {
 538                 tmp -= rcControlsConfig()->deadband;
 539             } else {
 540                 tmp = 0;
 541             }
 542             rcCommand[axis] = tmp;
 543         } else {
 544             if (tmp > rcControlsConfig()->yaw_deadband) {
 545                 tmp -= rcControlsConfig()->yaw_deadband;
 546             } else {
 547                 tmp = 0;
 548             }
 549             rcCommand[axis] = tmp * -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
 550         }
 551         if (rcData[axis] < rxConfig()->midrc) {
 552             rcCommand[axis] = -rcCommand[axis];
 553         }
 554     }
 555
 556     int32_t tmp;
 557     if (feature(FEATURE_3D)) {
 558         tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
 559         tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
 560     } else {
 561         tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
 562         tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);
 563     }
 564
 565     if (getLowVoltageCutoff()->enabled) {
 566         tmp = tmp * getLowVoltageCutoff()->percentage / 100;
 567     }
 568
 569     rcCommand[THROTTLE] = rcLookupThrottle(tmp);
 570
 571     if (feature(FEATURE_3D) && !failsafeIsActive()) {
 572         if (!flight3DConfig()->switched_mode3d) {
 573             if (IS_RC_MODE_ACTIVE(BOX3D)) {
 574                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 575                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
 576             }
 577         } else {
 578             if (IS_RC_MODE_ACTIVE(BOX3D)) {
 579                 reverseMotors = true;
 580                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 581                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MIN - rxConfig()->midrc);
 582             } else {
 583                 reverseMotors = false;
 584                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 585                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
 586             }
 587         }
 588     }
 589     if (FLIGHT_MODE(HEADFREE_MODE)) {
 590         static t_fp_vector_def  rcCommandBuff;
 591
 592         rcCommandBuff.X = rcCommand[ROLL];
 593         rcCommandBuff.Y = rcCommand[PITCH];
 594         if ((!FLIGHT_MODE(ANGLE_MODE) && (!FLIGHT_MODE(HORIZON_MODE)) && (!FLIGHT_MODE(GPS_RESCUE_MODE)))) {
 595             rcCommandBuff.Z = rcCommand[YAW];
 596         } else {
 597             rcCommandBuff.Z = 0;
 598         }
 599         imuQuaternionHeadfreeTransformVectorEarthToBody(&rcCommandBuff);
 600         rcCommand[ROLL] = rcCommandBuff.X;
 601         rcCommand[PITCH] = rcCommandBuff.Y;
 602         if ((!FLIGHT_MODE(ANGLE_MODE)&&(!FLIGHT_MODE(HORIZON_MODE)) && (!FLIGHT_MODE(GPS_RESCUE_MODE)))) {
 603             rcCommand[YAW] = rcCommandBuff.Z;
 604         }
 605     }
 606 }
 607
 608 void resetYawAxis(void)
 609 {
 610     rcCommand[YAW] = 0;
 611     setpointRate[YAW] = 0;
 612 }
 613
 614 bool isMotorsReversed(void)
 615 {
 616     return reverseMotors;
 617 }
 618
 619 void initRcProcessing(void)
 620 {
 621     for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
 622         const int16_t tmp = 10 * i - currentControlRateProfile->thrMid8;
 623         uint8_t y = 1;
 624         if (tmp > 0)
 625             y = 100 - currentControlRateProfile->thrMid8;
 626         if (tmp < 0)
 627             y = currentControlRateProfile->thrMid8;
 628         lookupThrottleRC[i] = 10 * currentControlRateProfile->thrMid8 + tmp * (100 - currentControlRateProfile->thrExpo8 + (int32_t) currentControlRateProfile->thrExpo8 * (tmp * tmp) / (y * y)) / 10;
 629         lookupThrottleRC[i] = PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
 630     }
 631
 632     switch (currentControlRateProfile->rates_type) {
 633     case RATES_TYPE_BETAFLIGHT:
 634     default:
 635         applyRates = applyBetaflightRates;
 636
 637         break;
 638     case RATES_TYPE_RACEFLIGHT:
 639         applyRates = applyRaceFlightRates;
 640
 641         break;
 642     }
 643
 644     interpolationChannels = 0;
 645     switch (rxConfig()->rcInterpolationChannels) {
 646     case INTERPOLATION_CHANNELS_RPYT:
 647         interpolationChannels |= THROTTLE_FLAG;
 648
 649         FALLTHROUGH;
 650     case INTERPOLATION_CHANNELS_RPY:
 651         interpolationChannels |= YAW_FLAG;
 652
 653         FALLTHROUGH;
 654     case INTERPOLATION_CHANNELS_RP:
 655         interpolationChannels |= ROLL_FLAG | PITCH_FLAG;
 656
 657         break;
 658     case INTERPOLATION_CHANNELS_RPT:
 659         interpolationChannels |= ROLL_FLAG | PITCH_FLAG;
 660
 661         FALLTHROUGH;
 662     case INTERPOLATION_CHANNELS_T:
 663         interpolationChannels |= THROTTLE_FLAG;
 664
 665         break;
 666     }
 667 }
 668
 669 #ifdef USE_RC_SMOOTHING_FILTER
 670 int rcSmoothingGetValue(int whichValue)
 671 {
 672     switch (whichValue) {
 673         case RC_SMOOTHING_VALUE_INPUT_ACTIVE:
 674             return rcSmoothingData.inputCutoffFrequency;
 675         case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE:
 676             return rcSmoothingData.derivativeCutoffFrequency;
 677         case RC_SMOOTHING_VALUE_AVERAGE_FRAME:
 678             return rcSmoothingData.averageFrameTimeUs;
 679         default:
 680             return 0;
 681     }
 682 }
 683 #endif // USE_RC_SMOOTHING_FILTER