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     #include "common/vector.h"
  30
  31     #include "config/feature.h"
  32     #include "pg/pg.h"
  33     #include "pg/pg_ids.h"
  34     #include "pg/rx.h"
  35
  36     #include "drivers/accgyro/accgyro.h"
  37     #include "drivers/compass/compass.h"
  38     #include "drivers/sensor.h"
  39
  40     #include "fc/rc_controls.h"
  41     #include "fc/rc_modes.h"
  42     #include "fc/runtime_config.h"
  43     #include "fc/rc.h"
  44
  45     #include "flight/imu.h"
  46     #include "flight/mixer.h"
  47     #include "flight/pid.h"
  48     #include "flight/position.h"
  49
  50     #include "io/gps.h"
  51
  52     #include "rx/rx.h"
  53
  54     #include "pg/autopilot.h"
  55
  56     #include "sensors/acceleration.h"
  57     #include "sensors/barometer.h"
  58     #include "sensors/compass.h"
  59     #include "sensors/gyro.h"
  60     #include "sensors/sensors.h"
  61
  62     void imuComputeRotationMatrix(void);
  63     void imuUpdateEulerAngles(void);
  64     void imuMahonyAHRSupdate(float dt,
  65                              float gx, float gy, float gz,
  66                              bool useAcc, float ax, float ay, float az,
  67                              float headingErrMag, float headingErrCog,
  68                              const float dcmKpGain);
  69     float imuCalcMagErr(void);
  70     float imuCalcCourseErr(float courseOverGround);
  71     extern quaternion_t q;
  72     extern matrix33_t rMat;
  73     extern bool attitudeIsEstablished;
  74
  75     PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
  76     PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0);
  77     PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
  78     PG_REGISTER(apConfig_t, apConfig, PG_AUTOPILOT, 0);
  79
  80     PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
  81         .enabledFeatures = 0
  82     );
  83 }
  84
  85 #include "unittest_macros.h"
  86 #include "gtest/gtest.h"
  87
  88 const float sqrt2over2 = sqrtf(2) / 2.0f;
  89
  90 void quaternion_from_axis_angle(quaternion_t* q, float angle, float x, float y, float z) {
  91     vector3_t a = {{x, y, z}};
  92     vector3Normalize(&a, &a);
  93     q->w = cos(angle / 2);
  94     q->x = a.x * sin(angle / 2);
  95     q->y = a.y * sin(angle / 2);
  96     q->z = a.z * sin(angle / 2);
  97 }
  98
  99 TEST(FlightImuTest, TestCalculateRotationMatrix)
 100 {
 101     #define TOL 1e-6
 102
 103     // No rotation
 104     q.w = 1.0f;
 105     q.x = 0.0f;
 106     q.y = 0.0f;
 107     q.z = 0.0f;
 108
 109     imuComputeRotationMatrix();
 110
 111     EXPECT_FLOAT_EQ(1.0f, rMat.m[0][0]);
 112     EXPECT_FLOAT_EQ(0.0f, rMat.m[0][1]);
 113     EXPECT_FLOAT_EQ(0.0f, rMat.m[0][2]);
 114     EXPECT_FLOAT_EQ(0.0f, rMat.m[1][0]);
 115     EXPECT_FLOAT_EQ(1.0f, rMat.m[1][1]);
 116     EXPECT_FLOAT_EQ(0.0f, rMat.m[1][2]);
 117     EXPECT_FLOAT_EQ(0.0f, rMat.m[2][0]);
 118     EXPECT_FLOAT_EQ(0.0f, rMat.m[2][1]);
 119     EXPECT_FLOAT_EQ(1.0f, rMat.m[2][2]);
 120
 121     // 90 degrees around Z axis
 122     q.w = sqrt2over2;
 123     q.x = 0.0f;
 124     q.y = 0.0f;
 125     q.z = sqrt2over2;
 126
 127     imuComputeRotationMatrix();
 128
 129     EXPECT_NEAR(0.0f, rMat.m[0][0], TOL);
 130     EXPECT_NEAR(-1.0f, rMat.m[0][1], TOL);
 131     EXPECT_NEAR(0.0f, rMat.m[0][2], TOL);
 132     EXPECT_NEAR(1.0f, rMat.m[1][0], TOL);
 133     EXPECT_NEAR(0.0f, rMat.m[1][1], TOL);
 134     EXPECT_NEAR(0.0f, rMat.m[1][2], TOL);
 135     EXPECT_NEAR(0.0f, rMat.m[2][0], TOL);
 136     EXPECT_NEAR(0.0f, rMat.m[2][1], TOL);
 137     EXPECT_NEAR(1.0f, rMat.m[2][2], TOL);
 138
 139     // 60 degrees around X axis
 140     q.w = 0.866f;
 141     q.x = 0.5f;
 142     q.y = 0.0f;
 143     q.z = 0.0f;
 144
 145     imuComputeRotationMatrix();
 146
 147     EXPECT_NEAR(1.0f, rMat.m[0][0], TOL);
 148     EXPECT_NEAR(0.0f, rMat.m[0][1], TOL);
 149     EXPECT_NEAR(0.0f, rMat.m[0][2], TOL);
 150     EXPECT_NEAR(0.0f, rMat.m[1][0], TOL);
 151     EXPECT_NEAR(0.5f, rMat.m[1][1], TOL);
 152     EXPECT_NEAR(-0.866f, rMat.m[1][2], TOL);
 153     EXPECT_NEAR(0.0f, rMat.m[2][0], TOL);
 154     EXPECT_NEAR(0.866f, rMat.m[2][1], TOL);
 155     EXPECT_NEAR(0.5f, rMat.m[2][2], TOL);
 156 }
 157
 158 TEST(FlightImuTest, TestUpdateEulerAngles)
 159 {
 160     // No rotation
 161     memset(&rMat, 0.0, sizeof(float) * 9);
 162
 163     imuUpdateEulerAngles();
 164
 165     EXPECT_EQ(0, attitude.values.roll);
 166     EXPECT_EQ(0, attitude.values.pitch);
 167     EXPECT_EQ(0, attitude.values.yaw);
 168
 169     // 45 degree yaw
 170     memset(&rMat, 0.0, sizeof(float) * 9);
 171     rMat.m[0][0] = sqrt2over2;
 172     rMat.m[0][1] = sqrt2over2;
 173     rMat.m[1][0] = -sqrt2over2;
 174     rMat.m[1][1] = sqrt2over2;
 175
 176     imuUpdateEulerAngles();
 177
 178     EXPECT_EQ(0, attitude.values.roll);
 179     EXPECT_EQ(0, attitude.values.pitch);
 180     EXPECT_EQ(450, attitude.values.yaw);
 181 }
 182
 183 TEST(FlightImuTest, TestSmallAngle)
 184 {
 185     const float r1 = 0.898;
 186     const float r2 = 0.438;
 187
 188     // given
 189     imuConfigMutable()->small_angle = 25;
 190     imuConfigure(0, 0);
 191     attitudeIsEstablished = true;
 192
 193     // and
 194     memset(&rMat, 0.0, sizeof(float) * 9);
 195
 196     // when
 197     imuComputeRotationMatrix();
 198
 199     // expect
 200     EXPECT_FALSE(isUpright());
 201
 202     // given
 203     rMat.m[0][0] = r1;
 204     rMat.m[0][2] = r2;
 205     rMat.m[2][0] = -r2;
 206     rMat.m[2][2] = r1;
 207
 208     // when
 209     imuComputeRotationMatrix();
 210
 211     // expect
 212     EXPECT_FALSE(isUpright());
 213
 214     // given
 215     memset(&rMat, 0.0, sizeof(float) * 9);
 216
 217     // when
 218     imuComputeRotationMatrix();
 219
 220     // expect
 221     EXPECT_FALSE(isUpright());
 222 }
 223
 224 testing::AssertionResult DoubleNearWrapPredFormat(const char* expr1, const char* expr2,
 225                                                   const char* abs_error_expr, const char* wrap_expr, double val1,
 226                                                   double val2, double abs_error, double wrap) {
 227     const double diff = remainder(val1 - val2, wrap);
 228     if (fabs(diff) <= abs_error) return testing::AssertionSuccess();
 229
 230     return testing::AssertionFailure()
 231         << "The difference between " << expr1 << " and " << expr2 << " is "
 232         << diff << " (wrapped to 0 .. " << wrap_expr << ")"
 233         << ", which exceeds " << abs_error_expr << ", where\n"
 234         << expr1 << " evaluates to " << val1 << ",\n"
 235         << expr2 << " evaluates to " << val2 << ", and\n"
 236         << abs_error_expr << " evaluates to " << abs_error << ".";
 237 }
 238
 239 #define EXPECT_NEAR_DEG(val1, val2, abs_error)                \
 240     EXPECT_PRED_FORMAT4(DoubleNearWrapPredFormat, val1, val2, \
 241                         abs_error, 360.0)
 242
 243 #define EXPECT_NEAR_RAD(val1, val2, abs_error)                \
 244     EXPECT_PRED_FORMAT4(DoubleNearWrapPredFormat, val1, val2, \
 245                         abs_error, 2 * M_PI)
 246
 247
 248
 249 class MahonyFixture : public ::testing::Test {
 250 protected:
 251     vector3_t gyro;
 252     bool useAcc;
 253     vector3_t acc;
 254     bool useMag;
 255     vector3_t magEF;
 256     float cogGain;
 257     float cogDeg;
 258     float dcmKp;
 259     float dt;
 260     void SetUp() override {
 261         vector3Zero(&gyro);
 262         useAcc = false;
 263         vector3Zero(&acc);
 264         cogGain = 0.0;   // no cog
 265         cogDeg  = 0.0;
 266         dcmKp = .25;     // default dcm_kp
 267         dt = 1e-2;       // 100Hz update
 268
 269         imuConfigure(0, 0);
 270         // level, poiting north
 271         setOrientationAA(0, {{1,0,0}});        // identity
 272     }
 273     virtual void setOrientationAA(float angleDeg, vector3_t axis) {
 274         quaternion_from_axis_angle(&q, DEGREES_TO_RADIANS(angleDeg), axis.x, axis.y, axis.z);
 275         imuComputeRotationMatrix();
 276     }
 277
 278     float wrap(float angle) {
 279         angle = fmod(angle, 360);
 280         if (angle < 0) angle += 360;
 281         return angle;
 282     }
 283     float angleDiffNorm(vector3_t *a, vector3_t* b, vector3_t weight = {{1,1,1}}) {
 284         vector3_t tmp;
 285         vector3Scale(&tmp, b, -1);
 286         vector3Add(&tmp, &tmp, a);
 287         for (int i = 0; i < 3; i++)
 288             tmp.v[i] *= weight.v[i];
 289         for (int i = 0; i < 3; i++)
 290             tmp.v[i] = std::remainder(tmp.v[i], 360.0);
 291         return vector3Norm(&tmp);
 292     }
 293     // run Mahony for some time
 294     // return time it took to get within 1deg from target
 295     float imuIntegrate(float runTime, vector3_t * target) {
 296         float alignTime = -1;
 297         for (float t = 0; t < runTime; t += dt) {
 298             //     if (fmod(t, 1) < dt) printf("MagBF=%.2f %.2f %.2f\n", magBF.x, magBF.y, magBF.z);
 299             float headingErrMag = 0;
 300             if (useMag) {   // not implemented yet
 301                 headingErrMag = imuCalcMagErr();
 302             }
 303             float headingErrCog = 0;
 304             if (cogGain > 0) {
 305                 headingErrCog = imuCalcCourseErr(DEGREES_TO_RADIANS(cogDeg)) * cogGain;
 306             }
 307
 308             imuMahonyAHRSupdate(dt,
 309                                 gyro.x, gyro.y, gyro.z,
 310                                 useAcc, acc.x, acc.y, acc.z,
 311                                 headingErrMag, headingErrCog,
 312                                 dcmKp);
 313             imuUpdateEulerAngles();
 314             // if (fmod(t, 1) < dt) printf("%3.1fs - %3.1f %3.1f %3.1f\n", t, attitude.values.roll / 10.0f, attitude.values.pitch / 10.0f, attitude.values.yaw / 10.0f);
 315             // remember how long it took
 316             if (alignTime < 0) {
 317                 vector3_t rpy = {{attitude.values.roll / 10.0f, attitude.values.pitch / 10.0f, attitude.values.yaw / 10.0f}};
 318                 float error = angleDiffNorm(&rpy, target);
 319                 if (error < 1)
 320                     alignTime = t;
 321             }
 322         }
 323         return alignTime;
 324     }
 325 };
 326
 327 class YawTest: public MahonyFixture, public testing::WithParamInterface<float> {
 328 };
 329
 330 TEST_P(YawTest, TestCogAlign)
 331 {
 332     cogGain = 1.0;
 333     cogDeg = GetParam();
 334     const float rollDeg = 30;    // 30deg pitch forward
 335     setOrientationAA(rollDeg, {{0, 1, 0}});
 336     vector3_t expect = {{0, rollDeg, wrap(cogDeg)}};
 337     // integrate IMU. about 25s is enough in worst case
 338     float alignTime = imuIntegrate(80, &expect);
 339
 340     imuUpdateEulerAngles();
 341     // quad stays level
 342     EXPECT_NEAR_DEG(attitude.values.roll / 10.0, expect.x, .1);
 343     EXPECT_NEAR_DEG(attitude.values.pitch / 10.0, expect.y, .1);
 344     // yaw is close to CoG direction
 345     EXPECT_NEAR_DEG(attitude.values.yaw / 10.0, expect.z, 1);  // error < 1 deg
 346     if (alignTime >= 0) {
 347         printf("[          ] Aligned to %.f deg in %.2fs\n", cogDeg, alignTime);
 348     }
 349 }
 350
 351 TEST_P(YawTest, TestMagAlign)
 352 {
 353     float initialAngle = GetParam();
 354
 355     // level, rotate to param heading
 356     quaternion_from_axis_angle(&q, -DEGREES_TO_RADIANS(initialAngle), 0, 0, 1);
 357     imuComputeRotationMatrix();
 358
 359     vector3_t expect = {{0, 0, 0}};    // expect zero yaw
 360
 361     vector3_t magBF = {{1, 0, .5}};    // use arbitrary Z component, point north
 362
 363     mag.magADC = magBF;
 364
 365     useMag = true;
 366     // integrate IMU. about 25s is enough in worst case
 367     float alignTime = imuIntegrate(30, &expect);
 368
 369     imuUpdateEulerAngles();
 370     // quad stays level
 371     EXPECT_NEAR_DEG(attitude.values.roll / 10.0, expect.x, .1);
 372     EXPECT_NEAR_DEG(attitude.values.pitch / 10.0, expect.y, .1);
 373     // yaw is close to north (0 deg)
 374     EXPECT_NEAR_DEG(attitude.values.yaw / 10.0, expect.z, 1.0);  // error < 1 deg
 375     if (alignTime >= 0) {
 376         printf("[          ] Aligned from %.f deg in %.2fs\n", initialAngle, alignTime);
 377     }
 378 }
 379
 380 INSTANTIATE_TEST_SUITE_P(
 381   TestAngles, YawTest,
 382   ::testing::Values(
 383       0, 45, -45, 90, 180, 270, 720+45
 384       ));
 385
 386 // STUBS
 387
 388 extern "C" {
 389     extern boxBitmask_t rcModeActivationMask;
 390     float rcCommand[4];
 391     float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 392
 393     gyro_t gyro;
 394     acc_t acc;
 395     mag_t mag;
 396
 397     gpsSolutionData_t gpsSol;
 398
 399     uint8_t debugMode;
 400     int16_t debug[DEBUG16_VALUE_COUNT];
 401
 402     uint8_t stateFlags;
 403     uint16_t flightModeFlags;
 404     uint8_t armingFlags;
 405
 406     pidProfile_t *currentPidProfile;
 407
 408     uint16_t enableFlightMode(flightModeFlags_e mask) {
 409         return flightModeFlags |= (mask);
 410     }
 411
 412     uint16_t disableFlightMode(flightModeFlags_e mask) {
 413         return flightModeFlags &= ~(mask);
 414     }
 415
 416     bool sensors(uint32_t mask) {
 417         return mask & SENSOR_ACC;
 418     };
 419
 420     uint32_t millis(void) { return 0; }
 421     uint32_t micros(void) { return 0; }
 422
 423     bool compassIsHealthy(void) { return true; }
 424     bool baroIsCalibrated(void) { return true; }
 425     void performBaroCalibrationCycle(void) {}
 426     float baroCalculateAltitude(void) { return 0; }
 427     bool gyroGetAccumulationAverage(float *) { return false; }
 428     bool accGetAccumulationAverage(float *) { return false; }
 429     void mixerSetThrottleAngleCorrection(int) {};
 430     bool gpsRescueIsRunning(void) { return false; }
 431     bool isFixedWing(void) { return false; }
 432     void pinioBoxTaskControl(void) {}
 433     void schedulerIgnoreTaskExecTime(void) {}
 434     void schedulerIgnoreTaskStateTime(void) {}
 435     void schedulerSetNextStateTime(timeDelta_t) {}
 436     bool schedulerGetIgnoreTaskExecTime() { return false; }
 437     float gyroGetFilteredDownsampled(int) { return 0.0f; }
 438     float baroUpsampleAltitude()  { return 0.0f; }
 439     float getBaroAltitude(void) { return 3000.0f; }
 440     float gpsRescueGetImuYawCogGain(void) { return 1.0f; }
 441     float getRcDeflectionAbs(int) { return 0.0f; }
 442 }