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

Merge pull request #10727 from ctzsnooze/PT2-PT3-options-for-filters

This commit is contained in:
Michael Keller 2021-05-25 01:53:53 +12:00 committed by GitHub
parent 6578cac89f 3b62b2e5d4
commit b414be320f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
29 changed files with 496 additions and 571 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
make/source.mk
View File

@ -93,7 +93,7 @@ COMMON_SRC = \
flight/gps_rescue.c \ flight/gps_rescue.c \
flight/gyroanalyse.c \ flight/gyroanalyse.c \
flight/imu.c \ flight/imu.c \
flight/interpolated_setpoint.c \ flight/feedforward.c \
flight/mixer.c \ flight/mixer.c \
flight/mixer_init.c \ flight/mixer_init.c \
flight/mixer_tricopter.c \ flight/mixer_tricopter.c \

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

@ -1384,15 +1384,17 @@ static bool blackboxWriteSysinfo(void)
#ifdef USE_INTEGRATED_YAW_CONTROL #ifdef USE_INTEGRATED_YAW_CONTROL
BLACKBOX_PRINT_HEADER_LINE("use_integrated_yaw", "%d", currentPidProfile->use_integrated_yaw); BLACKBOX_PRINT_HEADER_LINE("use_integrated_yaw", "%d", currentPidProfile->use_integrated_yaw);
#endif #endif
BLACKBOX_PRINT_HEADER_LINE("feedforward_transition", "%d", currentPidProfile->feedForwardTransition); BLACKBOX_PRINT_HEADER_LINE("ff_transition", "%d", currentPidProfile->feedforwardTransition);
BLACKBOX_PRINT_HEADER_LINE("feedforward_weight", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].F, BLACKBOX_PRINT_HEADER_LINE("ff_weight", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].F,
currentPidProfile->pid[PID_PITCH].F, currentPidProfile->pid[PID_PITCH].F,
currentPidProfile->pid[PID_YAW].F); currentPidProfile->pid[PID_YAW].F);
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
BLACKBOX_PRINT_HEADER_LINE("ff_interpolate_sp", "%d", currentPidProfile->ff_interpolate_sp); BLACKBOX_PRINT_HEADER_LINE("ff_averaging", "%d", currentPidProfile->feedforward_averaging);
BLACKBOX_PRINT_HEADER_LINE("ff_max_rate_limit", "%d", currentPidProfile->ff_max_rate_limit); BLACKBOX_PRINT_HEADER_LINE("ff_max_rate_limit", "%d", currentPidProfile->feedforward_max_rate_limit);
BLACKBOX_PRINT_HEADER_LINE("ff_smooth_factor", "%d", currentPidProfile->feedforward_smooth_factor);
BLACKBOX_PRINT_HEADER_LINE("ff_jitter_factor", "%d", currentPidProfile->feedforward_jitter_factor);
BLACKBOX_PRINT_HEADER_LINE("ff_boost", "%d", currentPidProfile->feedforward_boost);
#endif #endif
BLACKBOX_PRINT_HEADER_LINE("ff_boost", "%d", currentPidProfile->ff_boost);
BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit);
@ -1443,9 +1445,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware); BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware);
#endif #endif
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm); BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation", "%d", rxConfig()->rcInterpolation);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval", "%d", rxConfig()->rcInterpolationInterval);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_channels", "%d", rxConfig()->rcInterpolationChannels);
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold); BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold);
BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider); BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider);
BLACKBOX_PRINT_HEADER_LINE("use_unsynced_pwm", "%d", motorConfig()->dev.useUnsyncedPwm); BLACKBOX_PRINT_HEADER_LINE("use_unsynced_pwm", "%d", motorConfig()->dev.useUnsyncedPwm);
@ -1456,13 +1455,16 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures); BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_type", "%d", rxConfig()->rc_smoothing_type); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_mode", "%d", rxConfig()->rc_smoothing_mode);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_feedforward_hz", "%d", rcSmoothingData->ffCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_setpoint_hz", "%d", rcSmoothingData->setpointCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor_setpoint", "%d", rxConfig()->rc_smoothing_auto_factor_rpy);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_throttle_hz", "%d", rcSmoothingData->throttleCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor_throttle", "%d", rxConfig()->rc_smoothing_auto_factor_throttle);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_debug_axis", "%d", rcSmoothingData->debugAxis); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_debug_axis", "%d", rcSmoothingData->debugAxis);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rcSmoothingData->inputCutoffSetting, BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs_ff_sp_thr", "%d,%d,%d", rcSmoothingData->feedforwardCutoffFrequency,
rcSmoothingData->derivativeCutoffSetting); rcSmoothingData->setpointCutoffFrequency,
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor", "%d", rcSmoothingData->autoSmoothnessFactor); rcSmoothingData->throttleCutoffFrequency);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingData->inputCutoffFrequency,
rcSmoothingData->derivativeCutoffFrequency);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingData->averageFrameTimeUs); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingData->averageFrameTimeUs);
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
BLACKBOX_PRINT_HEADER_LINE("rates_type", "%d", currentControlRateProfile->rates_type); BLACKBOX_PRINT_HEADER_LINE("rates_type", "%d", currentControlRateProfile->rates_type);

18
src/main/cli/cli.c
View File

@ -4957,7 +4957,7 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline)
UNUSED(cmdline); UNUSED(cmdline);
rcSmoothingFilter_t *rcSmoothingData = getRcSmoothingData(); rcSmoothingFilter_t *rcSmoothingData = getRcSmoothingData();
cliPrint("# RC Smoothing Type: "); cliPrint("# RC Smoothing Type: ");
if (rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_FILTER) { if (rxConfig()->rc_smoothing_mode == ON) {
cliPrintLine("FILTER"); cliPrintLine("FILTER");
if (rcSmoothingAutoCalculate()) { if (rcSmoothingAutoCalculate()) {
const uint16_t avgRxFrameUs = rcSmoothingData->averageFrameTimeUs; const uint16_t avgRxFrameUs = rcSmoothingData->averageFrameTimeUs;
@ -4968,20 +4968,26 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline)
cliPrintLinef("%d.%03dms", avgRxFrameUs / 1000, avgRxFrameUs % 1000); cliPrintLinef("%d.%03dms", avgRxFrameUs / 1000, avgRxFrameUs % 1000);
} }
} }
cliPrintf("# Active input cutoff: %dhz ", rcSmoothingData->inputCutoffFrequency); cliPrintf("# Active setpoint cutoff: %dhz ", rcSmoothingData->setpointCutoffFrequency);
if (rcSmoothingData->inputCutoffSetting == 0) { if (rcSmoothingData->setpointCutoffSetting == 0) {
cliPrintLine("(auto)"); cliPrintLine("(auto)");
} else { } else {
cliPrintLine("(manual)"); cliPrintLine("(manual)");
} }
cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingData->derivativeCutoffFrequency); cliPrintf("# Active FF cutoff: %dhz (", rcSmoothingData->feedforwardCutoffFrequency);
if (rcSmoothingData->derivativeCutoffSetting == 0) { if (rcSmoothingData->ffCutoffSetting == 0) {
cliPrintLine("auto)");
} else {
cliPrintLine("manual)");
}
cliPrintf("# Active throttle cutoff: %dhz (", rcSmoothingData->throttleCutoffFrequency);
if (rcSmoothingData->ffCutoffSetting == 0) {
cliPrintLine("auto)"); cliPrintLine("auto)");
} else { } else {
cliPrintLine("manual)"); cliPrintLine("manual)");
} }
} else { } else {
cliPrintLine("INTERPOLATION"); cliPrintLine("OFF");
} }
} }
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER

53
src/main/cli/settings.c
View File

@ -291,22 +291,18 @@ static const char * const lookupTablePwmProtocol[] = {
"DISABLED" "DISABLED"
}; };
static const char * const lookupTableRcInterpolation[] = {
"OFF", "PRESET", "AUTO", "MANUAL"
};
static const char * const lookupTableRcInterpolationChannels[] = {
"RP", "RPY", "RPYT", "T", "RPT",
};
static const char * const lookupTableLowpassType[] = { static const char * const lookupTableLowpassType[] = {
"PT1", "PT1",
"BIQUAD", "BIQUAD",
"PT2",
"PT3",
}; };
static const char * const lookupTableDtermLowpassType[] = { static const char * const lookupTableDtermLowpassType[] = {
"PT1", "PT1",
"BIQUAD", "BIQUAD",
"PT2",
"PT3",
}; };
static const char * const lookupTableAntiGravityMode[] = { static const char * const lookupTableAntiGravityMode[] = {
@ -406,9 +402,6 @@ static const char * const lookupTableAcroTrainerDebug[] = {
#endif // USE_ACRO_TRAINER #endif // USE_ACRO_TRAINER
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
static const char * const lookupTableRcSmoothingType[] = {
"INTERPOLATION", "FILTER"
};
static const char * const lookupTableRcSmoothingDebug[] = { static const char * const lookupTableRcSmoothingDebug[] = {
"ROLL", "PITCH", "YAW", "THROTTLE" "ROLL", "PITCH", "YAW", "THROTTLE"
}; };
@ -479,8 +472,8 @@ static const char * const lookupTableOffOnAuto[] = {
"OFF", "ON", "AUTO" "OFF", "ON", "AUTO"
}; };
const char* const lookupTableInterpolatedSetpoint[] = { const char* const lookupTableFeedforwardAveraging[] = {
"OFF", "ON", "AVERAGED_2", "AVERAGED_3", "AVERAGED_4" "NONE", "2_POINT", "3_POINT", "4_POINT"
}; };
static const char* const lookupTableDshotBitbangedTimer[] = { static const char* const lookupTableDshotBitbangedTimer[] = {
@ -551,8 +544,6 @@ const lookupTableEntry_t lookupTables[] = {
#endif #endif
LOOKUP_TABLE_ENTRY(debugModeNames), LOOKUP_TABLE_ENTRY(debugModeNames),
LOOKUP_TABLE_ENTRY(lookupTablePwmProtocol), LOOKUP_TABLE_ENTRY(lookupTablePwmProtocol),
LOOKUP_TABLE_ENTRY(lookupTableRcInterpolation),
LOOKUP_TABLE_ENTRY(lookupTableRcInterpolationChannels),
LOOKUP_TABLE_ENTRY(lookupTableLowpassType), LOOKUP_TABLE_ENTRY(lookupTableLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableDtermLowpassType), LOOKUP_TABLE_ENTRY(lookupTableDtermLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableAntiGravityMode), LOOKUP_TABLE_ENTRY(lookupTableAntiGravityMode),
@ -597,7 +588,6 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableAcroTrainerDebug), LOOKUP_TABLE_ENTRY(lookupTableAcroTrainerDebug),
#endif // USE_ACRO_TRAINER #endif // USE_ACRO_TRAINER
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingType),
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDebug), LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDebug),
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_VTX_COMMON #ifdef USE_VTX_COMMON
@ -622,7 +612,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTablePositionAltSource), LOOKUP_TABLE_ENTRY(lookupTablePositionAltSource),
LOOKUP_TABLE_ENTRY(lookupTableOffOnAuto), LOOKUP_TABLE_ENTRY(lookupTableOffOnAuto),
LOOKUP_TABLE_ENTRY(lookupTableInterpolatedSetpoint), LOOKUP_TABLE_ENTRY(lookupTableFeedforwardAveraging),
LOOKUP_TABLE_ENTRY(lookupTableDshotBitbangedTimer), LOOKUP_TABLE_ENTRY(lookupTableDshotBitbangedTimer),
LOOKUP_TABLE_ENTRY(lookupTableOsdDisplayPortDevice), LOOKUP_TABLE_ENTRY(lookupTableOsdDisplayPortDevice),
@ -733,16 +723,15 @@ const clivalue_t valueTable[] = {
{ "rssi_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -100, 100 }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_offset) }, { "rssi_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -100, 100 }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_offset) },
{ "rssi_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_invert) }, { "rssi_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_invert) },
{ "rssi_src_frame_lpf_period", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_src_frame_lpf_period) }, { "rssi_src_frame_lpf_period", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_src_frame_lpf_period) },
{ "rc_interp", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_INTERPOLATION }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolation) },
{ "rc_interp_ch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_INTERPOLATION_CHANNELS }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolationChannels) },
{ "rc_interp_int", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 50 }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolationInterval) },
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
{ "rc_smoothing_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_TYPE }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_type) }, { "rc_smoothing", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_mode) },
{ "rc_smoothing_input_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_input_cutoff) }, { "rc_smoothing_auto_factor", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { RC_SMOOTHING_AUTO_FACTOR_MIN, RC_SMOOTHING_AUTO_FACTOR_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_auto_factor_rpy) },
{ "rc_smoothing_derivative_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_derivative_cutoff) }, { "rc_smoothing_auto_factor_throttle", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { RC_SMOOTHING_AUTO_FACTOR_MIN, RC_SMOOTHING_AUTO_FACTOR_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_auto_factor_throttle) },
{ "rc_smoothing_setpoint_cutoff", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_setpoint_cutoff) },
{ "rc_smoothing_feedforward_cutoff",VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_feedforward_cutoff) },
{ "rc_smoothing_throttle_cutoff", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_throttle_cutoff) },
{ "rc_smoothing_debug_axis", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DEBUG }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_debug_axis) }, { "rc_smoothing_debug_axis", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DEBUG }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_debug_axis) },
{ "rc_smoothing_auto_smoothness",VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { RC_SMOOTHING_AUTO_FACTOR_MIN, RC_SMOOTHING_AUTO_FACTOR_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_auto_factor) },
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
{ "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 90 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) }, { "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 90 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) },
@ -1062,7 +1051,7 @@ const clivalue_t valueTable[] = {
{ "anti_gravity_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ANTI_GRAVITY_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, antiGravityMode) }, { "anti_gravity_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ANTI_GRAVITY_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, antiGravityMode) },
{ "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) }, { "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) },
{ "anti_gravity_gain", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) }, { "anti_gravity_gain", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) },
{ "feedforward_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedForwardTransition) }, { "feedforward_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforwardTransition) },
{ "acc_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) }, { "acc_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) },
{ "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) }, { "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) },
{ "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) }, { "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) },
@ -1160,13 +1149,13 @@ const clivalue_t valueTable[] = {
{ "transient_throttle_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, transient_throttle_limit) }, { "transient_throttle_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, transient_throttle_limit) },
#endif #endif
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
{ "ff_interpolate_sp", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_INTERPOLATED_SP}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_sp) }, { "feedforward_averaging", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FEEDFORWARD_AVERAGING }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_averaging) },
{ "ff_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_max_rate_limit) }, { "feedforward_smooth_factor", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_smooth_factor) },
{ "ff_smooth_factor", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_smooth_factor) }, { "feedforward_jitter_reduction", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_jitter_factor) },
{ "ff_jitter_reduction", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_jitter_factor) }, { "feedforward_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_max_rate_limit) },
#endif #endif
{ "ff_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) }, { "feedforward_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_boost) },
#ifdef USE_DYN_IDLE #ifdef USE_DYN_IDLE
{ "dyn_idle_min_rpm", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_idle_min_rpm) }, { "dyn_idle_min_rpm", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_idle_min_rpm) },
@ -1185,7 +1174,7 @@ const clivalue_t valueTable[] = {
{ "simplified_pd_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, simplified_pd_ratio) }, { "simplified_pd_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, simplified_pd_ratio) },
{ "simplified_pd_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, simplified_pd_gain) }, { "simplified_pd_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, simplified_pd_gain) },
{ "simplified_dmin_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, simplified_dmin_ratio) }, { "simplified_dmin_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, simplified_dmin_ratio) },
{ "simplified_ff_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, simplified_ff_gain) }, { "simplified_feedforward_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, simplified_feedforward_gain) },
{ "simplified_dterm_filter", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, simplified_dterm_filter) }, { "simplified_dterm_filter", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, simplified_dterm_filter) },
{ "simplified_dterm_filter_multiplier", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, simplified_dterm_filter_multiplier) }, { "simplified_dterm_filter_multiplier", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, simplified_dterm_filter_multiplier) },

