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