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/maths.h"
  27 #include "common/axis.h"
  28 #include "common/utils.h"
  29 #include "common/filter.h"
  30
  31 #include "config/feature.h"
  32 #include "config/parameter_group.h"
  33 #include "config/parameter_group_ids.h"
  34
  35 #include "fc/config.h"
  36 #include "fc/controlrate_profile.h"
  37 #include "fc/fc_core.h"
  38 #include "fc/fc_rc.h"
  39 #include "fc/rc_controls.h"
  40 #include "fc/runtime_config.h"
  41
  42 #include "rx/rx.h"
  43
  44 #include "scheduler/scheduler.h"
  45
  46 #include "flight/failsafe.h"
  47 #include "flight/imu.h"
  48 #include "flight/pid.h"
  49
  50 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
  51 static float throttlePIDAttenuation;
  52
  53 float getSetpointRate(int axis) {
  54     return setpointRate[axis];
  55 }
  56
  57 float getRcDeflection(int axis) {
  58     return rcDeflection[axis];
  59 }
  60
  61 float getRcDeflectionAbs(int axis) {
  62     return rcDeflectionAbs[axis];
  63 }
  64
  65 float getThrottlePIDAttenuation(void) {
  66     return throttlePIDAttenuation;
  67 }
  68
  69 #define THROTTLE_LOOKUP_LENGTH 12
  70 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH];    // lookup table for expo & mid THROTTLE
  71
  72 void generateThrottleCurve(void)
  73 {
  74     uint8_t i;
  75
  76     for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
  77         int16_t tmp = 10 * i - currentControlRateProfile->thrMid8;
  78         uint8_t y = 1;
  79         if (tmp > 0)
  80             y = 100 - currentControlRateProfile->thrMid8;
  81         if (tmp < 0)
  82             y = currentControlRateProfile->thrMid8;
  83         lookupThrottleRC[i] = 10 * currentControlRateProfile->thrMid8 + tmp * (100 - currentControlRateProfile->thrExpo8 + (int32_t) currentControlRateProfile->thrExpo8 * (tmp * tmp) / (y * y)) / 10;
  84         lookupThrottleRC[i] = PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
  85     }
  86 }
  87
  88 int16_t rcLookupThrottle(int32_t tmp)
  89 {
  90     const int32_t tmp2 = tmp / 100;
  91     // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
  92     return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;
  93 }
  94
  95 #define SETPOINT_RATE_LIMIT 1998.0f
  96 #define RC_RATE_INCREMENTAL 14.54f
  97
  98 static void calculateSetpointRate(int axis)
  99 {
 100     uint8_t rcExpo;
 101     float rcRate;
 102     if (axis != YAW) {
 103         rcExpo = currentControlRateProfile->rcExpo8;
 104         rcRate = currentControlRateProfile->rcRate8 / 100.0f;
 105     } else {
 106         rcExpo = currentControlRateProfile->rcYawExpo8;
 107         rcRate = currentControlRateProfile->rcYawRate8 / 100.0f;
 108     }
 109     if (rcRate > 2.0f) {
 110         rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f);
 111     }
 112
 113     float rcCommandf = rcCommand[axis] / 500.0f;
 114     rcDeflection[axis] = rcCommandf;
 115     const float rcCommandfAbs = ABS(rcCommandf);
 116     rcDeflectionAbs[axis] = rcCommandfAbs;
 117
 118     if (rcExpo) {
 119         const float expof = rcExpo / 100.0f;
 120         rcCommandf = rcCommandf * power3(rcCommandfAbs) * expof + rcCommandf * (1-expof);
 121     }
 122
 123     float angleRate = 200.0f * rcRate * rcCommandf;
 124     if (currentControlRateProfile->rates[axis]) {
 125         const float rcSuperfactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
 126         angleRate *= rcSuperfactor;
 127     }
 128
 129     DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
 130
 131     setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec)
 132 }
 133
 134 static void scaleRcCommandToFpvCamAngle(void) {
 135     //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
 136     static uint8_t lastFpvCamAngleDegrees = 0;
 137     static float cosFactor = 1.0;
 138     static float sinFactor = 0.0;
 139
 140     if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees){
 141         lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
 142         cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD);
 143         sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
 144     }
 145
 146     float roll = setpointRate[ROLL];
 147     float yaw = setpointRate[YAW];
 148     setpointRate[ROLL] = constrainf(roll * cosFactor -  yaw * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
 149     setpointRate[YAW]  = constrainf(yaw  * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
 150 }
 151
 152 #define THROTTLE_BUFFER_MAX 20
 153 #define THROTTLE_DELTA_MS 100
 154
 155  static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) {
 156     static int index;
 157     static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX];
 158     const int rxRefreshRateMs = rxRefreshRate / 1000;
 159     const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX);
 160     const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold;
 161
 162     rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE];
 163     if (index >= indexMax)
 164         index = 0;
 165
 166     const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
 167
 168     if(ABS(rcCommandSpeed) > throttleVelocityThreshold)
 169         pidSetItermAccelerator(0.0001f * currentPidProfile->itermAcceleratorGain);
 170     else
 171         pidSetItermAccelerator(1.0f);
 172 }
 173
 174 void processRcCommand(void)
 175 {
 176     static int16_t lastCommand[4] = { 0, 0, 0, 0 };
 177     static int16_t deltaRC[4] = { 0, 0, 0, 0 };
 178     static int16_t factor, rcInterpolationFactor;
 179     static uint16_t currentRxRefreshRate;
 180     const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2;
 181     uint16_t rxRefreshRate;
 182     bool readyToCalculateRate = false;
 183     uint8_t readyToCalculateRateAxisCnt = 0;
 184
 185     if (isRXDataNew) {
 186         currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
 187         if (isAntiGravityModeActive()) {
 188             checkForThrottleErrorResetState(currentRxRefreshRate);
 189         }
 190     }
 191
 192     if (rxConfig()->rcInterpolation || flightModeFlags) {
 193          // Set RC refresh rate for sampling and channels to filter
 194         switch(rxConfig()->rcInterpolation) {
 195             case(RC_SMOOTHING_AUTO):
 196                 rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps
 197                 break;
 198             case(RC_SMOOTHING_MANUAL):
 199                 rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval;
 200                 break;
 201             case(RC_SMOOTHING_OFF):
 202             case(RC_SMOOTHING_DEFAULT):
 203             default:
 204                 rxRefreshRate = rxGetRefreshRate();
 205         }
 206
 207         if (isRXDataNew) {
 208             rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;
 209
 210             if (debugMode == DEBUG_RC_INTERPOLATION) {
 211                 for(int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis];
 212                 debug[3] = rxRefreshRate;
 213             }
 214
 215             for (int channel=ROLL; channel < interpolationChannels; channel++) {
 216                 deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
 217                 lastCommand[channel] = rcCommand[channel];
 218             }
 219
 220             factor = rcInterpolationFactor - 1;
 221         } else {
 222             factor--;
 223         }
 224
 225         // Interpolate steps of rcCommand
 226         if (factor > 0) {
 227             for (int channel=ROLL; channel < interpolationChannels; channel++) {
 228                 rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
 229                 readyToCalculateRateAxisCnt = MAX(channel,FD_YAW); // throttle channel doesn't require rate calculation
 230                 readyToCalculateRate = true;
 231             }
 232         } else {
 233             factor = 0;
 234         }
 235     } else {
 236         factor = 0; // reset factor in case of level modes flip flopping
 237     }
 238
 239     if (readyToCalculateRate || isRXDataNew) {
 240         if (isRXDataNew)
 241             readyToCalculateRateAxisCnt = FD_YAW;
 242
 243         for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++)
 244             calculateSetpointRate(axis);
 245
 246         // Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
 247         if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
 248             scaleRcCommandToFpvCamAngle();
 249
 250         isRXDataNew = false;
 251     }
 252 }
 253
 254 void updateRcCommands(void)
 255 {
 256     // PITCH & ROLL only dynamic PID adjustment,  depending on throttle value
 257     int32_t prop;
 258     if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
 259         prop = 100;
 260         throttlePIDAttenuation = 1.0f;
 261     } else {
 262         if (rcData[THROTTLE] < 2000) {
 263             prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
 264         } else {
 265             prop = 100 - currentControlRateProfile->dynThrPID;
 266         }
 267         throttlePIDAttenuation = prop / 100.0f;
 268     }
 269
 270     for (int axis = 0; axis < 3; axis++) {
 271         // non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
 272
 273         int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
 274         if (axis == ROLL || axis == PITCH) {
 275             if (tmp > rcControlsConfig()->deadband) {
 276                 tmp -= rcControlsConfig()->deadband;
 277             } else {
 278                 tmp = 0;
 279             }
 280             rcCommand[axis] = tmp;
 281         } else {
 282             if (tmp > rcControlsConfig()->yaw_deadband) {
 283                 tmp -= rcControlsConfig()->yaw_deadband;
 284             } else {
 285                 tmp = 0;
 286             }
 287             rcCommand[axis] = tmp * -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
 288         }
 289         if (rcData[axis] < rxConfig()->midrc) {
 290             rcCommand[axis] = -rcCommand[axis];
 291         }
 292     }
 293
 294     int32_t tmp;
 295     if (feature(FEATURE_3D)) {
 296         tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
 297         tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
 298     } else {
 299         tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
 300         tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);
 301     }
 302
 303     rcCommand[THROTTLE] = rcLookupThrottle(tmp);
 304
 305     if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
 306         fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 307         rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
 308     }
 309
 310     if (FLIGHT_MODE(HEADFREE_MODE)) {
 311         const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
 312         const float cosDiff = cos_approx(radDiff);
 313         const float sinDiff = sin_approx(radDiff);
 314         const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
 315         rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
 316         rcCommand[PITCH] = rcCommand_PITCH;
 317     }
 318 }
 319
 320 void resetYawAxis(void) {
 321     rcCommand[YAW] = 0;
 322     setpointRate[YAW] = 0;
 323 }