7
src/main/cli/settings.h
View File

@ -64,8 +64,6 @@ typedef enum {
#endif #endif
TABLE_DEBUG, TABLE_DEBUG,
TABLE_MOTOR_PWM_PROTOCOL, TABLE_MOTOR_PWM_PROTOCOL,
TABLE_RC_INTERPOLATION,
TABLE_RC_INTERPOLATION_CHANNELS,
TABLE_LOWPASS_TYPE, TABLE_LOWPASS_TYPE,
TABLE_DTERM_LOWPASS_TYPE, TABLE_DTERM_LOWPASS_TYPE,
TABLE_ANTI_GRAVITY_MODE, TABLE_ANTI_GRAVITY_MODE,
@ -110,7 +108,6 @@ typedef enum {
TABLE_ACRO_TRAINER_DEBUG, TABLE_ACRO_TRAINER_DEBUG,
#endif // USE_ACRO_TRAINER #endif // USE_ACRO_TRAINER
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
TABLE_RC_SMOOTHING_TYPE,
TABLE_RC_SMOOTHING_DEBUG, TABLE_RC_SMOOTHING_DEBUG,
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_VTX_COMMON #ifdef USE_VTX_COMMON
@ -133,7 +130,7 @@ typedef enum {
TABLE_GYRO_FILTER_DEBUG, TABLE_GYRO_FILTER_DEBUG,
TABLE_POSITION_ALT_SOURCE, TABLE_POSITION_ALT_SOURCE,
TABLE_OFF_ON_AUTO, TABLE_OFF_ON_AUTO,
TABLE_INTERPOLATED_SP, TABLE_FEEDFORWARD_AVERAGING,
TABLE_DSHOT_BITBANGED_TIMER, TABLE_DSHOT_BITBANGED_TIMER,
TABLE_OSD_DISPLAYPORT_DEVICE, TABLE_OSD_DISPLAYPORT_DEVICE,
#ifdef USE_OSD #ifdef USE_OSD
@ -260,7 +257,7 @@ extern const char * const lookupTableItermRelaxType[];
extern const char * const lookupTableOsdDisplayPortDevice[]; extern const char * const lookupTableOsdDisplayPortDevice[];
extern const char * const lookupTableInterpolatedSetpoint[]; extern const char * const lookupTableFeedforwardAveraging[];
extern const char * const lookupTableOffOn[]; extern const char * const lookupTableOffOn[];

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

@ -237,7 +237,7 @@ static uint8_t cmsx_simplified_i_gain;
static uint8_t cmsx_simplified_pd_ratio; static uint8_t cmsx_simplified_pd_ratio;
static uint8_t cmsx_simplified_pd_gain; static uint8_t cmsx_simplified_pd_gain;
static uint8_t cmsx_simplified_dmin_ratio; static uint8_t cmsx_simplified_dmin_ratio;
static uint8_t cmsx_simplified_ff_gain; static uint8_t cmsx_simplified_feedforward_gain;
static uint8_t cmsx_simplified_dterm_filter; static uint8_t cmsx_simplified_dterm_filter;
static uint8_t cmsx_simplified_dterm_filter_multiplier; static uint8_t cmsx_simplified_dterm_filter_multiplier;
@ -257,7 +257,7 @@ static const void *cmsx_simplifiedTuningOnEnter(displayPort_t *pDisp)
cmsx_simplified_pd_ratio = pidProfile->simplified_pd_ratio; cmsx_simplified_pd_ratio = pidProfile->simplified_pd_ratio;
cmsx_simplified_pd_gain = pidProfile->simplified_pd_gain; cmsx_simplified_pd_gain = pidProfile->simplified_pd_gain;
cmsx_simplified_dmin_ratio = pidProfile->simplified_dmin_ratio; cmsx_simplified_dmin_ratio = pidProfile->simplified_dmin_ratio;
cmsx_simplified_ff_gain = pidProfile->simplified_ff_gain; cmsx_simplified_feedforward_gain = pidProfile->simplified_feedforward_gain;
cmsx_simplified_dterm_filter = pidProfile->simplified_dterm_filter; cmsx_simplified_dterm_filter = pidProfile->simplified_dterm_filter;
cmsx_simplified_dterm_filter_multiplier = pidProfile->simplified_dterm_filter_multiplier; cmsx_simplified_dterm_filter_multiplier = pidProfile->simplified_dterm_filter_multiplier;
@ -281,7 +281,7 @@ static const void *cmsx_simplifiedTuningOnExit(displayPort_t *pDisp, const OSD_E
pidProfile->simplified_pd_ratio = cmsx_simplified_pd_ratio; pidProfile->simplified_pd_ratio = cmsx_simplified_pd_ratio;
pidProfile->simplified_pd_gain = cmsx_simplified_pd_gain; pidProfile->simplified_pd_gain = cmsx_simplified_pd_gain;
pidProfile->simplified_dmin_ratio = cmsx_simplified_dmin_ratio; pidProfile->simplified_dmin_ratio = cmsx_simplified_dmin_ratio;
pidProfile->simplified_ff_gain = cmsx_simplified_ff_gain; pidProfile->simplified_feedforward_gain = cmsx_simplified_feedforward_gain;
pidProfile->simplified_dterm_filter = cmsx_simplified_dterm_filter; pidProfile->simplified_dterm_filter = cmsx_simplified_dterm_filter;
pidProfile->simplified_dterm_filter_multiplier = cmsx_simplified_dterm_filter_multiplier; pidProfile->simplified_dterm_filter_multiplier = cmsx_simplified_dterm_filter_multiplier;
@ -305,7 +305,7 @@ static const void *cmsx_applySimplifiedTuning(displayPort_t *pDisp, const void *
pidProfile->simplified_pd_ratio = cmsx_simplified_pd_ratio; pidProfile->simplified_pd_ratio = cmsx_simplified_pd_ratio;
pidProfile->simplified_pd_gain = cmsx_simplified_pd_gain; pidProfile->simplified_pd_gain = cmsx_simplified_pd_gain;
pidProfile->simplified_dmin_ratio = cmsx_simplified_dmin_ratio; pidProfile->simplified_dmin_ratio = cmsx_simplified_dmin_ratio;
pidProfile->simplified_ff_gain = cmsx_simplified_ff_gain; pidProfile->simplified_feedforward_gain = cmsx_simplified_feedforward_gain;
pidProfile->simplified_dterm_filter = cmsx_simplified_dterm_filter; pidProfile->simplified_dterm_filter = cmsx_simplified_dterm_filter;
pidProfile->simplified_dterm_filter_multiplier = cmsx_simplified_dterm_filter_multiplier; pidProfile->simplified_dterm_filter_multiplier = cmsx_simplified_dterm_filter_multiplier;
@ -327,7 +327,7 @@ static const OSD_Entry cmsx_menuSimplifiedTuningEntries[] =
{ "PD RATIO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_pd_ratio, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 }, { "PD RATIO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_pd_ratio, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 },
{ "PD GAIN", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_pd_gain, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 }, { "PD GAIN", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_pd_gain, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 },
{ "DMIN RATIO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_dmin_ratio, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 }, { "DMIN RATIO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_dmin_ratio, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 },
{ "FF GAIN", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_ff_gain, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 }, { "FF GAIN", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_feedforward_gain, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 },
{ "-- SIMPLIFIED FILTER --", OME_Label, NULL, NULL, 0}, { "-- SIMPLIFIED FILTER --", OME_Label, NULL, NULL, 0},
{ "GYRO TUNING", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_simplified_gyro_filter, 1, lookupTableOffOn }, 0 }, { "GYRO TUNING", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_simplified_gyro_filter, 1, lookupTableOffOn }, 0 },
@ -489,8 +489,8 @@ static CMS_Menu cmsx_menuLaunchControl = {
}; };
#endif #endif
static uint8_t cmsx_feedForwardTransition; static uint8_t cmsx_feedforwardTransition;
static uint8_t cmsx_ff_boost; static uint8_t cmsx_feedforward_boost;
static uint8_t cmsx_angleStrength; static uint8_t cmsx_angleStrength;
static uint8_t cmsx_horizonStrength; static uint8_t cmsx_horizonStrength;
static uint8_t cmsx_horizonTransition; static uint8_t cmsx_horizonTransition;
@ -516,10 +516,10 @@ static uint8_t cmsx_iterm_relax_type;
static uint8_t cmsx_iterm_relax_cutoff; static uint8_t cmsx_iterm_relax_cutoff;
#endif #endif
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
static uint8_t cmsx_ff_interpolate_sp; static uint8_t cmsx_feedforward_averaging;
static uint8_t cmsx_ff_smooth_factor; static uint8_t cmsx_feedforward_smooth_factor;
static uint8_t cmsx_ff_jitter_factor; static uint8_t cmsx_feedforward_jitter_factor;
#endif #endif
static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp) static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
@ -530,8 +530,8 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
cmsx_feedForwardTransition = pidProfile->feedForwardTransition; cmsx_feedforwardTransition = pidProfile->feedforwardTransition;
cmsx_ff_boost = pidProfile->ff_boost; cmsx_feedforward_boost = pidProfile->feedforward_boost;
cmsx_angleStrength = pidProfile->pid[PID_LEVEL].P; cmsx_angleStrength = pidProfile->pid[PID_LEVEL].P;
cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I; cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I;
@ -559,10 +559,10 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
cmsx_iterm_relax_cutoff = pidProfile->iterm_relax_cutoff; cmsx_iterm_relax_cutoff = pidProfile->iterm_relax_cutoff;
#endif #endif
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
cmsx_ff_interpolate_sp = pidProfile->ff_interpolate_sp; cmsx_feedforward_averaging = pidProfile->feedforward_averaging;
cmsx_ff_smooth_factor = pidProfile->ff_smooth_factor; cmsx_feedforward_smooth_factor = pidProfile->feedforward_smooth_factor;
cmsx_ff_jitter_factor = pidProfile->ff_jitter_factor; cmsx_feedforward_jitter_factor = pidProfile->feedforward_jitter_factor;
#endif #endif
#ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
@ -577,9 +577,9 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
UNUSED(self); UNUSED(self);
pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
pidProfile->feedForwardTransition = cmsx_feedForwardTransition; pidProfile->feedforwardTransition = cmsx_feedforwardTransition;
pidInitConfig(currentPidProfile); pidInitConfig(currentPidProfile);
pidProfile->ff_boost = cmsx_ff_boost; pidProfile->feedforward_boost = cmsx_feedforward_boost;
pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength; pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength;
pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength; pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength;
@ -607,10 +607,10 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
pidProfile->iterm_relax_cutoff = cmsx_iterm_relax_cutoff; pidProfile->iterm_relax_cutoff = cmsx_iterm_relax_cutoff;
#endif #endif
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
pidProfile->ff_interpolate_sp = cmsx_ff_interpolate_sp; pidProfile->feedforward_averaging = cmsx_feedforward_averaging;
pidProfile->ff_smooth_factor = cmsx_ff_smooth_factor; pidProfile->feedforward_smooth_factor = cmsx_feedforward_smooth_factor;
pidProfile->ff_jitter_factor = cmsx_ff_jitter_factor; pidProfile->feedforward_jitter_factor = cmsx_feedforward_jitter_factor;
#endif #endif
#ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
@ -624,13 +624,13 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
static const OSD_Entry cmsx_menuProfileOtherEntries[] = { static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
{ "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 }, { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 },
{ "FF TRANS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_feedForwardTransition, 0, 100, 1, 10 }, 0 }, { "FF TRANSITION", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_feedforwardTransition, 0, 100, 1, 10 }, 0 },
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
{ "FF MODE", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_ff_interpolate_sp, 4, lookupTableInterpolatedSetpoint}, 0 }, { "FF AVERAGING", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_feedforward_averaging, 4, lookupTableFeedforwardAveraging}, 0 },
{ "FF SMOOTHNESS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_smooth_factor, 0, 75, 1 } , 0 }, { "FF SMOOTHNESS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_feedforward_smooth_factor, 0, 75, 1 } , 0 },
{ "FF JITTER", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_jitter_factor, 0, 20, 1 } , 0 }, { "FF JITTER", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_feedforward_jitter_factor, 0, 20, 1 } , 0 },
#endif #endif
{ "FF BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_boost, 0, 50, 1 } , 0 }, { "FF BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_feedforward_boost, 0, 50, 1 } , 0 },
{ "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 }, { "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 },
{ "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 }, { "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 },
{ "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 }, { "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 },

27
src/main/config/config.c
View File

