From a3951a3340e8a1360d36dda2dd59da1026e54a9c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 1 Feb 2017 10:32:27 +0000 Subject: [PATCH] Preparation for conversion to parameter groups 3 --- src/main/config/parameter_group.h | 24 +++++++++++++++++++ src/main/fc/fc_init.c | 7 +----- src/main/sensors/gyro.c | 36 ++++++++++++++++++++--------- src/main/sensors/gyro.h | 2 +- src/main/sensors/initialisation.c | 38 ++++++++++++------------------- src/main/sensors/initialisation.h | 6 +---- 6 files changed, 67 insertions(+), 46 deletions(-) diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 8ee75f996..33d49c6f4 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -140,6 +140,7 @@ extern const uint8_t __pg_resetdata_end[]; #endif // USE_PARAMETER_GROUPS +#ifdef USE_PARAMETER_GROUPS // Register system config #define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \ _type _name ## _System; \ @@ -242,6 +243,29 @@ extern const uint8_t __pg_resetdata_end[]; __VA_ARGS__ \ } \ /**/ +#else +#define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \ + _type _name ## _System + +#define PG_REGISTER(_type, _name, _pgn, _version) \ + PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = 0}) \ + /**/ + +#define PG_REGISTER_WITH_RESET_FN(_type, _name, _pgn, _version) \ + extern void pgResetFn_ ## _name(_type *); \ + PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name }) \ + /**/ + +#define PG_REGISTER_WITH_RESET_TEMPLATE(_type, _name, _pgn, _version) \ + extern const _type pgResetTemplate_ ## _name; \ + PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ + /**/ +#define PG_RESET_TEMPLATE(_type, _name, ...) \ + const _type pgResetTemplate_ ## _name PG_RESETDATA_ATTRIBUTES = { \ + __VA_ARGS__ \ + } \ + /**/ +#endif const pgRegistry_t* pgFind(pgn_t pgn); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index a5224e850..4fce84f0d 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -403,12 +403,7 @@ void init(void) } #endif -#ifdef SONAR - const sonarConfig_t *sonarConfig = sonarConfig(); -#else - const void *sonarConfig = NULL; -#endif - if (!sensorsAutodetect(gyroConfig(), accelerometerConfig(), compassConfig(), barometerConfig(), sonarConfig)) { + if (!sensorsAutodetect()) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 1fb1f6618..677bce6a8 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -72,9 +72,6 @@ gyro_t gyro; // gyro access functions static int32_t gyroADC[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; -#ifndef USE_PARAMETER_GROUPS -static const gyroConfig_t *gyroConfig; -#endif static uint16_t calibratingG = 0; static filterApplyFnPtr softLpfFilterApplyFn; @@ -86,6 +83,29 @@ static void *notchFilter2[3]; #define DEBUG_GYRO_CALIBRATION 3 +#ifdef STM32F10X +#define GYRO_SYNC_DENOM_DEFAULT 8 +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) +#define GYRO_SYNC_DENOM_DEFAULT 1 +#else +#define GYRO_SYNC_DENOM_DEFAULT 4 +#endif + +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); + +PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, + .gyro_lpf = GYRO_LPF_256HZ, + .gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT, + .gyro_soft_lpf_type = FILTER_PT1, + .gyro_soft_lpf_hz = 90, + .gyro_soft_notch_hz_1 = 400, + .gyro_soft_notch_cutoff_1 = 300, + .gyro_soft_notch_hz_2 = 200, + .gyro_soft_notch_cutoff_2 = 100, + .gyro_align = ALIGN_DEFAULT, + .gyroMovementCalibrationThreshold = 32 +); + static const extiConfig_t *selectMPUIntExtiConfig(void) { #if defined(MPU_INT_EXTI) @@ -236,13 +256,8 @@ static bool gyroDetect(gyroDev_t *dev) return true; } -bool gyroInit(const gyroConfig_t *gyroConfigToUse) +bool gyroInit(void) { -#ifdef USE_PARAMETER_GROUPS - (void)(gyroConfigToUse); -#else - gyroConfig = gyroConfigToUse; -#endif 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(); @@ -257,8 +272,7 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse) switch (detectedSensors[SENSOR_INDEX_GYRO]) { default: // gyro does not support 32kHz - // cast away constness, legitimate as this is cross-validation - ((gyroConfig_t*)gyroConfig)->gyro_use_32khz = false; + gyroConfigMutable()->gyro_use_32khz = false; break; case GYRO_MPU6500: case GYRO_MPU9250: diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 5bd68fcb9..94e86269b 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -63,7 +63,7 @@ typedef struct gyroConfig_s { PG_DECLARE(gyroConfig_t, gyroConfig); void gyroSetCalibrationCycles(void); -bool gyroInit(const gyroConfig_t *gyroConfigToUse); +bool gyroInit(void); void gyroInitFilters(void); void gyroUpdate(void); bool isGyroCalibrationComplete(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 01b396b2c..38d1dea7d 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,6 +24,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "fc/config.h" #include "fc/runtime_config.h" @@ -52,50 +54,40 @@ static bool sonarDetect(void) } #endif -bool sensorsAutodetect(const gyroConfig_t *gyroConfig, - const accelerometerConfig_t *accelerometerConfig, - const compassConfig_t *compassConfig, - const barometerConfig_t *barometerConfig, - const sonarConfig_t *sonarConfig) +bool sensorsAutodetect(void) { // gyro must be initialised before accelerometer - if (!gyroInit(gyroConfig)) { + if (!gyroInit()) { return false; } - accInit(accelerometerConfig, gyro.targetLooptime); + accInit(accelerometerConfig(), 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(compassConfig); + if (compassDetect(&mag.dev, compassConfig()->mag_hardware)) { + compassInit(compassConfig()); } -#else - UNUSED(compassConfig); #endif #ifdef BARO - baroDetect(&baro.dev, barometerConfig->baro_hardware); -#else - UNUSED(barometerConfig); + baroDetect(&baro.dev, barometerConfig()->baro_hardware); #endif #ifdef SONAR if (sonarDetect()) { - sonarInit(sonarConfig); + sonarInit(sonarConfig()); } -#else - UNUSED(sonarConfig); #endif - if (gyroConfig->gyro_align != ALIGN_DEFAULT) { - gyro.dev.gyroAlign = gyroConfig->gyro_align; + 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 (accelerometerConfig()->acc_align != ALIGN_DEFAULT) { + acc.dev.accAlign = accelerometerConfig()->acc_align; } - if (compassConfig->mag_align != ALIGN_DEFAULT) { - mag.dev.magAlign = compassConfig->mag_align; + if (compassConfig()->mag_align != ALIGN_DEFAULT) { + mag.dev.magAlign = compassConfig()->mag_align; } return true; diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h index e656294c6..9b5d04504 100644 --- a/src/main/sensors/initialisation.h +++ b/src/main/sensors/initialisation.h @@ -17,8 +17,4 @@ #pragma once -bool sensorsAutodetect(const gyroConfig_t *gyroConfig, - const accelerometerConfig_t *accConfig, - const compassConfig_t *compassConfig, - const barometerConfig_t *baroConfig, - const sonarConfig_t *sonarConfig); +bool sensorsAutodetect(void);