Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

Move gyro master config items to gyro config

Browse source
This commit is contained in:
Martin Budden 2016-11-28 18:15:46 +00:00
parent a6ca0bf09d
commit cce0efca2e
12 changed files with 56 additions and 65 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View file

@ -1379,7 +1379,7 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.profile[masterConfig.current_profile_index].rcControlsConfig.deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.profile[masterConfig.current_profile_index].rcControlsConfig.yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyroConfig.gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.gyro_soft_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.acc_soft_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.sensorSelectionConfig.acc_hardware);

12
src/main/cms/cms_menu_imu.c
View file

@ -470,9 +470,9 @@ static uint8_t cmsx_gyroLpf; // Global
static long cmsx_menuGyro_onEnter(void)
{
cmsx_gyroSync = masterConfig.gyroSync;
cmsx_gyroSyncDenom = masterConfig.gyroSyncDenominator;
cmsx_gyroLpf = masterConfig.gyro_lpf;
cmsx_gyroSync = masterConfig.gyroConfig.gyroSync;
cmsx_gyroSyncDenom = masterConfig.gyroConfig.gyroSyncDenominator;
cmsx_gyroLpf = masterConfig.gyroConfig.gyro_lpf;
return 0;
}
@ -481,9 +481,9 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self)
{
UNUSED(self);
masterConfig.gyroSync = cmsx_gyroSync;
masterConfig.gyroSyncDenominator = cmsx_gyroSyncDenom;
masterConfig.gyro_lpf = cmsx_gyroLpf;
masterConfig.gyroConfig.gyroSync = cmsx_gyroSync;
masterConfig.gyroConfig.gyroSyncDenominator = cmsx_gyroSyncDenom;
masterConfig.gyroConfig.gyro_lpf = cmsx_gyroLpf;
return 0;
}

24
src/main/config/config.c
View file