@ -354,33 +354,6 @@ static void validateAndFixConfig(void)
rxConfigMutable()->rssi_src_frame_errors = false; rxConfigMutable()->rssi_src_frame_errors = false;
} }
if (!rcSmoothingIsEnabled() || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_T) {
for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
pidProfilesMutable(i)->pid[PID_ROLL].F = 0;
pidProfilesMutable(i)->pid[PID_PITCH].F = 0;
}
}
if (!rcSmoothingIsEnabled() ||
(rxConfig()->rcInterpolationChannels != INTERPOLATION_CHANNELS_RPY &&
rxConfig()->rcInterpolationChannels != INTERPOLATION_CHANNELS_RPYT)) {
for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
pidProfilesMutable(i)->pid[PID_YAW].F = 0;
}
}
#if defined(USE_THROTTLE_BOOST)
if (!rcSmoothingIsEnabled() ||
!(rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPYT
|| rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_T
|| rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPT)) {
for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
pidProfilesMutable(i)->throttle_boost = 0;
}
}
#endif
if ( if (
featureIsConfigured(FEATURE_3D) || !featureIsConfigured(FEATURE_GPS) || mixerModeIsFixedWing(mixerConfig()->mixerMode) featureIsConfigured(FEATURE_3D) || !featureIsConfigured(FEATURE_GPS) || mixerModeIsFixedWing(mixerConfig()->mixerMode)
#if !defined(USE_GPS) || !defined(USE_GPS_RESCUE) #if !defined(USE_GPS) || !defined(USE_GPS_RESCUE)

4
src/main/config/simplified_tuning.c
View File

@ -40,7 +40,7 @@ static void calculateNewPidValues(pidProfile_t *pidProfile)
const int dMinDefaults[FLIGHT_DYNAMICS_INDEX_COUNT] = D_MIN_DEFAULT; const int dMinDefaults[FLIGHT_DYNAMICS_INDEX_COUNT] = D_MIN_DEFAULT;
const float masterMultiplier = pidProfile->simplified_master_multiplier / 100.0f; const float masterMultiplier = pidProfile->simplified_master_multiplier / 100.0f;
const float ffGain = pidProfile->simplified_ff_gain / 100.0f; const float feedforwardGain = pidProfile->simplified_feedforward_gain / 100.0f;
const float pdGain = pidProfile->simplified_pd_gain / 100.0f; const float pdGain = pidProfile->simplified_pd_gain / 100.0f;
const float iGain = pidProfile->simplified_i_gain / 100.0f; const float iGain = pidProfile->simplified_i_gain / 100.0f;
const float pdRatio = pidProfile->simplified_pd_ratio / 100.0f; const float pdRatio = pidProfile->simplified_pd_ratio / 100.0f;
@ -57,7 +57,7 @@ static void calculateNewPidValues(pidProfile_t *pidProfile)
} else { } else {
pidProfile->d_min[axis] = constrain(dMinDefaults[axis] * masterMultiplier * pdGain * rpRatio * dminRatio, 0, D_MIN_GAIN_MAX); pidProfile->d_min[axis] = constrain(dMinDefaults[axis] * masterMultiplier * pdGain * rpRatio * dminRatio, 0, D_MIN_GAIN_MAX);
} }
pidProfile->pid[axis].F = constrain(pidDefaults[axis].F * masterMultiplier * ffGain * rpRatio, 0, F_GAIN_MAX); pidProfile->pid[axis].F = constrain(pidDefaults[axis].F * masterMultiplier * feedforwardGain * rpRatio, 0, F_GAIN_MAX);
} }
} }

361
src/main/fc/rc.c
View File

@ -41,7 +41,7 @@
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/interpolated_setpoint.h" #include "flight/feedforward.h"
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#include "flight/pid_init.h" #include "flight/pid_init.h"
@ -57,14 +57,12 @@
typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs); typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs);
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
// Setpoint in degrees/sec before RC-Smoothing is applied
static float rawSetpoint[XYZ_AXIS_COUNT];
static float oldRcCommand[XYZ_AXIS_COUNT]; static float oldRcCommand[XYZ_AXIS_COUNT];
static bool isDuplicate[XYZ_AXIS_COUNT]; static bool isDuplicate[XYZ_AXIS_COUNT];
float rcCommandDelta[XYZ_AXIS_COUNT]; float rcCommandDelta[XYZ_AXIS_COUNT];
#endif #endif
static float rawSetpoint[XYZ_AXIS_COUNT];
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
static float throttlePIDAttenuation; static float throttlePIDAttenuation;
static bool reverseMotors = false; static bool reverseMotors = false;
@ -74,7 +72,6 @@ static bool isRxDataNew = false;
static float rcCommandDivider = 500.0f; static float rcCommandDivider = 500.0f;
static float rcCommandYawDivider = 500.0f; static float rcCommandYawDivider = 500.0f;
FAST_DATA_ZERO_INIT uint8_t interpolationChannels;
static FAST_DATA_ZERO_INIT bool newRxDataForFF; static FAST_DATA_ZERO_INIT bool newRxDataForFF;
enum { enum {
@ -93,13 +90,13 @@ enum {
#define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT 20 // Look for samples varying this much from the current detected frame rate to initiate retraining #define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT 20 // Look for samples varying this much from the current detected frame rate to initiate retraining
#define RC_SMOOTHING_RX_RATE_MIN_US 1000 // 1ms #define RC_SMOOTHING_RX_RATE_MIN_US 1000 // 1ms
#define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz #define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz
#define RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_INITIAL_HZ 100 // Initial value for "auto" when interpolated feedforward is enabled #define RC_SMOOTHING_FEEDFORWARD_INITIAL_HZ 100 // The value to use for "auto" when interpolated feedforward is enabled
static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData; static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
bool getShouldUpdateFf() bool getShouldUpdateFeedforward()
// only used in pid.c when interpolated_sp is active to initiate a new FF value // only used in pid.c, when feedforward is enabled, to initiate a new FF value
{ {
const bool updateFf = newRxDataForFF; const bool updateFf = newRxDataForFF;
if (newRxDataForFF == true){ if (newRxDataForFF == true){
@ -129,7 +126,7 @@ float getThrottlePIDAttenuation(void)
return throttlePIDAttenuation; return throttlePIDAttenuation;
} }
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
float getRawSetpoint(int axis) float getRawSetpoint(int axis)
{ {
return rawSetpoint[axis]; return rawSetpoint[axis];
@ -139,7 +136,6 @@ float getRcCommandDelta(int axis)
{ {
return rcCommandDelta[axis]; return rcCommandDelta[axis];
} }
#endif #endif
#define THROTTLE_LOOKUP_LENGTH 12 #define THROTTLE_LOOKUP_LENGTH 12
@ -240,43 +236,7 @@ float applyCurve(int axis, float deflection)
return applyRates(axis, deflection, fabsf(deflection)); return applyRates(axis, deflection, fabsf(deflection));
} }
static void calculateSetpointRate(int axis) static void scaleSetpointToFpvCamAngle(void)
{
float angleRate;
#ifdef USE_GPS_RESCUE
if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
// If GPS Rescue is active then override the setpointRate used in the
// pid controller with the value calculated from the desired heading logic.
angleRate = gpsRescueGetYawRate();
// Treat the stick input as centered to avoid any stick deflection base modifications (like acceleration limit)
rcDeflection[axis] = 0;
rcDeflectionAbs[axis] = 0;
} else
#endif
{
// scale rcCommandf to range [-1.0, 1.0]
float rcCommandf;
if (axis == FD_YAW) {
rcCommandf = rcCommand[axis] / rcCommandYawDivider;
} else {
rcCommandf = rcCommand[axis] / rcCommandDivider;
}
rcDeflection[axis] = rcCommandf;
const float rcCommandfAbs = fabsf(rcCommandf);
rcDeflectionAbs[axis] = rcCommandfAbs;
angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
}
// Rate limit from profile (deg/sec)
setpointRate[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]);
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
}
static void scaleRcCommandToFpvCamAngle(void)
{ {
//recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0; static uint8_t lastFpvCamAngleDegrees = 0;
@ -323,64 +283,6 @@ static void checkForThrottleErrorResetState(uint16_t rxRefreshRate)
} }
} }
static FAST_CODE uint8_t processRcInterpolation(void)
{
static FAST_DATA_ZERO_INIT float rcCommandInterp[4];
static FAST_DATA_ZERO_INIT float rcStepSize[4];
static FAST_DATA_ZERO_INIT int16_t rcInterpolationStepCount;
uint16_t rxRefreshRate;
uint8_t updatedChannel = 0;
if (rxConfig()->rcInterpolation) {
// Set RC refresh rate for sampling and channels to filter
switch (rxConfig()->rcInterpolation) {
case RC_SMOOTHING_AUTO:
rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps
break;
case RC_SMOOTHING_MANUAL:
rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval;
break;
case RC_SMOOTHING_OFF:
case RC_SMOOTHING_DEFAULT:
default:
rxRefreshRate = rxGetRefreshRate();
}
if (isRxDataNew && rxRefreshRate > 0) {
rcInterpolationStepCount = rxRefreshRate / targetPidLooptime;
for (int channel = 0; channel < PRIMARY_CHANNEL_COUNT; channel++) {
if ((1 << channel) & interpolationChannels) {
rcStepSize[channel] = (rcCommand[channel] - rcCommandInterp[channel]) / (float)rcInterpolationStepCount;
}
}
DEBUG_SET(DEBUG_RC_INTERPOLATION, 0, lrintf(rcCommand[0]));
DEBUG_SET(DEBUG_RC_INTERPOLATION, 1, lrintf(currentRxRefreshRate / 1000));
} else {
rcInterpolationStepCount--;
}
// Interpolate steps of rcCommand
if (rcInterpolationStepCount > 0) {
for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
if ((1 << updatedChannel) & interpolationChannels) {
rcCommandInterp[updatedChannel] += rcStepSize[updatedChannel];
rcCommand[updatedChannel] = rcCommandInterp[updatedChannel];
}
}
}
} else {
rcInterpolationStepCount = 0; // reset factor in case of level modes flip flopping
}
DEBUG_SET(DEBUG_RC_INTERPOLATION, 2, rcInterpolationStepCount);
return updatedChannel;
}
void updateRcRefreshRate(timeUs_t currentTimeUs) void updateRcRefreshRate(timeUs_t currentTimeUs)
{ {
static timeUs_t lastRxTimeUs; static timeUs_t lastRxTimeUs;
@ -400,9 +302,8 @@ uint16_t getCurrentRxRefreshRate(void)
} }
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
// Determine a cutoff frequency based on filter type and the calculated // Determine a cutoff frequency based on smoothness factor and calculated average rx frame time
// average rx frame time FAST_CODE_NOINLINE int calcAutoSmoothingCutoff(int avgRxFrameTimeUs, uint8_t autoSmoothnessFactor)
FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, uint8_t autoSmoothnessFactor)
{ {
if (avgRxFrameTimeUs > 0) { if (avgRxFrameTimeUs > 0) {
const float cutoffFactor = 1.5f / (1.0f + (autoSmoothnessFactor / 10.0f)); const float cutoffFactor = 1.5f / (1.0f + (autoSmoothnessFactor / 10.0f));
@ -426,35 +327,44 @@ static FAST_CODE bool rcSmoothingRxRateValid(int currentRxRefreshRate)
FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData) FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
{ {
const float dT = targetPidLooptime * 1e-6f; const float dT = targetPidLooptime * 1e-6f;
uint16_t oldCutoff = smoothingData->inputCutoffFrequency; uint16_t oldCutoff = smoothingData->setpointCutoffFrequency;
if (smoothingData->inputCutoffSetting == 0) { if (smoothingData->setpointCutoffSetting == 0) {
smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactor); smoothingData->setpointCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint);
}
if (smoothingData->throttleCutoffSetting == 0) {
smoothingData->throttleCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle);
} }
// initialize or update the input filter
if ((smoothingData->inputCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) { // initialize or update the Setpoint filter
if ((smoothingData->setpointCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
if ((1 << i) & interpolationChannels) { // only update channels specified by rc_interp_ch if (i < THROTTLE) { // Throttle handled by smoothing rcCommand
if (!smoothingData->filterInitialized) { if (!smoothingData->filterInitialized) {
pt3FilterInit((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT)); pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
} else { } else {
pt3FilterUpdateCutoff((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT)); pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
} }
} } else {
}
}
// update or initialize the derivative filter
oldCutoff = smoothingData->derivativeCutoffFrequency;
if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) {
smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactor);
}
if (!smoothingData->filterInitialized) { if (!smoothingData->filterInitialized) {
pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, smoothingData->debugAxis); pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
} else if (smoothingData->derivativeCutoffFrequency != oldCutoff) { } else {
pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency); pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
}
}
}
}
// update or initialize the FF filter
oldCutoff = smoothingData->feedforwardCutoffFrequency;
if (rcSmoothingData.ffCutoffSetting == 0) {
smoothingData->feedforwardCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint);
}
if (!smoothingData->filterInitialized) {
pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis);
} else if (smoothingData->feedforwardCutoffFrequency != oldCutoff) {
pidUpdateFeedforwardLpf(smoothingData->feedforwardCutoffFrequency);
} }
} }
@ -490,17 +400,16 @@ static FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothing
// examining the rx frame times completely // examining the rx frame times completely
FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void) FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
{ {
// if the input cutoff is 0 (auto) then we need to calculate cutoffs // if any rc smoothing cutoff is 0 (auto) then we need to calculate cutoffs
if ((rcSmoothingData.inputCutoffSetting == 0) || (rcSmoothingData.derivativeCutoffSetting == 0)) { if ((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.ffCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) {
return true; return true;
} }
return false; return false;
} }
static FAST_CODE uint8_t processRcSmoothingFilter(void) static FAST_CODE void processRcSmoothingFilter(void)
{ {
uint8_t updatedChannel = 0; static FAST_DATA_ZERO_INIT float rxDataToSmooth[4];
static FAST_DATA_ZERO_INIT float lastRxData[4];
static FAST_DATA_ZERO_INIT bool initialized; static FAST_DATA_ZERO_INIT bool initialized;
static FAST_DATA_ZERO_INIT timeMs_t validRxFrameTimeMs; static FAST_DATA_ZERO_INIT timeMs_t validRxFrameTimeMs;
static FAST_DATA_ZERO_INIT bool calculateCutoffs; static FAST_DATA_ZERO_INIT bool calculateCutoffs;
@ -510,23 +419,25 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
initialized = true; initialized = true;
rcSmoothingData.filterInitialized = false; rcSmoothingData.filterInitialized = false;
rcSmoothingData.averageFrameTimeUs = 0; rcSmoothingData.averageFrameTimeUs = 0;
rcSmoothingData.autoSmoothnessFactor = rxConfig()->rc_smoothing_auto_factor; rcSmoothingData.autoSmoothnessFactorSetpoint = rxConfig()->rc_smoothing_auto_factor_rpy;
rcSmoothingData.autoSmoothnessFactorThrottle = rxConfig()->rc_smoothing_auto_factor_throttle;
rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis; rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
rcSmoothingData.inputCutoffSetting = rxConfig()->rc_smoothing_input_cutoff; rcSmoothingData.setpointCutoffSetting = rxConfig()->rc_smoothing_setpoint_cutoff;
rcSmoothingData.derivativeCutoffSetting = rxConfig()->rc_smoothing_derivative_cutoff; rcSmoothingData.throttleCutoffSetting = rxConfig()->rc_smoothing_throttle_cutoff;
rcSmoothingData.ffCutoffSetting = rxConfig()->rc_smoothing_feedforward_cutoff;
rcSmoothingResetAccumulation(&rcSmoothingData); rcSmoothingResetAccumulation(&rcSmoothingData);
rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting;
rcSmoothingData.inputCutoffFrequency = rcSmoothingData.inputCutoffSetting; rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting;
if (rcSmoothingData.ffCutoffSetting == 0) {
if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) { // calculate and use an initial derivative cutoff until the RC interval is known
// calculate the initial derivative cutoff used for interpolated feedforward until RC interval is known const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactorSetpoint / 10.0f));
const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactor / 10.0f)); float ffCutoff = RC_SMOOTHING_FEEDFORWARD_INITIAL_HZ * cutoffFactor;
float derivativeCutoff = RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_INITIAL_HZ * cutoffFactor; // PT1 cutoff frequency rcSmoothingData.feedforwardCutoffFrequency = lrintf(ffCutoff);
rcSmoothingData.derivativeCutoffFrequency = lrintf(derivativeCutoff);
} else { } else {
rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting; rcSmoothingData.feedforwardCutoffFrequency = rcSmoothingData.ffCutoffSetting;
} }
if (rxConfig()->rc_smoothing_mode) {
calculateCutoffs = rcSmoothingAutoCalculate(); calculateCutoffs = rcSmoothingAutoCalculate();
// if we don't need to calculate cutoffs dynamically then the filters can be initialized now // if we don't need to calculate cutoffs dynamically then the filters can be initialized now
@ -535,22 +446,15 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
rcSmoothingData.filterInitialized = true; rcSmoothingData.filterInitialized = true;
} }
} }
}
if (isRxDataNew) { if (isRxDataNew) {
// for auto calculated filters we need to examine each rx frame interval
// store the new raw channel values
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
if ((1 << i) & interpolationChannels) {
lastRxData[i] = rcCommand[i];
}
}
// for dynamically calculated filters we need to examine each rx frame interval
if (calculateCutoffs) { if (calculateCutoffs) {
const timeMs_t currentTimeMs = millis(); const timeMs_t currentTimeMs = millis();
int sampleState = 0; int sampleState = 0;
// If the filter cutoffs are set to auto and we have good rx data, then determine the average rx frame rate // If the filter cutoffs in auto mode, and we have good rx data, then determine the average rx frame rate
// and use that to calculate the filter cutoff frequencies // and use that to calculate the filter cutoff frequencies
if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization
if (rxIsReceivingSignal() && rcSmoothingRxRateValid(currentRxRefreshRate)) { if (rxIsReceivingSignal() && rcSmoothingRxRateValid(currentRxRefreshRate)) {
@ -582,9 +486,11 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
// accumlate the sample into the average // accumlate the sample into the average
if (accumulateSample) { if (accumulateSample) {
if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) { if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) {
// the required number of samples were collected so set the filter cutoffs // the required number of samples were collected so set the filter cutoffs, but only if smoothing is active
if (rxConfig()->rc_smoothing_mode) {
rcSmoothingSetFilterCutoffs(&rcSmoothingData); rcSmoothingSetFilterCutoffs(&rcSmoothingData);
rcSmoothingData.filterInitialized = true; rcSmoothingData.filterInitialized = true;
}
validRxFrameTimeMs = 0; validRxFrameTimeMs = 0;
} }
} }
@ -604,35 +510,38 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); // indicates whether guard time is active DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); // indicates whether guard time is active
} }
} }
// Get new values to be smoothed
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
rxDataToSmooth[i] = i == THROTTLE ? rcCommand[i] : rawSetpoint[i];
if (i < THROTTLE) {
DEBUG_SET(DEBUG_RC_INTERPOLATION, i, lrintf(rxDataToSmooth[i]));
} else {
DEBUG_SET(DEBUG_RC_INTERPOLATION, i, ((lrintf(rxDataToSmooth[i])) - 1000));
}
}
} }
if (rcSmoothingData.filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) { if (rcSmoothingData.filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) {
// after training has completed then log the raw rc channel and the calculated // after training has completed then log the raw rc channel and the calculated
// average rx frame rate that was used to calculate the automatic filter cutoffs // average rx frame rate that was used to calculate the automatic filter cutoffs
DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rcSmoothingData.debugAxis]));
DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs); DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs);
} }
// each pid loop continue to apply the last received channel value to the filter // each pid loop, apply the last received channel value to the filter, if initialised - thanks @klutvott
for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) { for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value float *dst = i == THROTTLE ? &rcCommand[i] : &setpointRate[i];
if (rcSmoothingData.filterInitialized) { if (rcSmoothingData.filterInitialized) {
rcCommand[updatedChannel] = pt3FilterApply((pt3Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]); *dst = pt3FilterApply(&rcSmoothingData.filter[i], rxDataToSmooth[i]);
} else { } else {
// If filter isn't initialized yet then use the actual unsmoothed rx channel data // If filter isn't initialized yet, as in smoothing off, use the actual unsmoothed rx channel data
rcCommand[updatedChannel] = lastRxData[updatedChannel]; *dst = rxDataToSmooth[i];
} }
} }
}
return interpolationChannels;
} }
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
FAST_CODE void processRcCommand(void) FAST_CODE void processRcCommand(void)
{ {
uint8_t updatedChannel;
if (isRxDataNew) { if (isRxDataNew) {
newRxDataForFF = true; newRxDataForFF = true;
} }
@ -641,57 +550,55 @@ FAST_CODE void processRcCommand(void)
checkForThrottleErrorResetState(currentRxRefreshRate); checkForThrottleErrorResetState(currentRxRefreshRate);
} }
#ifdef USE_INTERPOLATED_SP
if (isRxDataNew) { if (isRxDataNew) {
for (int i = FD_ROLL; i <= FD_YAW; i++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
isDuplicate[i] = (oldRcCommand[i] == rcCommand[i]);
rcCommandDelta[i] = fabsf(rcCommand[i] - oldRcCommand[i]); isDuplicate[axis] = (oldRcCommand[axis] == rcCommand[axis]);
oldRcCommand[i] = rcCommand[i]; rcCommandDelta[axis] = fabsf(rcCommand[axis] - oldRcCommand[axis]);
oldRcCommand[axis] = rcCommand[axis];
float angleRate;
#ifdef USE_GPS_RESCUE
if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
// If GPS Rescue is active then override the setpointRate used in the
// pid controller with the value calculated from the desired heading logic.
angleRate = gpsRescueGetYawRate();
// Treat the stick input as centered to avoid any stick deflection base modifications (like acceleration limit)
rcDeflection[axis] = 0;
rcDeflectionAbs[axis] = 0;
} else
#endif
{
// scale rcCommandf to range [-1.0, 1.0]
float rcCommandf; float rcCommandf;
if (i == FD_YAW) { if (axis == FD_YAW) {
rcCommandf = rcCommand[i] / rcCommandYawDivider; rcCommandf = rcCommand[axis] / rcCommandYawDivider;
} else { } else {
rcCommandf = rcCommand[i] / rcCommandDivider; rcCommandf = rcCommand[axis] / rcCommandDivider;
} }
rcDeflection[axis] = rcCommandf;
const float rcCommandfAbs = fabsf(rcCommandf); const float rcCommandfAbs = fabsf(rcCommandf);
rawSetpoint[i] = applyRates(i, rcCommandf, rcCommandfAbs); rcDeflectionAbs[axis] = rcCommandfAbs;
}
}
#endif
switch (rxConfig()->rc_smoothing_type) { angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
#ifdef USE_RC_SMOOTHING_FILTER
case RC_SMOOTHING_TYPE_FILTER: }
updatedChannel = processRcSmoothingFilter(); rawSetpoint[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]);
break; DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
#endif // USE_RC_SMOOTHING_FILTER
case RC_SMOOTHING_TYPE_INTERPOLATION:
default:
updatedChannel = processRcInterpolation();
break;
} }
if (isRxDataNew || updatedChannel) { // adjust un-filtered setpoint steps to camera angle (mixing Roll and Yaw)
const uint8_t maxUpdatedAxis = isRxDataNew ? FD_YAW : MIN(updatedChannel, FD_YAW); // throttle channel doesn't require rate calculation
#if defined(SIMULATOR_BUILD)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunsafe-loop-optimizations"
#endif
for (int axis = FD_ROLL; axis <= maxUpdatedAxis; axis++) {
#if defined(SIMULATOR_BUILD)
#pragma GCC diagnostic pop
#endif
calculateSetpointRate(axis);
}
DEBUG_SET(DEBUG_RC_INTERPOLATION, 3, setpointRate[0]);
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) { if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleRcCommandToFpvCamAngle(); scaleSetpointToFpvCamAngle();
} }
} }
#ifdef USE_RC_SMOOTHING_FILTER
processRcSmoothingFilter();
#endif
isRxDataNew = false; isRxDataNew = false;
} }
@ -840,45 +747,13 @@ void initRcProcessing(void)
break; break;
} }
interpolationChannels = 0;
switch (rxConfig()->rcInterpolationChannels) {
case INTERPOLATION_CHANNELS_RPYT:
interpolationChannels |= THROTTLE_FLAG;
FALLTHROUGH;
case INTERPOLATION_CHANNELS_RPY:
interpolationChannels |= YAW_FLAG;
FALLTHROUGH;
case INTERPOLATION_CHANNELS_RP:
interpolationChannels |= ROLL_FLAG | PITCH_FLAG;
break;
case INTERPOLATION_CHANNELS_RPT:
interpolationChannels |= ROLL_FLAG | PITCH_FLAG;
FALLTHROUGH;
case INTERPOLATION_CHANNELS_T:
interpolationChannels |= THROTTLE_FLAG;
break;
}
#ifdef USE_YAW_SPIN_RECOVERY #ifdef USE_YAW_SPIN_RECOVERY
const int maxYawRate = (int)applyRates(FD_YAW, 1.0f, 1.0f); const int maxYawRate = (int)applyRates(FD_YAW, 1.0f, 1.0f);
initYawSpinRecovery(maxYawRate); initYawSpinRecovery(maxYawRate);
#endif #endif
} }
bool rcSmoothingIsEnabled(void) // send rc smoothing details to blackbox
{
return !(
#if defined(USE_RC_SMOOTHING_FILTER)
rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_INTERPOLATION &&
#endif
rxConfig()->rcInterpolation == RC_SMOOTHING_OFF);
}
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
rcSmoothingFilter_t *getRcSmoothingData(void) rcSmoothingFilter_t *getRcSmoothingData(void)
{ {
@ -886,6 +761,6 @@ rcSmoothingFilter_t *getRcSmoothingData(void)
} }
bool rcSmoothingInitializationComplete(void) { bool rcSmoothingInitializationComplete(void) {
return (rxConfig()->rc_smoothing_type != RC_SMOOTHING_TYPE_FILTER) || rcSmoothingData.filterInitialized; return rcSmoothingData.filterInitialized;
} }
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER

