src/test/unit/flight_imu_unittest.cc.txt

   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
  20 #include <limits.h>
  21
  22 extern "C" {
  23     #include "sensors/gyro.h"
  24     #include "sensors/compass.h"
  25     #include "sensors/acceleration.h"
  26
  27     #include "scheduler/scheduler.h"
  28     #include "fc/runtime_config.h"
  29
  30     #include "io/gps.h"
  31     #include "flight/pid.h"
  32     #include "flight/imu.h"
  33 }
  34
  35 #include "unittest_macros.h"
  36 #include "gtest/gtest.h"
  37
  38 extern "C" {
  39 STATIC_UNIT_TESTED void imuUpdateEulerAngles(void);
  40 STATIC_UNIT_TESTED void imuComputeQuaternionFromRPY(int16_t initialRoll, int16_t initialPitch, int16_t initialYaw);
  41 }
  42
  43 TEST(FlightImuTest, TestEulerAngleCalculation)
  44 {
  45     imuComputeQuaternionFromRPY(0, 0, 0);
  46     imuUpdateEulerAngles();
  47     EXPECT_NEAR(attitude.values.roll, 0, 1);
  48     EXPECT_NEAR(attitude.values.pitch, 0, 1);
  49     EXPECT_NEAR(attitude.values.yaw, 0, 1);
  50
  51     imuComputeQuaternionFromRPY(450, 450, 0);
  52     imuUpdateEulerAngles();
  53     EXPECT_NEAR(attitude.values.roll, 450, 1);
  54     EXPECT_NEAR(attitude.values.pitch, 450, 1);
  55     EXPECT_NEAR(attitude.values.yaw, 0, 1);
  56
  57     imuComputeQuaternionFromRPY(-450, -450, 0);
  58     imuUpdateEulerAngles();
  59     EXPECT_NEAR(attitude.values.roll, -450, 1);
  60     EXPECT_NEAR(attitude.values.pitch, -450, 1);
  61     EXPECT_NEAR(attitude.values.yaw, 0, 1);
  62
  63     imuComputeQuaternionFromRPY(1790, 0, 0);
  64     imuUpdateEulerAngles();
  65     EXPECT_NEAR(attitude.values.roll, 1790, 1);
  66     EXPECT_NEAR(attitude.values.pitch, 0, 1);
  67     EXPECT_NEAR(attitude.values.yaw, 0, 1);
  68
  69     imuComputeQuaternionFromRPY(-1790, 0, 0);
  70     imuUpdateEulerAngles();
  71     EXPECT_NEAR(attitude.values.roll, -1790, 1);
  72     EXPECT_NEAR(attitude.values.pitch, 0, 1);
  73     EXPECT_NEAR(attitude.values.yaw, 0, 1);
  74
  75     imuComputeQuaternionFromRPY(0, 0, 900);
  76     imuUpdateEulerAngles();
  77     EXPECT_NEAR(attitude.values.roll, 0, 1);
  78     EXPECT_NEAR(attitude.values.pitch, 0, 1);
  79     EXPECT_NEAR(attitude.values.yaw, 900, 1);
  80
  81     imuComputeQuaternionFromRPY(0, 0, 2700);
  82     imuUpdateEulerAngles();
  83     EXPECT_NEAR(attitude.values.roll, 0, 1);
  84     EXPECT_NEAR(attitude.values.pitch, 0, 1);
  85     EXPECT_NEAR(attitude.values.yaw, 2700, 1);
  86 }
  87
  88 // STUBS
  89
  90 extern "C" {
  91
  92 uint32_t stateFlags;
  93 uint32_t flightModeFlags;
  94 uint32_t armingFlags;
  95
  96 acc_t acc;
  97 mag_t mag;
  98
  99 gpsSolutionData_t gpsSol;
 100
 101 compassConfig_t compassConfig_System;
 102
 103 pidProfile_t* pidProfile_ProfileCurrent;
 104
 105 uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE };
 106
 107 bool sensors(uint32_t mask)
 108 {
 109     UNUSED(mask);
 110     return false;
 111 };
 112 uint32_t millis(void) { return 0; }
 113 timeDelta_t getLooptime(void) { return gyro.targetLooptime; }
 114 timeDelta_t getGyroLooptime(void) { return gyro.targetLooptime; }
 115 void schedulerResetTaskStatistics(cfTaskId_e) {}
 116 void sensorsSet(uint32_t) {}
 117 bool compassIsHealthy(void) { return true; }
 118 void accGetVibrationLevels(fpVector3_t *accVibeLevels)
 119 {
 120     accVibeLevels->x = fast_fsqrtf(acc.accVibeSq[X]);
 121     accVibeLevels->y = fast_fsqrtf(acc.accVibeSq[Y]);
 122     accVibeLevels->z = fast_fsqrtf(acc.accVibeSq[Z]);
 123 }
 124 void accGetMeasuredAcceleration(fpVector3_t *measuredAcc)
 125 {
 126     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 127         measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
 128     }
 129 }
 130 uint32_t accGetClipCount(void)
 131 {
 132     return acc.accClipCount;
 133 }
 134 void accUpdate(void)
 135 {
 136 }
 137 void resetHeadingHoldTarget(int16_t heading)
 138 {
 139     UNUSED(heading);
 140 }
 141 bool isGPSHeadingValid(void) { return true; }
 142 }