src/test/unit/flight_imu_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 #include <limits.h>
  21 #include <cmath>
  22
  23 extern "C" {
  24     #include "platform.h"
  25     #include "build/debug.h"
  26
  27     #include "common/axis.h"
  28     #include "common/maths.h"
  29
  30     #include "config/feature.h"
  31     #include "pg/pg.h"
  32     #include "pg/pg_ids.h"
  33     #include "pg/rx.h"
  34
  35     #include "drivers/accgyro/accgyro.h"
  36     #include "drivers/compass/compass.h"
  37     #include "drivers/sensor.h"
  38
  39     #include "fc/rc_controls.h"
  40     #include "fc/rc_modes.h"
  41     #include "fc/runtime_config.h"
  42
  43     #include "flight/mixer.h"
  44     #include "flight/pid.h"
  45     #include "flight/imu.h"
  46
  47     #include "io/gps.h"
  48
  49     #include "rx/rx.h"
  50
  51     #include "sensors/acceleration.h"
  52     #include "sensors/barometer.h"
  53     #include "sensors/compass.h"
  54     #include "sensors/gyro.h"
  55     #include "sensors/sensors.h"
  56
  57     void imuComputeRotationMatrix(void);
  58     void imuUpdateEulerAngles(void);
  59
  60     extern quaternion q;
  61     extern float rMat[3][3];
  62     extern bool attitudeIsEstablished;
  63
  64     PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
  65     PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0);
  66
  67     PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
  68         .enabledFeatures = 0
  69     );
  70 }
  71
  72 #include "unittest_macros.h"
  73 #include "gtest/gtest.h"
  74
  75 const float sqrt2over2 = sqrtf(2) / 2.0f;
  76
  77 TEST(FlightImuTest, TestCalculateRotationMatrix)
  78 {
  79     #define TOL 1e-6
  80
  81     // No rotation
  82     q.w = 1.0f;
  83     q.x = 0.0f;
  84     q.y = 0.0f;
  85     q.z = 0.0f;
  86
  87     imuComputeRotationMatrix();
  88
  89     EXPECT_FLOAT_EQ(1.0f, rMat[0][0]);
  90     EXPECT_FLOAT_EQ(0.0f, rMat[0][1]);
  91     EXPECT_FLOAT_EQ(0.0f, rMat[0][2]);
  92     EXPECT_FLOAT_EQ(0.0f, rMat[1][0]);
  93     EXPECT_FLOAT_EQ(1.0f, rMat[1][1]);
  94     EXPECT_FLOAT_EQ(0.0f, rMat[1][2]);
  95     EXPECT_FLOAT_EQ(0.0f, rMat[2][0]);
  96     EXPECT_FLOAT_EQ(0.0f, rMat[2][1]);
  97     EXPECT_FLOAT_EQ(1.0f, rMat[2][2]);
  98
  99     // 90 degrees around Z axis
 100     q.w = sqrt2over2;
 101     q.x = 0.0f;
 102     q.y = 0.0f;
 103     q.z = sqrt2over2;
 104
 105     imuComputeRotationMatrix();
 106
 107     EXPECT_NEAR(0.0f, rMat[0][0], TOL);
 108     EXPECT_NEAR(-1.0f, rMat[0][1], TOL);
 109     EXPECT_NEAR(0.0f, rMat[0][2], TOL);
 110     EXPECT_NEAR(1.0f, rMat[1][0], TOL);
 111     EXPECT_NEAR(0.0f, rMat[1][1], TOL);
 112     EXPECT_NEAR(0.0f, rMat[1][2], TOL);
 113     EXPECT_NEAR(0.0f, rMat[2][0], TOL);
 114     EXPECT_NEAR(0.0f, rMat[2][1], TOL);
 115     EXPECT_NEAR(1.0f, rMat[2][2], TOL);
 116
 117     // 60 degrees around X axis
 118     q.w = 0.866f;
 119     q.x = 0.5f;
 120     q.y = 0.0f;
 121     q.z = 0.0f;
 122
 123     imuComputeRotationMatrix();
 124
 125     EXPECT_NEAR(1.0f, rMat[0][0], TOL);
 126     EXPECT_NEAR(0.0f, rMat[0][1], TOL);
 127     EXPECT_NEAR(0.0f, rMat[0][2], TOL);
 128     EXPECT_NEAR(0.0f, rMat[1][0], TOL);
 129     EXPECT_NEAR(0.5f, rMat[1][1], TOL);
 130     EXPECT_NEAR(-0.866f, rMat[1][2], TOL);
 131     EXPECT_NEAR(0.0f, rMat[2][0], TOL);
 132     EXPECT_NEAR(0.866f, rMat[2][1], TOL);
 133     EXPECT_NEAR(0.5f, rMat[2][2], TOL);
 134 }
 135
 136 TEST(FlightImuTest, TestUpdateEulerAngles)
 137 {
 138     // No rotation
 139     memset(rMat, 0.0, sizeof(float) * 9);
 140
 141     imuUpdateEulerAngles();
 142
 143     EXPECT_EQ(0, attitude.values.roll);
 144     EXPECT_EQ(0, attitude.values.pitch);
 145     EXPECT_EQ(0, attitude.values.yaw);
 146
 147     // 45 degree yaw
 148     memset(rMat, 0.0, sizeof(float) * 9);
 149     rMat[0][0] = sqrt2over2;
 150     rMat[0][1] = sqrt2over2;
 151     rMat[1][0] = -sqrt2over2;
 152     rMat[1][1] = sqrt2over2;
 153
 154     imuUpdateEulerAngles();
 155
 156     EXPECT_EQ(0, attitude.values.roll);
 157     EXPECT_EQ(0, attitude.values.pitch);
 158     EXPECT_EQ(450, attitude.values.yaw);
 159 }
 160
 161 TEST(FlightImuTest, TestSmallAngle)
 162 {
 163     const float r1 = 0.898;
 164     const float r2 = 0.438;
 165
 166     // given
 167     imuConfigMutable()->small_angle = 25;
 168     imuConfigure(0, 0);
 169     attitudeIsEstablished = true;
 170
 171     // and
 172     memset(rMat, 0.0, sizeof(float) * 9);
 173
 174     // when
 175     imuComputeRotationMatrix();
 176
 177     // expect
 178     EXPECT_FALSE(isUpright());
 179
 180     // given
 181     rMat[0][0] = r1;
 182     rMat[0][2] = r2;
 183     rMat[2][0] = -r2;
 184     rMat[2][2] = r1;
 185
 186     // when
 187     imuComputeRotationMatrix();
 188
 189     // expect
 190     EXPECT_FALSE(isUpright());
 191
 192     // given
 193     memset(rMat, 0.0, sizeof(float) * 9);
 194
 195     // when
 196     imuComputeRotationMatrix();
 197
 198     // expect
 199     EXPECT_FALSE(isUpright());
 200 }
 201
 202 // STUBS
 203
 204 extern "C" {
 205 boxBitmask_t rcModeActivationMask;
 206 float rcCommand[4];
 207 float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 208
 209 gyro_t gyro;
 210 acc_t acc;
 211 mag_t mag;
 212
 213 gpsSolutionData_t gpsSol;
 214 int16_t GPS_verticalSpeedInCmS;
 215
 216 uint8_t debugMode;
 217 int16_t debug[DEBUG16_VALUE_COUNT];
 218
 219 uint8_t stateFlags;
 220 uint16_t flightModeFlags;
 221 uint8_t armingFlags;
 222
 223 pidProfile_t *currentPidProfile;
 224
 225 uint16_t enableFlightMode(flightModeFlags_e mask)
 226 {
 227     return flightModeFlags |= (mask);
 228 }
 229
 230 uint16_t disableFlightMode(flightModeFlags_e mask)
 231 {
 232     return flightModeFlags &= ~(mask);
 233 }
 234
 235 bool sensors(uint32_t mask)
 236 {
 237     return mask & SENSOR_ACC;
 238 };
 239
 240 uint32_t millis(void) { return 0; }
 241 uint32_t micros(void) { return 0; }
 242
 243 bool compassIsHealthy(void) { return true; }
 244 bool baroIsCalibrationComplete(void) { return true; }
 245 void performBaroCalibrationCycle(void) {}
 246 int32_t baroCalculateAltitude(void) { return 0; }
 247 bool gyroGetAccumulationAverage(float *) { return false; }
 248 bool accGetAccumulationAverage(float *) { return false; }
 249 void mixerSetThrottleAngleCorrection(int) {};
 250 bool gpsRescueIsRunning(void) { return false; }
 251 bool isFixedWing(void) { return false; }
 252 void pinioBoxTaskControl(void) {}
 253 void schedulerIgnoreTaskExecTime(void) {}
 254 void schedulerIgnoreTaskStateTime(void) {}
 255 void schedulerSetNextStateTime(timeDelta_t) {}
 256 bool schedulerGetIgnoreTaskExecTime() { return false; }
 257 }