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/gyroanalyse.c \
flight/imu.c \
flight/interpolated_setpoint.c \
flight/feedforward.c \
flight/mixer.c \
flight/mixer_init.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
BLACKBOX_PRINT_HEADER_LINE("use_integrated_yaw", "%d", currentPidProfile->use_integrated_yaw);
#endif
BLACKBOX_PRINT_HEADER_LINE("feedforward_transition", "%d", currentPidProfile->feedForwardTransition);
BLACKBOX_PRINT_HEADER_LINE("feedforward_weight", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].F,
BLACKBOX_PRINT_HEADER_LINE("ff_transition", "%d", currentPidProfile->feedforwardTransition);
BLACKBOX_PRINT_HEADER_LINE("ff_weight", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].F,
currentPidProfile->pid[PID_PITCH].F,
currentPidProfile->pid[PID_YAW].F);
#ifdef USE_INTERPOLATED_SP
BLACKBOX_PRINT_HEADER_LINE("ff_interpolate_sp", "%d", currentPidProfile->ff_interpolate_sp);
BLACKBOX_PRINT_HEADER_LINE("ff_max_rate_limit", "%d", currentPidProfile->ff_max_rate_limit);
#ifdef USE_FEEDFORWARD
BLACKBOX_PRINT_HEADER_LINE("ff_averaging", "%d", currentPidProfile->feedforward_averaging);
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
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", "%d", currentPidProfile->rateAccelLimit);
@ -1443,9 +1445,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware);
#endif
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("serialrx_provider", "%d", rxConfig()->serialrx_provider);
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);
#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_cutoffs", "%d, %d", rcSmoothingData->inputCutoffSetting,
rcSmoothingData->derivativeCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor", "%d", rcSmoothingData->autoSmoothnessFactor);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingData->inputCutoffFrequency,
rcSmoothingData->derivativeCutoffFrequency);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs_ff_sp_thr", "%d,%d,%d", rcSmoothingData->feedforwardCutoffFrequency,
rcSmoothingData->setpointCutoffFrequency,
rcSmoothingData->throttleCutoffFrequency);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingData->averageFrameTimeUs);
#endif // USE_RC_SMOOTHING_FILTER
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);
rcSmoothingFilter_t *rcSmoothingData = getRcSmoothingData();
cliPrint("# RC Smoothing Type: ");
if (rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_FILTER) {
if (rxConfig()->rc_smoothing_mode == ON) {
cliPrintLine("FILTER");
if (rcSmoothingAutoCalculate()) {
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);
}
}
cliPrintf("# Active input cutoff: %dhz ", rcSmoothingData->inputCutoffFrequency);
if (rcSmoothingData->inputCutoffSetting == 0) {
cliPrintf("# Active setpoint cutoff: %dhz ", rcSmoothingData->setpointCutoffFrequency);
if (rcSmoothingData->setpointCutoffSetting == 0) {
cliPrintLine("(auto)");
} else {
cliPrintLine("(manual)");
}
cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingData->derivativeCutoffFrequency);
if (rcSmoothingData->derivativeCutoffSetting == 0) {
cliPrintf("# Active FF cutoff: %dhz (", rcSmoothingData->feedforwardCutoffFrequency);
if (rcSmoothingData->ffCutoffSetting == 0) {
cliPrintLine("auto)");
} else {
cliPrintLine("manual)");
}
cliPrintf("# Active throttle cutoff: %dhz (", rcSmoothingData->throttleCutoffFrequency);
if (rcSmoothingData->ffCutoffSetting == 0) {
cliPrintLine("auto)");
} else {
cliPrintLine("manual)");
}
} else {
cliPrintLine("INTERPOLATION");
cliPrintLine("OFF");
}
}
#endif // USE_RC_SMOOTHING_FILTER

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

