Added gyro unit test

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) {

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

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 \

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <limits.h>
+#include <algorithm>
+
+extern "C" {
+    #include <platform.h>
+
+    #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;}
+}