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

Merge pull request #7373 from ctzsnooze/D_CUT 

D_CUT
This commit is contained in:
Michael Keller 2019-01-13 23:11:18 +13:00 committed by GitHub
parent 8e54c6711c 7e3e5649e1
commit c2ccf10675
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 94 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/build/debug.c
View File

@ -80,4 +80,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"RX_SPEKTRUM_SPI",
"DSHOT_TELEMETRY",
"RPM_FILTER",
"D_CUT",
};

1
src/main/build/debug.h
View File

@ -98,6 +98,7 @@ typedef enum {
DEBUG_RX_SPEKTRUM_SPI,
DEBUG_RPM_TELEMETRY,
DEBUG_RPM_FILTER,
DEBUG_D_CUT,
DEBUG_COUNT
} debugType_e;

13
src/main/cms/cms_menu_imu.c
View File

@ -548,6 +548,9 @@ static uint16_t cmsx_dterm_lowpass_hz;
static uint16_t cmsx_dterm_lowpass2_hz;
static uint16_t cmsx_dterm_notch_hz;
static uint16_t cmsx_dterm_notch_cutoff;
#ifdef USE_D_CUT
static uint8_t cmsx_dterm_cut_percent;
#endif
static uint16_t cmsx_yaw_lowpass_hz;
static long cmsx_FilterPerProfileRead(void)
@ -558,6 +561,9 @@ static long cmsx_FilterPerProfileRead(void)
cmsx_dterm_lowpass2_hz = pidProfile->dterm_lowpass2_hz;
cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz;
cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff;
#ifdef USE_D_CUT
cmsx_dterm_cut_percent = pidProfile->dterm_cut_percent;
#endif
cmsx_yaw_lowpass_hz = pidProfile->yaw_lowpass_hz;
return 0;
@ -573,6 +579,9 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self)
pidProfile->dterm_lowpass2_hz = cmsx_dterm_lowpass2_hz;
pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz;
pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
#ifdef USE_D_CUT
pidProfile->dterm_cut_percent = cmsx_dterm_cut_percent;
#endif
pidProfile->yaw_lowpass_hz = cmsx_yaw_lowpass_hz;
return 0;
@ -586,8 +595,10 @@ static OSD_Entry cmsx_menuFilterPerProfileEntries[] =
{ "DTERM LPF2", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz, 0, 500, 1 }, 0 },
{ "DTERM NF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz, 0, 500, 1 }, 0 },
{ "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff, 0, 500, 1 }, 0 },
#ifdef USE_D_CUT
{ "DTERM CUT", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_dterm_cut_percent, 0, 100, 1 }, 0 },
#endif
{ "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lowpass_hz, 0, 500, 1 }, 0 },
{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }
};

69
src/main/flight/pid.c
View File

@ -90,6 +90,9 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
#else
#define PID_PROCESS_DENOM_DEFAULT 2
#endif
#if defined(USE_D_CUT)
#define D_CUT_GAIN_FACTOR 0.00005f
#endif
#ifdef USE_RUNAWAY_TAKEOFF
PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
@ -115,14 +118,14 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 7);
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 8);
void resetPidProfile(pidProfile_t *pidProfile)
{
RESET_CONFIG(pidProfile_t, pidProfile,
.pid = {
[PID_ROLL] = { 46, 65, 30, 60 },
[PID_PITCH] = { 50, 75, 32, 60 },
[PID_ROLL] = { 46, 65, 40, 60 },
[PID_PITCH] = { 50, 75, 44, 60 },
[PID_YAW] = { 45, 100, 0, 100 },
[PID_LEVEL] = { 50, 50, 75, 0 },
[PID_MAG] = { 40, 0, 0, 0 },
@ -183,6 +186,10 @@ void resetPidProfile(pidProfile_t *pidProfile)
.use_integrated_yaw = false,
.integrated_yaw_relax = 200,
.thrustLinearization = 0,
.dterm_cut_percent = 65,
.dterm_cut_gain = 15,
.dterm_cut_range_hz = 40,
.dterm_cut_lowpass_hz = 7,
);
#ifdef USE_DYN_LPF
pidProfile->dterm_lowpass_hz = 150;
@ -238,12 +245,14 @@ static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn;
static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
#if defined(USE_ITERM_RELAX)
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT uint8_t itermRelax;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxType;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
#endif
#if defined(USE_ABSOLUTE_CONTROL)
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT float axisError[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float acGain;
@ -253,6 +262,12 @@ static FAST_RAM_ZERO_INIT float acCutoff;
static FAST_RAM_ZERO_INIT pt1Filter_t acLpf[XYZ_AXIS_COUNT];
#endif
#if defined(USE_D_CUT)
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermCutRangeApplyFn;
static FAST_RAM_ZERO_INIT biquadFilter_t dtermCutRange[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermCutLowpassApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t dtermCutLowpass[XYZ_AXIS_COUNT];
#endif
#ifdef USE_RC_SMOOTHING_FILTER
static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
@ -373,6 +388,19 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
#endif
#if defined(USE_D_CUT)
if (pidProfile->dterm_cut_percent == 0) {
dtermCutRangeApplyFn = nullFilterApply;
dtermCutLowpassApplyFn = nullFilterApply;
} else {
dtermCutRangeApplyFn = (filterApplyFnPtr)biquadFilterApply;
dtermCutLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&dtermCutRange[axis], pidProfile->dterm_cut_range_hz, targetPidLooptime);
pt1FilterInit(&dtermCutLowpass[axis], pt1FilterGain(pidProfile->dterm_cut_lowpass_hz, dT));
}
}
#endif
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
}
@ -495,6 +523,15 @@ static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
#endif
#ifdef USE_D_CUT
static FAST_RAM_ZERO_INIT float dtermCutPercent;
static FAST_RAM_ZERO_INIT float dtermCutPercentInv;
static FAST_RAM_ZERO_INIT float dtermCutGain;
static FAST_RAM_ZERO_INIT float dtermCutRangeHz;
static FAST_RAM_ZERO_INIT float dtermCutLowpassHz;
#endif
static FAST_RAM_ZERO_INIT float dtermCutFactor;
void pidInitConfig(const pidProfile_t *pidProfile)
{
if (pidProfile->feedForwardTransition == 0) {
@ -613,6 +650,15 @@ void pidInitConfig(const pidProfile_t *pidProfile)
thrustLinearizationB = (1.0f - thrustLinearization) / (2.0f * thrustLinearization);
}
#endif
#if defined(USE_D_CUT)
dtermCutPercent = pidProfile->dterm_cut_percent / 100.0f;
dtermCutPercentInv = 1.0f - dtermCutPercent;
dtermCutRangeHz = pidProfile->dterm_cut_range_hz;
dtermCutLowpassHz = pidProfile->dterm_cut_lowpass_hz;
dtermCutGain = pidProfile->dterm_cut_gain * dtermCutPercent * D_CUT_GAIN_FACTOR / dtermCutLowpassHz;
// lowpass included inversely in gain since stronger lowpass decreases peak effect
#endif
dtermCutFactor = 1.0f;
}
void pidInit(const pidProfile_t *pidProfile)
@ -1240,8 +1286,23 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
if (cmpTimeUs(currentTimeUs, levelModeStartTimeUs) > CRASH_RECOVERY_DETECTION_DELAY_US) {
detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
}
#if defined(USE_D_CUT)
if (dtermCutPercent){
dtermCutFactor = dtermCutRangeApplyFn((filter_t *) &dtermCutRange[axis], delta);
dtermCutFactor = fabsf(dtermCutFactor) * dtermCutGain;
dtermCutFactor = dtermCutLowpassApplyFn((filter_t *) &dtermCutLowpass[axis], dtermCutFactor);
dtermCutFactor = MIN(dtermCutFactor, 1.0f);
dtermCutFactor = dtermCutPercentInv + (dtermCutFactor * dtermCutPercent);
}
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_D_CUT, 2, lrintf(pidCoefficient[axis].Kd * dtermCutFactor * 10.0f / DTERM_SCALE));
} else if (axis == FD_PITCH) {
DEBUG_SET(DEBUG_D_CUT, 3, lrintf(pidCoefficient[axis].Kd * dtermCutFactor * 10.0f / DTERM_SCALE));
}
#endif
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor * dtermCutFactor;
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor;
} else {
pidData[axis].D = 0;
}

