Preparation for conversion to parameter groups 3
This commit is contained in:
parent
5d5845d9af
commit
a3951a3340
|
@ -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);
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue