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 #define BARO
  24
  25 extern "C" {
  26     #include "common/maths.h"
  27 }
  28
  29 #include "unittest_macros.h"
  30 #include "gtest/gtest.h"
  31
  32 TEST(MathsUnittest, TestConstrain)
  33 {
  34     // Within bounds
  35     EXPECT_EQ(constrain(0, 0, 0), 0);
  36     EXPECT_EQ(constrain(1, 1, 1), 1);
  37     EXPECT_EQ(constrain(1, 0, 2), 1);
  38
  39     // Equal to bottom bound.
  40     EXPECT_EQ(constrain(1, 1, 2), 1);
  41     // Equal to top bound.
  42     EXPECT_EQ(constrain(1, 0, 1), 1);
  43
  44     // Equal to both bottom and top bound.
  45     EXPECT_EQ(constrain(1, 1, 1), 1);
  46
  47     // Above top bound.
  48     EXPECT_EQ(constrain(2, 0, 1), 1);
  49     // Below bottom bound.
  50     EXPECT_EQ(constrain(0, 1, 2), 1);
  51 }
  52
  53 TEST(MathsUnittest, TestConstrainNegatives)
  54 {
  55     // Within bounds.
  56     EXPECT_EQ(constrain(-1, -1, -1), -1);
  57     EXPECT_EQ(constrain(-1, -2, 0), -1);
  58
  59     // Equal to bottom bound.
  60     EXPECT_EQ(constrain(-1, -1, 0), -1);
  61     // Equal to top bound.
  62     EXPECT_EQ(constrain(-1, -2, -1), -1);
  63
  64     // Equal to both bottom and top bound.
  65     EXPECT_EQ(constrain(-1, -1, -1), -1);
  66
  67     // Above top bound.
  68     EXPECT_EQ(constrain(-1, -3, -2), -2);
  69     // Below bottom bound.
  70     EXPECT_EQ(constrain(-3, -2, -1), -2);
  71 }
  72
  73 TEST(MathsUnittest, TestConstrainf)
  74 {
  75     // Within bounds.
  76     EXPECT_FLOAT_EQ(constrainf(1.0f, 0.0f, 2.0f), 1.0f);
  77
  78     // Equal to bottom bound.
  79     EXPECT_FLOAT_EQ(constrainf(1.0f, 1.0f, 2.0f), 1.0f);
  80     // Equal to top bound.
  81     EXPECT_FLOAT_EQ(constrainf(1.0f, 0.0f, 1.0f), 1.0f);
  82
  83     // Equal to both bottom and top bound.
  84     EXPECT_FLOAT_EQ(constrainf(1.0f, 1.0f, 1.0f), 1.0f);
  85
  86     // Above top bound.
  87     EXPECT_FLOAT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f);
  88     // Below bottom bound.
  89     EXPECT_FLOAT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f);
  90
  91     // Above bouth bounds.
  92     EXPECT_FLOAT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f);
  93     // Below bouth bounds.
  94     EXPECT_FLOAT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f);
  95 }
  96
  97 TEST(MathsUnittest, TestDegreesToRadians)
  98 {
  99     EXPECT_FLOAT_EQ(degreesToRadians(0), 0.0f);
 100     EXPECT_FLOAT_EQ(degreesToRadians(90), 0.5f * M_PIf);
 101     EXPECT_FLOAT_EQ(degreesToRadians(180), M_PIf);
 102     EXPECT_FLOAT_EQ(degreesToRadians(-180), - M_PIf);
 103 }
 104
 105 TEST(MathsUnittest, TestApplyDeadband)
 106 {
 107     EXPECT_EQ(applyDeadband(0, 0), 0);
 108     EXPECT_EQ(applyDeadband(1, 0), 1);
 109     EXPECT_EQ(applyDeadband(-1, 0), -1);
 110
 111     EXPECT_EQ(applyDeadband(0, 10), 0);
 112     EXPECT_EQ(applyDeadband(1, 10), 0);
 113     EXPECT_EQ(applyDeadband(10, 10), 0);
 114
 115     EXPECT_EQ(applyDeadband(11, 10), 1);
 116     EXPECT_EQ(applyDeadband(-11, 10), -1);
 117 }
 118
 119 void expectVectorsAreEqual(struct fp_vector *a, struct fp_vector *b)
 120 {
 121     EXPECT_FLOAT_EQ(a->X, b->X);
 122     EXPECT_FLOAT_EQ(a->Y, b->Y);
 123     EXPECT_FLOAT_EQ(a->Z, b->Z);
 124 }
 125
 126 TEST(MathsUnittest, TestRotateVectorWithNoAngle)
 127 {
 128     fp_vector vector = {1.0f, 0.0f, 0.0f};
 129     fp_angles_t euler_angles = {.raw={0.0f, 0.0f, 0.0f}};
 130
 131     rotateV(&vector, &euler_angles);
 132     fp_vector expected_result = {1.0f, 0.0f, 0.0f};
 133
 134     expectVectorsAreEqual(&vector, &expected_result);
 135 }
 136
 137 TEST(MathsUnittest, TestRotateVectorAroundAxis)
 138 {
 139     // Rotate a vector <1, 0, 0> around an each axis x y and z.
 140     fp_vector vector = {1.0f, 0.0f, 0.0f};
 141     fp_angles_t euler_angles = {.raw={90.0f, 0.0f, 0.0f}};
 142
 143     rotateV(&vector, &euler_angles);
 144     fp_vector expected_result = {1.0f, 0.0f, 0.0f};
 145
 146     expectVectorsAreEqual(&vector, &expected_result);
 147 }