@ -291,22 +291,18 @@ static const char * const lookupTablePwmProtocol[] = {
"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[] = {
"PT1",
"BIQUAD",
"PT2",
"PT3",
};
static const char * const lookupTableDtermLowpassType[] = {
"PT1",
"BIQUAD",
"PT2",
"PT3",
};
static const char * const lookupTableAntiGravityMode[] = {
@ -406,9 +402,6 @@ static const char * const lookupTableAcroTrainerDebug[] = {
#endif // USE_ACRO_TRAINER
#ifdef USE_RC_SMOOTHING_FILTER
static const char * const lookupTableRcSmoothingType[] = {
"INTERPOLATION", "FILTER"
};
static const char * const lookupTableRcSmoothingDebug[] = {
"ROLL", "PITCH", "YAW", "THROTTLE"
};
@ -479,8 +472,8 @@ static const char * const lookupTableOffOnAuto[] = {
"OFF", "ON", "AUTO"
};
const char* const lookupTableInterpolatedSetpoint[] = {
"OFF", "ON", "AVERAGED_2", "AVERAGED_3", "AVERAGED_4"
const char* const lookupTableFeedforwardAveraging[] = {
"NONE", "2_POINT", "3_POINT", "4_POINT"
};
static const char* const lookupTableDshotBitbangedTimer[] = {
@ -551,8 +544,6 @@ const lookupTableEntry_t lookupTables[] = {
#endif
LOOKUP_TABLE_ENTRY(debugModeNames),
LOOKUP_TABLE_ENTRY(lookupTablePwmProtocol),
LOOKUP_TABLE_ENTRY(lookupTableRcInterpolation),
LOOKUP_TABLE_ENTRY(lookupTableRcInterpolationChannels),
LOOKUP_TABLE_ENTRY(lookupTableLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableDtermLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableAntiGravityMode),
@ -597,7 +588,6 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableAcroTrainerDebug),
#endif // USE_ACRO_TRAINER
#ifdef USE_RC_SMOOTHING_FILTER
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingType),
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDebug),
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_VTX_COMMON
@ -622,7 +612,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTablePositionAltSource),
LOOKUP_TABLE_ENTRY(lookupTableOffOnAuto),
LOOKUP_TABLE_ENTRY(lookupTableInterpolatedSetpoint),
LOOKUP_TABLE_ENTRY(lookupTableFeedforwardAveraging),
LOOKUP_TABLE_ENTRY(lookupTableDshotBitbangedTimer),
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_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) },
{ "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
{ "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_input_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_input_cutoff) },
{ "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_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) },
{ "rc_smoothing", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_mode) },
{ "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_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) },
#endif // USE_RC_SMOOTHING_FILTER
{ "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_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) },
{ "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", 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) },
@ -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) },
#endif
#ifdef USE_INTERPOLATED_SP
{ "ff_interpolate_sp", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_INTERPOLATED_SP}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_sp) },
{ "ff_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_max_rate_limit) },
{ "ff_smooth_factor", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_smooth_factor) },
{ "ff_jitter_reduction", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_jitter_factor) },
#ifdef USE_FEEDFORWARD
{ "feedforward_averaging", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FEEDFORWARD_AVERAGING }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_averaging) },
{ "feedforward_smooth_factor", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_smooth_factor) },
{ "feedforward_jitter_reduction", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_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
{ "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
{ "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_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_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_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
TABLE_DEBUG,
TABLE_MOTOR_PWM_PROTOCOL,
TABLE_RC_INTERPOLATION,
TABLE_RC_INTERPOLATION_CHANNELS,
TABLE_LOWPASS_TYPE,
TABLE_DTERM_LOWPASS_TYPE,
TABLE_ANTI_GRAVITY_MODE,
@ -110,7 +108,6 @@ typedef enum {
TABLE_ACRO_TRAINER_DEBUG,
#endif // USE_ACRO_TRAINER
#ifdef USE_RC_SMOOTHING_FILTER
TABLE_RC_SMOOTHING_TYPE,
TABLE_RC_SMOOTHING_DEBUG,
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_VTX_COMMON
@ -133,7 +130,7 @@ typedef enum {
TABLE_GYRO_FILTER_DEBUG,
TABLE_POSITION_ALT_SOURCE,
TABLE_OFF_ON_AUTO,
TABLE_INTERPOLATED_SP,
TABLE_FEEDFORWARD_AVERAGING,
TABLE_DSHOT_BITBANGED_TIMER,
TABLE_OSD_DISPLAYPORT_DEVICE,
#ifdef USE_OSD
@ -260,7 +257,7 @@ extern const char * const lookupTableItermRelaxType[];
extern const char * const lookupTableOsdDisplayPortDevice[];
extern const char * const lookupTableInterpolatedSetpoint[];
extern const char * const lookupTableFeedforwardAveraging[];
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_gain;
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_multiplier;
@ -257,7 +257,7 @@ static const void *cmsx_simplifiedTuningOnEnter(displayPort_t *pDisp)
cmsx_simplified_pd_ratio = pidProfile->simplified_pd_ratio;
cmsx_simplified_pd_gain = pidProfile->simplified_pd_gain;
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_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_gain = cmsx_simplified_pd_gain;
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_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_gain = cmsx_simplified_pd_gain;
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_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 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 },
{ "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},
{ "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
static uint8_t cmsx_feedForwardTransition;
static uint8_t cmsx_ff_boost;
static uint8_t cmsx_feedforwardTransition;
static uint8_t cmsx_feedforward_boost;
static uint8_t cmsx_angleStrength;
static uint8_t cmsx_horizonStrength;
static uint8_t cmsx_horizonTransition;
@ -516,10 +516,10 @@ static uint8_t cmsx_iterm_relax_type;
static uint8_t cmsx_iterm_relax_cutoff;
#endif
#ifdef USE_INTERPOLATED_SP
static uint8_t cmsx_ff_interpolate_sp;
static uint8_t cmsx_ff_smooth_factor;
static uint8_t cmsx_ff_jitter_factor;
#ifdef USE_FEEDFORWARD
static uint8_t cmsx_feedforward_averaging;
static uint8_t cmsx_feedforward_smooth_factor;
static uint8_t cmsx_feedforward_jitter_factor;
#endif
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);
cmsx_feedForwardTransition = pidProfile->feedForwardTransition;
cmsx_ff_boost = pidProfile->ff_boost;
cmsx_feedforwardTransition = pidProfile->feedforwardTransition;
cmsx_feedforward_boost = pidProfile->feedforward_boost;
cmsx_angleStrength = pidProfile->pid[PID_LEVEL].P;
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;
#endif
#ifdef USE_INTERPOLATED_SP
cmsx_ff_interpolate_sp = pidProfile->ff_interpolate_sp;
cmsx_ff_smooth_factor = pidProfile->ff_smooth_factor;
cmsx_ff_jitter_factor = pidProfile->ff_jitter_factor;
#ifdef USE_FEEDFORWARD
cmsx_feedforward_averaging = pidProfile->feedforward_averaging;
cmsx_feedforward_smooth_factor = pidProfile->feedforward_smooth_factor;
cmsx_feedforward_jitter_factor = pidProfile->feedforward_jitter_factor;
#endif
#ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
@ -577,9 +577,9 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
UNUSED(self);
pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
pidProfile->feedForwardTransition = cmsx_feedForwardTransition;
pidProfile->feedforwardTransition = cmsx_feedforwardTransition;
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].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;
#endif
#ifdef USE_INTERPOLATED_SP
pidProfile->ff_interpolate_sp = cmsx_ff_interpolate_sp;
pidProfile->ff_smooth_factor = cmsx_ff_smooth_factor;
pidProfile->ff_jitter_factor = cmsx_ff_jitter_factor;
#ifdef USE_FEEDFORWARD
pidProfile->feedforward_averaging = cmsx_feedforward_averaging;
pidProfile->feedforward_smooth_factor = cmsx_feedforward_smooth_factor;
pidProfile->feedforward_jitter_factor = cmsx_feedforward_jitter_factor;
#endif
#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[] = {
{ "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 },
{ "FF TRANS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_feedForwardTransition, 0, 100, 1, 10 }, 0 },
#ifdef USE_INTERPOLATED_SP
{ "FF MODE", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_ff_interpolate_sp, 4, lookupTableInterpolatedSetpoint}, 0 },
{ "FF SMOOTHNESS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_smooth_factor, 0, 75, 1 } , 0 },
{ "FF JITTER", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_jitter_factor, 0, 20, 1 } , 0 },
{ "FF TRANSITION", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_feedforwardTransition, 0, 100, 1, 10 }, 0 },
#ifdef USE_FEEDFORWARD
{ "FF AVERAGING", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_feedforward_averaging, 4, lookupTableFeedforwardAveraging}, 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_feedforward_jitter_factor, 0, 20, 1 } , 0 },
#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 },
{ "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 },

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

@ -354,33 +354,6 @@ static void validateAndFixConfig(void)
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 (
featureIsConfigured(FEATURE_3D) || !featureIsConfigured(FEATURE_GPS) || mixerModeIsFixedWing(mixerConfig()->mixerMode)
#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 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 iGain = pidProfile->simplified_i_gain / 100.0f;
const float pdRatio = pidProfile->simplified_pd_ratio / 100.0f;
@ -57,7 +57,7 @@ static void calculateNewPidValues(pidProfile_t *pidProfile)
} else {
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);
}
}

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

@ -41,7 +41,7 @@
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/interpolated_setpoint.h"
#include "flight/feedforward.h"
#include "flight/gps_rescue.h"
#include "flight/pid_init.h"
@ -57,14 +57,12 @@
typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs);
#ifdef USE_INTERPOLATED_SP
// Setpoint in degrees/sec before RC-Smoothing is applied
static float rawSetpoint[XYZ_AXIS_COUNT];
#ifdef USE_FEEDFORWARD
static float oldRcCommand[XYZ_AXIS_COUNT];
static bool isDuplicate[XYZ_AXIS_COUNT];
float rcCommandDelta[XYZ_AXIS_COUNT];
#endif
static float rawSetpoint[XYZ_AXIS_COUNT];
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
static float throttlePIDAttenuation;
static bool reverseMotors = false;
@ -74,7 +72,6 @@ static bool isRxDataNew = false;
static float rcCommandDivider = 500.0f;
static float rcCommandYawDivider = 500.0f;
FAST_DATA_ZERO_INIT uint8_t interpolationChannels;
static FAST_DATA_ZERO_INIT bool newRxDataForFF;
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_MIN_US 1000 // 1ms
#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;
#endif // USE_RC_SMOOTHING_FILTER
bool getShouldUpdateFf()
// only used in pid.c when interpolated_sp is active to initiate a new FF value
bool getShouldUpdateFeedforward()
// only used in pid.c, when feedforward is enabled, to initiate a new FF value
{
const bool updateFf = newRxDataForFF;
if (newRxDataForFF == true){
@ -129,7 +126,7 @@ float getThrottlePIDAttenuation(void)
return throttlePIDAttenuation;
}
#ifdef USE_INTERPOLATED_SP
#ifdef USE_FEEDFORWARD
float getRawSetpoint(int axis)
{
return rawSetpoint[axis];
@ -139,7 +136,6 @@ float getRcCommandDelta(int axis)
{
return rcCommandDelta[axis];
}
#endif
#define THROTTLE_LOOKUP_LENGTH 12
@ -240,43 +236,7 @@ float applyCurve(int axis, float deflection)
return applyRates(axis, deflection, fabsf(deflection));
}
static void calculateSetpointRate(int 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;
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)
static void scaleSetpointToFpvCamAngle(void)
{
//recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
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)
{
static timeUs_t lastRxTimeUs;
@ -400,9 +302,8 @@ uint16_t getCurrentRxRefreshRate(void)
}
#ifdef USE_RC_SMOOTHING_FILTER
// Determine a cutoff frequency based on filter type and the calculated
// average rx frame time
FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, uint8_t autoSmoothnessFactor)
// Determine a cutoff frequency based on smoothness factor and calculated average rx frame time
FAST_CODE_NOINLINE int calcAutoSmoothingCutoff(int avgRxFrameTimeUs, uint8_t autoSmoothnessFactor)
{
if (avgRxFrameTimeUs > 0) {
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)
{
const float dT = targetPidLooptime * 1e-6f;
uint16_t oldCutoff = smoothingData->inputCutoffFrequency;
uint16_t oldCutoff = smoothingData->setpointCutoffFrequency;
if (smoothingData->inputCutoffSetting == 0) {
smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactor);
if (smoothingData->setpointCutoffSetting == 0) {
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++) {
if ((1 << i) & interpolationChannels) { // only update channels specified by rc_interp_ch
if (i < THROTTLE) { // Throttle handled by smoothing rcCommand
if (!smoothingData->filterInitialized) {
pt3FilterInit((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT));
pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
} else {
pt3FilterUpdateCutoff((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT));
pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
}
} else {
if (!smoothingData->filterInitialized) {
pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
} else {
pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
}
}
}
}
// 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);
// update or initialize the FF filter
oldCutoff = smoothingData->feedforwardCutoffFrequency;
if (rcSmoothingData.ffCutoffSetting == 0) {
smoothingData->feedforwardCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint);
}
if (!smoothingData->filterInitialized) {
pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, smoothingData->debugAxis);
} else if (smoothingData->derivativeCutoffFrequency != oldCutoff) {
pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency);
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
FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
{
// if the input cutoff is 0 (auto) then we need to calculate cutoffs
if ((rcSmoothingData.inputCutoffSetting == 0) || (rcSmoothingData.derivativeCutoffSetting == 0)) {
// if any rc smoothing cutoff is 0 (auto) then we need to calculate cutoffs
if ((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.ffCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) {
return true;
}
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 lastRxData[4];
static FAST_DATA_ZERO_INIT float rxDataToSmooth[4];
static FAST_DATA_ZERO_INIT bool initialized;
static FAST_DATA_ZERO_INIT timeMs_t validRxFrameTimeMs;
static FAST_DATA_ZERO_INIT bool calculateCutoffs;
@ -510,47 +419,42 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
initialized = true;
rcSmoothingData.filterInitialized = false;
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.inputCutoffSetting = rxConfig()->rc_smoothing_input_cutoff;
rcSmoothingData.derivativeCutoffSetting = rxConfig()->rc_smoothing_derivative_cutoff;
rcSmoothingData.setpointCutoffSetting = rxConfig()->rc_smoothing_setpoint_cutoff;
rcSmoothingData.throttleCutoffSetting = rxConfig()->rc_smoothing_throttle_cutoff;
rcSmoothingData.ffCutoffSetting = rxConfig()->rc_smoothing_feedforward_cutoff;
rcSmoothingResetAccumulation(&rcSmoothingData);
rcSmoothingData.inputCutoffFrequency = rcSmoothingData.inputCutoffSetting;
if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) {
// calculate the initial derivative cutoff used for interpolated feedforward until RC interval is known
const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactor / 10.0f));
float derivativeCutoff = RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_INITIAL_HZ * cutoffFactor; // PT1 cutoff frequency
rcSmoothingData.derivativeCutoffFrequency = lrintf(derivativeCutoff);
rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting;
rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting;
if (rcSmoothingData.ffCutoffSetting == 0) {
// calculate and use an initial derivative cutoff until the RC interval is known
const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactorSetpoint / 10.0f));
float ffCutoff = RC_SMOOTHING_FEEDFORWARD_INITIAL_HZ * cutoffFactor;
rcSmoothingData.feedforwardCutoffFrequency = lrintf(ffCutoff);
} else {
rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting;
rcSmoothingData.feedforwardCutoffFrequency = rcSmoothingData.ffCutoffSetting;
}
calculateCutoffs = rcSmoothingAutoCalculate();
if (rxConfig()->rc_smoothing_mode) {
calculateCutoffs = rcSmoothingAutoCalculate();
// if we don't need to calculate cutoffs dynamically then the filters can be initialized now
if (!calculateCutoffs) {
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
rcSmoothingData.filterInitialized = true;
// if we don't need to calculate cutoffs dynamically then the filters can be initialized now
if (!calculateCutoffs) {
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
rcSmoothingData.filterInitialized = true;
}
}
}
if (isRxDataNew) {
// 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
// for auto calculated filters we need to examine each rx frame interval
if (calculateCutoffs) {
const timeMs_t currentTimeMs = millis();
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
if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization
if (rxIsReceivingSignal() && rcSmoothingRxRateValid(currentRxRefreshRate)) {
@ -582,9 +486,11 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
// accumlate the sample into the average
if (accumulateSample) {
if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) {
// the required number of samples were collected so set the filter cutoffs
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
rcSmoothingData.filterInitialized = true;
// 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);
rcSmoothingData.filterInitialized = true;
}
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
}
}
// 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)) {
// 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
DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rcSmoothingData.debugAxis]));
DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs);
}
// each pid loop continue to apply the last received channel value to the filter
for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value
if (rcSmoothingData.filterInitialized) {
rcCommand[updatedChannel] = pt3FilterApply((pt3Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
} else {
// If filter isn't initialized yet then use the actual unsmoothed rx channel data
rcCommand[updatedChannel] = lastRxData[updatedChannel];
}
// each pid loop, apply the last received channel value to the filter, if initialised - thanks @klutvott
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
float *dst = i == THROTTLE ? &rcCommand[i] : &setpointRate[i];
if (rcSmoothingData.filterInitialized) {
*dst = pt3FilterApply(&rcSmoothingData.filter[i], rxDataToSmooth[i]);
} else {
// If filter isn't initialized yet, as in smoothing off, use the actual unsmoothed rx channel data
*dst = rxDataToSmooth[i];
}
}
return interpolationChannels;
}
#endif // USE_RC_SMOOTHING_FILTER
FAST_CODE void processRcCommand(void)
{
uint8_t updatedChannel;
if (isRxDataNew) {
newRxDataForFF = true;
}
@ -641,57 +550,55 @@ FAST_CODE void processRcCommand(void)
checkForThrottleErrorResetState(currentRxRefreshRate);
}
#ifdef USE_INTERPOLATED_SP
if (isRxDataNew) {
for (int i = FD_ROLL; i <= FD_YAW; i++) {
isDuplicate[i] = (oldRcCommand[i] == rcCommand[i]);
rcCommandDelta[i] = fabsf(rcCommand[i] - oldRcCommand[i]);
oldRcCommand[i] = rcCommand[i];
float rcCommandf;
if (i == FD_YAW) {
rcCommandf = rcCommand[i] / rcCommandYawDivider;
} else {
rcCommandf = rcCommand[i] / rcCommandDivider;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
isDuplicate[axis] = (oldRcCommand[axis] == rcCommand[axis]);
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;
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);
}
const float rcCommandfAbs = fabsf(rcCommandf);
rawSetpoint[i] = applyRates(i, rcCommandf, rcCommandfAbs);
}
}
#endif
switch (rxConfig()->rc_smoothing_type) {
#ifdef USE_RC_SMOOTHING_FILTER
case RC_SMOOTHING_TYPE_FILTER:
updatedChannel = processRcSmoothingFilter();
break;
#endif // USE_RC_SMOOTHING_FILTER
case RC_SMOOTHING_TYPE_INTERPOLATION:
default:
updatedChannel = processRcInterpolation();
break;
}
if (isRxDataNew || updatedChannel) {
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);
rawSetpoint[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]);
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
}
DEBUG_SET(DEBUG_RC_INTERPOLATION, 3, setpointRate[0]);
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
// adjust un-filtered setpoint steps to camera angle (mixing Roll and Yaw)
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleRcCommandToFpvCamAngle();
scaleSetpointToFpvCamAngle();
}
}
#ifdef USE_RC_SMOOTHING_FILTER
processRcSmoothingFilter();
#endif
isRxDataNew = false;
}
@ -840,45 +747,13 @@ void initRcProcessing(void)
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
const int maxYawRate = (int)applyRates(FD_YAW, 1.0f, 1.0f);
initYawSpinRecovery(maxYawRate);
#endif
}
bool rcSmoothingIsEnabled(void)
{
return !(
#if defined(USE_RC_SMOOTHING_FILTER)
rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_INTERPOLATION &&
#endif
rxConfig()->rcInterpolation == RC_SMOOTHING_OFF);
}
// send rc smoothing details to blackbox
#ifdef USE_RC_SMOOTHING_FILTER
rcSmoothingFilter_t *getRcSmoothingData(void)
{
@ -886,6 +761,6 @@ rcSmoothingFilter_t *getRcSmoothingData(void)
}
bool rcSmoothingInitializationComplete(void) {
return (rxConfig()->rc_smoothing_type != RC_SMOOTHING_TYPE_FILTER) || rcSmoothingData.filterInitialized;
return rcSmoothingData.filterInitialized;
}
#endif // USE_RC_SMOOTHING_FILTER

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

