flight/libraries/math/mathmisc.h

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilot Math Utilities
   4  * @{
   5  * @addtogroup Reuseable math functions
   6  * @{
   7  *
   8  * @file       mathmisc.h
   9  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  10  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  11  * @brief      Reuseable math functions
  12  *
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  *****************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 #ifndef MATHMISC_H
  33 #define MATHMISC_H
  34
  35 #include <math.h>
  36 #include <stdint.h>
  37
  38 typedef struct {
  39     float p1;
  40     float p2;
  41     float new_sma;
  42     float new_smsa;
  43 } pw_variance_t;
  44
  45 /***
  46  * initialize pseudo windowed
  47  * @param variance the instance to be initialized
  48  * @param window_size size of the sample window
  49  */
  50 void pseudo_windowed_variance_init(pw_variance_t *variance, int32_t window_size);
  51
  52 /***
  53  * Push a new sample
  54  * @param variance the working instance
  55  * @param sample the new sample
  56  */
  57 static inline void pseudo_windowed_variance_push_sample(pw_variance_t *variance, float sample)
  58 {
  59     variance->new_sma  = variance->new_sma * variance->p2 + sample * variance->p1;
  60     variance->new_smsa = variance->new_smsa * variance->p2 + sample * sample * variance->p1;
  61 }
  62
  63 /***
  64  * Get the current variance value
  65  * @param variance the working instance
  66  * @return
  67  */
  68 float pseudo_windowed_variance_get(pw_variance_t *variance);
  69
  70 // returns min(boundary1,boundary2) if val<min(boundary1,boundary2)
  71 // returns max(boundary1,boundary2) if val>max(boundary1,boundary2)
  72 // returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2)
  73 static inline float boundf(float val, float boundary1, float boundary2)
  74 {
  75     if (boundary1 > boundary2) {
  76         if (!(val >= boundary2)) {
  77             return boundary2;
  78         } else if (!(val <= boundary1)) {
  79             return boundary1;
  80         }
  81     } else {
  82         if (!(val >= boundary1)) {
  83             return boundary1;
  84         } else if (!(val <= boundary2)) {
  85             return boundary2;
  86         }
  87     }
  88     return val;
  89 }
  90
  91 static inline float squaref(float x)
  92 {
  93     return x * x;
  94 }
  95
  96 static inline float vector_lengthf(float *vector, const uint8_t dim)
  97 {
  98     float length = 0.0f;
  99
 100     for (int t = 0; t < dim; t++) {
 101         length += squaref(vector[t]);
 102     }
 103     return sqrtf(length);
 104 }
 105
 106 static inline void vector_normalizef(float *vector, const uint8_t dim)
 107 {
 108     float length = vector_lengthf(vector, dim);
 109
 110     if (length <= 0.0f || isnan(length)) {
 111         return;
 112     }
 113     for (int t = 0; t < dim; t++) {
 114         vector[t] /= length;
 115     }
 116 }
 117
 118 typedef struct pointf {
 119     float x;
 120     float y;
 121 } pointf;
 122
 123 // Returns the y value, given x, on the line passing through the points p0 and p1.
 124 static inline float y_on_line(float x, const pointf *p0, const pointf *p1)
 125 {
 126     // Setup line y = m * x + b.
 127     const float dY1 = p1->y - p0->y;
 128     const float dX1 = p1->x - p0->x;
 129     const float m   = dY1 / dX1; // == dY0 / dX0 == (p0.y - b) / (p0.x - 0.0f) ==>
 130     const float b   = p0->y - m * p0->x;
 131
 132     // Get the y value on the line.
 133     return m * x + b;
 134 }
 135
 136 // Returns the y value, given x, on the curve defined by the points array.
 137 // The fist and last line of the curve extends beyond the first resp. last points.
 138 static inline float y_on_curve(float x, const pointf points[], int num_points)
 139 {
 140     // Find the two points x is within.
 141     // If x is smaller than the first point's x value, use the first line of the curve.
 142     // If x is larger than the last point's x value, user the last line of the curve.
 143     int end_point = num_points - 1;
 144
 145     for (int i = 1; i < num_points; i++) {
 146         if (x < points[i].x) {
 147             end_point = i;
 148             break;
 149         }
 150     }
 151
 152     // Find the y value on the selected line.
 153     return y_on_line(x, &points[end_point - 1], &points[end_point]);
 154 }
 155
 156 static inline float invsqrtf(float number)
 157 {
 158     float y;
 159
 160     y = 1.0f / sqrtf(number);
 161     return y;
 162 }
 163
 164 /**
 165  * Ultrafast pow() aproximation needed for expo
 166  * Based on Algorithm by Martin Ankerl
 167  */
 168 static inline float fastPow(float a, float b)
 169 {
 170     union {
 171         double  d;
 172         int32_t x[2];
 173     } u = { (double)a };
 174     u.x[1] = (int32_t)(b * (u.x[1] - 1072632447) + 1072632447);
 175     u.x[0] = 0;
 176     return (float)u.d;
 177 }
 178
 179 #endif /* MATHMISC_H */