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/feedforward.h"
  46 #include "flight/gps_rescue.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
  61 #ifdef USE_FEEDFORWARD
  62 static float oldRcCommand[XYZ_AXIS_COUNT];
  63 static bool isDuplicate[XYZ_AXIS_COUNT];
  64 float rcCommandDelta[XYZ_AXIS_COUNT];
  65 #endif
  66 static float rawSetpoint[XYZ_AXIS_COUNT];
  67 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
  68 static bool reverseMotors = false;
  69 static applyRatesFn *applyRates;
  70 static uint16_t currentRxRefreshRate;
  71 static bool isRxDataNew = false;
  72 static bool isRxRateValid = false;
  73 static float rcCommandDivider = 500.0f;
  74 static float rcCommandYawDivider = 500.0f;
  75
  76 static FAST_DATA_ZERO_INIT bool newRxDataForFF;
  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_RC_SMOOTHING_FILTER
  86 #define RC_SMOOTHING_CUTOFF_MIN_HZ              15    // Minimum rc smoothing cutoff frequency
  87 #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
  88 #define RC_SMOOTHING_FILTER_TRAINING_SAMPLES    50    // Number of rx frame rate samples to average during initial training
  89 #define RC_SMOOTHING_FILTER_RETRAINING_SAMPLES  20    // Number of rx frame rate samples to average during frame rate changes
  90 #define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS   1000  // Additional time to wait after receiving first valid rx frame before initial training starts
  91 #define RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS 2000  // Guard time to wait after retraining to prevent retraining again too quickly
  92 #define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT     20    // Look for samples varying this much from the current detected frame rate to initiate retraining
  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 static float rcDeflectionSmoothed[3];
  97 #endif // USE_RC_SMOOTHING_FILTER
  98
  99 #define RC_RX_RATE_MIN_US                       950   // 0.950ms to fit 1kHz without an issue
 100 #define RC_RX_RATE_MAX_US                       65500 // 65.5ms or 15.26hz
 101
 102 bool getShouldUpdateFeedforward(void)
 103 // only used in pid.c, when feedforward is enabled, to initiate a new FF value
 104 {
 105     const bool updateFf = newRxDataForFF;
 106     if (newRxDataForFF == true){
 107         newRxDataForFF = false;
 108     }
 109     return updateFf;
 110 }
 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 float getRcDeflection(int axis)
 122 {
 123 #ifdef USE_RC_SMOOTHING_FILTER
 124     return rcDeflectionSmoothed[axis];
 125 #else
 126     return rcDeflection[axis];
 127 #endif
 128 }
 129
 130 float getRcDeflectionAbs(int axis)
 131 {
 132     return rcDeflectionAbs[axis];
 133 }
 134
 135 #ifdef USE_FEEDFORWARD
 136 float getRawSetpoint(int axis)
 137 {
 138     return rawSetpoint[axis];
 139 }
 140
 141 float getRcCommandDelta(int axis)
 142 {
 143     return rcCommandDelta[axis];
 144 }
 145
 146 bool getRxRateValid(void)
 147 {
 148     return isRxRateValid;
 149 }
 150 #endif
 151
 152 #define THROTTLE_LOOKUP_LENGTH 12
 153 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH];    // lookup table for expo & mid THROTTLE
 154
 155 static int16_t rcLookupThrottle(int32_t tmp)
 156 {
 157     const int32_t tmp2 = tmp / 100;
 158     // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
 159     return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;
 160 }
 161
 162 #define SETPOINT_RATE_LIMIT 1998
 163 STATIC_ASSERT(CONTROL_RATE_CONFIG_RATE_LIMIT_MAX <= SETPOINT_RATE_LIMIT, 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, SETPOINT_RATE_LIMIT);
 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, SETPOINT_RATE_LIMIT);
 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, SETPOINT_RATE_LIMIT);
 240     }
 241
 242     return angleRate;
 243 }
 244
 245 float applyCurve(int axis, float deflection)
 246 {
 247     return applyRates(axis, deflection, fabsf(deflection));
 248 }
 249
 250 static void scaleRawSetpointToFpvCamAngle(void)
 251 {
 252     //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
 253     static uint8_t lastFpvCamAngleDegrees = 0;
 254     static float cosFactor = 1.0;
 255     static float sinFactor = 0.0;
 256
 257     if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees) {
 258         lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
 259         cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD);
 260         sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
 261     }
 262
 263     float roll = rawSetpoint[ROLL];
 264     float yaw = rawSetpoint[YAW];
 265     rawSetpoint[ROLL] = constrainf(roll * cosFactor -  yaw * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f);
 266     rawSetpoint[YAW]  = constrainf(yaw  * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f);
 267 }
 268
 269 void updateRcRefreshRate(timeUs_t currentTimeUs)
 270 {
 271     static timeUs_t lastRxTimeUs;
 272
 273     timeDelta_t frameAgeUs;
 274     timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs);
 275
 276     if (!frameDeltaUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
 277         frameDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
 278     }
 279
 280     DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX));
 281     DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX));
 282
 283     lastRxTimeUs = currentTimeUs;
 284     isRxRateValid = (frameDeltaUs >= RC_RX_RATE_MIN_US && frameDeltaUs <= RC_RX_RATE_MAX_US);
 285     currentRxRefreshRate = constrain(frameDeltaUs, RC_RX_RATE_MIN_US, RC_RX_RATE_MAX_US);
 286 }
 287
 288 uint16_t getCurrentRxRefreshRate(void)
 289 {
 290     return currentRxRefreshRate;
 291 }
 292
 293 #ifdef USE_RC_SMOOTHING_FILTER
 294 // Determine a cutoff frequency based on smoothness factor and calculated average rx frame time
 295 FAST_CODE_NOINLINE int calcAutoSmoothingCutoff(int avgRxFrameTimeUs, uint8_t autoSmoothnessFactor)
 296 {
 297     if (avgRxFrameTimeUs > 0) {
 298         const float cutoffFactor = 1.5f / (1.0f + (autoSmoothnessFactor / 10.0f));
 299         float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f));  // link frequency
 300         cutoff = cutoff * cutoffFactor;
 301         return lrintf(cutoff);
 302     } else {
 303         return 0;
 304     }
 305 }
 306
 307 // Initialize or update the filters base on either the manually selected cutoff, or
 308 // the auto-calculated cutoff frequency based on detected rx frame rate.
 309 FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
 310 {
 311     const float dT = targetPidLooptime * 1e-6f;
 312     uint16_t oldCutoff = smoothingData->setpointCutoffFrequency;
 313
 314     if (smoothingData->setpointCutoffSetting == 0) {
 315         smoothingData->setpointCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
 316     }
 317     if (smoothingData->throttleCutoffSetting == 0) {
 318         smoothingData->throttleCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle));
 319     }
 320
 321     // initialize or update the Setpoint filter
 322     if ((smoothingData->setpointCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
 323         for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
 324             if (i < THROTTLE) { // Throttle handled by smoothing rcCommand
 325                 if (!smoothingData->filterInitialized) {
 326                     pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
 327                 } else {
 328                     pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
 329                 }
 330             } else {
 331                 if (!smoothingData->filterInitialized) {
 332                     pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
 333                 } else {
 334                     pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
 335                 }
 336             }
 337         }
 338
 339         // initialize or update the Level filter
 340         for (int i = FD_ROLL; i < FD_YAW; i++) {
 341             if (!smoothingData->filterInitialized) {
 342                 pt3FilterInit(&smoothingData->filterDeflection[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
 343             } else {
 344                 pt3FilterUpdateCutoff(&smoothingData->filterDeflection[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
 345             }
 346         }
 347     }
 348
 349     // update or initialize the FF filter
 350     oldCutoff = smoothingData->feedforwardCutoffFrequency;
 351     if (rcSmoothingData.ffCutoffSetting == 0) {
 352         smoothingData->feedforwardCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
 353     }
 354     if (!smoothingData->filterInitialized) {
 355         pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis);
 356     } else if (smoothingData->feedforwardCutoffFrequency != oldCutoff) {
 357         pidUpdateFeedforwardLpf(smoothingData->feedforwardCutoffFrequency);
 358     }
 359 }
 360
 361 FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(rcSmoothingFilter_t *smoothingData)
 362 {
 363     smoothingData->training.sum = 0;
 364     smoothingData->training.count = 0;
 365     smoothingData->training.min = UINT16_MAX;
 366     smoothingData->training.max = 0;
 367 }
 368
 369 // Accumulate the rx frame time samples. Once we've collected enough samples calculate the
 370 // average and return true.
 371 static FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothingData, int rxFrameTimeUs)
 372 {
 373     smoothingData->training.sum += rxFrameTimeUs;
 374     smoothingData->training.count++;
 375     smoothingData->training.max = MAX(smoothingData->training.max, rxFrameTimeUs);
 376     smoothingData->training.min = MIN(smoothingData->training.min, rxFrameTimeUs);
 377
 378     // if we've collected enough samples then calculate the average and reset the accumulation
 379     const int sampleLimit = (rcSmoothingData.filterInitialized) ? RC_SMOOTHING_FILTER_RETRAINING_SAMPLES : RC_SMOOTHING_FILTER_TRAINING_SAMPLES;
 380     if (smoothingData->training.count >= sampleLimit) {
 381         smoothingData->training.sum = smoothingData->training.sum - smoothingData->training.min - smoothingData->training.max; // Throw out high and low samples
 382         smoothingData->averageFrameTimeUs = lrintf(smoothingData->training.sum / (smoothingData->training.count - 2));
 383         rcSmoothingResetAccumulation(smoothingData);
 384         return true;
 385     }
 386     return false;
 387 }
 388
 389 // Determine if we need to caclulate filter cutoffs. If not then we can avoid
 390 // examining the rx frame times completely
 391 FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
 392 {
 393     // if any rc smoothing cutoff is 0 (auto) then we need to calculate cutoffs
 394     if ((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.ffCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) {
 395         return true;
 396     }
 397     return false;
 398 }
 399
 400 static FAST_CODE void processRcSmoothingFilter(void)
 401 {
 402     static FAST_DATA_ZERO_INIT float rxDataToSmooth[4];
 403     static FAST_DATA_ZERO_INIT bool initialized;
 404     static FAST_DATA_ZERO_INIT timeMs_t validRxFrameTimeMs;
 405     static FAST_DATA_ZERO_INIT bool calculateCutoffs;
 406
 407     // first call initialization
 408     if (!initialized) {
 409         initialized = true;
 410         rcSmoothingData.filterInitialized = false;
 411         rcSmoothingData.averageFrameTimeUs = 0;
 412         rcSmoothingData.autoSmoothnessFactorSetpoint = rxConfig()->rc_smoothing_auto_factor_rpy;
 413         rcSmoothingData.autoSmoothnessFactorThrottle = rxConfig()->rc_smoothing_auto_factor_throttle;
 414         rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
 415         rcSmoothingData.setpointCutoffSetting = rxConfig()->rc_smoothing_setpoint_cutoff;
 416         rcSmoothingData.throttleCutoffSetting = rxConfig()->rc_smoothing_throttle_cutoff;
 417         rcSmoothingData.ffCutoffSetting = rxConfig()->rc_smoothing_feedforward_cutoff;
 418         rcSmoothingResetAccumulation(&rcSmoothingData);
 419         rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting;
 420         rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting;
 421         if (rcSmoothingData.ffCutoffSetting == 0) {
 422             // calculate and use an initial derivative cutoff until the RC interval is known
 423             const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactorSetpoint / 10.0f));
 424             float ffCutoff = RC_SMOOTHING_FEEDFORWARD_INITIAL_HZ * cutoffFactor;
 425             rcSmoothingData.feedforwardCutoffFrequency = lrintf(ffCutoff);
 426         } else {
 427             rcSmoothingData.feedforwardCutoffFrequency = rcSmoothingData.ffCutoffSetting;
 428         }
 429
 430         if (rxConfig()->rc_smoothing_mode) {
 431             calculateCutoffs = rcSmoothingAutoCalculate();
 432
 433             // if we don't need to calculate cutoffs dynamically then the filters can be initialized now
 434             if (!calculateCutoffs) {
 435                 rcSmoothingSetFilterCutoffs(&rcSmoothingData);
 436                 rcSmoothingData.filterInitialized = true;
 437             }
 438         }
 439     }
 440
 441     if (isRxDataNew) {
 442         // for auto calculated filters we need to examine each rx frame interval
 443         if (calculateCutoffs) {
 444             const timeMs_t currentTimeMs = millis();
 445             int sampleState = 0;
 446
 447             // If the filter cutoffs in auto mode, and we have good rx data, then determine the average rx frame rate
 448             // and use that to calculate the filter cutoff frequencies
 449             if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization
 450                 if (rxIsReceivingSignal() && isRxRateValid) {
 451
 452                     // set the guard time expiration if it's not set
 453                     if (validRxFrameTimeMs == 0) {
 454                         validRxFrameTimeMs = currentTimeMs + (rcSmoothingData.filterInitialized ? RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS : RC_SMOOTHING_FILTER_TRAINING_DELAY_MS);
 455                     } else {
 456                         sampleState = 1;
 457                     }
 458
 459                     // if the guard time has expired then process the rx frame time
 460                     if (currentTimeMs > validRxFrameTimeMs) {
 461                         sampleState = 2;
 462                         bool accumulateSample = true;
 463
 464                         // During initial training process all samples.
 465                         // During retraining check samples to determine if they vary by more than the limit percentage.
 466                         if (rcSmoothingData.filterInitialized) {
 467                             const float percentChange = (abs(currentRxRefreshRate - rcSmoothingData.averageFrameTimeUs) / (float)rcSmoothingData.averageFrameTimeUs) * 100;
 468                             if (percentChange < RC_SMOOTHING_RX_RATE_CHANGE_PERCENT) {
 469                                 // We received a sample that wasn't more than the limit percent so reset the accumulation
 470                                 // During retraining we need a contiguous block of samples that are all significantly different than the current average
 471                                 rcSmoothingResetAccumulation(&rcSmoothingData);
 472                                 accumulateSample = false;
 473                             }
 474                         }
 475
 476                         // accumlate the sample into the average
 477                         if (accumulateSample) {
 478                             if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) {
 479                                 // the required number of samples were collected so set the filter cutoffs, but only if smoothing is active
 480                                 if (rxConfig()->rc_smoothing_mode) {
 481                                     rcSmoothingSetFilterCutoffs(&rcSmoothingData);
 482                                     rcSmoothingData.filterInitialized = true;
 483                                 }
 484                                 validRxFrameTimeMs = 0;
 485                             }
 486                         }
 487
 488                     }
 489                 } else {
 490                     // we have either stopped receiving rx samples (failsafe?) or the sample time is unreasonable so reset the accumulation
 491                     rcSmoothingResetAccumulation(&rcSmoothingData);
 492                 }
 493             }
 494
 495             // rx frame rate training blackbox debugging
 496             DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxRefreshRate);              // log each rx frame interval
 497             DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.training.count);    // log the training step count
 498             DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.averageFrameTimeUs);// the current calculated average
 499             DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState);                       // indicates whether guard time is active
 500         }
 501         // Get new values to be smoothed
 502         for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
 503             rxDataToSmooth[i] = i == THROTTLE ? rcCommand[i] : rawSetpoint[i];
 504             if (i < THROTTLE) {
 505                 DEBUG_SET(DEBUG_RC_INTERPOLATION, i, lrintf(rxDataToSmooth[i]));
 506             } else {
 507                 DEBUG_SET(DEBUG_RC_INTERPOLATION, i, ((lrintf(rxDataToSmooth[i])) - 1000));
 508             }
 509         }
 510     }
 511
 512     if (rcSmoothingData.filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) {
 513         // after training has completed then log the raw rc channel and the calculated
 514         // average rx frame rate that was used to calculate the automatic filter cutoffs
 515         DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs);
 516     }
 517
 518     // each pid loop, apply the last received channel value to the filter, if initialised - thanks @klutvott
 519     for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
 520         float *dst = i == THROTTLE ? &rcCommand[i] : &setpointRate[i];
 521         if (rcSmoothingData.filterInitialized) {
 522             *dst = pt3FilterApply(&rcSmoothingData.filter[i], rxDataToSmooth[i]);
 523         } else {
 524             // If filter isn't initialized yet, as in smoothing off, use the actual unsmoothed rx channel data
 525             *dst = rxDataToSmooth[i];
 526         }
 527     }
 528
 529     // for ANGLE and HORIZON, smooth rcDeflection on pitch and roll to avoid setpoint steps
 530     bool smoothingNeeded = (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && rcSmoothingData.filterInitialized;
 531     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 532         if (smoothingNeeded && axis < FD_YAW) {
 533             rcDeflectionSmoothed[axis] = pt3FilterApply(&rcSmoothingData.filterDeflection[axis], rcDeflection[axis]);
 534         } else {
 535             rcDeflectionSmoothed[axis] = rcDeflection[axis];
 536         }
 537     }
 538
 539 }
 540 #endif // USE_RC_SMOOTHING_FILTER
 541
 542 FAST_CODE void processRcCommand(void)
 543 {
 544     if (isRxDataNew) {
 545         newRxDataForFF = true;
 546         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 547
 548 #ifdef USE_FEEDFORWARD
 549             isDuplicate[axis] = (oldRcCommand[axis] == rcCommand[axis]);
 550             rcCommandDelta[axis] = (rcCommand[axis] - oldRcCommand[axis]);
 551             oldRcCommand[axis] = rcCommand[axis];
 552 #endif
 553
 554             float angleRate;
 555
 556 #ifdef USE_GPS_RESCUE
 557             if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
 558                 // If GPS Rescue is active then override the setpointRate used in the
 559                 // pid controller with the value calculated from the desired heading logic.
 560                 angleRate = gpsRescueGetYawRate();
 561                 // Treat the stick input as centered to avoid any stick deflection base modifications (like acceleration limit)
 562                 rcDeflection[axis] = 0;
 563                 rcDeflectionAbs[axis] = 0;
 564             } else
 565 #endif
 566             {
 567                 // scale rcCommandf to range [-1.0, 1.0]
 568                 float rcCommandf;
 569                 if (axis == FD_YAW) {
 570                     rcCommandf = rcCommand[axis] / rcCommandYawDivider;
 571                 } else {
 572                     rcCommandf = rcCommand[axis] / rcCommandDivider;
 573                 }
 574
 575                 rcDeflection[axis] = rcCommandf;
 576                 const float rcCommandfAbs = fabsf(rcCommandf);
 577                 rcDeflectionAbs[axis] = rcCommandfAbs;
 578
 579                 angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
 580
 581             }
 582             rawSetpoint[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]);
 583             DEBUG_SET(DEBUG_ANGLERATE, axis, lrintf(angleRate));
 584         }
 585         // adjust raw setpoint steps to camera angle (mixing Roll and Yaw)
 586         if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
 587             scaleRawSetpointToFpvCamAngle();
 588         }
 589     }
 590
 591 #ifdef USE_RC_SMOOTHING_FILTER
 592     processRcSmoothingFilter();
 593 #endif
 594
 595     isRxDataNew = false;
 596 }
 597
 598 FAST_CODE_NOINLINE void updateRcCommands(void)
 599 {
 600     isRxDataNew = true;
 601
 602     for (int axis = 0; axis < 3; axis++) {
 603         // non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
 604
 605         float tmp = MIN(fabsf(rcData[axis] - rxConfig()->midrc), 500.0f);
 606         if (axis == ROLL || axis == PITCH) {
 607             if (tmp > rcControlsConfig()->deadband) {
 608                 tmp -= rcControlsConfig()->deadband;
 609             } else {
 610                 tmp = 0;
 611             }
 612             rcCommand[axis] = tmp;
 613         } else {
 614             if (tmp > rcControlsConfig()->yaw_deadband) {
 615                 tmp -= rcControlsConfig()->yaw_deadband;
 616             } else {
 617                 tmp = 0;
 618             }
 619             rcCommand[axis] = tmp * -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
 620         }
 621         if (rcData[axis] < rxConfig()->midrc) {
 622             rcCommand[axis] = -rcCommand[axis];
 623         }
 624     }
 625
 626     int32_t tmp;
 627     if (featureIsEnabled(FEATURE_3D)) {
 628         tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
 629         tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
 630     } else {
 631         tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
 632         tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);
 633     }
 634
 635     if (getLowVoltageCutoff()->enabled) {
 636         tmp = tmp * getLowVoltageCutoff()->percentage / 100;
 637     }
 638
 639     rcCommand[THROTTLE] = rcLookupThrottle(tmp);
 640
 641     if (featureIsEnabled(FEATURE_3D) && !failsafeIsActive()) {
 642         if (!flight3DConfig()->switched_mode3d) {
 643             if (IS_RC_MODE_ACTIVE(BOX3D)) {
 644                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 645                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
 646             }
 647         } else {
 648             if (IS_RC_MODE_ACTIVE(BOX3D)) {
 649                 reverseMotors = true;
 650                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 651                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MIN - rxConfig()->midrc);
 652             } else {
 653                 reverseMotors = false;
 654                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 655                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
 656             }
 657         }
 658     }
 659     if (FLIGHT_MODE(HEADFREE_MODE)) {
 660         static t_fp_vector_def  rcCommandBuff;
 661
 662         rcCommandBuff.X = rcCommand[ROLL];
 663         rcCommandBuff.Y = rcCommand[PITCH];
 664         if ((!FLIGHT_MODE(ANGLE_MODE) && (!FLIGHT_MODE(HORIZON_MODE)) && (!FLIGHT_MODE(GPS_RESCUE_MODE)))) {
 665             rcCommandBuff.Z = rcCommand[YAW];
 666         } else {
 667             rcCommandBuff.Z = 0;
 668         }
 669         imuQuaternionHeadfreeTransformVectorEarthToBody(&rcCommandBuff);
 670         rcCommand[ROLL] = rcCommandBuff.X;
 671         rcCommand[PITCH] = rcCommandBuff.Y;
 672         if ((!FLIGHT_MODE(ANGLE_MODE)&&(!FLIGHT_MODE(HORIZON_MODE)) && (!FLIGHT_MODE(GPS_RESCUE_MODE)))) {
 673             rcCommand[YAW] = rcCommandBuff.Z;
 674         }
 675     }
 676 }
 677
 678 void resetYawAxis(void)
 679 {
 680     rcCommand[YAW] = 0;
 681     setpointRate[YAW] = 0;
 682 }
 683
 684 bool isMotorsReversed(void)
 685 {
 686     return reverseMotors;
 687 }
 688
 689 void initRcProcessing(void)
 690 {
 691     rcCommandDivider = 500.0f - rcControlsConfig()->deadband;
 692     rcCommandYawDivider = 500.0f - rcControlsConfig()->yaw_deadband;
 693
 694     for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
 695         const int16_t tmp = 10 * i - currentControlRateProfile->thrMid8;
 696         uint8_t y = 1;
 697         if (tmp > 0)
 698             y = 100 - currentControlRateProfile->thrMid8;
 699         if (tmp < 0)
 700             y = currentControlRateProfile->thrMid8;
 701         lookupThrottleRC[i] = 10 * currentControlRateProfile->thrMid8 + tmp * (100 - currentControlRateProfile->thrExpo8 + (int32_t) currentControlRateProfile->thrExpo8 * (tmp * tmp) / (y * y)) / 10;
 702         lookupThrottleRC[i] = PWM_RANGE_MIN + PWM_RANGE * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
 703     }
 704
 705     switch (currentControlRateProfile->rates_type) {
 706     case RATES_TYPE_BETAFLIGHT:
 707     default:
 708         applyRates = applyBetaflightRates;
 709         break;
 710     case RATES_TYPE_RACEFLIGHT:
 711         applyRates = applyRaceFlightRates;
 712         break;
 713     case RATES_TYPE_KISS:
 714         applyRates = applyKissRates;
 715         break;
 716     case RATES_TYPE_ACTUAL:
 717         applyRates = applyActualRates;
 718         break;
 719     case RATES_TYPE_QUICK:
 720         applyRates = applyQuickRates;
 721         break;
 722     }
 723
 724 #ifdef USE_YAW_SPIN_RECOVERY
 725     const int maxYawRate = (int)applyRates(FD_YAW, 1.0f, 1.0f);
 726     initYawSpinRecovery(maxYawRate);
 727 #endif
 728 }
 729
 730 // send rc smoothing details to blackbox
 731 #ifdef USE_RC_SMOOTHING_FILTER
 732 rcSmoothingFilter_t *getRcSmoothingData(void)
 733 {
 734     return &rcSmoothingData;
 735 }
 736
 737 bool rcSmoothingInitializationComplete(void)
 738 {
 739     return rcSmoothingData.filterInitialized;
 740 }
 741 #endif // USE_RC_SMOOTHING_FILTER