@ -24,14 +24,6 @@
#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
#define RC_SMOOTHING_AUTO_FACTOR_MIN 0
#define RC_SMOOTHING_AUTO_FACTOR_MAX 250
@ -46,13 +38,12 @@ void updateRcCommands(void);
void resetYawAxis(void);
void initRcProcessing(void);
bool isMotorsReversed(void);
bool rcSmoothingIsEnabled(void);
rcSmoothingFilter_t *getRcSmoothingData(void);
bool rcSmoothingAutoCalculate(void);
bool rcSmoothingInitializationComplete(void);
float getRawSetpoint(int axis);
float getRcCommandDelta(int axis);
float applyCurve(int axis, float deflection);
bool getShouldUpdateFf();
bool getShouldUpdateFeedforward();
void updateRcRefreshRate(timeUs_t currentTimeUs);
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);
break;
case ADJUSTMENT_FEEDFORWARD_TRANSITION:
newValue = constrain(currentPidProfile->feedForwardTransition + delta, 1, 100); // FIXME magic numbers repeated in cli.c
currentPidProfile->feedForwardTransition = newValue;
newValue = constrain(currentPidProfile->feedforwardTransition + delta, 1, 100); // FIXME magic numbers repeated in cli.c
currentPidProfile->feedforwardTransition = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
break;
default:
@ -579,7 +579,7 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus
break;
case ADJUSTMENT_FEEDFORWARD_TRANSITION:
newValue = constrain(value, 1, 100); // FIXME magic numbers repeated in cli.c
currentPidProfile->feedForwardTransition = newValue;
currentPidProfile->feedforwardTransition = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
break;
default:

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

