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