4
src/main/flight/pid.h
View File

@ -160,6 +160,10 @@ typedef struct pidProfile_s {
uint8_t use_integrated_yaw; // Selects whether the yaw pidsum should integrated
uint8_t integrated_yaw_relax; // Specifies how much integrated yaw should be reduced to offset the drag based yaw component
uint8_t thrustLinearization; // Compensation factor for pid linearization
uint8_t dterm_cut_percent; // Amount to cut D by with no gyro activity, zero disables, 20 means cut 20%, 50 means cut 50%
uint8_t dterm_cut_gain; // Gain factor for amount of gyro activity required to remove the dterm cut
uint8_t dterm_cut_range_hz; // Biquad to prevent high frequency gyro noise from removing the dterm cut
uint8_t dterm_cut_lowpass_hz; // First order lowpass to delay and smooth dterm cut factor
} pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);

7
src/main/interface/settings.c
View File

@ -962,6 +962,13 @@ const clivalue_t valueTable[] = {
{ "integrated_yaw_relax", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, integrated_yaw_relax) },
#endif
#ifdef USE_D_CUT
{ "dterm_cut_percent", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_percent) },
{ "dterm_cut_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_gain) },
{ "dterm_cut_range_hz", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_range_hz) },
{ "dterm_cut_lowpass_hz", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_lowpass_hz) },
#endif
#ifdef USE_LAUNCH_CONTROL
{ "launch_control_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LAUNCH_CONTROL_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlMode) },
{ "launch_trigger_allow_reset", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlAllowTriggerReset) },

1
src/main/target/LUX_RACE/target.h
View File

@ -51,6 +51,7 @@
//#undef USE_BOARD_INFO
#undef USE_EXTENDED_CMS_MENUS
#undef USE_D_CUT
//#undef USE_RTC_TIME
#undef USE_RX_MSP
//#undef USE_ESC_SENSOR_INFO

1
src/main/target/OMNIBUS/target.h
View File

@ -31,6 +31,7 @@
#undef USE_RC_SMOOTHING_FILTER
#undef USE_DYN_LPF
#undef USE_D_CUT
#undef USE_ITERM_RELAX
#undef USE_RC_SMOOTHING_FILTER

1
src/main/target/SPRACINGF3NEO/target.h
View File

@ -37,6 +37,7 @@
#undef USE_ITERM_RELAX
#undef USE_RC_SMOOTHING_FILTER
#undef USE_THRUST_LINEARIZATION
#undef USE_D_CUT
#undef USE_HUFFMAN
#undef USE_PINIO

1
src/main/target/common_pre.h
View File

@ -202,6 +202,7 @@
#define USE_DYN_LPF
#define USE_INTEGRATED_YAW_CONTROL
#define USE_THRUST_LINEARIZATION
#define USE_D_CUT
#ifdef USE_SERIALRX_SPEKTRUM
#define USE_SPEKTRUM_BIND