@ -59,16 +59,9 @@ typedef enum {
} rollPitchStatus_e;
typedef enum {
RC_SMOOTHING_OFF = 0,
RC_SMOOTHING_DEFAULT,
RC_SMOOTHING_AUTO,
RC_SMOOTHING_MANUAL
} rcSmoothing_t;
typedef enum {
RC_SMOOTHING_TYPE_INTERPOLATION,
RC_SMOOTHING_TYPE_FILTER
} rcSmoothingType_e;
OFF,
ON
} rcSmoothingMode_e;
#define ROL_LO (1 << (2 * ROLL))
#define ROL_CE (3 << (2 * ROLL))
@ -107,14 +100,17 @@ typedef struct rcSmoothingFilterTraining_s {
typedef struct rcSmoothingFilter_s {
bool filterInitialized;
pt3Filter_t filter[4];
uint8_t inputCutoffSetting;
uint16_t inputCutoffFrequency;
uint8_t derivativeCutoffSetting;
uint16_t derivativeCutoffFrequency;
uint8_t setpointCutoffSetting;
uint8_t throttleCutoffSetting;
uint16_t setpointCutoffFrequency;
uint16_t throttleCutoffFrequency;
uint8_t ffCutoffSetting;
uint16_t feedforwardCutoffFrequency;
int averageFrameTimeUs;
rcSmoothingFilterTraining_t training;
uint8_t debugAxis;
uint8_t autoSmoothnessFactor;
uint8_t autoSmoothnessFactorSetpoint;
uint8_t autoSmoothnessFactorThrottle;
} rcSmoothingFilter_t;
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 "platform.h"
#ifdef USE_INTERPOLATED_SP
#ifdef USE_FEEDFORWARD
#include "build/debug.h"
@ -31,7 +31,7 @@
#include "flight/pid.h"
#include "interpolated_setpoint.h"
#include "feedforward.h"
static float setpointDeltaImpl[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 ffMaxRate[XYZ_AXIS_COUNT];
void interpolatedSpInit(const pidProfile_t *pidProfile) {
const float ffMaxRateScale = pidProfile->ff_max_rate_limit * 0.01f;
averagingCount = pidProfile->ff_interpolate_sp;
void feedforwardInit(const pidProfile_t *pidProfile) {
const float ffMaxRateScale = pidProfile->feedforward_max_rate_limit * 0.01f;
averagingCount = pidProfile->feedforward_averaging + 1;
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
ffMaxRate[i] = applyCurve(i, 1.0f);
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) {
float rcCommandDelta = getRcCommandDelta(axis);
@ -99,7 +99,7 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
} else {
// movement!
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
ffAttenuator = 0.0f;
}
@ -138,26 +138,26 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
}
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 2 is interpolated setpoint, above
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;
// apply averaging
if (type == FF_INTERPOLATE_ON) {
setpointDelta[axis] = setpointDeltaImpl[axis];
} else {
if (feedforwardAveraging) {
setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDeltaImpl[axis]);
} else {
setpointDelta[axis] = setpointDeltaImpl[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) {
case FD_ROLL:
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;
}
bool shouldApplyFfLimits(int axis)
bool shouldApplyFeedforwardLimits(int axis)
{
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 "flight/pid.h"
void interpolatedSpInit(const pidProfile_t *pidProfile);
float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type);
float applyFfLimit(int axis, float value, float Kp, float currentPidSetpoint);
bool shouldApplyFfLimits(int axis);
void feedforwardInit(const pidProfile_t *pidProfile);
float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging);
float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint);
bool shouldApplyFeedforwardLimits(int axis);

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

@ -48,7 +48,7 @@
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/rpm_filter.h"
#include "flight/interpolated_setpoint.h"
#include "flight/feedforward.h"
#include "io/gps.h"
@ -141,7 +141,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.itermWindupPointPercent = 100,
.pidAtMinThrottle = PID_STABILISATION_ON,
.levelAngleLimit = 55,
.feedForwardTransition = 0,
.feedforwardTransition = 0,
.yawRateAccelLimit = 0,
.rateAccelLimit = 0,
.itermThrottleThreshold = 250,
@ -202,11 +202,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
.dyn_idle_i_gain = 50,
.dyn_idle_d_gain = 50,
.dyn_idle_max_increase = 150,
.ff_interpolate_sp = FF_INTERPOLATE_ON,
.ff_max_rate_limit = 100,
.ff_smooth_factor = 37,
.ff_jitter_factor = 7,
.ff_boost = 15,
.feedforward_averaging = FEEDFORWARD_AVERAGING_OFF,
.feedforward_max_rate_limit = 100,
.feedforward_smooth_factor = 37,
.feedforward_jitter_factor = 7,
.feedforward_boost = 15,
.dyn_lpf_curve_expo = 5,
.level_race_mode = false,
.vbat_sag_compensation = 0,
@ -217,7 +217,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.simplified_pd_ratio = SIMPLIFIED_TUNING_DEFAULT,
.simplified_pd_gain = 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_multiplier = SIMPLIFIED_TUNING_DEFAULT,
);
@ -261,7 +261,7 @@ float pidGetFfBoostFactor()
return pidRuntime.ffBoostFactor;
}
#ifdef USE_INTERPOLATED_SP
#ifdef USE_FEEDFORWARD
float pidGetFfSmoothFactor()
{
return pidRuntime.ffSmoothFactor;
@ -636,14 +636,14 @@ STATIC_UNIT_TESTED void rotateItermAndAxisError()
}
#ifdef USE_RC_SMOOTHING_FILTER
float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelta)
float FAST_CODE applyRcSmoothingFeedforwardFilter(int axis, float pidSetpointDelta)
{
float ret = pidSetpointDelta;
if (axis == pidRuntime.rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
}
if (pidRuntime.setpointDerivativeLpfInitialized) {
ret = pt3FilterApply(&pidRuntime.setpointDerivativePt3[axis], pidSetpointDelta);
if (pidRuntime.feedforwardLpfInitialized) {
ret = pt3FilterApply(&pidRuntime.feedforwardPt3[axis], pidSetpointDelta);
if (axis == pidRuntime.rcSmoothingDebugAxis) {
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();
#endif
#ifdef USE_INTERPOLATED_SP
#ifdef USE_FEEDFORWARD
bool newRcFrame = false;
if (getShouldUpdateFf()) {
if (getShouldUpdateFeedforward()) {
newRcFrame = true;
}
#endif
@ -1025,22 +1025,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// -----calculate pidSetpointDelta
float pidSetpointDelta = 0;
#ifdef USE_INTERPOLATED_SP
if (pidRuntime.ffFromInterpolatedSetpoint) {
pidSetpointDelta = interpolatedSpApply(axis, newRcFrame, pidRuntime.ffFromInterpolatedSetpoint);
} else {
pidSetpointDelta = currentPidSetpoint - pidRuntime.previousPidSetpoint[axis];
}
#else
pidSetpointDelta = currentPidSetpoint - pidRuntime.previousPidSetpoint[axis];
#ifdef USE_FEEDFORWARD
pidSetpointDelta = feedforwardApply(axis, newRcFrame, pidRuntime.feedforwardAveraging);
#endif
pidRuntime.previousPidSetpoint[axis] = currentPidSetpoint;
#ifdef USE_RC_SMOOTHING_FILTER
pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
#endif // USE_RC_SMOOTHING_FILTER
// -----calculate D component
// disable D if launch control is active
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
#ifdef USE_ABSOLUTE_CONTROL
// include abs control correction in FF
// include abs control correction in feedforward
pidSetpointDelta += setpointCorrection - pidRuntime.oldSetpointCorrection[axis];
pidRuntime.oldSetpointCorrection[axis] = setpointCorrection;
#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
const float feedforwardGain = (flightModeFlags || launchControlActive) ? 0.0f : pidRuntime.pidCoefficient[axis].Kf;
if (feedforwardGain > 0) {
// no transition if feedForwardTransition == 0
float transition = pidRuntime.feedForwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * pidRuntime.feedForwardTransition) : 1;
// no transition if feedforwardTransition == 0
float transition = pidRuntime.feedforwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * pidRuntime.feedforwardTransition) : 1;
float feedForward = feedforwardGain * transition * pidSetpointDelta * pidRuntime.pidFrequency;
#ifdef USE_INTERPOLATED_SP
pidData[axis].F = shouldApplyFfLimits(axis) ?
applyFfLimit(axis, feedForward, pidRuntime.pidCoefficient[axis].Kp, currentPidSetpoint) : feedForward;
#ifdef USE_FEEDFORWARD
pidData[axis].F = shouldApplyFeedforwardLimits(axis) ?
applyFeedforwardLimit(axis, feedForward, pidRuntime.pidCoefficient[axis].Kp, currentPidSetpoint) : feedForward;
#else
pidData[axis].F = feedForward;
#endif
#ifdef USE_RC_SMOOTHING_FILTER
pidData[axis].F = applyRcSmoothingFeedforwardFilter(axis, pidData[axis].F);
#endif // USE_RC_SMOOTHING_FILTER
} else {
pidData[axis].F = 0;
}
@ -1239,15 +1231,27 @@ void dynLpfDTermUpdate(float throttle)
} else {
cutoffFreq = fmax(dynThrottle(throttle) * pidRuntime.dynLpfMax, pidRuntime.dynLpfMin);
}
if (pidRuntime.dynLpfFilter == DYN_LPF_PT1) {
switch (pidRuntime.dynLpfFilter) {
case DYN_LPF_PT1:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
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++) {
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,
} itermRelaxType_e;
typedef enum ffInterpolationType_e {
FF_INTERPOLATE_OFF,
FF_INTERPOLATE_ON,
FF_INTERPOLATE_AVG2,
FF_INTERPOLATE_AVG3,
FF_INTERPOLATE_AVG4
} ffInterpolationType_t;
typedef enum feedforwardAveraging_e {
FEEDFORWARD_AVERAGING_OFF,
FEEDFORWARD_AVERAGING_2_POINT,
FEEDFORWARD_AVERAGING_3_POINT,
FEEDFORWARD_AVERAGING_4_POINT,
} feedforwardAveraging_t;
#define MAX_PROFILE_NAME_LENGTH 8u
@ -162,7 +161,7 @@ typedef struct pidProfile_s {
uint16_t crash_delay; // ms
uint8_t crash_recovery_angle; // degrees
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 itermLimit;
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)
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 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
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_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 ff_max_rate_limit; // Maximum setpoint rate percentage for FF
uint8_t ff_smooth_factor; // Amount of smoothing for interpolated FF steps
uint8_t ff_jitter_factor; // Number of RC steps below which to attenuate FF
uint8_t feedforward_averaging; // Number of packets to average when averaging is on
uint8_t feedforward_max_rate_limit; // Maximum setpoint rate percentage for feedforward
uint8_t feedforward_smooth_factor; // Amount of lowpass type smoothing for feedforward steps
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 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
@ -222,7 +221,7 @@ typedef struct pidProfile_s {
uint8_t simplified_pd_ratio;
uint8_t simplified_pd_gain;
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_multiplier;
@ -254,6 +253,8 @@ typedef struct pidAxisData_s {
typedef union dtermLowpass_u {
pt1Filter_t pt1Filter;
biquadFilter_t biquadFilter;
pt2Filter_t pt2Filter;
pt3Filter_t pt3Filter;
} dtermLowpass_t;
typedef struct pidCoefficient_s {
@ -286,7 +287,7 @@ typedef struct pidRuntime_s {
float ffBoostFactor;
float itermAccelerator;
uint16_t itermAcceleratorGain;
float feedForwardTransition;
float feedforwardTransition;
pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
float levelGain;
float horizonGain;
@ -341,8 +342,8 @@ typedef struct pidRuntime_s {
#endif
#ifdef USE_RC_SMOOTHING_FILTER
pt3Filter_t setpointDerivativePt3[XYZ_AXIS_COUNT];
bool setpointDerivativeLpfInitialized;
pt3Filter_t feedforwardPt3[XYZ_AXIS_COUNT];
bool feedforwardLpfInitialized;
uint8_t rcSmoothingDebugAxis;
uint8_t rcSmoothingFilterType;
#endif // USE_RC_SMOOTHING_FILTER
@ -383,8 +384,8 @@ typedef struct pidRuntime_s {
float airmodeThrottleOffsetLimit;
#endif
#ifdef USE_INTERPOLATED_SP
ffInterpolationType_t ffFromInterpolatedSetpoint;
#ifdef USE_FEEDFORWARD
feedforwardAveraging_t feedforwardAveraging;
float ffSmoothFactor;
float ffJitterFactor;
#endif

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

@ -36,7 +36,7 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/interpolated_setpoint.h"
#include "flight/feedforward.h"
#include "flight/pid.h"
#include "flight/rpm_filter.h"
@ -110,7 +110,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
#endif
if (dterm_lowpass_hz > 0 && dterm_lowpass_hz < pidFrequencyNyquist) {
if (dterm_lowpass_hz > 0) {
switch (pidProfile->dterm_filter_type) {
case FILTER_PT1:
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
@ -119,13 +119,29 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
break;
case FILTER_BIQUAD:
if (pidProfile->dterm_lowpass_hz < pidFrequencyNyquist) {
#ifdef USE_DYN_LPF
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
#else
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
#endif
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
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++) {
biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime);
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;
default:
@ -137,9 +153,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
//2nd Dterm Lowpass Filter
if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) {
pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
} else {
if (pidProfile->dterm_lowpass2_hz > 0) {
switch (pidProfile->dterm_filter2_type) {
case FILTER_PT1:
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
@ -148,18 +162,36 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
break;
case FILTER_BIQUAD:
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
if (pidProfile->dterm_lowpass2_hz < pidFrequencyNyquist) {
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
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++) {
biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime);
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;
default:
pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
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;
} else {
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.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)
@ -221,22 +253,22 @@ void pidInit(const pidProfile_t *pidProfile)
}
#ifdef USE_RC_SMOOTHING_FILTER
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis)
void pidInitFeedforwardLpf(uint16_t filterCutoff, uint8_t debugAxis)
{
pidRuntime.rcSmoothingDebugAxis = debugAxis;
if (filterCutoff > 0) {
pidRuntime.setpointDerivativeLpfInitialized = true;
pidRuntime.feedforwardLpfInitialized = true;
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) {
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)
{
if (pidProfile->feedForwardTransition == 0) {
pidRuntime.feedForwardTransition = 0;
if (pidProfile->feedforwardTransition == 0) {
pidRuntime.feedforwardTransition = 0;
} else {
pidRuntime.feedForwardTransition = 100.0f / pidProfile->feedForwardTransition;
pidRuntime.feedforwardTransition = 100.0f / pidProfile->feedforwardTransition;
}
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pidRuntime.pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
@ -333,6 +365,12 @@ void pidInitConfig(const pidProfile_t *pidProfile)
case FILTER_BIQUAD:
pidRuntime.dynLpfFilter = DYN_LPF_BIQUAD;
break;
case FILTER_PT2:
pidRuntime.dynLpfFilter = DYN_LPF_PT2;
break;
case FILTER_PT3:
pidRuntime.dynLpfFilter = DYN_LPF_PT3;
break;
default:
pidRuntime.dynLpfFilter = DYN_LPF_NONE;
break;
@ -383,16 +421,16 @@ void pidInitConfig(const pidProfile_t *pidProfile)
pidRuntime.airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
#endif
#ifdef USE_INTERPOLATED_SP
pidRuntime.ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp;
if (pidProfile->ff_smooth_factor) {
pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f;
#ifdef USE_FEEDFORWARD
pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging;
if (pidProfile->feedforward_smooth_factor) {
pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->feedforward_smooth_factor) / 100.0f;
} else {
// 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;
interpolatedSpInit(pidProfile);
pidRuntime.ffJitterFactor = pidProfile->feedforward_jitter_factor;
feedforwardInit(pidProfile);
#endif
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 pidInitConfig(const pidProfile_t *pidProfile);
void pidSetItermAccelerator(float newItermAccelerator);
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis);
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
void pidInitFeedforwardLpf(uint16_t filterCutoff, uint8_t debugAxis);
void pidUpdateFeedforwardLpf(uint16_t filterCutoff);
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);
sbufWriteU16(dst, rxConfig()->rx_min_usec);
sbufWriteU16(dst, rxConfig()->rx_max_usec);
sbufWriteU8(dst, rxConfig()->rcInterpolation);
sbufWriteU8(dst, rxConfig()->rcInterpolationInterval);
sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rcInterpolation)
sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rcInterpolationInterval)
sbufWriteU16(dst, rxConfig()->airModeActivateThreshold * 10 + 1000);
#ifdef USE_RX_SPI
sbufWriteU8(dst, rxSpiConfig()->rx_spi_protocol);
@ -1500,13 +1500,13 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, 0);
#endif
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)
sbufWriteU8(dst, rxConfig()->rc_smoothing_type);
sbufWriteU8(dst, rxConfig()->rc_smoothing_input_cutoff);
sbufWriteU8(dst, rxConfig()->rc_smoothing_derivative_cutoff);
sbufWriteU8(dst, rxConfig()->rc_smoothing_input_type);
sbufWriteU8(dst, rxConfig()->rc_smoothing_derivative_type);
sbufWriteU8(dst, rxConfig()->rc_smoothing_mode);
sbufWriteU8(dst, rxConfig()->rc_smoothing_setpoint_cutoff);
sbufWriteU8(dst, rxConfig()->rc_smoothing_feedforward_cutoff);
sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_input_type
sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_derivative_type
#else
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
@ -1521,7 +1521,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
#endif
// Added in MSP API 1.42
#if defined(USE_RC_SMOOTHING_FILTER)
sbufWriteU8(dst, rxConfig()->rc_smoothing_auto_factor);
sbufWriteU8(dst, rxConfig()->rc_smoothing_auto_factor_rpy);
#else
sbufWriteU8(dst, 0);
#endif
@ -1816,7 +1816,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit
sbufWriteU8(dst, 0); // reserved
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); // reserved
sbufWriteU8(dst, 0); // reserved
@ -1892,14 +1892,14 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, 0);
#endif
// Added in MSP API 1.44
#if defined(USE_INTERPOLATED_SP)
sbufWriteU8(dst, currentPidProfile->ff_interpolate_sp);
sbufWriteU8(dst, currentPidProfile->ff_smooth_factor);
#if defined(USE_FEEDFORWARD)
sbufWriteU8(dst, currentPidProfile->feedforward_averaging);
sbufWriteU8(dst, currentPidProfile->feedforward_smooth_factor);
#else
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
#endif
sbufWriteU8(dst, currentPidProfile->ff_boost);
sbufWriteU8(dst, currentPidProfile->feedforward_boost);
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
sbufWriteU8(dst, currentPidProfile->vbat_sag_compensation);
#else
@ -2140,7 +2140,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_
sbufWriteU8(dst, currentPidProfile->simplified_pd_ratio);
sbufWriteU8(dst, currentPidProfile->simplified_pd_gain);
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_multiplier);
@ -2703,7 +2703,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU16(src); // was pidProfile.yaw_p_limit
sbufReadU8(src); // reserved
sbufReadU8(src); // was vbatPidCompensation
currentPidProfile->feedForwardTransition = sbufReadU8(src);
currentPidProfile->feedforwardTransition = sbufReadU8(src);
sbufReadU8(src); // was low byte of currentPidProfile->dtermSetpointWeight
sbufReadU8(src); // reserved
sbufReadU8(src); // reserved
@ -2797,14 +2797,14 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
}
if (sbufBytesRemaining(src) >= 5) {
// Added in MSP API 1.44
#if defined(USE_INTERPOLATED_SP)
currentPidProfile->ff_interpolate_sp = sbufReadU8(src);
currentPidProfile->ff_smooth_factor = sbufReadU8(src);
#if defined(USE_FEEDFORWARD)
currentPidProfile->feedforward_averaging = sbufReadU8(src);
currentPidProfile->feedforward_smooth_factor = sbufReadU8(src);
#else
sbufReadU8(src);
sbufReadU8(src);
#endif
currentPidProfile->ff_boost = sbufReadU8(src);
currentPidProfile->feedforward_boost = sbufReadU8(src);
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
currentPidProfile->vbat_sag_compensation = sbufReadU8(src);
#else
@ -3115,7 +3115,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
currentPidProfile->simplified_pd_ratio = sbufReadU8(src);
currentPidProfile->simplified_pd_gain = 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_multiplier = sbufReadU8(src);
@ -3234,8 +3234,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
rxConfigMutable()->rx_max_usec = sbufReadU16(src);
}
if (sbufBytesRemaining(src) >= 4) {
rxConfigMutable()->rcInterpolation = sbufReadU8(src);
rxConfigMutable()->rcInterpolationInterval = sbufReadU8(src);
sbufReadU8(src); // not required in API 1.44, was rxConfigMutable()->rcInterpolation
sbufReadU8(src); // not required in API 1.44, was rxConfigMutable()->rcInterpolationInterval
rxConfigMutable()->airModeActivateThreshold = (sbufReadU16(src) - 1000) / 10;
}
if (sbufBytesRemaining(src) >= 6) {
@ -3254,13 +3254,13 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
}
if (sbufBytesRemaining(src) >= 6) {
// 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)
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_type, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_input_cutoff, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_derivative_cutoff, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_input_type, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_derivative_type, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_mode, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_setpoint_cutoff, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_feedforward_cutoff, sbufReadU8(src));
sbufReadU8(src); // not required in API 1.44, was rc_smoothing_input_type
sbufReadU8(src); // not required in API 1.44, was rc_smoothing_derivative_type
#else
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
// able to remove the constraint at some point in the future once the affected versions are deprecated
// 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
sbufReadU8(src);
#endif

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

@ -34,7 +34,7 @@
#include "rx/rx.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)
{
RESET_CONFIG_2(rxConfig_t, rxConfig,
@ -56,17 +56,16 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.rssi_offset = 0,
.rssi_invert = 0,
.rssi_src_frame_lpf_period = 30,
.rcInterpolation = RC_SMOOTHING_AUTO,
.rcInterpolationChannels = INTERPOLATION_CHANNELS_RPYT,
.rcInterpolationInterval = 19,
.fpvCamAngleDegrees = 0,
.airModeActivateThreshold = 25,
.max_aux_channel = DEFAULT_AUX_CHANNEL_COUNT,
.rc_smoothing_type = RC_SMOOTHING_TYPE_FILTER,
.rc_smoothing_input_cutoff = 0, // automatically calculate the cutoff by default
.rc_smoothing_derivative_cutoff = 0, // automatically calculate the cutoff by default
.rc_smoothing_debug_axis = ROLL, // default to debug logging for the roll axis
.rc_smoothing_auto_factor = 30,
.rc_smoothing_mode = ON,
.rc_smoothing_setpoint_cutoff = 0,
.rc_smoothing_feedforward_cutoff = 0,
.rc_smoothing_throttle_cutoff = 0,
.rc_smoothing_debug_axis = ROLL,
.rc_smoothing_auto_factor_rpy = 30,
.rc_smoothing_auto_factor_throttle = 30,
.srxl2_unit_id = 1,
.srxl2_baud_fast = true,
.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 mincheck; // minimum 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 airModeActivateThreshold; // Throttle setpoint percent where airmode gets activated
@ -53,13 +50,13 @@ typedef struct rxConfig_s {
uint8_t max_aux_channel;
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
uint8_t rc_smoothing_type; // Determines the smoothing algorithm to use: INTERPOLATION or FILTER
uint8_t rc_smoothing_input_cutoff; // Filter cutoff frequency for the input filter (0 = auto)
uint8_t rc_smoothing_derivative_cutoff; // Filter cutoff frequency for the setpoint weight derivative filter (0 = auto)
uint8_t rc_smoothing_mode; // Whether filter based rc smoothing is on or off
uint8_t rc_smoothing_setpoint_cutoff; // Filter cutoff frequency for the setpoint 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_input_type; // Input filter type (0 = PT1, 1 = BIQUAD)
uint8_t rc_smoothing_derivative_type; // Derivative filter type (0 = OFF, 1 = PT1, 2 = BIQUAD)
uint8_t rc_smoothing_auto_factor; // Used to adjust the "smoothness" determined by the auto cutoff calculations
uint8_t rc_smoothing_auto_factor_rpy; // Used to adjust the "smoothness" determined by the auto cutoff calculations
uint8_t rc_smoothing_auto_factor_throttle; // 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 srxl2_unit_id; // Spektrum SRXL2 RX unit id

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

@ -635,17 +635,29 @@ void dynLpfGyroUpdate(float throttle)
} else {
cutoffFreq = fmax(dynThrottle(throttle) * gyro.dynLpfMax, gyro.dynLpfMin);
}
if (gyro.dynLpfFilter == DYN_LPF_PT1) {
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
const float gyroDt = gyro.targetLooptime * 1e-6f;
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
const float gyroDt = gyro.targetLooptime * 1e-6f;
switch (gyro.dynLpfFilter) {
case DYN_LPF_PT1:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
}
} else if (gyro.dynLpfFilter == DYN_LPF_BIQUAD) {
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
break;
case DYN_LPF_BIQUAD:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
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"
#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_GYRO_MIN_HZ_DEFAULT 200
@ -51,6 +51,8 @@
typedef union gyroLowpassFilter_u {
pt1Filter_t pt1FilterState;
biquadFilter_t biquadFilterState;
pt2Filter_t pt2FilterState;
pt3Filter_t pt3FilterState;
} gyroLowpassFilter_t;
typedef enum gyroDetectionFlags_e {
@ -147,7 +149,9 @@ enum {
enum {
DYN_LPF_NONE = 0,
DYN_LPF_PT1,
DYN_LPF_BIQUAD
DYN_LPF_BIQUAD,
DYN_LPF_PT2,
DYN_LPF_PT3,
};
typedef enum {

32
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.
*lowpassFilterApplyFn = nullFilterApply;
// If lowpass cutoff has been specified and is less than the Nyquist frequency
if (lpfHz && lpfHz <= gyroFrequencyNyquist) {
// If lowpass cutoff has been specified
if (lpfHz) {
switch (type) {
case FILTER_PT1:
*lowpassFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
@ -192,13 +192,29 @@ static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_
ret = true;
break;
case FILTER_BIQUAD:
if (lpfHz <= gyroFrequencyNyquist) {
#ifdef USE_DYN_LPF
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
#else
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
#endif
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, looptime);
}
ret = true;
}
break;
case FILTER_PT2:
*lowpassFilterApplyFn = (filterApplyFnPtr) pt2FilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, looptime);
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;
@ -218,6 +234,12 @@ static void dynLpfFilterInit()
case FILTER_BIQUAD:
gyro.dynLpfFilter = DYN_LPF_BIQUAD;
break;
case FILTER_PT2:
gyro.dynLpfFilter = DYN_LPF_PT2;
break;
case FILTER_PT3:
gyro.dynLpfFilter = DYN_LPF_PT3;
break;
default:
gyro.dynLpfFilter = DYN_LPF_NONE;
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_autoreset = 1;
rxConfigMutable()->mincheck = 1025;
rxConfigMutable()->rcInterpolation = RC_SMOOTHING_MANUAL;
rxConfigMutable()->rcInterpolationInterval = 14;
parseRcChannels("TAER1234", rxConfigMutable());
mixerConfigMutable()->yaw_motors_reversed = true;
@ -133,7 +131,7 @@ void targetConfiguration(void)
pidProfile->dterm_notch_hz = 0;
pidProfile->pid[PID_PITCH].F = 100;
pidProfile->pid[PID_ROLL].F = 100;
pidProfile->feedForwardTransition = 0;
pidProfile->feedforwardTransition = 0;
/* Anti-Gravity */
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_ROLL].F = 200;
pidProfile->feedForwardTransition = 50;
pidProfile->feedforwardTransition = 50;
}
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_PROFILE_NAMES
#define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol
#define USE_INTERPOLATED_SP
#define USE_FEEDFORWARD
#define USE_CUSTOM_BOX_NAMES
#define USE_BATTERY_VOLTAGE_SAG_COMPENSATION
#define USE_RX_MSP_OVERRIDE

3
src/test/Makefile
View File

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

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

@ -26,6 +26,7 @@
bool simulatedAirmodeEnabled = true;
float simulatedSetpointRate[3] = { 0,0,0 };
float simulatedPrevSetpointRate[3] = { 0,0,0 };
float simulatedRcDeflection[3] = { 0,0,0 };
float simulatedThrottlePIDAttenuation = 1.0f;
float simulatedMotorMixRange = 0.0f;
@ -88,12 +89,26 @@ extern "C" {
void beeperConfirmationBeeps(uint8_t) { }
bool isLaunchControlActive(void) {return unitLaunchControlActive; }
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(Kp);
UNUSED(currentPidSetpoint);
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) { }
}
@ -124,7 +139,7 @@ void setDefaultTestSettings(void) {
pidProfile->itermWindupPointPercent = 50;
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
pidProfile->levelAngleLimit = 55;
pidProfile->feedForwardTransition = 100;
pidProfile->feedforwardTransition = 100;
pidProfile->yawRateAccelLimit = 100;
pidProfile->rateAccelLimit = 0;
pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH;
@ -199,6 +214,7 @@ void resetTest(void) {
}
void setStickPosition(int axis, float stickRatio) {
simulatedPrevSetpointRate[axis] = simulatedSetpointRate[axis];
simulatedSetpointRate[axis] = 1998.0f * stickRatio;
simulatedRcDeflection[axis] = stickRatio;
}
@ -519,7 +535,7 @@ TEST(pidControllerTest, testFeedForward) {
EXPECT_NEAR(2232.78, pidData[FD_ROLL].F, calculateTolerance(2232.78));
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
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(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];
pidController(pidProfile, currentTestTime());
}