From f2b423525d4e8e534d5d347efeb5cbc70ad39a32 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 5 Feb 2017 22:24:13 +0000 Subject: [PATCH] Split gyro device out of gyro sensor struct --- src/main/drivers/accgyro_fake.c | 4 +- src/main/fc/fc_core.c | 5 +- src/main/fc/fc_msp.c | 2 +- src/main/sensors/acceleration.c | 13 +-- src/main/sensors/compass.c | 17 +++- src/main/sensors/compass.h | 6 +- src/main/sensors/gyro.c | 107 ++++++++++++++++--------- src/main/sensors/gyro.h | 9 ++- src/main/sensors/initialisation.c | 15 +--- src/main/sensors/sensors.h | 1 - src/main/target/COLIBRI_RACE/i2c_bst.c | 2 +- src/main/telemetry/frsky.c | 2 +- src/main/telemetry/ibus.c | 2 +- src/test/Makefile | 54 +++++++++++++ src/test/unit/platform.h | 5 ++ src/test/unit/target.h | 1 + 16 files changed, 168 insertions(+), 77 deletions(-) diff --git a/src/main/drivers/accgyro_fake.c b/src/main/drivers/accgyro_fake.c index 9e661f55a..105f6a33e 100644 --- a/src/main/drivers/accgyro_fake.c +++ b/src/main/drivers/accgyro_fake.c @@ -20,6 +20,8 @@ #include "platform.h" +#ifdef USE_FAKE_GYRO + #include "common/axis.h" #include "common/utils.h" @@ -27,8 +29,6 @@ #include "accgyro_fake.h" -#ifdef USE_FAKE_GYRO - static int16_t fakeGyroADC[XYZ_AXIS_COUNT]; static void fakeGyroInit(gyroDev_t *gyro) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 47535bb6f..5d4b2a04c 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -93,7 +93,6 @@ int16_t headFreeModeHold; uint8_t motorControlEnable = false; -int16_t telemTemperature1; // gyro sensor temperature static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero bool isRXDataNew; @@ -470,8 +469,8 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs) if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} // Read out gyro temperature if used for telemmetry - if (feature(FEATURE_TELEMETRY) && gyro.dev.temperature) { - gyro.dev.temperature(&gyro.dev, &telemTemperature1); + if (feature(FEATURE_TELEMETRY)) { + gyroReadTemperature(); } #ifdef MAG diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b8a7d5aeb..2a946b2c9 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -664,7 +664,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, acc.accSmooth[i] / scale); } for (int i = 0; i < 3; i++) { - sbufWriteU16(dst, lrintf(gyro.gyroADCf[i] / gyro.dev.scale)); + sbufWriteU16(dst, gyroRateDps(i)); } for (int i = 0; i < 3; i++) { sbufWriteU16(dst, mag.magADC[i]); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 0d9289ede..f9af837dc 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -35,18 +35,18 @@ #include "drivers/accgyro_bma280.h" #include "drivers/accgyro_fake.h" #include "drivers/accgyro_l3g4200d.h" +#include "drivers/accgyro_l3gd20.h" +#include "drivers/accgyro_lsm303dlhc.h" #include "drivers/accgyro_mma845x.h" #include "drivers/accgyro_mpu.h" #include "drivers/accgyro_mpu3050.h" #include "drivers/accgyro_mpu6050.h" #include "drivers/accgyro_mpu6500.h" -#include "drivers/accgyro_l3gd20.h" -#include "drivers/accgyro_lsm303dlhc.h" -#include "drivers/bus_spi.h" #include "drivers/accgyro_spi_icm20689.h" #include "drivers/accgyro_spi_mpu6000.h" #include "drivers/accgyro_spi_mpu6500.h" #include "drivers/accgyro_spi_mpu9250.h" +#include "drivers/bus_spi.h" #include "drivers/system.h" #include "fc/config.h" @@ -250,8 +250,8 @@ bool accInit(uint32_t gyroSamplingInverval) { memset(&acc, 0, sizeof(acc)); // copy over the common gyro mpu settings - acc.dev.mpuConfiguration = gyro.dev.mpuConfiguration; - acc.dev.mpuDetectionResult = gyro.dev.mpuDetectionResult; + acc.dev.mpuConfiguration = *gyroMpuConfiguration(); + acc.dev.mpuDetectionResult = *gyroMpuDetectionResult(); if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) { return false; } @@ -278,6 +278,9 @@ bool accInit(uint32_t gyroSamplingInverval) biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval); } } + if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) { + acc.dev.accAlign = accelerometerConfig()->acc_align; + } return true; } diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 8e84e44b4..2c012a944 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -44,6 +44,7 @@ #include "hardware_revision.h" #endif +magDev_t magDev; mag_t mag; // mag access functions #ifdef MAG @@ -146,17 +147,25 @@ retry: return true; } -void compassInit(void) +bool compassInit(void) { // initialize and calibration. turn on led during mag calibration (calibration routine blinks it) // calculate magnetic declination + mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. + if (!compassDetect(&magDev, compassConfig()->mag_hardware)) { + return false; + } const int16_t deg = compassConfig()->mag_declination / 100; const int16_t min = compassConfig()->mag_declination % 100; mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units LED1_ON; - mag.dev.init(); + magDev.init(); LED1_OFF; magInit = 1; + if (compassConfig()->mag_align != ALIGN_DEFAULT) { + magDev.magAlign = compassConfig()->mag_align; + } + return true; } void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero) @@ -165,11 +174,11 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero) static flightDynamicsTrims_t magZeroTempMin; static flightDynamicsTrims_t magZeroTempMax; - mag.dev.read(magADCRaw); + magDev.read(magADCRaw); for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { mag.magADC[axis] = magADCRaw[axis]; } - alignSensors(mag.magADC, mag.dev.magAlign); + alignSensors(mag.magADC, magDev.magAlign); if (STATE(CALIBRATE_MAG)) { tCal = currentTime; diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 4f95e9db5..7f4002fc6 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -18,7 +18,7 @@ #pragma once #include "config/parameter_group.h" -#include "drivers/compass.h" +#include "drivers/sensor.h" #include "sensors/sensors.h" @@ -32,7 +32,6 @@ typedef enum { } magSensor_e; typedef struct mag_s { - magDev_t dev; int32_t magADC[XYZ_AXIS_COUNT]; float magneticDeclination; } mag_t; @@ -49,8 +48,7 @@ typedef struct compassConfig_s { PG_DECLARE(compassConfig_t, compassConfig); -bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse); -void compassInit(void); +bool compassInit(void); union flightDynamicsTrims_u; void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 677bce6a8..ee076a035 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -36,13 +36,13 @@ #include "drivers/accgyro_bma280.h" #include "drivers/accgyro_fake.h" #include "drivers/accgyro_l3g4200d.h" +#include "drivers/accgyro_l3gd20.h" +#include "drivers/accgyro_lsm303dlhc.h" #include "drivers/accgyro_mma845x.h" #include "drivers/accgyro_mpu.h" #include "drivers/accgyro_mpu3050.h" #include "drivers/accgyro_mpu6050.h" #include "drivers/accgyro_mpu6500.h" -#include "drivers/accgyro_l3gd20.h" -#include "drivers/accgyro_lsm303dlhc.h" #include "drivers/accgyro_spi_icm20689.h" #include "drivers/accgyro_spi_mpu6000.h" #include "drivers/accgyro_spi_mpu6500.h" @@ -59,19 +59,22 @@ #include "scheduler/scheduler.h" -#include "sensors/sensors.h" #include "sensors/boardalignment.h" #include "sensors/gyro.h" +#include "sensors/sensors.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif -gyro_t gyro; // gyro access functions +gyro_t gyro; + +STATIC_UNIT_TESTED gyroDev_t gyroDev0; +static int16_t gyroTemperature0; static int32_t gyroADC[XYZ_AXIS_COUNT]; -static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; +STATIC_UNIT_TESTED int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static uint16_t calibratingG = 0; static filterApplyFnPtr softLpfFilterApplyFn; @@ -106,6 +109,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyroMovementCalibrationThreshold = 32 ); +#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) static const extiConfig_t *selectMPUIntExtiConfig(void) { #if defined(MPU_INT_EXTI) @@ -117,8 +121,18 @@ static const extiConfig_t *selectMPUIntExtiConfig(void) return NULL; #endif } +#endif -static bool gyroDetect(gyroDev_t *dev) +const mpuConfiguration_t *gyroMpuConfiguration(void) +{ + return &gyroDev0.mpuConfiguration; +} +const mpuDetectionResult_t *gyroMpuDetectionResult(void) +{ + return &gyroDev0.mpuDetectionResult; +} + +STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) { gyroSensor_e gyroHardware = GYRO_DEFAULT; @@ -246,34 +260,29 @@ static bool gyroDetect(gyroDev_t *dev) gyroHardware = GYRO_NONE; } - if (gyroHardware == GYRO_NONE) { - return false; + if (gyroHardware != GYRO_NONE) { + detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; + sensorsSet(SENSOR_GYRO); } - detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; - sensorsSet(SENSOR_GYRO); - return true; + return gyroHardware; } bool gyroInit(void) { memset(&gyro, 0, sizeof(gyro)); #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) - gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig(); - mpuDetect(&gyro.dev); - mpuResetFn = gyro.dev.mpuConfiguration.resetFn; + gyroDev0.mpuIntExtiConfig = selectMPUIntExtiConfig(); + mpuDetect(&gyroDev0); + mpuResetFn = gyroDev0.mpuConfiguration.resetFn; #endif - - if (!gyroDetect(&gyro.dev)) { + const gyroSensor_e gyroHardware = gyroDetect(&gyroDev0); + if (gyroHardware == GYRO_NONE) { return false; } - switch (detectedSensors[SENSOR_INDEX_GYRO]) { - default: - // gyro does not support 32kHz - gyroConfigMutable()->gyro_use_32khz = false; - break; + switch (gyroHardware) { case GYRO_MPU6500: case GYRO_MPU9250: case GYRO_ICM20689: @@ -281,12 +290,19 @@ bool gyroInit(void) case GYRO_ICM20602: // do nothing, as gyro supports 32kHz break; + default: + // gyro does not support 32kHz + gyroConfigMutable()->gyro_use_32khz = false; + break; } // Must set gyro sample rate before initialisation - gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz); - gyro.dev.lpf = gyroConfig()->gyro_lpf; - gyro.dev.init(&gyro.dev); + gyro.targetLooptime = gyroSetSampleRate(&gyroDev0, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz); + gyroDev0.lpf = gyroConfig()->gyro_lpf; + gyroDev0.init(&gyroDev0); + if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { + gyroDev0.gyroAlign = gyroConfig()->gyro_align; + } gyroInitFilters(); return true; } @@ -369,7 +385,7 @@ void gyroSetCalibrationCycles(void) calibratingG = gyroCalculateCalibratingCycles(); } -static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold) +STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold) { static int32_t g[3]; static stdev_t var[3]; @@ -445,27 +461,27 @@ static bool gyroUpdateISR(gyroDev_t* gyroDev) void gyroUpdate(void) { // range: +/- 8192; +/- 2000 deg/sec - if (gyro.dev.update) { + if (gyroDev0.update) { // if the gyro update function is set then return, since the gyro is read in gyroUpdateISR return; } - if (!gyro.dev.read(&gyro.dev)) { + if (!gyroDev0.read(&gyroDev0)) { return; } - gyro.dev.dataReady = false; + gyroDev0.dataReady = false; // move gyro data into 32-bit variables to avoid overflows in calculations - gyroADC[X] = gyro.dev.gyroADCRaw[X]; - gyroADC[Y] = gyro.dev.gyroADCRaw[Y]; - gyroADC[Z] = gyro.dev.gyroADCRaw[Z]; + gyroADC[X] = gyroDev0.gyroADCRaw[X]; + gyroADC[Y] = gyroDev0.gyroADCRaw[Y]; + gyroADC[Z] = gyroDev0.gyroADCRaw[Z]; - alignSensors(gyroADC, gyro.dev.gyroAlign); + alignSensors(gyroADC, gyroDev0.gyroAlign); const bool calibrationComplete = isGyroCalibrationComplete(); if (calibrationComplete) { #if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL) // SPI-based gyro so can read and update in ISR if (gyroConfig()->gyro_isr_update) { - mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR); + mpuGyroSetIsrUpdate(&gyroDev0, gyroUpdateISR); return; } #endif @@ -479,7 +495,7 @@ void gyroUpdate(void) for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADC[axis] -= gyroZero[axis]; // scale gyro output to degrees per second - float gyroADCf = (float)gyroADC[axis] * gyro.dev.scale; + float gyroADCf = (float)gyroADC[axis] * gyroDev0.scale; // Apply LPF DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); @@ -493,8 +509,25 @@ void gyroUpdate(void) } if (!calibrationComplete) { - gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyro.dev.scale); - gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyro.dev.scale); - gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyro.dev.scale); + gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyroDev0.scale); + gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.scale); + gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyroDev0.scale); } } + +void gyroReadTemperature(void) +{ + if (gyroDev0.temperature) { + gyroDev0.temperature(&gyroDev0, &gyroTemperature0); + } +} + +int16_t gyroGetTemperature(void) +{ + return gyroTemperature0; +} + +int16_t gyroRateDps(int axis) +{ + return lrintf(gyro.gyroADCf[axis] / gyroDev0.scale); +} diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 94e86269b..44ae0a798 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -18,7 +18,6 @@ #pragma once #include "config/parameter_group.h" -#include "drivers/accgyro.h" #include "common/axis.h" typedef enum { @@ -38,7 +37,6 @@ typedef enum { } gyroSensor_e; typedef struct gyro_s { - gyroDev_t dev; uint32_t targetLooptime; float gyroADCf[XYZ_AXIS_COUNT]; } gyro_t; @@ -62,8 +60,13 @@ typedef struct gyroConfig_s { PG_DECLARE(gyroConfig_t, gyroConfig); -void gyroSetCalibrationCycles(void); bool gyroInit(void); void gyroInitFilters(void); void gyroUpdate(void); +const mpuConfiguration_t *gyroMpuConfiguration(void); +const mpuDetectionResult_t *gyroMpuDetectionResult(void); +void gyroSetCalibrationCycles(void); bool isGyroCalibrationComplete(void); +void gyroReadTemperature(void); +int16_t gyroGetTemperature(void); +int16_t gyroRateDps(int axis); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 4220fab3b..bbccc9414 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -63,11 +63,8 @@ bool sensorsAutodetect(void) accInit(gyro.targetLooptime); - mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. #ifdef MAG - if (compassDetect(&mag.dev, compassConfig()->mag_hardware)) { - compassInit(); - } + compassInit(); #endif #ifdef BARO @@ -80,15 +77,5 @@ bool sensorsAutodetect(void) } #endif - if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { - gyro.dev.gyroAlign = gyroConfig()->gyro_align; - } - if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) { - acc.dev.accAlign = accelerometerConfig()->acc_align; - } - if (compassConfig()->mag_align != ALIGN_DEFAULT) { - mag.dev.magAlign = compassConfig()->mag_align; - } - return true; } diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index a7e6f1030..a93446511 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -25,7 +25,6 @@ typedef enum { SENSOR_INDEX_COUNT } sensorIndex_e; -extern int16_t telemTemperature1; //FIXME move to temp sensor...? extern uint8_t detectedSensors[SENSOR_INDEX_COUNT]; typedef struct int16_flightDynamicsTrims_s { diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 550f69b97..3d84971bc 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -613,7 +613,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite16(acc.accSmooth[i] / scale); } for (i = 0; i < 3; i++) { - bstWrite16(lrintf(gyro.gyroADCf[i] /gyro.dev.scale)); + bstWrite16(gyroRateDps(i)); } for (i = 0; i < 3; i++) { bstWrite16(mag.magADC[i]); diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index ef1877e82..932f4bdc0 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -223,7 +223,7 @@ static void sendTemperature1(void) #elif defined(BARO) serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf #else - serialize16(telemTemperature1 / 10); + serialize16(gyroGetTemperature() / 10); #endif } diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c index 9bb193454..42c8b40d8 100644 --- a/src/main/telemetry/ibus.c +++ b/src/main/telemetry/ibus.c @@ -300,7 +300,7 @@ static void dispatchMeasurementReply(ibusAddress_t address) #ifdef BARO value = (baro.baroTemperature + 5) / 10; // +5 to make integer division rounding correct #else - value = telemTemperature1 * 10; + value = gyroGetTemperature() * 10; #endif sendIbusMeasurement(address, value + IBUS_TEMPERATURE_OFFSET); break; diff --git a/src/test/Makefile b/src/test/Makefile index 26b95987c..da633d863 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -675,6 +675,60 @@ $(OBJECT_DIR)/alignsensor_unittest : \ $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/build/debug.o : \ + $(USER_DIR)/build/debug.c \ + $(USER_DIR)/build/debug.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/build/debug.c -o $@ + +$(OBJECT_DIR)/drivers/gyro_sync.o : \ + $(USER_DIR)/drivers/gyro_sync.c \ + $(USER_DIR)/drivers/gyro_sync.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/gyro_sync.c -o $@ + +$(OBJECT_DIR)/drivers/accgyro_fake.o : \ + $(USER_DIR)/drivers/accgyro_fake.c \ + $(USER_DIR)/drivers/accgyro_fake.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/accgyro_fake.c -o $@ + +$(OBJECT_DIR)/sensors/gyro.o : \ + $(USER_DIR)/sensors/gyro.c \ + $(USER_DIR)/sensors/gyro.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/gyro.c -o $@ + +$(OBJECT_DIR)/sensor_gyro_unittest.o : \ + $(TEST_DIR)/sensor_gyro_unittest.cc \ + $(USER_DIR)/sensors/gyro.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/sensor_gyro_unittest.cc -o $@ + +$(OBJECT_DIR)/sensor_gyro_unittest : \ + $(OBJECT_DIR)/build/debug.o \ + $(OBJECT_DIR)/common/filter.o \ + $(OBJECT_DIR)/common/maths.o \ + $(OBJECT_DIR)/drivers/accgyro_fake.o \ + $(OBJECT_DIR)/drivers/gyro_sync.o \ + $(OBJECT_DIR)/sensors/boardalignment.o \ + $(OBJECT_DIR)/sensors/gyro.o \ + $(OBJECT_DIR)/sensor_gyro_unittest.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ + + $(OBJECT_DIR)/build/version.o : \ $(USER_DIR)/build/version.c \ $(USER_DIR)/build/version.h \ diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 6b5f77acf..e78f88ef0 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -65,6 +65,11 @@ uint8_t DMA_GetFlagStatus(void *); void DMA_Cmd(DMA_Channel_TypeDef*, FunctionalState ); void DMA_ClearFlag(uint32_t); +typedef struct +{ + void* test; +} SPI_TypeDef; + typedef struct { void* test; diff --git a/src/test/unit/target.h b/src/test/unit/target.h index 741529284..e4d607342 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -19,6 +19,7 @@ #define CMS #define CMS_MAX_DEVICE 4 +#define USE_FAKE_GYRO #define MAG #define BARO #define GPS