@ -402,7 +402,7 @@ uint32_t getPidUpdateRate(void) {
if (masterConfig.asyncMode == ASYNC_MODE_NONE) {
return getGyroUpdateRate();
} else {
return masterConfig.looptime;
return masterConfig.gyroConfig.looptime;
}
}
@ -495,7 +495,7 @@ static void resetConf(void)
masterConfig.dcm_ki_acc = 50; // 0.005 * 10000
masterConfig.dcm_kp_mag = 10000; // 1.00 * 10000
masterConfig.dcm_ki_mag = 0; // 0.00 * 10000
masterConfig.gyro_lpf = 3; // INV_FILTER_42HZ, In case of ST gyro, will default to 32Hz instead
masterConfig.gyroConfig.gyro_lpf = 3; // INV_FILTER_42HZ, In case of ST gyro, will default to 32Hz instead
resetAccelerometerTrims(&masterConfig.sensorTrims.accZero, &masterConfig.sensorTrims.accGain);
@ -572,10 +572,10 @@ static void resetConf(void)
resetSerialConfig(&masterConfig.serialConfig);
masterConfig.looptime = 2000;
masterConfig.gyroConfig.looptime = 2000;
masterConfig.i2c_overclock = 0;
masterConfig.gyroSync = 0;
masterConfig.gyroSyncDenominator = 2;
masterConfig.gyroConfig.gyroSync = 0;
masterConfig.gyroConfig.gyroSyncDenominator = 2;
#ifdef ASYNC_GYRO_PROCESSING
masterConfig.accTaskFrequency = ACC_TASK_FREQUENCY_DEFAULT;
@ -783,7 +783,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);
gyroUseConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf_hz);
masterConfig.gyroConfig.gyro_soft_lpf_hz = currentProfile->pidProfile.gyro_soft_lpf_hz;
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);
@ -905,7 +905,7 @@ void validateAndFixConfig(void)
* When async processing mode is enabled, gyroSync has to be forced to "ON"
*/
if (getAsyncMode() != ASYNC_MODE_NONE) {
masterConfig.gyroSync = 1;
masterConfig.gyroConfig.gyroSync = 1;
}
#endif
@ -917,16 +917,16 @@ void validateAndFixConfig(void)
uint8_t denominatorLimit = 2;
if (masterConfig.gyro_lpf == 0) {
if (masterConfig.gyroConfig.gyro_lpf == 0) {
denominatorLimit = 16;
}
if (masterConfig.gyroSyncDenominator < denominatorLimit) {
masterConfig.gyroSyncDenominator = denominatorLimit;
if (masterConfig.gyroConfig.gyroSyncDenominator < denominatorLimit) {
masterConfig.gyroConfig.gyroSyncDenominator = denominatorLimit;
}
if (masterConfig.looptime < 2000) {
masterConfig.looptime = 2000;
if (masterConfig.gyroConfig.looptime < 2000) {
masterConfig.gyroConfig.looptime = 2000;
}
}

5
src/main/config/config_master.h
View file

@ -74,10 +74,7 @@ typedef struct master_s {
uint8_t mixerMode;
uint32_t enabledFeatures;
uint8_t persistentFlags;
uint16_t looptime; // imu loop time in us
uint8_t i2c_overclock; // Overclock i2c Bus for faster IMU readings
uint8_t gyroSync; // Enable interrupt based loop
uint8_t gyroSyncDenominator; // Gyro sync Denominator
#ifdef ASYNC_GYRO_PROCESSING
uint16_t accTaskFrequency;
@ -107,8 +104,6 @@ typedef struct master_s {
uint16_t dcm_kp_mag; // DCM filter proportional gain ( x 10000) for magnetometer and GPS heading
uint16_t dcm_ki_mag; // DCM filter integral gain ( x 10000) for magnetometer and GPS heading
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
gyroConfig_t gyroConfig;
barometerConfig_t barometerConfig;

5
src/main/fc/fc_init.c
View file

@ -476,10 +476,7 @@ void init(void)
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
&masterConfig.sensorSelectionConfig,
currentProfile->mag_declination,
masterConfig.looptime,
masterConfig.gyro_lpf,
masterConfig.gyroSync,
masterConfig.gyroSyncDenominator)) {
&masterConfig.gyroConfig)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);

16
src/main/fc/fc_msp.c
View file

@ -632,7 +632,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_LOOP_TIME:
sbufWriteU16(dst, masterConfig.looptime);
sbufWriteU16(dst, masterConfig.gyroConfig.looptime);
break;
case MSP_RC_TUNING:
@ -1002,7 +1002,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_ADVANCED_CONFIG:
sbufWriteU8(dst, masterConfig.gyroSyncDenominator);
sbufWriteU8(dst, masterConfig.gyroConfig.gyroSyncDenominator);
sbufWriteU8(dst, 1); // BF: masterConfig.pid_process_denom
sbufWriteU8(dst, 1); // BF: masterConfig.motorConfig.useUnsyncedPwm
sbufWriteU8(dst, masterConfig.motorConfig.motorPwmProtocol);
@ -1012,7 +1012,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#else
sbufWriteU16(dst, 0);
#endif
sbufWriteU8(dst, masterConfig.gyroSync);
sbufWriteU8(dst, masterConfig.gyroConfig.gyroSync);
break;
case MSP_FILTER_CONFIG :
@ -1065,7 +1065,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, 0);
#endif
sbufWriteU16(dst, masterConfig.mixerConfig.yaw_jump_prevention_limit);
sbufWriteU8(dst, masterConfig.gyro_lpf);
sbufWriteU8(dst, masterConfig.gyroConfig.gyro_lpf);
sbufWriteU8(dst, currentProfile->pidProfile.acc_soft_lpf_hz);
sbufWriteU8(dst, 0); //reserved
sbufWriteU8(dst, 0); //reserved
@ -1190,7 +1190,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_LOOP_TIME:
masterConfig.looptime = sbufReadU16(src);
masterConfig.gyroConfig.looptime = sbufReadU16(src);
break;
case MSP_SET_PID_CONTROLLER:
@ -1380,7 +1380,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_ADVANCED_CONFIG:
masterConfig.gyroSyncDenominator = sbufReadU8(src);
masterConfig.gyroConfig.gyroSyncDenominator = sbufReadU8(src);
sbufReadU8(src); // BF: masterConfig.pid_process_denom
sbufReadU8(src); // BF: masterConfig.motorConfig.useUnsyncedPwm
masterConfig.motorConfig.motorPwmProtocol = sbufReadU8(src);
@ -1390,7 +1390,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#else
sbufReadU16(src);
#endif
masterConfig.gyroSync = sbufReadU8(src);
masterConfig.gyroConfig.gyroSync = sbufReadU8(src);
break;
case MSP_SET_FILTER_CONFIG :
@ -1446,7 +1446,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
sbufReadU8(src);
#endif
masterConfig.mixerConfig.yaw_jump_prevention_limit = sbufReadU16(src);
masterConfig.gyro_lpf = sbufReadU8(src);
masterConfig.gyroConfig.gyro_lpf = sbufReadU8(src);
currentProfile->pidProfile.acc_soft_lpf_hz = sbufReadU8(src);
sbufReadU8(src); //reserved
sbufReadU8(src); //reserved

2
src/main/fc/mw.c
View file

