src/main/common/pwl.c

   1 /*
   2  * This file is part of Betaflight.
   3  *
   4  * Betaflight is free software. You can redistribute this software
   5  * and/or modify this software under the terms of the GNU General
   6  * Public License as published by the Free Software Foundation,
   7  * either version 3 of the License, or (at your option) any later
   8  * version.
   9  *
  10  * Betaflight is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  *
  14  * See the GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public
  17  * License along with this software.
  18  *
  19  * If not, see <http://www.gnu.org/licenses/>.
  20  */
  21
  22 #include "platform.h"
  23
  24 #include "pwl.h"
  25
  26 void pwlInitialize(pwl_t *pwl, float *yValues, int numPoints, float xMin, float xMax) {
  27     pwl->yValues = yValues;
  28     pwl->numPoints = numPoints;
  29     pwl->xMin = xMin;
  30     pwl->xMax = xMax;
  31     pwl->dx = (xMax - xMin) / (numPoints - 1);
  32 }
  33
  34 void pwlFill(pwl_t *pwl, float (*function)(float, void*), void *args)
  35 {
  36     for (int i = 0; i < pwl->numPoints; ++i) {
  37         const float x = pwl->xMin + i * pwl->dx;
  38         pwl->yValues[i] = function(x, args);
  39     }
  40 }
  41
  42 float pwlInterpolate(const pwl_t *pwl, float x)
  43 {
  44     if (x <= pwl->xMin) {
  45         return pwl->yValues[0];
  46     }
  47
  48     if (x >= pwl->xMax) {
  49         return pwl->yValues[pwl->numPoints - 1];
  50     }
  51
  52     const int index = (int)((x - pwl->xMin) / pwl->dx);
  53     if (index >= pwl->numPoints - 1) {
  54         return pwl->yValues[pwl->numPoints - 1];
  55     }
  56
  57     const float x0 = pwl->xMin + index * pwl->dx;
  58     const float y0 = pwl->yValues[index];
  59     const float y1 = pwl->yValues[index + 1];
  60
  61     return y0 + (x - x0) * (y1 - y0) / pwl->dx;
  62 }