11
src/main/fc/rc.h
View File

@ -24,14 +24,6 @@
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
typedef enum {
INTERPOLATION_CHANNELS_RP,
INTERPOLATION_CHANNELS_RPY,
INTERPOLATION_CHANNELS_RPYT,
INTERPOLATION_CHANNELS_T,
INTERPOLATION_CHANNELS_RPT,
} interpolationChannels_e;
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
#define RC_SMOOTHING_AUTO_FACTOR_MIN 0 #define RC_SMOOTHING_AUTO_FACTOR_MIN 0
#define RC_SMOOTHING_AUTO_FACTOR_MAX 250 #define RC_SMOOTHING_AUTO_FACTOR_MAX 250
@ -46,13 +38,12 @@ void updateRcCommands(void);
void resetYawAxis(void); void resetYawAxis(void);
void initRcProcessing(void); void initRcProcessing(void);
bool isMotorsReversed(void); bool isMotorsReversed(void);
bool rcSmoothingIsEnabled(void);
rcSmoothingFilter_t *getRcSmoothingData(void); rcSmoothingFilter_t *getRcSmoothingData(void);
bool rcSmoothingAutoCalculate(void); bool rcSmoothingAutoCalculate(void);
bool rcSmoothingInitializationComplete(void); bool rcSmoothingInitializationComplete(void);
float getRawSetpoint(int axis); float getRawSetpoint(int axis);
float getRcCommandDelta(int axis); float getRcCommandDelta(int axis);
float applyCurve(int axis, float deflection); float applyCurve(int axis, float deflection);
bool getShouldUpdateFf(); bool getShouldUpdateFeedforward();
void updateRcRefreshRate(timeUs_t currentTimeUs); void updateRcRefreshRate(timeUs_t currentTimeUs);
uint16_t getCurrentRxRefreshRate(void); uint16_t getCurrentRxRefreshRate(void);

6
src/main/fc/rc_adjustments.c
View File

@ -417,8 +417,8 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue);
break; break;
case ADJUSTMENT_FEEDFORWARD_TRANSITION: case ADJUSTMENT_FEEDFORWARD_TRANSITION:
newValue = constrain(currentPidProfile->feedForwardTransition + delta, 1, 100); // FIXME magic numbers repeated in cli.c newValue = constrain(currentPidProfile->feedforwardTransition + delta, 1, 100); // FIXME magic numbers repeated in cli.c
currentPidProfile->feedForwardTransition = newValue; currentPidProfile->feedforwardTransition = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
break; break;
default: default:
@ -579,7 +579,7 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus
break; break;
case ADJUSTMENT_FEEDFORWARD_TRANSITION: case ADJUSTMENT_FEEDFORWARD_TRANSITION:
newValue = constrain(value, 1, 100); // FIXME magic numbers repeated in cli.c newValue = constrain(value, 1, 100); // FIXME magic numbers repeated in cli.c
currentPidProfile->feedForwardTransition = newValue; currentPidProfile->feedforwardTransition = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
break; break;
default: default:

26
src/main/fc/rc_controls.h
View File