@ -520,7 +520,7 @@ void taskGyro(uint32_t currentTime) {
// To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
const uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
if (masterConfig.gyroSync) {
if (masterConfig.gyroConfig.gyroSync) {
while (true) {
#ifdef ASYNC_GYRO_PROCESSING
if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTime)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {

8
src/main/io/serial_cli.c
View file

@ -623,10 +623,10 @@ typedef struct {
} clivalue_t;
const clivalue_t valueTable[] = {
{ "looptime", VAR_UINT16 | MASTER_VALUE, &masterConfig.looptime, .config.minmax = {0, 9000}, 0 },
{ "looptime", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.looptime, .config.minmax = {0, 9000}, 0 },
{ "i2c_overclock", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.i2c_overclock, .config.lookup = { TABLE_OFF_ON }, 0 },
{ "gyro_sync", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroSync, .config.lookup = { TABLE_OFF_ON } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroSyncDenominator, .config.minmax = { 1, 32 } },
{ "gyro_sync", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyroSync, .config.lookup = { TABLE_OFF_ON } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroSyncDenominator, .config.minmax = { 1, 32 } },
#ifdef ASYNC_GYRO_PROCESSING
{ "acc_task_frequency", VAR_UINT16 | MASTER_VALUE, &masterConfig.accTaskFrequency, .config.minmax = { ACC_TASK_FREQUENCY_MIN, ACC_TASK_FREQUENCY_MAX } },
@ -795,7 +795,7 @@ const clivalue_t valueTable[] = {
{ "align_board_pitch", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.pitchDeciDegrees, .config.minmax = { -1800, 3600 }, 0 },
{ "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDeciDegrees, .config.minmax = { -1800, 3600 }, 0 },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 }, 0 },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp_acc, .config.minmax = { 0, 65535 }, 0 },

18
src/main/sensors/gyro.c
View file

@ -45,26 +45,20 @@ static const gyroConfig_t *gyroConfig;
static uint16_t calibratingG = 0;
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static uint8_t gyroSoftLpfHz = 0;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz)
{
gyroConfig = gyroConfigToUse;
gyroSoftLpfHz = gyro_soft_lpf_hz;
}
void gyroInit(void)
void gyroInit(const gyroConfig_t *gyroConfigToUse)
{
/*
* After refactoring this function is always called after gyro sampling rate is known, so
* no additional condition is required
*/
if (gyroSoftLpfHz) {
gyroConfig = gyroConfigToUse;
if (gyroConfig->gyro_soft_lpf_hz) {
for (int axis = 0; axis < 3; axis++) {
#ifdef ASYNC_GYRO_PROCESSING
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, getGyroUpdateRate());
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, getGyroUpdateRate());
#else
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
#endif
}
}
@ -150,7 +144,7 @@ void gyroUpdate(void)
gyroADC[axis] = gyroADCRaw[axis];
}
if (gyroSoftLpfHz) {
if (gyroConfig->gyro_soft_lpf_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] = lrintf(biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]));
}

8
src/main/sensors/gyro.h
View file

@ -39,11 +39,15 @@ extern int32_t gyroADC[XYZ_AXIS_COUNT];
typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
uint16_t looptime; // imu loop time in us
uint8_t gyroSync; // Enable interrupt based loop
uint8_t gyroSyncDenominator; // Gyro sync Denominator
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint8_t gyro_soft_lpf_hz;
} gyroConfig_t;
void gyroInit(void);
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t initialGyroLpfCutHz);
void gyroInit(const gyroConfig_t *gyroConfigToUse);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);

12
src/main/sensors/initialisation.c
View file

@ -678,10 +678,7 @@ static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentC
bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
const sensorSelectionConfig_t *sensorSelectionConfig,
int16_t magDeclinationFromConfig,
uint32_t looptime,
uint8_t gyroLpf,
uint8_t gyroSync,
uint8_t gyroSyncDenominator)
const gyroConfig_t *gyroConfig)
{
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
@ -696,9 +693,10 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
}
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.targetLooptime = gyroSetSampleRate(looptime, gyroLpf, gyroSync, gyroSyncDenominator); // Set gyro sample rate before initialisation
gyro.init(gyroLpf); // driver initialisation
gyroInit(); // sensor initialisation
// Set gyro sample rate before initialisation
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->looptime, gyroConfig->gyro_lpf, gyroConfig->gyroSync, gyroConfig->gyroSyncDenominator);
gyro.init(gyroConfig->gyro_lpf); // driver initialisation
gyroInit(gyroConfig); // sensor initialisation
if (detectAcc(sensorSelectionConfig->acc_hardware)) {
acc.acc_1G = 256; // set default

9
src/main/sensors/initialisation.h
View file

@ -17,7 +17,10 @@
#pragma once
bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
const sensorSelectionConfig_t *sensorSelectionConfig,
struct sensorAlignmentConfig_s;
struct sensorSelectionConfig_s;
struct gyroConfig_s;
bool sensorsAutodetect(const struct sensorAlignmentConfig_s *sensorAlignmentConfig,
const struct sensorSelectionConfig_s *sensorSelectionConfig,
int16_t magDeclinationFromConfig,
uint32_t looptime, uint8_t gyroLpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);
const struct gyroConfig_s *gyroConfig);