src/main/fc/fc_rc.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <math.h>
  21
  22 #include "platform.h"
  23
  24 #include "build/debug.h"
  25
  26 #include "common/axis.h"
  27 #include "common/maths.h"
  28 #include "common/utils.h"
  29
  30 #include "config/feature.h"
  31
  32 #include "fc/config.h"
  33 #include "fc/controlrate_profile.h"
  34 #include "fc/fc_core.h"
  35 #include "fc/fc_rc.h"
  36 #include "fc/rc_controls.h"
  37 #include "fc/rc_modes.h"
  38 #include "fc/runtime_config.h"
  39
  40 #include "flight/failsafe.h"
  41 #include "flight/imu.h"
  42 #include "flight/pid.h"
  43 #include "rx/rx.h"
  44
  45 #include "scheduler/scheduler.h"
  46
  47 #include "sensors/battery.h"
  48
  49 typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs);
  50
  51 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
  52 static float throttlePIDAttenuation;
  53 static bool reverseMotors = false;
  54 static applyRatesFn *applyRates;
  55
  56 float getSetpointRate(int axis)
  57 {
  58     return setpointRate[axis];
  59 }
  60
  61 float getRcDeflection(int axis)
  62 {
  63     return rcDeflection[axis];
  64 }
  65
  66 float getRcDeflectionAbs(int axis)
  67 {
  68     return rcDeflectionAbs[axis];
  69 }
  70
  71 float getThrottlePIDAttenuation(void)
  72 {
  73     return throttlePIDAttenuation;
  74 }
  75
  76 #define THROTTLE_LOOKUP_LENGTH 12
  77 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH];    // lookup table for expo & mid THROTTLE
  78
  79 static int16_t rcLookupThrottle(int32_t tmp)
  80 {
  81     const int32_t tmp2 = tmp / 100;
  82     // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
  83     return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;
  84 }
  85
  86 #define SETPOINT_RATE_LIMIT 1998.0f
  87 #define RC_RATE_INCREMENTAL 14.54f
  88
  89 float applyBetaflightRates(const int axis, float rcCommandf, const float rcCommandfAbs)
  90 {
  91     if (currentControlRateProfile->rcExpo[axis]) {
  92         const float expof = currentControlRateProfile->rcExpo[axis] / 100.0f;
  93         rcCommandf = rcCommandf * power3(rcCommandfAbs) * expof + rcCommandf * (1 - expof);
  94     }
  95
  96     float rcRate = currentControlRateProfile->rcRates[axis] / 100.0f;
  97     if (rcRate > 2.0f) {
  98         rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f);
  99     }
 100     float angleRate = 200.0f * rcRate * rcCommandf;
 101     if (currentControlRateProfile->rates[axis]) {
 102         const float rcSuperfactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
 103         angleRate *= rcSuperfactor;
 104     }
 105
 106     return angleRate;
 107 }
 108
 109 float applyRaceFlightRates(const int axis, float rcCommandf, const float rcCommandfAbs)
 110 {
 111     // -1.0 to 1.0 ranged and curved
 112     rcCommandf = ((1.0f + 0.01f * currentControlRateProfile->rcExpo[axis] * (rcCommandf * rcCommandf - 1.0f)) * rcCommandf);
 113     // convert to -2000 to 2000 range using acro+ modifier
 114     float angleRate = 10.0f * currentControlRateProfile->rcRates[axis] * rcCommandf;
 115     angleRate = angleRate * (1 + rcCommandfAbs * (float)currentControlRateProfile->rates[axis] * 0.01f);
 116
 117     return angleRate;
 118 }
 119
 120 static void calculateSetpointRate(int axis)
 121 {
 122     // scale rcCommandf to range [-1.0, 1.0]
 123     float rcCommandf = rcCommand[axis] / 500.0f;
 124     rcDeflection[axis] = rcCommandf;
 125     const float rcCommandfAbs = ABS(rcCommandf);
 126     rcDeflectionAbs[axis] = rcCommandfAbs;
 127
 128     float angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
 129
 130     DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
 131
 132     setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec)
 133 }
 134
 135 static void scaleRcCommandToFpvCamAngle(void)
 136 {
 137     //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
 138     static uint8_t lastFpvCamAngleDegrees = 0;
 139     static float cosFactor = 1.0;
 140     static float sinFactor = 0.0;
 141
 142     if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees) {
 143         lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
 144         cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD);
 145         sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
 146     }
 147
 148     float roll = setpointRate[ROLL];
 149     float yaw = setpointRate[YAW];
 150     setpointRate[ROLL] = constrainf(roll * cosFactor -  yaw * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
 151     setpointRate[YAW]  = constrainf(yaw  * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
 152 }
 153
 154 #define THROTTLE_BUFFER_MAX 20
 155 #define THROTTLE_DELTA_MS 100
 156
 157 static void checkForThrottleErrorResetState(uint16_t rxRefreshRate)
 158 {
 159     static int index;
 160     static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX];
 161
 162     const int rxRefreshRateMs = rxRefreshRate / 1000;
 163     const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX);
 164     const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold;
 165
 166     rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE];
 167     if (index >= indexMax) {
 168         index = 0;
 169     }
 170
 171     const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
 172
 173     if (ABS(rcCommandSpeed) > throttleVelocityThreshold) {
 174         pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
 175     } else {
 176         pidSetItermAccelerator(1.0f);
 177     }
 178 }
 179
 180 FAST_CODE NOINLINE void processRcCommand(void)
 181 {
 182     static float rcCommandInterp[4] = { 0, 0, 0, 0 };
 183     static float rcStepSize[4] = { 0, 0, 0, 0 };
 184     static int16_t rcInterpolationStepCount;
 185     static uint16_t currentRxRefreshRate;
 186
 187     if (isRXDataNew) {
 188         currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
 189         if (isAntiGravityModeActive()) {
 190             checkForThrottleErrorResetState(currentRxRefreshRate);
 191         }
 192     }
 193
 194     const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT"
 195     uint16_t rxRefreshRate;
 196     uint8_t updatedChannel = 0;
 197
 198     if (rxConfig()->rcInterpolation) {
 199          // Set RC refresh rate for sampling and channels to filter
 200         switch (rxConfig()->rcInterpolation) {
 201         case RC_SMOOTHING_AUTO:
 202             rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps
 203             break;
 204         case RC_SMOOTHING_MANUAL:
 205             rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval;
 206             break;
 207         case RC_SMOOTHING_OFF:
 208         case RC_SMOOTHING_DEFAULT:
 209         default:
 210             rxRefreshRate = rxGetRefreshRate();
 211         }
 212
 213         if (isRXDataNew && rxRefreshRate > 0) {
 214             rcInterpolationStepCount = rxRefreshRate / targetPidLooptime;
 215
 216             for (int channel = ROLL; channel < interpolationChannels; channel++) {
 217                 rcStepSize[channel] = (rcCommand[channel] - rcCommandInterp[channel]) / (float)rcInterpolationStepCount;
 218             }
 219
 220             if (debugMode == DEBUG_RC_INTERPOLATION) {
 221                 debug[0] = lrintf(rcCommand[0]);
 222                 debug[1] = lrintf(getTaskDeltaTime(TASK_RX) / 1000);
 223                 //debug[1] = lrintf(rcCommandInterp[0]);
 224                 //debug[1] = lrintf(rcStepSize[0]*100);
 225             }
 226         } else {
 227             rcInterpolationStepCount--;
 228         }
 229
 230         // Interpolate steps of rcCommand
 231         if (rcInterpolationStepCount > 0) {
 232             for (updatedChannel = ROLL; updatedChannel < interpolationChannels; updatedChannel++) {
 233                 rcCommandInterp[updatedChannel] += rcStepSize[updatedChannel];
 234                 rcCommand[updatedChannel] = rcCommandInterp[updatedChannel];
 235             }
 236         }
 237     } else {
 238         rcInterpolationStepCount = 0; // reset factor in case of level modes flip flopping
 239     }
 240
 241     if (isRXDataNew || updatedChannel) {
 242         const uint8_t maxUpdatedAxis = isRXDataNew ? FD_YAW : MIN(updatedChannel, FD_YAW); // throttle channel doesn't require rate calculation
 243 #if defined(SITL)
 244 #pragma GCC diagnostic push
 245 #pragma GCC diagnostic ignored "-Wunsafe-loop-optimizations"
 246 #endif
 247         for (int axis = FD_ROLL; axis <= maxUpdatedAxis; axis++) {
 248 #if defined(SITL)
 249 #pragma GCC diagnostic pop
 250 #endif
 251             calculateSetpointRate(axis);
 252         }
 253
 254         if (debugMode == DEBUG_RC_INTERPOLATION) {
 255             debug[2] = rcInterpolationStepCount;
 256             debug[3] = setpointRate[0];
 257         }
 258
 259         // Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
 260         if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
 261             scaleRcCommandToFpvCamAngle();
 262         }
 263     }
 264
 265     if (isRXDataNew) {
 266         isRXDataNew = false;
 267     }
 268 }
 269
 270 FAST_CODE NOINLINE void updateRcCommands(void)
 271 {
 272     // PITCH & ROLL only dynamic PID adjustment,  depending on throttle value
 273     int32_t prop;
 274     if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
 275         prop = 100;
 276         throttlePIDAttenuation = 1.0f;
 277     } else {
 278         if (rcData[THROTTLE] < 2000) {
 279             prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
 280         } else {
 281             prop = 100 - currentControlRateProfile->dynThrPID;
 282         }
 283         throttlePIDAttenuation = prop / 100.0f;
 284     }
 285
 286     for (int axis = 0; axis < 3; axis++) {
 287         // non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
 288
 289         int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
 290         if (axis == ROLL || axis == PITCH) {
 291             if (tmp > rcControlsConfig()->deadband) {
 292                 tmp -= rcControlsConfig()->deadband;
 293             } else {
 294                 tmp = 0;
 295             }
 296             rcCommand[axis] = tmp;
 297         } else {
 298             if (tmp > rcControlsConfig()->yaw_deadband) {
 299                 tmp -= rcControlsConfig()->yaw_deadband;
 300             } else {
 301                 tmp = 0;
 302             }
 303             rcCommand[axis] = tmp * -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
 304         }
 305         if (rcData[axis] < rxConfig()->midrc) {
 306             rcCommand[axis] = -rcCommand[axis];
 307         }
 308     }
 309
 310     int32_t tmp;
 311     if (feature(FEATURE_3D)) {
 312         tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
 313         tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
 314     } else {
 315         tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
 316         tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);
 317     }
 318
 319     if (getLowVoltageCutoff()->enabled) {
 320         tmp = tmp * getLowVoltageCutoff()->percentage / 100;
 321     }
 322
 323     rcCommand[THROTTLE] = rcLookupThrottle(tmp);
 324
 325     if (feature(FEATURE_3D) && !failsafeIsActive()) {
 326         if (!flight3DConfig()->switched_mode3d) {
 327             if (IS_RC_MODE_ACTIVE(BOX3D)) {
 328                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 329                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
 330             }
 331         } else {
 332             if (IS_RC_MODE_ACTIVE(BOX3D)) {
 333                 reverseMotors = true;
 334                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 335                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MIN - rxConfig()->midrc);
 336             } else {
 337                 reverseMotors = false;
 338                 fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 339                 rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
 340             }
 341         }
 342     }
 343     if (FLIGHT_MODE(HEADFREE_MODE)) {
 344         static t_fp_vector_def  rcCommandBuff;
 345
 346         rcCommandBuff.X = rcCommand[ROLL];
 347         rcCommandBuff.Y = rcCommand[PITCH];
 348         if ((!FLIGHT_MODE(ANGLE_MODE)&&(!FLIGHT_MODE(HORIZON_MODE)))) {
 349             rcCommandBuff.Z = rcCommand[YAW];
 350         } else {
 351             rcCommandBuff.Z = 0;
 352         }
 353         imuQuaternionHeadfreeTransformVectorEarthToBody(&rcCommandBuff);
 354         rcCommand[ROLL] = rcCommandBuff.X;
 355         rcCommand[PITCH] = rcCommandBuff.Y;
 356         if ((!FLIGHT_MODE(ANGLE_MODE)&&(!FLIGHT_MODE(HORIZON_MODE)))) {
 357             rcCommand[YAW] = rcCommandBuff.Z;
 358         }
 359     }
 360 }
 361
 362 void resetYawAxis(void)
 363 {
 364     rcCommand[YAW] = 0;
 365     setpointRate[YAW] = 0;
 366 }
 367
 368 bool isMotorsReversed(void)
 369 {
 370     return reverseMotors;
 371 }
 372
 373 void initRcProcessing(void)
 374 {
 375     for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
 376         const int16_t tmp = 10 * i - currentControlRateProfile->thrMid8;
 377         uint8_t y = 1;
 378         if (tmp > 0)
 379             y = 100 - currentControlRateProfile->thrMid8;
 380         if (tmp < 0)
 381             y = currentControlRateProfile->thrMid8;
 382         lookupThrottleRC[i] = 10 * currentControlRateProfile->thrMid8 + tmp * (100 - currentControlRateProfile->thrExpo8 + (int32_t) currentControlRateProfile->thrExpo8 * (tmp * tmp) / (y * y)) / 10;
 383         lookupThrottleRC[i] = PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
 384     }
 385
 386     switch (currentControlRateProfile->rates_type) {
 387     case RATES_TYPE_BETAFLIGHT:
 388     default:
 389         applyRates = applyBetaflightRates;
 390
 391         break;
 392     case RATES_TYPE_RACEFLIGHT:
 393         applyRates = applyRaceFlightRates;
 394
 395         break;
 396     }
 397 }