@ -59,16 +59,9 @@ typedef enum {
} rollPitchStatus_e; } rollPitchStatus_e;
typedef enum { typedef enum {
RC_SMOOTHING_OFF = 0, OFF,
RC_SMOOTHING_DEFAULT, ON
RC_SMOOTHING_AUTO, } rcSmoothingMode_e;
RC_SMOOTHING_MANUAL
} rcSmoothing_t;
typedef enum {
RC_SMOOTHING_TYPE_INTERPOLATION,
RC_SMOOTHING_TYPE_FILTER
} rcSmoothingType_e;
#define ROL_LO (1 << (2 * ROLL)) #define ROL_LO (1 << (2 * ROLL))
#define ROL_CE (3 << (2 * ROLL)) #define ROL_CE (3 << (2 * ROLL))
@ -107,14 +100,17 @@ typedef struct rcSmoothingFilterTraining_s {
typedef struct rcSmoothingFilter_s { typedef struct rcSmoothingFilter_s {
bool filterInitialized; bool filterInitialized;
pt3Filter_t filter[4]; pt3Filter_t filter[4];
uint8_t inputCutoffSetting; uint8_t setpointCutoffSetting;
uint16_t inputCutoffFrequency; uint8_t throttleCutoffSetting;
uint8_t derivativeCutoffSetting; uint16_t setpointCutoffFrequency;
uint16_t derivativeCutoffFrequency; uint16_t throttleCutoffFrequency;
uint8_t ffCutoffSetting;
uint16_t feedforwardCutoffFrequency;
int averageFrameTimeUs; int averageFrameTimeUs;
rcSmoothingFilterTraining_t training; rcSmoothingFilterTraining_t training;
uint8_t debugAxis; uint8_t debugAxis;
uint8_t autoSmoothnessFactor; uint8_t autoSmoothnessFactorSetpoint;
uint8_t autoSmoothnessFactorThrottle;
} rcSmoothingFilter_t; } rcSmoothingFilter_t;
typedef struct rcControlsConfig_s { typedef struct rcControlsConfig_s {

28
src/main/flight/interpolated_setpoint.c → src/main/flight/feedforward.c
View File

@ -21,7 +21,7 @@
#include <math.h> #include <math.h>
#include "platform.h" #include "platform.h"
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
#include "build/debug.h" #include "build/debug.h"
@ -31,7 +31,7 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "interpolated_setpoint.h" #include "feedforward.h"
static float setpointDeltaImpl[XYZ_AXIS_COUNT]; static float setpointDeltaImpl[XYZ_AXIS_COUNT];
static float setpointDelta[XYZ_AXIS_COUNT]; static float setpointDelta[XYZ_AXIS_COUNT];
@ -54,9 +54,9 @@ static uint8_t averagingCount;
static float ffMaxRateLimit[XYZ_AXIS_COUNT]; static float ffMaxRateLimit[XYZ_AXIS_COUNT];
static float ffMaxRate[XYZ_AXIS_COUNT]; static float ffMaxRate[XYZ_AXIS_COUNT];
void interpolatedSpInit(const pidProfile_t *pidProfile) { void feedforwardInit(const pidProfile_t *pidProfile) {
const float ffMaxRateScale = pidProfile->ff_max_rate_limit * 0.01f; const float ffMaxRateScale = pidProfile->feedforward_max_rate_limit * 0.01f;
averagingCount = pidProfile->ff_interpolate_sp; averagingCount = pidProfile->feedforward_averaging + 1;
for (int i = 0; i < XYZ_AXIS_COUNT; i++) { for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
ffMaxRate[i] = applyCurve(i, 1.0f); ffMaxRate[i] = applyCurve(i, 1.0f);
ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale; ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale;
@ -64,7 +64,7 @@ void interpolatedSpInit(const pidProfile_t *pidProfile) {
} }
} }
FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type) { FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging) {
if (newRcFrame) { if (newRcFrame) {
float rcCommandDelta = getRcCommandDelta(axis); float rcCommandDelta = getRcCommandDelta(axis);
@ -99,7 +99,7 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
} else { } else {
// movement! // movement!
if (prevDuplicatePacket[axis] == true) { if (prevDuplicatePacket[axis] == true) {
// don't boost the packet after a duplicate, the FF alone is enough, usually // don't boost the packet after a duplicate, the feedforward alone is enough, usually
// in part because after a duplicate, the raw up-step is large, so the jitter attenuator is less active // in part because after a duplicate, the raw up-step is large, so the jitter attenuator is less active
ffAttenuator = 0.0f; ffAttenuator = 0.0f;
} }
@ -138,26 +138,26 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
} }
if (axis == FD_ROLL) { if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100.0f)); // base FF DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100.0f)); // base feedforward
DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(boostAmount * 100.0f)); // boost amount DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(boostAmount * 100.0f)); // boost amount
// debug 2 is interpolated setpoint, above // debug 2 is interpolated setpoint, above
DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, lrintf(rcCommandDelta * 100.0f)); // rcCommand packet difference DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, lrintf(rcCommandDelta * 100.0f)); // rcCommand packet difference
} }
// add boost to base feed forward // add boost to base feedforward
setpointDeltaImpl[axis] += boostAmount; setpointDeltaImpl[axis] += boostAmount;
// apply averaging // apply averaging
if (type == FF_INTERPOLATE_ON) { if (feedforwardAveraging) {
setpointDelta[axis] = setpointDeltaImpl[axis];
} else {
setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDeltaImpl[axis]); setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDeltaImpl[axis]);
} else {
setpointDelta[axis] = setpointDeltaImpl[axis];
} }
} }
return setpointDelta[axis]; return setpointDelta[axis];
} }
FAST_CODE_NOINLINE float applyFfLimit(int axis, float value, float Kp, float currentPidSetpoint) { FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint) {
switch (axis) { switch (axis) {
case FD_ROLL: case FD_ROLL:
DEBUG_SET(DEBUG_FF_LIMIT, 0, value); DEBUG_SET(DEBUG_FF_LIMIT, 0, value);
@ -182,7 +182,7 @@ FAST_CODE_NOINLINE float applyFfLimit(int axis, float value, float Kp, float cur
return value; return value;
} }
bool shouldApplyFfLimits(int axis) bool shouldApplyFeedforwardLimits(int axis)
{ {
return ffMaxRateLimit[axis] != 0.0f && axis < FD_YAW; return ffMaxRateLimit[axis] != 0.0f && axis < FD_YAW;
} }

8
src/main/flight/interpolated_setpoint.h → src/main/flight/feedforward.h
View File

@ -25,7 +25,7 @@
#include "common/axis.h" #include "common/axis.h"
#include "flight/pid.h" #include "flight/pid.h"
void interpolatedSpInit(const pidProfile_t *pidProfile); void feedforwardInit(const pidProfile_t *pidProfile);
float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type); float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging);
float applyFfLimit(int axis, float value, float Kp, float currentPidSetpoint); float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint);
bool shouldApplyFfLimits(int axis); bool shouldApplyFeedforwardLimits(int axis);

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

@ -48,7 +48,7 @@
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
#include "flight/interpolated_setpoint.h" #include "flight/feedforward.h"
#include "io/gps.h" #include "io/gps.h"
@ -141,7 +141,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.itermWindupPointPercent = 100, .itermWindupPointPercent = 100,
.pidAtMinThrottle = PID_STABILISATION_ON, .pidAtMinThrottle = PID_STABILISATION_ON,
.levelAngleLimit = 55, .levelAngleLimit = 55,
.feedForwardTransition = 0, .feedforwardTransition = 0,
.yawRateAccelLimit = 0, .yawRateAccelLimit = 0,
.rateAccelLimit = 0, .rateAccelLimit = 0,
.itermThrottleThreshold = 250, .itermThrottleThreshold = 250,
@ -202,11 +202,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
.dyn_idle_i_gain = 50, .dyn_idle_i_gain = 50,
.dyn_idle_d_gain = 50, .dyn_idle_d_gain = 50,
.dyn_idle_max_increase = 150, .dyn_idle_max_increase = 150,
.ff_interpolate_sp = FF_INTERPOLATE_ON, .feedforward_averaging = FEEDFORWARD_AVERAGING_OFF,
.ff_max_rate_limit = 100, .feedforward_max_rate_limit = 100,
.ff_smooth_factor = 37, .feedforward_smooth_factor = 37,
.ff_jitter_factor = 7, .feedforward_jitter_factor = 7,
.ff_boost = 15, .feedforward_boost = 15,
.dyn_lpf_curve_expo = 5, .dyn_lpf_curve_expo = 5,
.level_race_mode = false, .level_race_mode = false,
.vbat_sag_compensation = 0, .vbat_sag_compensation = 0,
@ -217,7 +217,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.simplified_pd_ratio = SIMPLIFIED_TUNING_DEFAULT, .simplified_pd_ratio = SIMPLIFIED_TUNING_DEFAULT,
.simplified_pd_gain = SIMPLIFIED_TUNING_DEFAULT, .simplified_pd_gain = SIMPLIFIED_TUNING_DEFAULT,
.simplified_dmin_ratio = SIMPLIFIED_TUNING_DEFAULT, .simplified_dmin_ratio = SIMPLIFIED_TUNING_DEFAULT,
.simplified_ff_gain = SIMPLIFIED_TUNING_DEFAULT, .simplified_feedforward_gain = SIMPLIFIED_TUNING_DEFAULT,
.simplified_dterm_filter = false, .simplified_dterm_filter = false,
.simplified_dterm_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT, .simplified_dterm_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT,
); );
@ -261,7 +261,7 @@ float pidGetFfBoostFactor()
return pidRuntime.ffBoostFactor; return pidRuntime.ffBoostFactor;
} }
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
float pidGetFfSmoothFactor() float pidGetFfSmoothFactor()
{ {
return pidRuntime.ffSmoothFactor; return pidRuntime.ffSmoothFactor;
@ -636,14 +636,14 @@ STATIC_UNIT_TESTED void rotateItermAndAxisError()
} }
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelta) float FAST_CODE applyRcSmoothingFeedforwardFilter(int axis, float pidSetpointDelta)
{ {
float ret = pidSetpointDelta; float ret = pidSetpointDelta;
if (axis == pidRuntime.rcSmoothingDebugAxis) { if (axis == pidRuntime.rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f)); DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
} }
if (pidRuntime.setpointDerivativeLpfInitialized) { if (pidRuntime.feedforwardLpfInitialized) {
ret = pt3FilterApply(&pidRuntime.setpointDerivativePt3[axis], pidSetpointDelta); ret = pt3FilterApply(&pidRuntime.feedforwardPt3[axis], pidSetpointDelta);
if (axis == pidRuntime.rcSmoothingDebugAxis) { if (axis == pidRuntime.rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f)); DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f));
} }
@ -912,9 +912,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
rpmFilterUpdate(); rpmFilterUpdate();
#endif #endif
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
bool newRcFrame = false; bool newRcFrame = false;
if (getShouldUpdateFf()) { if (getShouldUpdateFeedforward()) {
newRcFrame = true; newRcFrame = true;
} }
#endif #endif
@ -1025,22 +1025,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// -----calculate pidSetpointDelta // -----calculate pidSetpointDelta
float pidSetpointDelta = 0; float pidSetpointDelta = 0;
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
if (pidRuntime.ffFromInterpolatedSetpoint) { pidSetpointDelta = feedforwardApply(axis, newRcFrame, pidRuntime.feedforwardAveraging);
pidSetpointDelta = interpolatedSpApply(axis, newRcFrame, pidRuntime.ffFromInterpolatedSetpoint);
} else {
pidSetpointDelta = currentPidSetpoint - pidRuntime.previousPidSetpoint[axis];
}
#else
pidSetpointDelta = currentPidSetpoint - pidRuntime.previousPidSetpoint[axis];
#endif #endif
pidRuntime.previousPidSetpoint[axis] = currentPidSetpoint; pidRuntime.previousPidSetpoint[axis] = currentPidSetpoint;
#ifdef USE_RC_SMOOTHING_FILTER
pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
#endif // USE_RC_SMOOTHING_FILTER
// -----calculate D component // -----calculate D component
// disable D if launch control is active // disable D if launch control is active
if ((pidRuntime.pidCoefficient[axis].Kd > 0) && !launchControlActive) { if ((pidRuntime.pidCoefficient[axis].Kd > 0) && !launchControlActive) {
@ -1106,7 +1095,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// -----calculate feedforward component // -----calculate feedforward component
#ifdef USE_ABSOLUTE_CONTROL #ifdef USE_ABSOLUTE_CONTROL
// include abs control correction in FF // include abs control correction in feedforward
pidSetpointDelta += setpointCorrection - pidRuntime.oldSetpointCorrection[axis]; pidSetpointDelta += setpointCorrection - pidRuntime.oldSetpointCorrection[axis];
pidRuntime.oldSetpointCorrection[axis] = setpointCorrection; pidRuntime.oldSetpointCorrection[axis] = setpointCorrection;
#endif #endif
@ -1114,16 +1103,19 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// Only enable feedforward for rate mode and if launch control is inactive // Only enable feedforward for rate mode and if launch control is inactive
const float feedforwardGain = (flightModeFlags || launchControlActive) ? 0.0f : pidRuntime.pidCoefficient[axis].Kf; const float feedforwardGain = (flightModeFlags || launchControlActive) ? 0.0f : pidRuntime.pidCoefficient[axis].Kf;
if (feedforwardGain > 0) { if (feedforwardGain > 0) {
// no transition if feedForwardTransition == 0 // no transition if feedforwardTransition == 0
float transition = pidRuntime.feedForwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * pidRuntime.feedForwardTransition) : 1; float transition = pidRuntime.feedforwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * pidRuntime.feedforwardTransition) : 1;
float feedForward = feedforwardGain * transition * pidSetpointDelta * pidRuntime.pidFrequency; float feedForward = feedforwardGain * transition * pidSetpointDelta * pidRuntime.pidFrequency;
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
pidData[axis].F = shouldApplyFfLimits(axis) ? pidData[axis].F = shouldApplyFeedforwardLimits(axis) ?
applyFfLimit(axis, feedForward, pidRuntime.pidCoefficient[axis].Kp, currentPidSetpoint) : feedForward; applyFeedforwardLimit(axis, feedForward, pidRuntime.pidCoefficient[axis].Kp, currentPidSetpoint) : feedForward;
#else #else
pidData[axis].F = feedForward; pidData[axis].F = feedForward;
#endif #endif
#ifdef USE_RC_SMOOTHING_FILTER
pidData[axis].F = applyRcSmoothingFeedforwardFilter(axis, pidData[axis].F);
#endif // USE_RC_SMOOTHING_FILTER
} else { } else {
pidData[axis].F = 0; pidData[axis].F = 0;
} }
@ -1239,15 +1231,27 @@ void dynLpfDTermUpdate(float throttle)
} else { } else {
cutoffFreq = fmax(dynThrottle(throttle) * pidRuntime.dynLpfMax, pidRuntime.dynLpfMin); cutoffFreq = fmax(dynThrottle(throttle) * pidRuntime.dynLpfMax, pidRuntime.dynLpfMin);
} }
switch (pidRuntime.dynLpfFilter) {
if (pidRuntime.dynLpfFilter == DYN_LPF_PT1) { case DYN_LPF_PT1:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, pidRuntime.dT)); pt1FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, pidRuntime.dT));
} }
} else if (pidRuntime.dynLpfFilter == DYN_LPF_BIQUAD) { break;
case DYN_LPF_BIQUAD:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterUpdateLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime); biquadFilterUpdateLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
} }
break;
case DYN_LPF_PT2:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt2FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt2Filter, pt2FilterGain(cutoffFreq, pidRuntime.dT));
}
break;
case DYN_LPF_PT3:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt3FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt3Filter, pt3FilterGain(cutoffFreq, pidRuntime.dT));
}
break;
} }
} }
} }

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

