src/main/io/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
  21 #include "rx/rx.h"
  22 #include "io/rc_controls.h"
  23 #include "io/escservo.h"
  24
  25 #include "io/rc_curves.h"
  26
  27 #include "config/config.h"
  28
  29 #define THROTTLE_LOOKUP_LENGTH 12
  30 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH];    // lookup table for expo & mid THROTTLE
  31
  32 void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig)
  33 {
  34     uint8_t i;
  35     uint16_t minThrottle = (feature(FEATURE_3D && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) ? PWM_RANGE_MIN : escAndServoConfig->minthrottle);
  36
  37     for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
  38         int16_t tmp = 10 * i - controlRateConfig->thrMid8;
  39         uint8_t y = 1;
  40         if (tmp > 0)
  41             y = 100 - controlRateConfig->thrMid8;
  42         if (tmp < 0)
  43             y = controlRateConfig->thrMid8;
  44         lookupThrottleRC[i] = 10 * controlRateConfig->thrMid8 + tmp * (100 - controlRateConfig->thrExpo8 + (int32_t) controlRateConfig->thrExpo8 * (tmp * tmp) / (y * y)) / 10;
  45         lookupThrottleRC[i] = minThrottle + (int32_t) (escAndServoConfig->maxthrottle - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
  46     }
  47 }
  48
  49 int16_t rcLookupPitchRoll(int32_t tmp, controlRateConfig_t *controlRateConfig)
  50 {
  51     float tmpf = tmp / 100.0f;
  52     return (int16_t)((2500.0f + (float)controlRateConfig->rcExpo8 * (tmpf * tmpf - 25.0f)) * tmpf * (float)(controlRateConfig->rcRate8) / 2500.0f );
  53 }
  54
  55 int16_t rcLookupYaw(int32_t tmp, controlRateConfig_t *controlRateConfig)
  56 {
  57     float tmpf = tmp / 100.0f;
  58     return (int16_t)((2500.0f + (float)controlRateConfig->rcYawExpo8* (tmpf * tmpf - 25.0f)) * tmpf / 25.0f );
  59 }
  60
  61 int16_t rcLookupThrottle(int32_t tmp)
  62 {
  63     const int32_t tmp2 = tmp / 100;
  64     // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
  65     return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;
  66 }
  67