src/main/flight/rate_dynamics.c

   1 /*
   2  * This file is part of the INAV distribution https://github.com/iNavFlight/inav.
   3  *
   4  * This program 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, version 3.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License
  14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  15  *
  16  * The code in this file is a derivative work of EmuFlight distribution https://github.com/emuflight/EmuFlight/
  17  *
  18  */
  19 #include "platform.h"
  20
  21 #ifdef USE_RATE_DYNAMICS
  22
  23 #include <stdlib.h>
  24 #include "rate_dynamics.h"
  25 #include "fc/controlrate_profile.h"
  26 #include <math.h>
  27
  28 static FASTRAM float lastRcCommandData[3];
  29 static FASTRAM float iterm[3];
  30
  31 FAST_CODE static float calculateK(const float k, const float dT) {
  32     if (k == 0.0f) {
  33         return 0;
  34     }
  35     // scale so it feels like running at 62.5hz (16ms) regardless of the current rx rate
  36
  37     // The original code is:
  38     // const float rxRate = 1.0f / dT;
  39     // const float rxRateFactor = (rxRate / 62.5f) * rxRate;
  40     // const float freq = k / ((1.0f / rxRateFactor) * (1.0f - k));
  41     // const float RC = 1.0f / freq;
  42     // return dT / (RC + dT);
  43
  44     // This can be simplified to (while saving 128B of flash on F722):
  45
  46     return k / (62.5f * dT * (1 - k) + k);
  47 }
  48
  49 FAST_CODE int applyRateDynamics(int rcCommand, const int axis, const float dT) {
  50     if (
  51         currentControlRateProfile->rateDynamics.sensitivityCenter != 100 ||
  52         currentControlRateProfile->rateDynamics.sensitivityEnd != 100 ||
  53         currentControlRateProfile->rateDynamics.weightCenter > 0 ||
  54         currentControlRateProfile->rateDynamics.weightEnd > 0
  55     ) {
  56
  57         float pterm_centerStick, pterm_endStick, pterm, iterm_centerStick, iterm_endStick, dterm_centerStick, dterm_endStick, dterm;
  58         float rcCommandPercent;
  59         float rcCommandError;
  60         float inverseRcCommandPercent;
  61
  62         rcCommandPercent = abs(rcCommand) / 500.0f; // make rcCommandPercent go from 0 to 1
  63         inverseRcCommandPercent = 1.0f - rcCommandPercent;
  64
  65         pterm_centerStick = inverseRcCommandPercent * rcCommand * (currentControlRateProfile->rateDynamics.sensitivityCenter / 100.0f); // valid pterm values are between 50-150
  66         pterm_endStick = rcCommandPercent * rcCommand * (currentControlRateProfile->rateDynamics.sensitivityEnd / 100.0f);
  67         pterm = pterm_centerStick + pterm_endStick;
  68         rcCommandError = rcCommand - (pterm + iterm[axis]);
  69         rcCommand = pterm; // add this fake pterm to the rcCommand
  70
  71         iterm_centerStick = inverseRcCommandPercent * rcCommandError * calculateK(currentControlRateProfile->rateDynamics.correctionCenter / 100.0f, dT); // valid iterm values are between 0-95
  72         iterm_endStick = rcCommandPercent * rcCommandError * calculateK(currentControlRateProfile->rateDynamics.correctionEnd / 100.0f, dT);
  73         iterm[axis] += iterm_centerStick + iterm_endStick;
  74         rcCommand = rcCommand + iterm[axis]; // add the iterm to the rcCommand
  75
  76         dterm_centerStick = inverseRcCommandPercent * (lastRcCommandData[axis] - rcCommand) * calculateK(currentControlRateProfile->rateDynamics.weightCenter / 100.0f, dT); // valid dterm values are between 0-95
  77         dterm_endStick = rcCommandPercent * (lastRcCommandData[axis] - rcCommand) * calculateK(currentControlRateProfile->rateDynamics.weightEnd / 100.0f, dT);
  78         dterm = dterm_centerStick + dterm_endStick;
  79         rcCommand = rcCommand + dterm; // add dterm to the rcCommand (this is real dterm)
  80
  81         lastRcCommandData[axis] = rcCommand;
  82     }
  83     return rcCommand;
  84 }
  85
  86 #endif