rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Moved further gyro parameters into gyroConfig_t

This commit is contained in:
Martin Budden 2016-11-24 07:24:17 +00:00
parent 0f082201f4
commit 78e6130aab
10 changed files with 41 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/blackbox/blackbox.c
View File

@ -1199,7 +1199,7 @@ static bool blackboxWriteSysinfo()
);
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime);
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.gyro_sync_denom);
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.gyroConfig.gyro_sync_denom);
BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", masterConfig.pid_process_denom);
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8);
@ -1262,7 +1262,7 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.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_soft_type:%d", masterConfig.gyroConfig.gyro_soft_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", masterConfig.gyroConfig.gyro_soft_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_hz_1,

3
src/main/config/config_master.h
View File

@ -92,9 +92,6 @@ typedef struct master_s {
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
uint16_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_sync_denom; // Gyro sample divider
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)

14
src/main/fc/config.c
View File

@ -564,15 +564,15 @@ void createDefaultConfig(master_t *config)
config->current_profile_index = 0; // default profile
config->dcm_kp = 2500; // 1.0 * 10000
config->dcm_ki = 0; // 0.003 * 10000
config->gyro_lpf = GYRO_LPF_256HZ; // 256HZ default
config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ; // 256HZ default
#ifdef STM32F10X
config->gyro_sync_denom = 8;
config->gyroConfig.gyro_sync_denom = 8;
config->pid_process_denom = 1;
#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689)
config->gyro_sync_denom = 1;
config->gyroConfig.gyro_sync_denom = 1;
config->pid_process_denom = 4;
#else
config->gyro_sync_denom = 4;
config->gyroConfig.gyro_sync_denom = 4;
config->pid_process_denom = 2;
#endif
config->gyroConfig.gyro_soft_lpf_type = FILTER_PT1;
@ -830,8 +830,6 @@ void activateConfig(void)
&currentProfile->pidProfile
);
gyroUseConfig(&masterConfig.gyroConfig);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);
#endif
@ -1003,9 +1001,9 @@ void validateAndFixGyroConfig(void)
masterConfig.gyroConfig.gyro_soft_notch_hz_2 = 0;
}
if (masterConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyro_lpf != GYRO_LPF_NONE) {
if (masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_NONE) {
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
masterConfig.gyro_sync_denom = 1;
masterConfig.gyroConfig.gyro_sync_denom = 1;
}
}

9
src/main/fc/fc_msp.c
View File

@ -1081,11 +1081,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_ADVANCED_CONFIG:
if (masterConfig.gyro_lpf) {
if (masterConfig.gyroConfig.gyro_lpf) {
sbufWriteU8(dst, 8); // If gyro_lpf != OFF then looptime is set to 1000
sbufWriteU8(dst, 1);
} else {
sbufWriteU8(dst, masterConfig.gyro_sync_denom);
sbufWriteU8(dst, masterConfig.gyroConfig.gyro_sync_denom);
sbufWriteU8(dst, masterConfig.pid_process_denom);
}
sbufWriteU8(dst, masterConfig.motorConfig.useUnsyncedPwm);
@ -1433,7 +1433,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_ADVANCED_CONFIG:
masterConfig.gyro_sync_denom = sbufReadU8(src);
masterConfig.gyroConfig.gyro_sync_denom = sbufReadU8(src);
masterConfig.pid_process_denom = sbufReadU8(src);
masterConfig.motorConfig.useUnsyncedPwm = sbufReadU8(src);
#ifdef USE_DSHOT
@ -1460,8 +1460,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
}
// reinitialize the gyro filters with the new values
validateAndFixGyroConfig();
gyroUseConfig(&masterConfig.gyroConfig);
gyroInit();
gyroInit(&masterConfig.gyroConfig);
// reinitialize the PID filters with the new values
pidInitFilters(&currentProfile->pidProfile);
break;

4
src/main/io/serial_cli.c
View File

@ -807,8 +807,8 @@ const clivalue_t valueTable[] = {
{ "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDegrees, .config.minmax = { -180, 360 } },
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } },

3
src/main/main.c
View File

@ -425,8 +425,7 @@ void init(void)
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
&masterConfig.sensorSelectionConfig,
masterConfig.compassConfig.mag_declination,
masterConfig.gyro_lpf,
masterConfig.gyro_sync_denom,
&masterConfig.gyroConfig,
sonarConfig)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);

28
src/main/sensors/gyro.c
View File

