src/test/unit/sensor_gyro_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 #include <stdbool.h>
  20
  21 #include <limits.h>
  22 #include <algorithm>
  23
  24 extern "C" {
  25     #include <platform.h>
  26
  27     #include "build/build_config.h"
  28     #include "build/debug.h"
  29     #include "common/axis.h"
  30     #include "common/maths.h"
  31     #include "common/calibration.h"
  32     #include "common/utils.h"
  33     #include "drivers/accgyro/accgyro_fake.h"
  34     #include "drivers/logging_codes.h"
  35     #include "io/beeper.h"
  36     #include "scheduler/scheduler.h"
  37     #include "sensors/gyro.h"
  38     #include "sensors/acceleration.h"
  39     #include "sensors/sensors.h"
  40     #include "fc/rc_controls.h"
  41     #include "flight/mixer.h"
  42
  43     extern zeroCalibrationVector_t gyroCalibration;
  44     extern gyroDev_t gyroDev[];
  45
  46     STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware);
  47     STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration);
  48 }
  49
  50 #include "unittest_macros.h"
  51 #include "gtest/gtest.h"
  52
  53 TEST(SensorGyro, Detect)
  54 {
  55     const gyroSensor_e detected = gyroDetect(&gyroDev[0], GYRO_AUTODETECT);
  56     EXPECT_EQ(GYRO_FAKE, detected);
  57 }
  58
  59 TEST(SensorGyro, Init)
  60 {
  61     const bool initialised = gyroInit();
  62     EXPECT_EQ(true, initialised);
  63     EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
  64 }
  65
  66 TEST(SensorGyro, Read)
  67 {
  68     gyroInit();
  69     EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
  70     fakeGyroSet(5, 6, 7);
  71     const bool read = gyroDev[0].readFn(&gyroDev[0]);
  72     EXPECT_EQ(true, read);
  73     EXPECT_EQ(5, gyroDev[0].gyroADCRaw[X]);
  74     EXPECT_EQ(6, gyroDev[0].gyroADCRaw[Y]);
  75     EXPECT_EQ(7, gyroDev[0].gyroADCRaw[Z]);
  76 }
  77
  78 TEST(SensorGyro, Calibrate)
  79 {
  80     gyroInit();
  81     gyroStartCalibration();
  82     fakeGyroSet(5, 6, 7);
  83     const bool read = gyroDev[0].readFn(&gyroDev[0]);
  84     EXPECT_EQ(true, read);
  85     EXPECT_EQ(5, gyroDev[0].gyroADCRaw[X]);
  86     EXPECT_EQ(6, gyroDev[0].gyroADCRaw[Y]);
  87     EXPECT_EQ(7, gyroDev[0].gyroADCRaw[Z]);
  88     gyroDev[0].gyroZero[X] = 8;
  89     gyroDev[0].gyroZero[Y] = 9;
  90     gyroDev[0].gyroZero[Z] = 10;
  91     performGyroCalibration(&gyroDev[0], &gyroCalibration);
  92     EXPECT_EQ(0, gyroDev[0].gyroZero[X]);
  93     EXPECT_EQ(0, gyroDev[0].gyroZero[Y]);
  94     EXPECT_EQ(0, gyroDev[0].gyroZero[Z]);
  95     EXPECT_EQ(false, gyroIsCalibrationComplete());
  96     while (!gyroIsCalibrationComplete()) {
  97         performGyroCalibration(&gyroDev[0], &gyroCalibration);
  98     }
  99     EXPECT_EQ(5, gyroDev[0].gyroZero[X]);
 100     EXPECT_EQ(6, gyroDev[0].gyroZero[Y]);
 101     EXPECT_EQ(7, gyroDev[0].gyroZero[Z]);
 102 }
 103
 104 TEST(SensorGyro, Update)
 105 {
 106     gyroInit();
 107     gyroStartCalibration();
 108     EXPECT_EQ(false, gyroIsCalibrationComplete());
 109     gyroInit();
 110     fakeGyroSet(5, 6, 7);
 111     gyroUpdate();
 112     EXPECT_EQ(false, gyroIsCalibrationComplete());
 113     while (!gyroIsCalibrationComplete()) {
 114         gyroUpdate();
 115     }
 116     EXPECT_EQ(true, gyroIsCalibrationComplete());
 117     EXPECT_EQ(5, gyroDev[0].gyroZero[X]);
 118     EXPECT_EQ(6, gyroDev[0].gyroZero[Y]);
 119     EXPECT_EQ(7, gyroDev[0].gyroZero[Z]);
 120     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
 121     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
 122     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
 123     gyroUpdate();
 124     // expect zero values since gyro is calibrated
 125     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
 126     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
 127     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
 128     fakeGyroSet(15, 26, 97);
 129     gyroUpdate();
 130     EXPECT_FLOAT_EQ(10 * gyroDev[0].scale, gyro.gyroADCf[X]); // gyroADCf values are scaled
 131     EXPECT_FLOAT_EQ(20 * gyroDev[0].scale, gyro.gyroADCf[Y]);
 132     EXPECT_FLOAT_EQ(90 * gyroDev[0].scale, gyro.gyroADCf[Z]);
 133 }
 134
 135
 136 // STUBS
 137
 138 extern "C" {
 139 static timeMs_t milliTime = 0;
 140 timeMs_t millis(void) {return milliTime++;}
 141 uint32_t micros(void) {return 0;}
 142 void beeper(beeperMode_e) {}
 143 uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE };
 144 timeDelta_t getLooptime(void) {return gyro.targetLooptime;}
 145 timeDelta_t getGyroLooptime(void) {return gyro.targetLooptime;}
 146 void sensorsSet(uint32_t) {}
 147 void schedulerResetTaskStatistics(cfTaskId_e) {}
 148 }