src/test/unit/sensor_gyro_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
  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/utils.h"
  32     #include "common/vector.h"
  33     #include "drivers/accgyro/accgyro_virtual.h"
  34     #include "drivers/accgyro/accgyro_mpu.h"
  35     #include "drivers/sensor.h"
  36     #include "io/beeper.h"
  37     #include "pg/pg.h"
  38     #include "pg/pg_ids.h"
  39     #include "scheduler/scheduler.h"
  40     #include "sensors/gyro.h"
  41     #include "sensors/gyro_init.h"
  42     #include "sensors/acceleration.h"
  43     #include "sensors/sensors.h"
  44
  45     STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev);
  46     struct gyroSensor_s;
  47     STATIC_UNIT_TESTED void performGyroCalibration(struct gyroSensor_s *gyroSensor, uint8_t gyroMovementCalibrationThreshold);
  48     STATIC_UNIT_TESTED bool virtualGyroRead(gyroDev_t *gyro);
  49
  50     uint8_t debugMode;
  51     int16_t debug[DEBUG16_VALUE_COUNT];
  52 }
  53
  54 #include "unittest_macros.h"
  55 #include "gtest/gtest.h"
  56 extern gyroSensor_s * const gyroSensorPtr;
  57 extern gyroDev_t * const gyroDevPtr;
  58
  59
  60 TEST(SensorGyro, Detect)
  61 {
  62     const gyroHardware_e detected = gyroDetect(gyroDevPtr);
  63     EXPECT_EQ(GYRO_VIRTUAL, detected);
  64 }
  65
  66 TEST(SensorGyro, Init)
  67 {
  68     pgResetAll();
  69     const bool initialised = gyroInit();
  70     EXPECT_TRUE(initialised);
  71     EXPECT_EQ(GYRO_VIRTUAL, detectedSensors[SENSOR_INDEX_GYRO]);
  72 }
  73
  74 TEST(SensorGyro, Read)
  75 {
  76     pgResetAll();
  77     gyroInit();
  78     virtualGyroSet(gyroDevPtr, 5, 6, 7);
  79     const bool read = gyroDevPtr->readFn(gyroDevPtr);
  80     EXPECT_TRUE(read);
  81     EXPECT_EQ(5, gyroDevPtr->gyroADCRaw[X]);
  82     EXPECT_EQ(6, gyroDevPtr->gyroADCRaw[Y]);
  83     EXPECT_EQ(7, gyroDevPtr->gyroADCRaw[Z]);
  84 }
  85
  86 TEST(SensorGyro, Calibrate)
  87 {
  88     pgResetAll();
  89     gyroInit();
  90     gyroSetTargetLooptime(1);
  91     virtualGyroSet(gyroDevPtr, 5, 6, 7);
  92     const bool read = gyroDevPtr->readFn(gyroDevPtr);
  93     EXPECT_TRUE(read);
  94     EXPECT_EQ(5, gyroDevPtr->gyroADCRaw[X]);
  95     EXPECT_EQ(6, gyroDevPtr->gyroADCRaw[Y]);
  96     EXPECT_EQ(7, gyroDevPtr->gyroADCRaw[Z]);
  97     static const int gyroMovementCalibrationThreshold = 32;
  98     gyroDevPtr->gyroZero[X] = 8;
  99     gyroDevPtr->gyroZero[Y] = 9;
 100     gyroDevPtr->gyroZero[Z] = 10;
 101     performGyroCalibration(gyroSensorPtr, gyroMovementCalibrationThreshold);
 102     EXPECT_EQ(8, gyroDevPtr->gyroZero[X]);
 103     EXPECT_EQ(9, gyroDevPtr->gyroZero[Y]);
 104     EXPECT_EQ(10, gyroDevPtr->gyroZero[Z]);
 105     gyroStartCalibration(false);
 106     EXPECT_FALSE(gyroIsCalibrationComplete());
 107     while (!gyroIsCalibrationComplete()) {
 108         gyroDevPtr->readFn(gyroDevPtr);
 109         performGyroCalibration(gyroSensorPtr, gyroMovementCalibrationThreshold);
 110     }
 111     EXPECT_EQ(5, gyroDevPtr->gyroZero[X]);
 112     EXPECT_EQ(6, gyroDevPtr->gyroZero[Y]);
 113     EXPECT_EQ(7, gyroDevPtr->gyroZero[Z]);
 114 }
 115
 116 TEST(SensorGyro, Update)
 117 {
 118     pgResetAll();
 119     // turn off filters
 120     gyroConfigMutable()->gyro_lpf1_static_hz = 0;
 121     gyroConfigMutable()->gyro_lpf2_static_hz = 0;
 122     gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
 123     gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
 124     gyroInit();
 125     gyroSetTargetLooptime(1);
 126     gyroDevPtr->readFn = virtualGyroRead;
 127     gyroStartCalibration(false);
 128     EXPECT_FALSE(gyroIsCalibrationComplete());
 129
 130     virtualGyroSet(gyroDevPtr, 5, 6, 7);
 131     gyroUpdate();
 132     while (!gyroIsCalibrationComplete()) {
 133         virtualGyroSet(gyroDevPtr, 5, 6, 7);
 134         gyroUpdate();
 135     }
 136     EXPECT_TRUE(gyroIsCalibrationComplete());
 137     EXPECT_EQ(5, gyroDevPtr->gyroZero[X]);
 138     EXPECT_EQ(6, gyroDevPtr->gyroZero[Y]);
 139     EXPECT_EQ(7, gyroDevPtr->gyroZero[Z]);
 140     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
 141     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
 142     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
 143     gyroUpdate();
 144     // expect zero values since gyro is calibrated
 145     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
 146     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
 147     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
 148     virtualGyroSet(gyroDevPtr, 15, 26, 97);
 149     gyroUpdate();
 150     EXPECT_NEAR(10 * gyroDevPtr->scale, gyro.gyroADC[X], 1e-3); // gyro.gyroADC values are scaled
 151     EXPECT_NEAR(20 * gyroDevPtr->scale, gyro.gyroADC[Y], 1e-3);
 152     EXPECT_NEAR(90 * gyroDevPtr->scale, gyro.gyroADC[Z], 1e-3);
 153 }
 154
 155 // STUBS
 156
 157 extern "C" {
 158
 159 uint32_t micros(void) {return 0;}
 160 void beeper(beeperMode_e) {}
 161 uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE };
 162 timeDelta_t getGyroUpdateRate(void) {return gyro.targetLooptime;}
 163 void sensorsSet(uint32_t) {}
 164 void schedulerResetTaskStatistics(taskId_e) {}
 165 int getArmingDisableFlags(void) {return 0;}
 166 void writeEEPROM(void) {}
 167 }