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