Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

revert some USE_GYRO_DATA_ANALYSE and USE_GYRO_LPF2 gating #936

Merged
merged 1 commit into from
Oct 27, 2023
Merged

revert some USE_GYRO_DATA_ANALYSE and USE_GYRO_LPF2 gating #936

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions src/main/flight/pid.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -197,10 +197,8 @@ void resetPidProfile(pidProfile_t *pidProfile) {
.dterm_ABG_half_life = 50,
.emuGravityGain = 50,
.angle_filter = 100,
#ifdef USE_GYRO_DATA_ANALYSE
.dtermDynNotch = false,
.dterm_dyn_notch_q = 400,
#endif
);
}

Expand Down Expand Up @@ -237,7 +235,9 @@ static FAST_RAM filterApplyFnPtr angleSetpointFilterApplyFn = nullFilterApply;
static FAST_RAM_ZERO_INIT pt1Filter_t angleSetpointFilter[2];
static FAST_RAM filterApplyFnPtr dtermABGapplyFn = nullFilterApply;
static FAST_RAM_ZERO_INIT alphaBetaGammaFilter_t dtermABG[XYZ_AXIS_COUNT];
#ifdef USE_GYRO_DATA_ANALYSE
static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT][5];
#endif

#if defined(USE_ITERM_RELAX)
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
Expand Down Expand Up @@ -673,7 +673,9 @@ static FAST_RAM_ZERO_INIT float stickMovement[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float lastRcDeflectionAbs[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float previousError[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float previousMeasurement[XYZ_AXIS_COUNT];
#ifdef USE_GYRO_DATA_ANALYSE
static FAST_RAM_ZERO_INIT float previousNotchCenterFreq[XYZ_AXIS_COUNT][5];
#endif
static FAST_RAM_ZERO_INIT timeUs_t crashDetectedAtUs;

void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) {
Expand Down
2 changes: 0 additions & 2 deletions src/main/flight/pid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -155,10 +155,8 @@ typedef struct pidProfile_s {
uint16_t dterm_ABG_alpha;
uint16_t dterm_ABG_boost;
uint8_t dterm_ABG_half_life;
#ifdef USE_GYRO_DATA_ANALYSE
uint8_t dtermDynNotch;
uint16_t dterm_dyn_notch_q;
#endif
} pidProfile_t;

#ifndef USE_OSD_SLAVE
Expand Down
22 changes: 0 additions & 22 deletions src/main/interface/msp.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1833,37 +1833,20 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
gyroConfigMutable()->gyro_lowpass_hz[ROLL] = sbufReadU16(src);
gyroConfigMutable()->gyro_lowpass_hz[PITCH] = sbufReadU16(src);
gyroConfigMutable()->gyro_lowpass_hz[YAW] = sbufReadU16(src);
#ifdef USE_GYRO_LPF2
gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = sbufReadU16(src);
gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = sbufReadU16(src);
gyroConfigMutable()->gyro_lowpass2_hz[YAW] = sbufReadU16(src);
#else
sbufReadU16(src);
sbufReadU16(src);
sbufReadU16(src);
#endif
gyroConfigMutable()->gyro_lowpass_type = sbufReadU8(src);
#ifdef USE_GYRO_LPF2
gyroConfigMutable()->gyro_lowpass2_type = sbufReadU8(src);
#else
sbufReadU8(src);
#endif
currentPidProfile->dFilter[ROLL].dLpf2 = sbufReadU16(src);
currentPidProfile->dFilter[PITCH].dLpf2 = sbufReadU16(src);
currentPidProfile->dFilter[YAW].dLpf2 = sbufReadU16(src);
//MSP 1.51 removes SmartDTermSmoothing and WitchCraft
//MSP 1.51 adds and refactors dynamic_filter
#ifdef USE_GYRO_DATA_ANALYSE
gyroConfigMutable()->dyn_notch_count = sbufReadU8(src); //dynamic_gyro_notch_count
gyroConfigMutable()->dyn_notch_q = sbufReadU16(src);
gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
gyroConfigMutable()->dyn_notch_max_hz = sbufReadU16(src); //dynamic_gyro_notch_max_hz
#else
sbufReadU8(src);
sbufReadU16(src);
sbufReadU16(src);
sbufReadU16(src);
#endif
//end 1.51 add/refactor dynamic_filter
//MSP 1.51
gyroConfigMutable()->gyro_ABG_alpha = sbufReadU16(src);
Expand All @@ -1875,13 +1858,8 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
currentPidProfile->dterm_ABG_half_life = sbufReadU8(src);
//end MSP 1.51
//MSP 1.51 dynamic dTerm notch
#ifdef USE_GYRO_DATA_ANALYSE
currentPidProfile->dtermDynNotch = sbufReadU8(src); //dterm_dyn_notch_enable
currentPidProfile->dterm_dyn_notch_q = sbufReadU16(src); //dterm_dyn_notch_q
#else
sbufReadU8(src);
sbufReadU16(src);
#endif
//end MSP 1.51 dynamic dTerm notch
}
// reinitialize the gyro filters with the new values
Expand Down
4 changes: 0 additions & 4 deletions src/main/interface/settings.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -394,11 +394,9 @@ static const char *const lookupTableMixerImplType[] = {
"LEGACY", "SMOOTH", "2PASS"
};

#ifdef USE_GYRO_DATA_ANALYSE
static const char *const lookupTableDynNotchAxisType[] = {
"RP", "RPY"
};
#endif

#define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }

Expand Down Expand Up @@ -489,9 +487,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableOsdLogoOnArming),
#endif
LOOKUP_TABLE_ENTRY(lookupTableMixerImplType),
#ifdef USE_GYRO_DATA_ANALYSE
LOOKUP_TABLE_ENTRY(lookupTableDynNotchAxisType),
#endif
};