@ -121,13 +121,12 @@ typedef enum {
ITERM_RELAX_TYPE_COUNT, ITERM_RELAX_TYPE_COUNT,
} itermRelaxType_e; } itermRelaxType_e;
typedef enum ffInterpolationType_e { typedef enum feedforwardAveraging_e {
FF_INTERPOLATE_OFF, FEEDFORWARD_AVERAGING_OFF,
FF_INTERPOLATE_ON, FEEDFORWARD_AVERAGING_2_POINT,
FF_INTERPOLATE_AVG2, FEEDFORWARD_AVERAGING_3_POINT,
FF_INTERPOLATE_AVG3, FEEDFORWARD_AVERAGING_4_POINT,
FF_INTERPOLATE_AVG4 } feedforwardAveraging_t;
} ffInterpolationType_t;
#define MAX_PROFILE_NAME_LENGTH 8u #define MAX_PROFILE_NAME_LENGTH 8u
@ -162,7 +161,7 @@ typedef struct pidProfile_s {
uint16_t crash_delay; // ms uint16_t crash_delay; // ms
uint8_t crash_recovery_angle; // degrees uint8_t crash_recovery_angle; // degrees
uint8_t crash_recovery_rate; // degree/second uint8_t crash_recovery_rate; // degree/second
uint8_t feedForwardTransition; // Feed forward weight transition uint8_t feedforwardTransition; // Feedforward attenuation around centre sticks
uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase
uint16_t itermLimit; uint16_t itermLimit;
uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz
@ -198,7 +197,7 @@ typedef struct pidProfile_s {
uint8_t motor_output_limit; // Upper limit of the motor output (percent) uint8_t motor_output_limit; // Upper limit of the motor output (percent)
int8_t auto_profile_cell_count; // Cell count for this profile to be used with if auto PID profile switching is used int8_t auto_profile_cell_count; // Cell count for this profile to be used with if auto PID profile switching is used
uint8_t transient_throttle_limit; // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise uint8_t transient_throttle_limit; // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise
uint8_t ff_boost; // amount of high-pass filtered FF to add to FF, 100 means 100% added uint8_t feedforward_boost; // amount of setpoint acceleration to add to feedforward, 10 means 100% added
char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile
uint8_t dyn_idle_min_rpm; // minimum motor speed enforced by the dynamic idle controller uint8_t dyn_idle_min_rpm; // minimum motor speed enforced by the dynamic idle controller
@ -207,10 +206,10 @@ typedef struct pidProfile_s {
uint8_t dyn_idle_d_gain; // D gain for corrections around rapid changes in rpm uint8_t dyn_idle_d_gain; // D gain for corrections around rapid changes in rpm
uint8_t dyn_idle_max_increase; // limit on maximum possible increase in motor idle drive during active control uint8_t dyn_idle_max_increase; // limit on maximum possible increase in motor idle drive during active control
uint8_t ff_interpolate_sp; // Calculate FF from interpolated setpoint uint8_t feedforward_averaging; // Number of packets to average when averaging is on
uint8_t ff_max_rate_limit; // Maximum setpoint rate percentage for FF uint8_t feedforward_max_rate_limit; // Maximum setpoint rate percentage for feedforward
uint8_t ff_smooth_factor; // Amount of smoothing for interpolated FF steps uint8_t feedforward_smooth_factor; // Amount of lowpass type smoothing for feedforward steps
uint8_t ff_jitter_factor; // Number of RC steps below which to attenuate FF uint8_t feedforward_jitter_factor; // Number of RC steps below which to attenuate feedforward
uint8_t dyn_lpf_curve_expo; // set the curve for dynamic dterm lowpass filter uint8_t dyn_lpf_curve_expo; // set the curve for dynamic dterm lowpass filter
uint8_t level_race_mode; // NFE race mode - when true pitch setpoint calcualtion is gyro based in level mode uint8_t level_race_mode; // NFE race mode - when true pitch setpoint calcualtion is gyro based in level mode
uint8_t vbat_sag_compensation; // Reduce motor output by this percentage of the maximum compensation amount uint8_t vbat_sag_compensation; // Reduce motor output by this percentage of the maximum compensation amount
@ -222,7 +221,7 @@ typedef struct pidProfile_s {
uint8_t simplified_pd_ratio; uint8_t simplified_pd_ratio;
uint8_t simplified_pd_gain; uint8_t simplified_pd_gain;
uint8_t simplified_dmin_ratio; uint8_t simplified_dmin_ratio;
uint8_t simplified_ff_gain; uint8_t simplified_feedforward_gain;
uint8_t simplified_dterm_filter; uint8_t simplified_dterm_filter;
uint8_t simplified_dterm_filter_multiplier; uint8_t simplified_dterm_filter_multiplier;
@ -254,6 +253,8 @@ typedef struct pidAxisData_s {
typedef union dtermLowpass_u { typedef union dtermLowpass_u {
pt1Filter_t pt1Filter; pt1Filter_t pt1Filter;
biquadFilter_t biquadFilter; biquadFilter_t biquadFilter;
pt2Filter_t pt2Filter;
pt3Filter_t pt3Filter;
} dtermLowpass_t; } dtermLowpass_t;
typedef struct pidCoefficient_s { typedef struct pidCoefficient_s {
@ -286,7 +287,7 @@ typedef struct pidRuntime_s {
float ffBoostFactor; float ffBoostFactor;
float itermAccelerator; float itermAccelerator;
uint16_t itermAcceleratorGain; uint16_t itermAcceleratorGain;
float feedForwardTransition; float feedforwardTransition;
pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT]; pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
float levelGain; float levelGain;
float horizonGain; float horizonGain;
@ -341,8 +342,8 @@ typedef struct pidRuntime_s {
#endif #endif
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
pt3Filter_t setpointDerivativePt3[XYZ_AXIS_COUNT]; pt3Filter_t feedforwardPt3[XYZ_AXIS_COUNT];
bool setpointDerivativeLpfInitialized; bool feedforwardLpfInitialized;
uint8_t rcSmoothingDebugAxis; uint8_t rcSmoothingDebugAxis;
uint8_t rcSmoothingFilterType; uint8_t rcSmoothingFilterType;
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
@ -383,8 +384,8 @@ typedef struct pidRuntime_s {
float airmodeThrottleOffsetLimit; float airmodeThrottleOffsetLimit;
#endif #endif
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
ffInterpolationType_t ffFromInterpolatedSetpoint; feedforwardAveraging_t feedforwardAveraging;
float ffSmoothFactor; float ffSmoothFactor;
float ffJitterFactor; float ffJitterFactor;
#endif #endif

82
src/main/flight/pid_init.c
View File

@ -36,7 +36,7 @@
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/interpolated_setpoint.h" #include "flight/feedforward.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
@ -110,7 +110,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
} }
#endif #endif
if (dterm_lowpass_hz > 0 && dterm_lowpass_hz < pidFrequencyNyquist) { if (dterm_lowpass_hz > 0) {
switch (pidProfile->dterm_filter_type) { switch (pidProfile->dterm_filter_type) {
case FILTER_PT1: case FILTER_PT1:
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply; pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
@ -119,6 +119,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
} }
break; break;
case FILTER_BIQUAD: case FILTER_BIQUAD:
if (pidProfile->dterm_lowpass_hz < pidFrequencyNyquist) {
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
#else #else
@ -127,6 +128,21 @@ void pidInitFilters(const pidProfile_t *pidProfile)
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime); biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime);
} }
} else {
pidRuntime.dtermLowpassApplyFn = nullFilterApply;
}
break;
case FILTER_PT2:
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt2FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt2FilterInit(&pidRuntime.dtermLowpass[axis].pt2Filter, pt2FilterGain(dterm_lowpass_hz, pidRuntime.dT));
}
break;
case FILTER_PT3:
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt3FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt3FilterInit(&pidRuntime.dtermLowpass[axis].pt3Filter, pt3FilterGain(dterm_lowpass_hz, pidRuntime.dT));
}
break; break;
default: default:
pidRuntime.dtermLowpassApplyFn = nullFilterApply; pidRuntime.dtermLowpassApplyFn = nullFilterApply;
@ -137,9 +153,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
} }
//2nd Dterm Lowpass Filter //2nd Dterm Lowpass Filter
if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) { if (pidProfile->dterm_lowpass2_hz > 0) {
pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
} else {
switch (pidProfile->dterm_filter2_type) { switch (pidProfile->dterm_filter2_type) {
case FILTER_PT1: case FILTER_PT1:
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
@ -148,18 +162,36 @@ void pidInitFilters(const pidProfile_t *pidProfile)
} }
break; break;
case FILTER_BIQUAD: case FILTER_BIQUAD:
if (pidProfile->dterm_lowpass2_hz < pidFrequencyNyquist) {
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply; pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime); biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime);
} }
} else {
pidRuntime.dtermLowpassApplyFn = nullFilterApply;
}
break;
case FILTER_PT2:
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt2FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt2FilterInit(&pidRuntime.dtermLowpass2[axis].pt2Filter, pt2FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT));
}
break;
case FILTER_PT3:
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt3FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt3FilterInit(&pidRuntime.dtermLowpass2[axis].pt3Filter, pt3FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT));
}
break; break;
default: default:
pidRuntime.dtermLowpass2ApplyFn = nullFilterApply; pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
break; break;
} }
} else {
pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
} }
if (pidProfile->yaw_lowpass_hz == 0 || pidProfile->yaw_lowpass_hz > pidFrequencyNyquist) { if (pidProfile->yaw_lowpass_hz == 0) {
pidRuntime.ptermYawLowpassApplyFn = nullFilterApply; pidRuntime.ptermYawLowpassApplyFn = nullFilterApply;
} else { } else {
pidRuntime.ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply; pidRuntime.ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
@ -207,7 +239,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT)); pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT));
pt1FilterInit(&pidRuntime.antiGravitySmoothLpf, pt1FilterGain(ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF, pidRuntime.dT)); pt1FilterInit(&pidRuntime.antiGravitySmoothLpf, pt1FilterGain(ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF, pidRuntime.dT));
pidRuntime.ffBoostFactor = (float)pidProfile->ff_boost / 10.0f; pidRuntime.ffBoostFactor = (float)pidProfile->feedforward_boost / 10.0f;
} }
void pidInit(const pidProfile_t *pidProfile) void pidInit(const pidProfile_t *pidProfile)
@ -221,22 +253,22 @@ void pidInit(const pidProfile_t *pidProfile)
} }
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis) void pidInitFeedforwardLpf(uint16_t filterCutoff, uint8_t debugAxis)
{ {
pidRuntime.rcSmoothingDebugAxis = debugAxis; pidRuntime.rcSmoothingDebugAxis = debugAxis;
if (filterCutoff > 0) { if (filterCutoff > 0) {
pidRuntime.setpointDerivativeLpfInitialized = true; pidRuntime.feedforwardLpfInitialized = true;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt3FilterInit(&pidRuntime.setpointDerivativePt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT)); pt3FilterInit(&pidRuntime.feedforwardPt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
} }
} }
} }
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff) void pidUpdateFeedforwardLpf(uint16_t filterCutoff)
{ {
if (filterCutoff > 0) { if (filterCutoff > 0) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt3FilterUpdateCutoff(&pidRuntime.setpointDerivativePt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT)); pt3FilterUpdateCutoff(&pidRuntime.feedforwardPt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
} }
} }
} }
@ -244,10 +276,10 @@ void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
void pidInitConfig(const pidProfile_t *pidProfile) void pidInitConfig(const pidProfile_t *pidProfile)
{ {
if (pidProfile->feedForwardTransition == 0) { if (pidProfile->feedforwardTransition == 0) {
pidRuntime.feedForwardTransition = 0; pidRuntime.feedforwardTransition = 0;
} else { } else {
pidRuntime.feedForwardTransition = 100.0f / pidProfile->feedForwardTransition; pidRuntime.feedforwardTransition = 100.0f / pidProfile->feedforwardTransition;
} }
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pidRuntime.pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P; pidRuntime.pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
@ -333,6 +365,12 @@ void pidInitConfig(const pidProfile_t *pidProfile)
case FILTER_BIQUAD: case FILTER_BIQUAD:
pidRuntime.dynLpfFilter = DYN_LPF_BIQUAD; pidRuntime.dynLpfFilter = DYN_LPF_BIQUAD;
break; break;
case FILTER_PT2:
pidRuntime.dynLpfFilter = DYN_LPF_PT2;
break;
case FILTER_PT3:
pidRuntime.dynLpfFilter = DYN_LPF_PT3;
break;
default: default:
pidRuntime.dynLpfFilter = DYN_LPF_NONE; pidRuntime.dynLpfFilter = DYN_LPF_NONE;
break; break;
@ -383,16 +421,16 @@ void pidInitConfig(const pidProfile_t *pidProfile)
pidRuntime.airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f; pidRuntime.airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
#endif #endif
#ifdef USE_INTERPOLATED_SP #ifdef USE_FEEDFORWARD
pidRuntime.ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp; pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging;
if (pidProfile->ff_smooth_factor) { if (pidProfile->feedforward_smooth_factor) {
pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f; pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->feedforward_smooth_factor) / 100.0f;
} else { } else {
// set automatically according to boost amount, limit to 0.5 for auto // set automatically according to boost amount, limit to 0.5 for auto
pidRuntime.ffSmoothFactor = MAX(0.5f, 1.0f - ((float)pidProfile->ff_boost) * 2.0f / 100.0f); pidRuntime.ffSmoothFactor = MAX(0.5f, 1.0f - ((float)pidProfile->feedforward_boost) * 2.0f / 100.0f);
} }
pidRuntime.ffJitterFactor = pidProfile->ff_jitter_factor; pidRuntime.ffJitterFactor = pidProfile->feedforward_jitter_factor;
interpolatedSpInit(pidProfile); feedforwardInit(pidProfile);
#endif #endif
pidRuntime.levelRaceMode = pidProfile->level_race_mode; pidRuntime.levelRaceMode = pidProfile->level_race_mode;

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

@ -24,6 +24,6 @@ void pidInit(const pidProfile_t *pidProfile);
void pidInitFilters(const pidProfile_t *pidProfile); void pidInitFilters(const pidProfile_t *pidProfile);
void pidInitConfig(const pidProfile_t *pidProfile); void pidInitConfig(const pidProfile_t *pidProfile);
void pidSetItermAccelerator(float newItermAccelerator); void pidSetItermAccelerator(float newItermAccelerator);
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis); void pidInitFeedforwardLpf(uint16_t filterCutoff, uint8_t debugAxis);
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff); void pidUpdateFeedforwardLpf(uint16_t filterCutoff);
void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex); void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);

