diff --git a/src/main/drivers/accgyro/accgyro_fake.c b/src/main/drivers/accgyro/accgyro_fake.c index eb4d2f6b76..5fc807259b 100644 --- a/src/main/drivers/accgyro/accgyro_fake.c +++ b/src/main/drivers/accgyro/accgyro_fake.c @@ -27,6 +27,8 @@ #ifdef USE_FAKE_GYRO +#include "build/build_config.h" + #include "common/axis.h" #include "common/utils.h" @@ -59,7 +61,7 @@ void fakeGyroSet(gyroDev_t *gyro, int16_t x, int16_t y, int16_t z) gyroDevUnLock(gyro); } -static bool fakeGyroRead(gyroDev_t *gyro) +STATIC_UNIT_TESTED bool fakeGyroRead(gyroDev_t *gyro) { gyroDevLock(gyro); if (gyro->dataReady == false) { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index eaec195631..94de90c2b9 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -117,7 +117,11 @@ typedef struct gyroSensor_s { bool overflowDetected; } gyroSensor_t; -static FAST_RAM gyroSensor_t gyroSensor1; +STATIC_UNIT_TESTED FAST_RAM gyroSensor_t gyroSensor1; +#ifdef UNIT_TEST +STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyroSensor1; +STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev; +#endif static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); @@ -424,6 +428,7 @@ bool gyroInit(void) gyroDebugMode = DEBUG_NONE; break; } + firstArmingCalibrationWasStarted = false; memset(&gyro, 0, sizeof(gyro)); return gyroInitSensor(&gyroSensor1); } diff --git a/src/test/Makefile b/src/test/Makefile index d2d82b5d07..d07b7a1f32 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -210,6 +210,16 @@ scheduler_unittest_SRC := \ $(USER_DIR)/common/streambuf.c +sensor_gyro_unittest_SRC := \ + $(USER_DIR)/sensors/gyro.c \ + $(USER_DIR)/sensors/boardalignment.c \ + $(USER_DIR)/build/debug.c \ + $(USER_DIR)/common/filter.c \ + $(USER_DIR)/common/maths.c \ + $(USER_DIR)/drivers/accgyro/accgyro_fake.c \ + $(USER_DIR)/drivers/accgyro/gyro_sync.c \ + $(USER_DIR)/pg/pg.c + telemetry_crsf_unittest_SRC := \ $(USER_DIR)/rx/crsf.c \ $(USER_DIR)/telemetry/crsf.c \ diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc new file mode 100644 index 0000000000..a7a4c86e5b --- /dev/null +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -0,0 +1,160 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include +#include + +extern "C" { + #include + + #include "build/build_config.h" + #include "build/debug.h" + #include "common/axis.h" + #include "common/maths.h" + #include "common/utils.h" + #include "drivers/accgyro/accgyro_fake.h" + #include "drivers/accgyro/accgyro_mpu.h" + #include "drivers/sensor.h" + #include "io/beeper.h" + #include "pg/pg.h" + #include "pg/pg_ids.h" + #include "scheduler/scheduler.h" + #include "sensors/gyro.h" + #include "sensors/acceleration.h" + #include "sensors/sensors.h" + + STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev); + struct gyroSensor_s; + STATIC_UNIT_TESTED void performGyroCalibration(struct gyroSensor_s *gyroSensor, uint8_t gyroMovementCalibrationThreshold); + STATIC_UNIT_TESTED bool fakeGyroRead(gyroDev_t *gyro); +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" +extern gyroSensor_s * const gyroSensorPtr; +extern gyroDev_t * const gyroDevPtr; + + +TEST(SensorGyro, Detect) +{ + const gyroSensor_e detected = gyroDetect(gyroDevPtr); + EXPECT_EQ(GYRO_FAKE, detected); + EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]); +} + +TEST(SensorGyro, Init) +{ + pgResetAll(); + const bool initialised = gyroInit(); + EXPECT_EQ(true, initialised); + EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]); +} + +TEST(SensorGyro, Read) +{ + pgResetAll(); + gyroInit(); + fakeGyroSet(gyroDevPtr, 5, 6, 7); + const bool read = gyroDevPtr->readFn(gyroDevPtr); + EXPECT_EQ(true, read); + EXPECT_EQ(5, gyroDevPtr->gyroADCRaw[X]); + EXPECT_EQ(6, gyroDevPtr->gyroADCRaw[Y]); + EXPECT_EQ(7, gyroDevPtr->gyroADCRaw[Z]); +} + +TEST(SensorGyro, Calibrate) +{ + pgResetAll(); + gyroInit(); + fakeGyroSet(gyroDevPtr, 5, 6, 7); + const bool read = gyroDevPtr->readFn(gyroDevPtr); + EXPECT_EQ(true, read); + EXPECT_EQ(5, gyroDevPtr->gyroADCRaw[X]); + EXPECT_EQ(6, gyroDevPtr->gyroADCRaw[Y]); + EXPECT_EQ(7, gyroDevPtr->gyroADCRaw[Z]); + static const int gyroMovementCalibrationThreshold = 32; + gyroDevPtr->gyroZero[X] = 8; + gyroDevPtr->gyroZero[Y] = 9; + gyroDevPtr->gyroZero[Z] = 10; + performGyroCalibration(gyroSensorPtr, gyroMovementCalibrationThreshold); + EXPECT_EQ(8, gyroDevPtr->gyroZero[X]); + EXPECT_EQ(9, gyroDevPtr->gyroZero[Y]); + EXPECT_EQ(10, gyroDevPtr->gyroZero[Z]); + gyroStartCalibration(false); + EXPECT_EQ(false, isGyroCalibrationComplete()); + while (!isGyroCalibrationComplete()) { + gyroDevPtr->readFn(gyroDevPtr); + performGyroCalibration(gyroSensorPtr, gyroMovementCalibrationThreshold); + } + EXPECT_EQ(5, gyroDevPtr->gyroZero[X]); + EXPECT_EQ(6, gyroDevPtr->gyroZero[Y]); + EXPECT_EQ(7, gyroDevPtr->gyroZero[Z]); +} + +TEST(SensorGyro, Update) +{ + pgResetAll(); + // turn off filters + gyroConfigMutable()->gyro_soft_lpf_hz = 0; + gyroConfigMutable()->gyro_soft_notch_hz_1 = 0; + gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; + gyroInit(); + gyroDevPtr->readFn = fakeGyroRead; + gyroStartCalibration(false); + EXPECT_EQ(false, isGyroCalibrationComplete()); + + timeUs_t currentTimeUs = 0; + fakeGyroSet(gyroDevPtr, 5, 6, 7); + gyroUpdate(currentTimeUs); + while (!isGyroCalibrationComplete()) { + fakeGyroSet(gyroDevPtr, 5, 6, 7); + gyroUpdate(currentTimeUs); + } + EXPECT_EQ(true, isGyroCalibrationComplete()); + EXPECT_EQ(5, gyroDevPtr->gyroZero[X]); + EXPECT_EQ(6, gyroDevPtr->gyroZero[Y]); + EXPECT_EQ(7, gyroDevPtr->gyroZero[Z]); + EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]); + EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]); + EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]); + gyroUpdate(currentTimeUs); + // expect zero values since gyro is calibrated + EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]); + EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]); + EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]); + fakeGyroSet(gyroDevPtr, 15, 26, 97); + gyroUpdate(currentTimeUs); + EXPECT_FLOAT_EQ(10 * gyroDevPtr->scale, gyro.gyroADCf[X]); // gyroADCf values are scaled + EXPECT_FLOAT_EQ(20 * gyroDevPtr->scale, gyro.gyroADCf[Y]); + EXPECT_FLOAT_EQ(90 * gyroDevPtr->scale, gyro.gyroADCf[Z]); +} + +// STUBS + +extern "C" { + +uint32_t micros(void) {return 0;} +void beeper(beeperMode_e) {} +uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE }; +timeDelta_t getGyroUpdateRate(void) {return gyro.targetLooptime;} +void sensorsSet(uint32_t) {} +void schedulerResetTaskStatistics(cfTaskId_e) {} +int getArmingDisableFlags(void) {return 0;} +}