#undef LOOKUP_TABLE_ENTRY
Expand Down
2 changes: 0 additions & 2 deletions src/main/interface/settings.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -112,9 +112,7 @@ typedef enum {
TABLE_OSD_LOGO_ON_ARMING,
#endif
TABLE_MIXER_IMPL_TYPE,
#ifdef USE_GYRO_DATA_ANALYSE
TABLE_DYN_NOTCH_AXIS_TYPE,
#endif
LOOKUP_TABLE_COUNT
} lookupTableIndex_e;

Expand Down
8 changes: 0 additions & 8 deletions src/main/sensors/gyro.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -238,12 +238,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lowpass_hz[ROLL] = 0,
.gyro_lowpass_hz[PITCH] = 0,
.gyro_lowpass_hz[YAW] = 0,
#ifdef USE_GYRO_LPF2
.gyro_lowpass2_type = FILTER_PT1,
.gyro_lowpass2_hz[ROLL] = 0,
.gyro_lowpass2_hz[PITCH] = 0,
.gyro_lowpass2_hz[YAW] = 0,
#endif
.gyro_high_fsr = false,
.gyro_use_32khz = true,
.gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT,
Expand All @@ -254,13 +252,11 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
.yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO,
.yaw_spin_threshold = 1950,
#ifdef USE_GYRO_DATA_ANALYSE
.dyn_notch_axis = RPY,
.dyn_notch_q = 400,
.dyn_notch_count = 3, // default of 3 is similar to the matrix filter.
.dyn_notch_min_hz = 150,
.dyn_notch_max_hz = 600,
#endif
.imuf_mode = GTBCM_GYRO_ACC_FILTER_F,
.imuf_rate = IMUF_RATE_16K,
.imuf_roll_q = 6000,
Expand Down Expand Up @@ -293,12 +289,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lowpass_hz[ROLL] = 115,
.gyro_lowpass_hz[PITCH] = 115,
.gyro_lowpass_hz[YAW] = 105,
#ifdef USE_GYRO_LPF2
.gyro_lowpass2_type = FILTER_PT1,
.gyro_lowpass2_hz[ROLL] = 0,
.gyro_lowpass2_hz[PITCH] = 0,
.gyro_lowpass2_hz[YAW] = 0,
#endif
.gyro_high_fsr = false,
.gyro_use_32khz = false,
.gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT,
Expand All @@ -314,13 +308,11 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_offset_yaw = 0,
.yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO,
.yaw_spin_threshold = 1950,
#ifdef USE_GYRO_DATA_ANALYSE
.dyn_notch_axis = RPY,
.dyn_notch_q = 350,
.dyn_notch_count = 3, // default of 3 is similar to the matrix filter.
.dyn_notch_min_hz = 150,
.dyn_notch_max_hz = 600,
#endif
.gyro_ABG_alpha = 0,
.gyro_ABG_boost = 275,
.gyro_ABG_half_life = 50,
Expand Down
8 changes: 0 additions & 8 deletions src/main/sensors/gyro.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,12 +113,10 @@ typedef struct smithPredictor_s {
} smithPredictor_t;
#endif // USE_SMITH_PREDICTOR

#ifdef USE_GYRO_DATA_ANALYSE
typedef enum {
RP = 0,
RPY = 1
} dynamicGyroAxisType_e;
#endif

typedef struct gyroConfig_s {
uint8_t gyro_align; // gyro alignment
Expand All @@ -132,9 +130,7 @@ typedef struct gyroConfig_s {
uint8_t gyro_to_use;

uint16_t gyro_lowpass_hz[XYZ_AXIS_COUNT];
#ifdef USE_GYRO_LPF2
uint16_t gyro_lowpass2_hz[XYZ_AXIS_COUNT];
#endif

uint16_t gyro_ABG_alpha;
uint16_t gyro_ABG_boost;
Expand All @@ -149,21 +145,17 @@ typedef struct gyroConfig_s {

// Lowpass primary/secondary
uint8_t gyro_lowpass_type;
#ifdef USE_GYRO_LPF2
uint8_t gyro_lowpass2_type;
#endif

uint8_t yaw_spin_recovery;
int16_t yaw_spin_threshold;

uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
#if defined(USE_GYRO_DATA_ANALYSE)
uint8_t dyn_notch_axis;
uint16_t dyn_notch_q;
uint8_t dyn_notch_count;
uint16_t dyn_notch_min_hz;
uint16_t dyn_notch_max_hz;
#endif
#if defined(USE_GYRO_IMUF9001)
uint16_t imuf_mode;
uint16_t imuf_rate;
Expand Down
Loading