60
src/main/msp/msp.c
View File

@ -1487,8 +1487,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, rxConfig()->spektrum_sat_bind); sbufWriteU8(dst, rxConfig()->spektrum_sat_bind);
sbufWriteU16(dst, rxConfig()->rx_min_usec); sbufWriteU16(dst, rxConfig()->rx_min_usec);
sbufWriteU16(dst, rxConfig()->rx_max_usec); sbufWriteU16(dst, rxConfig()->rx_max_usec);
sbufWriteU8(dst, rxConfig()->rcInterpolation); sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rcInterpolation)
sbufWriteU8(dst, rxConfig()->rcInterpolationInterval); sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rcInterpolationInterval)
sbufWriteU16(dst, rxConfig()->airModeActivateThreshold * 10 + 1000); sbufWriteU16(dst, rxConfig()->airModeActivateThreshold * 10 + 1000);
#ifdef USE_RX_SPI #ifdef USE_RX_SPI
sbufWriteU8(dst, rxSpiConfig()->rx_spi_protocol); sbufWriteU8(dst, rxSpiConfig()->rx_spi_protocol);
@ -1500,13 +1500,13 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
#endif #endif
sbufWriteU8(dst, rxConfig()->fpvCamAngleDegrees); sbufWriteU8(dst, rxConfig()->fpvCamAngleDegrees);
sbufWriteU8(dst, rxConfig()->rcInterpolationChannels); sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rcSmoothingChannels
#if defined(USE_RC_SMOOTHING_FILTER) #if defined(USE_RC_SMOOTHING_FILTER)
sbufWriteU8(dst, rxConfig()->rc_smoothing_type); sbufWriteU8(dst, rxConfig()->rc_smoothing_mode);
sbufWriteU8(dst, rxConfig()->rc_smoothing_input_cutoff); sbufWriteU8(dst, rxConfig()->rc_smoothing_setpoint_cutoff);
sbufWriteU8(dst, rxConfig()->rc_smoothing_derivative_cutoff); sbufWriteU8(dst, rxConfig()->rc_smoothing_feedforward_cutoff);
sbufWriteU8(dst, rxConfig()->rc_smoothing_input_type); sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_input_type
sbufWriteU8(dst, rxConfig()->rc_smoothing_derivative_type); sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_derivative_type
#else #else
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
@ -1521,7 +1521,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
#endif #endif
// Added in MSP API 1.42 // Added in MSP API 1.42
#if defined(USE_RC_SMOOTHING_FILTER) #if defined(USE_RC_SMOOTHING_FILTER)
sbufWriteU8(dst, rxConfig()->rc_smoothing_auto_factor); sbufWriteU8(dst, rxConfig()->rc_smoothing_auto_factor_rpy);
#else #else
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
#endif #endif
@ -1816,7 +1816,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, 0); // was vbatPidCompensation sbufWriteU8(dst, 0); // was vbatPidCompensation
sbufWriteU8(dst, currentPidProfile->feedForwardTransition); sbufWriteU8(dst, currentPidProfile->feedforwardTransition);
sbufWriteU8(dst, 0); // was low byte of currentPidProfile->dtermSetpointWeight sbufWriteU8(dst, 0); // was low byte of currentPidProfile->dtermSetpointWeight
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
@ -1892,14 +1892,14 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
#endif #endif
// Added in MSP API 1.44 // Added in MSP API 1.44
#if defined(USE_INTERPOLATED_SP) #if defined(USE_FEEDFORWARD)
sbufWriteU8(dst, currentPidProfile->ff_interpolate_sp); sbufWriteU8(dst, currentPidProfile->feedforward_averaging);
sbufWriteU8(dst, currentPidProfile->ff_smooth_factor); sbufWriteU8(dst, currentPidProfile->feedforward_smooth_factor);
#else #else
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
#endif #endif
sbufWriteU8(dst, currentPidProfile->ff_boost); sbufWriteU8(dst, currentPidProfile->feedforward_boost);
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION) #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
sbufWriteU8(dst, currentPidProfile->vbat_sag_compensation); sbufWriteU8(dst, currentPidProfile->vbat_sag_compensation);
#else #else
@ -2140,7 +2140,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_
sbufWriteU8(dst, currentPidProfile->simplified_pd_ratio); sbufWriteU8(dst, currentPidProfile->simplified_pd_ratio);
sbufWriteU8(dst, currentPidProfile->simplified_pd_gain); sbufWriteU8(dst, currentPidProfile->simplified_pd_gain);
sbufWriteU8(dst, currentPidProfile->simplified_dmin_ratio); sbufWriteU8(dst, currentPidProfile->simplified_dmin_ratio);
sbufWriteU8(dst, currentPidProfile->simplified_ff_gain); sbufWriteU8(dst, currentPidProfile->simplified_feedforward_gain);
sbufWriteU8(dst, currentPidProfile->simplified_dterm_filter); sbufWriteU8(dst, currentPidProfile->simplified_dterm_filter);
sbufWriteU8(dst, currentPidProfile->simplified_dterm_filter_multiplier); sbufWriteU8(dst, currentPidProfile->simplified_dterm_filter_multiplier);
@ -2703,7 +2703,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU16(src); // was pidProfile.yaw_p_limit sbufReadU16(src); // was pidProfile.yaw_p_limit
sbufReadU8(src); // reserved sbufReadU8(src); // reserved
sbufReadU8(src); // was vbatPidCompensation sbufReadU8(src); // was vbatPidCompensation
currentPidProfile->feedForwardTransition = sbufReadU8(src); currentPidProfile->feedforwardTransition = sbufReadU8(src);
sbufReadU8(src); // was low byte of currentPidProfile->dtermSetpointWeight sbufReadU8(src); // was low byte of currentPidProfile->dtermSetpointWeight
sbufReadU8(src); // reserved sbufReadU8(src); // reserved
sbufReadU8(src); // reserved sbufReadU8(src); // reserved
@ -2797,14 +2797,14 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
} }
if (sbufBytesRemaining(src) >= 5) { if (sbufBytesRemaining(src) >= 5) {
// Added in MSP API 1.44 // Added in MSP API 1.44
#if defined(USE_INTERPOLATED_SP) #if defined(USE_FEEDFORWARD)
currentPidProfile->ff_interpolate_sp = sbufReadU8(src); currentPidProfile->feedforward_averaging = sbufReadU8(src);
currentPidProfile->ff_smooth_factor = sbufReadU8(src); currentPidProfile->feedforward_smooth_factor = sbufReadU8(src);
#else #else
sbufReadU8(src); sbufReadU8(src);
sbufReadU8(src); sbufReadU8(src);
#endif #endif
currentPidProfile->ff_boost = sbufReadU8(src); currentPidProfile->feedforward_boost = sbufReadU8(src);
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION) #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
currentPidProfile->vbat_sag_compensation = sbufReadU8(src); currentPidProfile->vbat_sag_compensation = sbufReadU8(src);
#else #else
@ -3115,7 +3115,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
currentPidProfile->simplified_pd_ratio = sbufReadU8(src); currentPidProfile->simplified_pd_ratio = sbufReadU8(src);
currentPidProfile->simplified_pd_gain = sbufReadU8(src); currentPidProfile->simplified_pd_gain = sbufReadU8(src);
currentPidProfile->simplified_dmin_ratio = sbufReadU8(src); currentPidProfile->simplified_dmin_ratio = sbufReadU8(src);
currentPidProfile->simplified_ff_gain = sbufReadU8(src); currentPidProfile->simplified_feedforward_gain = sbufReadU8(src);
currentPidProfile->simplified_dterm_filter = sbufReadU8(src); currentPidProfile->simplified_dterm_filter = sbufReadU8(src);
currentPidProfile->simplified_dterm_filter_multiplier = sbufReadU8(src); currentPidProfile->simplified_dterm_filter_multiplier = sbufReadU8(src);
@ -3234,8 +3234,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
rxConfigMutable()->rx_max_usec = sbufReadU16(src); rxConfigMutable()->rx_max_usec = sbufReadU16(src);
} }
if (sbufBytesRemaining(src) >= 4) { if (sbufBytesRemaining(src) >= 4) {
rxConfigMutable()->rcInterpolation = sbufReadU8(src); sbufReadU8(src); // not required in API 1.44, was rxConfigMutable()->rcInterpolation
rxConfigMutable()->rcInterpolationInterval = sbufReadU8(src); sbufReadU8(src); // not required in API 1.44, was rxConfigMutable()->rcInterpolationInterval
rxConfigMutable()->airModeActivateThreshold = (sbufReadU16(src) - 1000) / 10; rxConfigMutable()->airModeActivateThreshold = (sbufReadU16(src) - 1000) / 10;
} }
if (sbufBytesRemaining(src) >= 6) { if (sbufBytesRemaining(src) >= 6) {
@ -3254,13 +3254,13 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
} }
if (sbufBytesRemaining(src) >= 6) { if (sbufBytesRemaining(src) >= 6) {
// Added in MSP API 1.40 // Added in MSP API 1.40
rxConfigMutable()->rcInterpolationChannels = sbufReadU8(src); sbufReadU8(src); // not required in API 1.44, was rxConfigMutable()->rcSmoothingChannels
#if defined(USE_RC_SMOOTHING_FILTER) #if defined(USE_RC_SMOOTHING_FILTER)
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_type, sbufReadU8(src)); configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_mode, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_input_cutoff, sbufReadU8(src)); configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_setpoint_cutoff, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_derivative_cutoff, sbufReadU8(src)); configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_feedforward_cutoff, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_input_type, sbufReadU8(src)); sbufReadU8(src); // not required in API 1.44, was rc_smoothing_input_type
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_derivative_type, sbufReadU8(src)); sbufReadU8(src); // not required in API 1.44, was rc_smoothing_derivative_type
#else #else
sbufReadU8(src); sbufReadU8(src);
sbufReadU8(src); sbufReadU8(src);
@ -3285,7 +3285,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
// the 10.6 configurator where it was possible to submit an invalid out-of-range value. We might be // the 10.6 configurator where it was possible to submit an invalid out-of-range value. We might be
// able to remove the constraint at some point in the future once the affected versions are deprecated // able to remove the constraint at some point in the future once the affected versions are deprecated
// enough that the risk is low. // enough that the risk is low.
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_auto_factor, constrain(sbufReadU8(src), RC_SMOOTHING_AUTO_FACTOR_MIN, RC_SMOOTHING_AUTO_FACTOR_MAX)); configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_auto_factor_rpy, constrain(sbufReadU8(src), RC_SMOOTHING_AUTO_FACTOR_MIN, RC_SMOOTHING_AUTO_FACTOR_MAX));
#else #else
sbufReadU8(src); sbufReadU8(src);
#endif #endif

17
src/main/pg/rx.c
View File

@ -34,7 +34,7 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/rx_spi.h" #include "rx/rx_spi.h"
PG_REGISTER_WITH_RESET_FN(rxConfig_t, rxConfig, PG_RX_CONFIG, 2); PG_REGISTER_WITH_RESET_FN(rxConfig_t, rxConfig, PG_RX_CONFIG, 3);
void pgResetFn_rxConfig(rxConfig_t *rxConfig) void pgResetFn_rxConfig(rxConfig_t *rxConfig)
{ {
RESET_CONFIG_2(rxConfig_t, rxConfig, RESET_CONFIG_2(rxConfig_t, rxConfig,
@ -56,17 +56,16 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.rssi_offset = 0, .rssi_offset = 0,
.rssi_invert = 0, .rssi_invert = 0,
.rssi_src_frame_lpf_period = 30, .rssi_src_frame_lpf_period = 30,
.rcInterpolation = RC_SMOOTHING_AUTO,
.rcInterpolationChannels = INTERPOLATION_CHANNELS_RPYT,
.rcInterpolationInterval = 19,
.fpvCamAngleDegrees = 0, .fpvCamAngleDegrees = 0,
.airModeActivateThreshold = 25, .airModeActivateThreshold = 25,
.max_aux_channel = DEFAULT_AUX_CHANNEL_COUNT, .max_aux_channel = DEFAULT_AUX_CHANNEL_COUNT,
.rc_smoothing_type = RC_SMOOTHING_TYPE_FILTER, .rc_smoothing_mode = ON,
.rc_smoothing_input_cutoff = 0, // automatically calculate the cutoff by default .rc_smoothing_setpoint_cutoff = 0,
.rc_smoothing_derivative_cutoff = 0, // automatically calculate the cutoff by default .rc_smoothing_feedforward_cutoff = 0,
.rc_smoothing_debug_axis = ROLL, // default to debug logging for the roll axis .rc_smoothing_throttle_cutoff = 0,
.rc_smoothing_auto_factor = 30, .rc_smoothing_debug_axis = ROLL,
.rc_smoothing_auto_factor_rpy = 30,
.rc_smoothing_auto_factor_throttle = 30,
.srxl2_unit_id = 1, .srxl2_unit_id = 1,
.srxl2_baud_fast = true, .srxl2_baud_fast = true,
.sbus_baud_fast = false, .sbus_baud_fast = false,

15
src/main/pg/rx.h
View File

@ -42,9 +42,6 @@ typedef struct rxConfig_s {
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t mincheck; // minimum rc end uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end uint16_t maxcheck; // maximum rc end
uint8_t rcInterpolation;
uint8_t rcInterpolationChannels;
uint8_t rcInterpolationInterval;
uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands
uint8_t airModeActivateThreshold; // Throttle setpoint percent where airmode gets activated uint8_t airModeActivateThreshold; // Throttle setpoint percent where airmode gets activated
@ -53,13 +50,13 @@ typedef struct rxConfig_s {
uint8_t max_aux_channel; uint8_t max_aux_channel;
uint8_t rssi_src_frame_errors; // true to use frame drop flags in the rx protocol uint8_t rssi_src_frame_errors; // true to use frame drop flags in the rx protocol
int8_t rssi_offset; // offset applied to the RSSI value before it is returned int8_t rssi_offset; // offset applied to the RSSI value before it is returned
uint8_t rc_smoothing_type; // Determines the smoothing algorithm to use: INTERPOLATION or FILTER uint8_t rc_smoothing_mode; // Whether filter based rc smoothing is on or off
uint8_t rc_smoothing_input_cutoff; // Filter cutoff frequency for the input filter (0 = auto) uint8_t rc_smoothing_setpoint_cutoff; // Filter cutoff frequency for the setpoint filter (0 = auto)
uint8_t rc_smoothing_derivative_cutoff; // Filter cutoff frequency for the setpoint weight derivative filter (0 = auto) uint8_t rc_smoothing_feedforward_cutoff; // Filter cutoff frequency for the feedforward filter (0 = auto)
uint8_t rc_smoothing_throttle_cutoff; // Filter cutoff frequency for the setpoint filter (0 = auto)
uint8_t rc_smoothing_debug_axis; // Axis to log as debug values when debug_mode = RC_SMOOTHING uint8_t rc_smoothing_debug_axis; // Axis to log as debug values when debug_mode = RC_SMOOTHING
uint8_t rc_smoothing_input_type; // Input filter type (0 = PT1, 1 = BIQUAD) uint8_t rc_smoothing_auto_factor_rpy; // Used to adjust the "smoothness" determined by the auto cutoff calculations
uint8_t rc_smoothing_derivative_type; // Derivative filter type (0 = OFF, 1 = PT1, 2 = BIQUAD) uint8_t rc_smoothing_auto_factor_throttle; // Used to adjust the "smoothness" determined by the auto cutoff calculations
uint8_t rc_smoothing_auto_factor; // Used to adjust the "smoothness" determined by the auto cutoff calculations
uint8_t rssi_src_frame_lpf_period; // Period of the cutoff frequency for the source frame RSSI filter (in 0.1 s) uint8_t rssi_src_frame_lpf_period; // Period of the cutoff frequency for the source frame RSSI filter (in 0.1 s)
uint8_t srxl2_unit_id; // Spektrum SRXL2 RX unit id uint8_t srxl2_unit_id; // Spektrum SRXL2 RX unit id

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

@ -635,17 +635,29 @@ void dynLpfGyroUpdate(float throttle)
} else { } else {
cutoffFreq = fmax(dynThrottle(throttle) * gyro.dynLpfMax, gyro.dynLpfMin); cutoffFreq = fmax(dynThrottle(throttle) * gyro.dynLpfMax, gyro.dynLpfMin);
} }
if (gyro.dynLpfFilter == DYN_LPF_PT1) {
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
const float gyroDt = gyro.targetLooptime * 1e-6f; const float gyroDt = gyro.targetLooptime * 1e-6f;
switch (gyro.dynLpfFilter) {
case DYN_LPF_PT1:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt)); pt1FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
} }
} else if (gyro.dynLpfFilter == DYN_LPF_BIQUAD) { break;
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); case DYN_LPF_BIQUAD:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterUpdateLPF(&gyro.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime); biquadFilterUpdateLPF(&gyro.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
} }
break;
case DYN_LPF_PT2:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt2FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt2FilterState, pt2FilterGain(cutoffFreq, gyroDt));
}
break;
case DYN_LPF_PT3:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt3FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt3FilterState, pt3FilterGain(cutoffFreq, gyroDt));
}
break;
} }
} }
} }

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