@ -44,7 +44,6 @@ float gyroADCf[XYZ_AXIS_COUNT];
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
static const gyroConfig_t *gyroConfig;
static float gyroSoftNotchQ1, gyroSoftNotchQ2;
static uint16_t calibratingG = 0;
static filterApplyFnPtr softLpfFilterApplyFn;
@ -54,18 +53,7 @@ static void *notchFilter1[3];
static filterApplyFnPtr notchFilter2ApplyFn;
static void *notchFilter2[3];
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse)
{
gyroConfig = gyroConfigToUse;
if (gyroConfig->gyro_soft_notch_hz_1) {
gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1);
}
if (gyroConfig->gyro_soft_notch_hz_2) {
gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2);
}
}
void gyroInit(void)
void gyroInit(const gyroConfig_t *gyroConfigToUse)
{
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
@ -73,6 +61,8 @@ void gyroInit(void)
static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
gyroConfig = gyroConfigToUse;
softLpfFilterApplyFn = nullFilterApply;
notchFilter1ApplyFn = nullFilterApply;
notchFilter2ApplyFn = nullFilterApply;
@ -82,36 +72,38 @@ void gyroInit(void)
softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) {
softLpfFilter[axis] = &gyroFilterLPF[axis];
biquadFilterInitLPF(softLpfFilter[axis], gyroSoftLpfHz, gyro.targetLooptime);
biquadFilterInitLPF(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
}
} else if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) {
softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
const float gyroDt = (float) gyro.targetLooptime * 0.000001f;
for (int axis = 0; axis < 3; axis++) {
softLpfFilter[axis] = &gyroFilterPt1[axis];
pt1FilterInit(softLpfFilter[axis], gyroSoftLpfHz, gyroDt);
pt1FilterInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyroDt);
}
} else {
softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
for (int axis = 0; axis < 3; axis++) {
softLpfFilter[axis] = &gyroDenoiseState[axis];
firFilterDenoiseInit(softLpfFilter[axis], gyroSoftLpfHz, gyro.targetLooptime);
firFilterDenoiseInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
}
}
}
if (gyroConfig->gyro_soft_notch_hz_1) {
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1);
for (int axis = 0; axis < 3; axis++) {
notchFilter1[axis] = &gyroFilterNotch_1[axis];
biquadFilterInit(notchFilter1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
}
}
if (gyroConfig->gyro_soft_notch_hz_2) {
notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2);
for (int axis = 0; axis < 3; axis++) {
notchFilter2[axis] = &gyroFilterNotch_2[axis];
biquadFilterInit(notchFilter2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
}
}
}

7
src/main/sensors/gyro.h
View File

@ -42,17 +42,18 @@ extern float gyroADCf[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.
uint8_t gyro_sync_denom; // Gyro sample divider
uint8_t gyro_soft_lpf_type;
uint8_t gyro_soft_lpf_hz;
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint16_t gyro_soft_notch_hz_1;
uint16_t gyro_soft_notch_cutoff_1;
uint16_t gyro_soft_notch_hz_2;
uint16_t gyro_soft_notch_cutoff_2;
uint8_t gyro_soft_lpf_type;
} gyroConfig_t;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse);
void gyroSetCalibrationCycles(void);
void gyroInit(void);
void gyroInit(const gyroConfig_t *gyroConfigToUse);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);

11
src/main/sensors/initialisation.c
View File

@ -638,11 +638,10 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
}
}
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
sensorSelectionConfig_t *sensorSelectionConfig,
int16_t magDeclinationFromConfig,
uint8_t gyroLpf,
uint8_t gyroSyncDenominator,
const gyroConfig_t *gyroConfig,
const sonarConfig_t *sonarConfig)
{
memset(&acc, 0, sizeof(acc));
@ -662,9 +661,9 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
// Now time to init things
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation
gyro.init(gyroLpf); // driver initialisation
gyroInit(); // sensor initialisation
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation
gyro.init(gyroConfig->gyro_lpf); // driver initialisation
gyroInit(gyroConfig); // sensor initialisation
if (detectAcc(sensorSelectionConfig->acc_hardware)) {
acc.acc_1G = 256; // set default

8
src/main/sensors/initialisation.h
View File

@ -17,6 +17,12 @@
#pragma once
struct sensorAlignmentConfig_s;
struct sensorSelectionConfig_s;
struct gyroConfig_s;
struct sonarConfig_s;
bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig, const struct sensorSelectionConfig_s *sensorSelectionConfig, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator, const struct sonarConfig_s *sonarConfig);
bool sensorsAutodetect(const sensorAlignmentConfig_s *sensorAlignmentConfig,
const struct sensorSelectionConfig_s *sensorSelectionConfig,
int16_t magDeclinationFromConfig,
const struct gyroConfig_s *gyroConfig,
const struct sonarConfig_s *sonarConfig);