src/test/unit/maths_unittest.cc

   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 <stdint.h>
  19 #include <stdbool.h>
  20
  21 #include <limits.h>
  22
  23 #include <math.h>
  24
  25 #define USE_BARO
  26
  27 extern "C" {
  28     #include "common/maths.h"
  29 }
  30
  31 #include "unittest_macros.h"
  32 #include "gtest/gtest.h"
  33
  34
  35 TEST(MathsUnittest, TestScaleRange)
  36 {
  37     // Within bounds
  38     EXPECT_EQ(scaleRange(0, 0, 10, 0, 100), 0);
  39     EXPECT_EQ(scaleRange(10, 0, 10, 0, 100), 100);
  40     EXPECT_EQ(scaleRange(0, 0, 100, 0, 10), 0);
  41     EXPECT_EQ(scaleRange(100, 0, 100, 0, 10), 10);
  42
  43     // Scale up
  44     EXPECT_EQ(scaleRange(1, 0, 10, 0, 100), 10);
  45     EXPECT_EQ(scaleRange(2, 0, 10, 0, 100), 20);
  46     EXPECT_EQ(scaleRange(5, 0, 10, 0, 100), 50);
  47
  48     // Scale down
  49     EXPECT_EQ(scaleRange(10, 0, 100, 0, 10), 1);
  50     EXPECT_EQ(scaleRange(20, 0, 100, 0, 10), 2);
  51     EXPECT_EQ(scaleRange(50, 0, 100, 0, 10), 5);
  52 }
  53
  54 TEST(MathsUnittest, TestScaleRangeNegatives)
  55 {
  56     // Within bounds
  57     EXPECT_EQ(scaleRange(0, -10, 0, -100, 0), 0);
  58     EXPECT_EQ(scaleRange(-10, -10, 0, -100, 0), -100);
  59     EXPECT_EQ(scaleRange(0, -100, 0, -10, 0), 0);
  60     EXPECT_EQ(scaleRange(-100, -100, 0, -10, 0), -10);
  61
  62     // Scale up
  63     EXPECT_EQ(scaleRange(-1, -10, 0, -100, 0), -10);
  64     EXPECT_EQ(scaleRange(-2, -10, 0, -100, 0), -20);
  65     EXPECT_EQ(scaleRange(-5, -10, 0, -100, 0), -50);
  66
  67     // Scale down
  68     EXPECT_EQ(scaleRange(-10, -100, 0, -10, 0), -1);
  69     EXPECT_EQ(scaleRange(-20, -100, 0, -10, 0), -2);
  70     EXPECT_EQ(scaleRange(-50, -100, 0, -10, 0), -5);
  71 }
  72
  73 TEST(MathsUnittest, TestScaleRangeNegativePositive)
  74 {
  75     // Within bounds
  76     EXPECT_EQ(scaleRange(0, -10, 0, 0, 100), 100);
  77     EXPECT_EQ(scaleRange(-10, -10, 0, 0, 100), 0);
  78     EXPECT_EQ(scaleRange(0, -100, 0, 0, 10), 10);
  79     EXPECT_EQ(scaleRange(-100, -100, 0, 0, 10), 0);
  80
  81     // Scale up
  82     EXPECT_EQ(scaleRange(-1, -10, 0, 0, 100), 90);
  83     EXPECT_EQ(scaleRange(-2, -10, 0, 0, 100), 80);
  84     EXPECT_EQ(scaleRange(-5, -10, 0, 0, 100), 50);
  85
  86     // Scale down
  87     EXPECT_EQ(scaleRange(-10, -100, 0, 0, 10), 9);
  88     EXPECT_EQ(scaleRange(-20, -100, 0, 0, 10), 8);
  89     EXPECT_EQ(scaleRange(-50, -100, 0, 0, 10), 5);
  90 }
  91
  92 TEST(MathsUnittest, TestScaleRangeReverse)
  93 {
  94     // Within bounds
  95     EXPECT_EQ(scaleRange(0, 0, 10, 100, 0), 100);
  96     EXPECT_EQ(scaleRange(10, 0, 10, 100, 0), 0);
  97     EXPECT_EQ(scaleRange(0, 0, 100, 10, 0), 10);
  98     EXPECT_EQ(scaleRange(100, 0, 100, 10, 0), 0);
  99
 100     // Scale up
 101     EXPECT_EQ(scaleRange(1, 0, 10, 100, 0), 90);
 102     EXPECT_EQ(scaleRange(2, 0, 10, 100, 0), 80);
 103     EXPECT_EQ(scaleRange(5, 0, 10, 100, 0), 50);
 104
 105     // Scale down
 106     EXPECT_EQ(scaleRange(10, 0, 100, 10, 0), 9);
 107     EXPECT_EQ(scaleRange(20, 0, 100, 10, 0), 8);
 108     EXPECT_EQ(scaleRange(50, 0, 100, 10, 0), 5);
 109 }
 110
 111 TEST(MathsUnittest, TestConstrain)
 112 {
 113     // Within bounds
 114     EXPECT_EQ(constrain(0, 0, 0), 0);
 115     EXPECT_EQ(constrain(1, 1, 1), 1);
 116     EXPECT_EQ(constrain(1, 0, 2), 1);
 117
 118     // Equal to bottom bound.
 119     EXPECT_EQ(constrain(1, 1, 2), 1);
 120     // Equal to top bound.
 121     EXPECT_EQ(constrain(1, 0, 1), 1);
 122
 123     // Equal to both bottom and top bound.
 124     EXPECT_EQ(constrain(1, 1, 1), 1);
 125
 126     // Above top bound.
 127     EXPECT_EQ(constrain(2, 0, 1), 1);
 128     // Below bottom bound.
 129     EXPECT_EQ(constrain(0, 1, 2), 1);
 130 }
 131
 132 TEST(MathsUnittest, TestConstrainNegatives)
 133 {
 134     // Within bounds.
 135     EXPECT_EQ(constrain(-1, -1, -1), -1);
 136     EXPECT_EQ(constrain(-1, -2, 0), -1);
 137
 138     // Equal to bottom bound.
 139     EXPECT_EQ(constrain(-1, -1, 0), -1);
 140     // Equal to top bound.
 141     EXPECT_EQ(constrain(-1, -2, -1), -1);
 142
 143     // Equal to both bottom and top bound.
 144     EXPECT_EQ(constrain(-1, -1, -1), -1);
 145
 146     // Above top bound.
 147     EXPECT_EQ(constrain(-1, -3, -2), -2);
 148     // Below bottom bound.
 149     EXPECT_EQ(constrain(-3, -2, -1), -2);
 150 }
 151
 152 TEST(MathsUnittest, TestConstrainf)
 153 {
 154     // Within bounds.
 155     EXPECT_FLOAT_EQ(constrainf(1.0f, 0.0f, 2.0f), 1.0f);
 156
 157     // Equal to bottom bound.
 158     EXPECT_FLOAT_EQ(constrainf(1.0f, 1.0f, 2.0f), 1.0f);
 159     // Equal to top bound.
 160     EXPECT_FLOAT_EQ(constrainf(1.0f, 0.0f, 1.0f), 1.0f);
 161
 162     // Equal to both bottom and top bound.
 163     EXPECT_FLOAT_EQ(constrainf(1.0f, 1.0f, 1.0f), 1.0f);
 164
 165     // Above top bound.
 166     EXPECT_FLOAT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f);
 167     // Below bottom bound.
 168     EXPECT_FLOAT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f);
 169
 170     // Above bouth bounds.
 171     EXPECT_FLOAT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f);
 172     // Below bouth bounds.
 173     EXPECT_FLOAT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f);
 174 }
 175
 176 TEST(MathsUnittest, TestDegreesToRadians)
 177 {
 178     EXPECT_FLOAT_EQ(degreesToRadians(0), 0.0f);
 179     EXPECT_FLOAT_EQ(degreesToRadians(90), 0.5f * M_PIf);
 180     EXPECT_FLOAT_EQ(degreesToRadians(180), M_PIf);
 181     EXPECT_FLOAT_EQ(degreesToRadians(-180), - M_PIf);
 182 }
 183
 184 TEST(MathsUnittest, TestApplyDeadband)
 185 {
 186     EXPECT_EQ(applyDeadband(0, 0), 0);
 187     EXPECT_EQ(applyDeadband(1, 0), 1);
 188     EXPECT_EQ(applyDeadband(-1, 0), -1);
 189
 190     EXPECT_EQ(applyDeadband(0, 10), 0);
 191     EXPECT_EQ(applyDeadband(1, 10), 0);
 192     EXPECT_EQ(applyDeadband(10, 10), 0);
 193
 194     EXPECT_EQ(applyDeadband(11, 10), 1);
 195     EXPECT_EQ(applyDeadband(-11, 10), -1);
 196 }
 197
 198 void expectVectorsAreEqual(struct fp_vector *a, struct fp_vector *b, float absTol)
 199 {
 200     EXPECT_NEAR(a->X, b->X, absTol);
 201     EXPECT_NEAR(a->Y, b->Y, absTol);
 202     EXPECT_NEAR(a->Z, b->Z, absTol);
 203 }
 204
 205 #if defined(FAST_MATH) || defined(VERY_FAST_MATH)
 206 TEST(MathsUnittest, TestFastTrigonometrySinCos)
 207 {
 208     double sinError = 0;
 209     for (float x = -10 * M_PI; x < 10 * M_PI; x += M_PI / 300) {
 210         float approxResult = sin_approx(x);
 211         double libmResult = sin(x);
 212         sinError = MAX(sinError, fabs(approxResult - libmResult));
 213     }
 214     printf("sin_approx maximum absolute error = %e\n", sinError);
 215     EXPECT_LE(sinError, 3e-6);
 216
 217     double cosError = 0;
 218     for (float x = -10 * M_PI; x < 10 * M_PI; x += M_PI / 300) {
 219         float approxResult = cos_approx(x);
 220         double libmResult = cos(x);
 221         cosError = MAX(cosError, fabs(approxResult - libmResult));
 222     }
 223     printf("cos_approx maximum absolute error = %e\n", cosError);
 224     EXPECT_LE(cosError, 3.5e-6);
 225 }
 226
 227 TEST(MathsUnittest, TestFastTrigonometryATan2)
 228 {
 229     double error = 0;
 230     for (float x = -1.0f; x < 1.0f; x += 0.01) {
 231         for (float y = -1.0f; x < 1.0f; x += 0.001) {
 232             float approxResult = atan2_approx(y, x);
 233             double libmResult = atan2(y, x);
 234             error = MAX(error, fabs(approxResult - libmResult));
 235         }
 236     }
 237     printf("atan2_approx maximum absolute error = %e rads (%e degree)\n", error, error / M_PI * 180.0f);
 238     EXPECT_LE(error, 1e-6);
 239 }
 240
 241 TEST(MathsUnittest, TestFastTrigonometryACos)
 242 {
 243     double error = 0;
 244     for (float x = -1.0f; x < 1.0f; x += 0.001) {
 245         float approxResult = acos_approx(x);
 246         double libmResult = acos(x);
 247         error = MAX(error, fabs(approxResult - libmResult));
 248     }
 249     printf("acos_approx maximum absolute error = %e rads (%e degree)\n", error, error / M_PI * 180.0f);
 250     EXPECT_LE(error, 1e-4);
 251 }
 252 #endif