@ -36,7 +36,7 @@
#include "pg/pg.h" #include "pg/pg.h"
#define FILTER_FREQUENCY_MAX 4000 // maximum frequency for filter cutoffs (nyquist limit of 8K max sampling) #define FILTER_FREQUENCY_MAX 1000 // so little filtering above 1000hz that if the user wants less delay, they must disable the filter
#define DYN_LPF_FILTER_FREQUENCY_MAX 1000 #define DYN_LPF_FILTER_FREQUENCY_MAX 1000
#define DYN_LPF_GYRO_MIN_HZ_DEFAULT 200 #define DYN_LPF_GYRO_MIN_HZ_DEFAULT 200
@ -51,6 +51,8 @@
typedef union gyroLowpassFilter_u { typedef union gyroLowpassFilter_u {
pt1Filter_t pt1FilterState; pt1Filter_t pt1FilterState;
biquadFilter_t biquadFilterState; biquadFilter_t biquadFilterState;
pt2Filter_t pt2FilterState;
pt3Filter_t pt3FilterState;
} gyroLowpassFilter_t; } gyroLowpassFilter_t;
typedef enum gyroDetectionFlags_e { typedef enum gyroDetectionFlags_e {
@ -147,7 +149,9 @@ enum {
enum { enum {
DYN_LPF_NONE = 0, DYN_LPF_NONE = 0,
DYN_LPF_PT1, DYN_LPF_PT1,
DYN_LPF_BIQUAD DYN_LPF_BIQUAD,
DYN_LPF_PT2,
DYN_LPF_PT3,
}; };
typedef enum { typedef enum {

26
src/main/sensors/gyro_init.c
View File

@ -181,8 +181,8 @@ static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_
// type. It will be overridden for positive cases. // type. It will be overridden for positive cases.
*lowpassFilterApplyFn = nullFilterApply; *lowpassFilterApplyFn = nullFilterApply;
// If lowpass cutoff has been specified and is less than the Nyquist frequency // If lowpass cutoff has been specified
if (lpfHz && lpfHz <= gyroFrequencyNyquist) { if (lpfHz) {
switch (type) { switch (type) {
case FILTER_PT1: case FILTER_PT1:
*lowpassFilterApplyFn = (filterApplyFnPtr) pt1FilterApply; *lowpassFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
@ -192,6 +192,7 @@ static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_
ret = true; ret = true;
break; break;
case FILTER_BIQUAD: case FILTER_BIQUAD:
if (lpfHz <= gyroFrequencyNyquist) {
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1; *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
#else #else
@ -201,6 +202,21 @@ static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_
biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, looptime); biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, looptime);
} }
ret = true; ret = true;
}
break;
case FILTER_PT2:
*lowpassFilterApplyFn = (filterApplyFnPtr) pt2FilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt2FilterInit(&lowpassFilter[axis].pt2FilterState, gain);
}
ret = true;
break;
case FILTER_PT3:
*lowpassFilterApplyFn = (filterApplyFnPtr) pt3FilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt3FilterInit(&lowpassFilter[axis].pt3FilterState, gain);
}
ret = true;
break; break;
} }
} }
@ -218,6 +234,12 @@ static void dynLpfFilterInit()
case FILTER_BIQUAD: case FILTER_BIQUAD:
gyro.dynLpfFilter = DYN_LPF_BIQUAD; gyro.dynLpfFilter = DYN_LPF_BIQUAD;
break; break;
case FILTER_PT2:
gyro.dynLpfFilter = DYN_LPF_PT2;
break;
case FILTER_PT3:
gyro.dynLpfFilter = DYN_LPF_PT3;
break;
default: default:
gyro.dynLpfFilter = DYN_LPF_NONE; gyro.dynLpfFilter = DYN_LPF_NONE;
break; break;

4
src/main/target/ALIENWHOOP/config.c
View File

@ -83,8 +83,6 @@ void targetConfiguration(void)
rxConfigMutable()->spektrum_sat_bind = 5; // DSM2 11ms rxConfigMutable()->spektrum_sat_bind = 5; // DSM2 11ms
rxConfigMutable()->spektrum_sat_bind_autoreset = 1; rxConfigMutable()->spektrum_sat_bind_autoreset = 1;
rxConfigMutable()->mincheck = 1025; rxConfigMutable()->mincheck = 1025;
rxConfigMutable()->rcInterpolation = RC_SMOOTHING_MANUAL;
rxConfigMutable()->rcInterpolationInterval = 14;
parseRcChannels("TAER1234", rxConfigMutable()); parseRcChannels("TAER1234", rxConfigMutable());
mixerConfigMutable()->yaw_motors_reversed = true; mixerConfigMutable()->yaw_motors_reversed = true;
@ -133,7 +131,7 @@ void targetConfiguration(void)
pidProfile->dterm_notch_hz = 0; pidProfile->dterm_notch_hz = 0;
pidProfile->pid[PID_PITCH].F = 100; pidProfile->pid[PID_PITCH].F = 100;
pidProfile->pid[PID_ROLL].F = 100; pidProfile->pid[PID_ROLL].F = 100;
pidProfile->feedForwardTransition = 0; pidProfile->feedforwardTransition = 0;
/* Anti-Gravity */ /* Anti-Gravity */
pidProfile->itermThrottleThreshold = 500; pidProfile->itermThrottleThreshold = 500;

2
src/main/target/NAZE/config.c
View File

@ -88,7 +88,7 @@ void targetConfiguration(void)
pidProfile->pid[PID_PITCH].F = 200; pidProfile->pid[PID_PITCH].F = 200;
pidProfile->pid[PID_ROLL].F = 200; pidProfile->pid[PID_ROLL].F = 200;
pidProfile->feedForwardTransition = 50; pidProfile->feedforwardTransition = 50;
} }
for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) {

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

@ -379,7 +379,7 @@
#define USE_PERSISTENT_STATS #define USE_PERSISTENT_STATS
#define USE_PROFILE_NAMES #define USE_PROFILE_NAMES
#define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol #define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol
#define USE_INTERPOLATED_SP #define USE_FEEDFORWARD
#define USE_CUSTOM_BOX_NAMES #define USE_CUSTOM_BOX_NAMES
#define USE_BATTERY_VOLTAGE_SAG_COMPENSATION #define USE_BATTERY_VOLTAGE_SAG_COMPENSATION
#define USE_RX_MSP_OVERRIDE #define USE_RX_MSP_OVERRIDE

3
src/test/Makefile
View File

@ -388,7 +388,8 @@ pid_unittest_DEFINES := \
USE_ITERM_RELAX= \ USE_ITERM_RELAX= \
USE_RC_SMOOTHING_FILTER= \ USE_RC_SMOOTHING_FILTER= \
USE_ABSOLUTE_CONTROL= \ USE_ABSOLUTE_CONTROL= \
USE_LAUNCH_CONTROL= USE_LAUNCH_CONTROL= \
USE_FEEDFORWARD=
rcdevice_unittest_DEFINES := \ rcdevice_unittest_DEFINES := \
USE_RCDEVICE= USE_RCDEVICE=

30
src/test/unit/pid_unittest.cc
View File

@ -26,6 +26,7 @@
bool simulatedAirmodeEnabled = true; bool simulatedAirmodeEnabled = true;
float simulatedSetpointRate[3] = { 0,0,0 }; float simulatedSetpointRate[3] = { 0,0,0 };
float simulatedPrevSetpointRate[3] = { 0,0,0 };
float simulatedRcDeflection[3] = { 0,0,0 }; float simulatedRcDeflection[3] = { 0,0,0 };
float simulatedThrottlePIDAttenuation = 1.0f; float simulatedThrottlePIDAttenuation = 1.0f;
float simulatedMotorMixRange = 0.0f; float simulatedMotorMixRange = 0.0f;
@ -88,12 +89,26 @@ extern "C" {
void beeperConfirmationBeeps(uint8_t) { } void beeperConfirmationBeeps(uint8_t) { }
bool isLaunchControlActive(void) {return unitLaunchControlActive; } bool isLaunchControlActive(void) {return unitLaunchControlActive; }
void disarm(flightLogDisarmReason_e) { } void disarm(flightLogDisarmReason_e) { }
float applyFFLimit(int axis, float value, float Kp, float currentPidSetpoint) { float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint)
{
UNUSED(axis); UNUSED(axis);
UNUSED(Kp); UNUSED(Kp);
UNUSED(currentPidSetpoint); UNUSED(currentPidSetpoint);
return value; return value;
} }
void feedforwardInit(const pidProfile_t) { }
float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging)
{
UNUSED(newRcFrame);
UNUSED(feedforwardAveraging);
return simulatedSetpointRate[axis] - simulatedPrevSetpointRate[axis];
}
bool shouldApplyFeedforwardLimits(int axis)
{
UNUSED(axis);
return true;
}
bool getShouldUpdateFeedforward() { return true; }
void initRcProcessing(void) { } void initRcProcessing(void) { }
} }
@ -124,7 +139,7 @@ void setDefaultTestSettings(void) {
pidProfile->itermWindupPointPercent = 50; pidProfile->itermWindupPointPercent = 50;
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
pidProfile->levelAngleLimit = 55; pidProfile->levelAngleLimit = 55;
pidProfile->feedForwardTransition = 100; pidProfile->feedforwardTransition = 100;
pidProfile->yawRateAccelLimit = 100; pidProfile->yawRateAccelLimit = 100;
pidProfile->rateAccelLimit = 0; pidProfile->rateAccelLimit = 0;
pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH; pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH;
@ -199,6 +214,7 @@ void resetTest(void) {
} }
void setStickPosition(int axis, float stickRatio) { void setStickPosition(int axis, float stickRatio) {
simulatedPrevSetpointRate[axis] = simulatedSetpointRate[axis];
simulatedSetpointRate[axis] = 1998.0f * stickRatio; simulatedSetpointRate[axis] = 1998.0f * stickRatio;
simulatedRcDeflection[axis] = stickRatio; simulatedRcDeflection[axis] = stickRatio;
} }
@ -519,7 +535,7 @@ TEST(pidControllerTest, testFeedForward) {
EXPECT_NEAR(2232.78, pidData[FD_ROLL].F, calculateTolerance(2232.78)); EXPECT_NEAR(2232.78, pidData[FD_ROLL].F, calculateTolerance(2232.78));
EXPECT_NEAR(-2061.03, pidData[FD_PITCH].F, calculateTolerance(-2061.03)); EXPECT_NEAR(-2061.03, pidData[FD_PITCH].F, calculateTolerance(-2061.03));
EXPECT_NEAR(-82.52, pidData[FD_YAW].F, calculateTolerance(-82.5)); EXPECT_NEAR(-2061.03, pidData[FD_YAW].F, calculateTolerance(-2061.03));
// Match the stick to gyro to stop error // Match the stick to gyro to stop error
setStickPosition(FD_ROLL, 0.5f); setStickPosition(FD_ROLL, 0.5f);
@ -530,9 +546,13 @@ TEST(pidControllerTest, testFeedForward) {
EXPECT_NEAR(-558.20, pidData[FD_ROLL].F, calculateTolerance(-558.20)); EXPECT_NEAR(-558.20, pidData[FD_ROLL].F, calculateTolerance(-558.20));
EXPECT_NEAR(515.26, pidData[FD_PITCH].F, calculateTolerance(515.26)); EXPECT_NEAR(515.26, pidData[FD_PITCH].F, calculateTolerance(515.26));
EXPECT_NEAR(-41.26, pidData[FD_YAW].F, calculateTolerance(-41.26)); EXPECT_NEAR(515.26, pidData[FD_YAW].F, calculateTolerance(515.26));
for (int loop =0; loop <= 15; loop++) { setStickPosition(FD_ROLL, 0.0f);
setStickPosition(FD_PITCH, 0.0f);
setStickPosition(FD_YAW, 0.0f);
for (int loop = 0; loop <= 15; loop++) {
gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL]; gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL];
pidController(pidProfile, currentTestTime()); pidController(pidProfile, currentTestTime());
} }