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;}
+}