src/main/fc/rc_curves.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 "common/maths.h"
  25
  26 #include "fc/controlrate_profile.h"
  27 #include "fc/rc_controls.h"
  28 #include "fc/rc_curves.h"
  29
  30 #include "flight/mixer.h"
  31
  32 #include "rx/rx.h"
  33
  34 #define THROTTLE_LOOKUP_LENGTH 11
  35
  36 static EXTENDED_FASTRAM int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH];    // lookup table for expo & mid THROTTLE
  37 int16_t lookupThrottleRCMid;                         // THROTTLE curve mid point
  38
  39 void generateThrottleCurve(const controlRateConfig_t *controlRateConfig)
  40 {
  41     const int minThrottle = getThrottleIdleValue();
  42     lookupThrottleRCMid = minThrottle + (int32_t)(getMaxThrottle() - minThrottle) * controlRateConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE]
  43
  44     for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
  45         const int16_t tmp = 10 * i - controlRateConfig->throttle.rcMid8;
  46         uint8_t y = 1;
  47         if (tmp > 0)
  48             y = 100 - controlRateConfig->throttle.rcMid8;
  49         if (tmp < 0)
  50             y = controlRateConfig->throttle.rcMid8;
  51         lookupThrottleRC[i] = 10 * controlRateConfig->throttle.rcMid8 + tmp * (100 - controlRateConfig->throttle.rcExpo8 + (int32_t) controlRateConfig->throttle.rcExpo8 * (tmp * tmp) / (y * y)) / 10;
  52         lookupThrottleRC[i] = minThrottle + (int32_t) (getMaxThrottle() - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
  53     }
  54 }
  55
  56 int16_t rcLookup(int32_t stickDeflection, uint8_t expo)
  57 {
  58     float tmpf = stickDeflection / 100.0f;
  59     return lrintf((2500.0f + (float)expo * (tmpf * tmpf - 25.0f)) * tmpf / 25.0f);
  60 }
  61
  62 uint16_t rcLookupThrottle(uint16_t absoluteDeflection)
  63 {
  64     if (absoluteDeflection > 999)
  65         return getMaxThrottle();
  66
  67     const uint8_t lookupStep = absoluteDeflection / 100;
  68     return lookupThrottleRC[lookupStep] + (absoluteDeflection - lookupStep * 100) * (lookupThrottleRC[lookupStep + 1] - lookupThrottleRC[lookupStep]) / 100;
  69 }
  70
  71 int16_t rcLookupThrottleMid(void)
  72 {
  73     return lookupThrottleRCMid;
  74 }