Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 11:59:58 +03:00
Code Issues Wiki Activity

Refactor Feedforward Angle and RC Smoothing - mashup of 12578 and 12594 (#12605)

Browse source 
* Refactor Feedforward Angle and RC Smoothing

* update rc_smoothing at regular intervals

* add Earth Ref to OSD, update pid and rate PG

* Initialise filters correctly

* refactoring to improve performance

* Save 24 cycles in Horizon calculations, other optimisations

At a cost of 40 bytes

* save 25 cycles and 330 bytes in rc_smoothing

* feedforward max rate improvements

* typo fix

* Karatebrot's review suggestions  part one

* Karatebrot's excellent suggestions part 2

* more efficient if we calculate inverse at init time

Co-Authored-By: Jan Post <post@stud.tu-darmstadt.de>

* Horizon delay, to ease it in when returning sticks to centre

* fix unit tests after horizon changes

Co-Authored-By: 4712 <4712@users.noreply.github.com>

* horizon_delay_ms, default 500

* fix unit test for feedforward from setpointDelta

* Final optimisations - thanks @Karatebrot for your advice

* increase horizon level strength default to 75 now we have the delay

* restore Makefile value which allowed local make test on mac

---------

Co-authored-by: Jan Post <post@stud.tu-darmstadt.de>
Co-authored-by: 4712 <4712@users.noreply.github.com>
This commit is contained in:
ctzsnooze 2023-04-24 06:03:18 +10:00 • committed by GitHub
parent 445758f3ec
commit 34057bfbc2
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
19 changed files with 558 additions and 856 deletions
Download patch file Download diff file Expand all files Collapse all files

1
make/source.mk
View file

@ -89,7 +89,6 @@ COMMON_SRC = \
flight/gps_rescue.c \ flight/gps_rescue.c \
flight/dyn_notch_filter.c \ flight/dyn_notch_filter.c \
flight/imu.c \ flight/imu.c \
flight/feedforward.c \
flight/mixer.c \ flight/mixer.c \
flight/mixer_init.c \ flight/mixer_init.c \
flight/mixer_tricopter.c \ flight/mixer_tricopter.c \

12
src/main/blackbox/blackbox.c
View file

@ -1403,13 +1403,12 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_BOOST, "%d", currentPidProfile->feedforward_boost); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_BOOST, "%d", currentPidProfile->feedforward_boost);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_MAX_RATE_LIMIT, "%d", currentPidProfile->feedforward_max_rate_limit); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_MAX_RATE_LIMIT, "%d", currentPidProfile->feedforward_max_rate_limit);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_FEEDFORWARD, "%d", currentPidProfile->pid[PID_LEVEL].F); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_FEEDFORWARD, "%d", currentPidProfile->pid[PID_LEVEL].F);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_FF_SMOOTHING_MS, "%d", currentPidProfile->angle_feedforward_smoothing_ms); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_FF_SMOOTHING_MS, "%d", currentPidProfile->angle_feedforward_smoothing_ms);
#endif #endif
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_ROLL_EXPO, "%d", currentControlRateProfile->levelExpo[ROLL]);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_PITCH_EXPO, "%d", currentControlRateProfile->levelExpo[PITCH]);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_LIMIT, "%d", currentPidProfile->angle_limit); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_LIMIT, "%d", currentPidProfile->angle_limit);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_EARTH_REF, "%d", currentPidProfile->angle_earth_ref); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_EARTH_REF, "%d", currentPidProfile->angle_earth_ref);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HORIZON_LIMIT_DEGREES, "%d", currentPidProfile->horizon_limit_degrees); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HORIZON_LIMIT_DEGREES, "%d", currentPidProfile->horizon_limit_degrees);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HORIZON_DELAY_MS, "%d", currentPidProfile->horizon_delay_ms);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT_YAW, "%d", currentPidProfile->yawRateAccelLimit); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT_YAW, "%d", currentPidProfile->yawRateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT, "%d", currentPidProfile->rateAccelLimit); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT, "%d", currentPidProfile->rateAccelLimit);
@ -1481,14 +1480,15 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR, "%d", rxConfig()->rc_smoothing_auto_factor_rpy); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR, "%d", rxConfig()->rc_smoothing_auto_factor_rpy);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE, "%d", rxConfig()->rc_smoothing_auto_factor_throttle); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE, "%d", rxConfig()->rc_smoothing_auto_factor_throttle);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_FEEDFORWARD_CUTOFF, "%d", rcSmoothingData->ffCutoffSetting); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_FEEDFORWARD_CUTOFF, "%d", rcSmoothingData->feedforwardCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF, "%d", rcSmoothingData->setpointCutoffSetting); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF, "%d", rcSmoothingData->setpointCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF, "%d", rcSmoothingData->throttleCutoffSetting); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF, "%d", rcSmoothingData->throttleCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_DEBUG_AXIS, "%d", rcSmoothingData->debugAxis); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_DEBUG_AXIS, "%d", rcSmoothingData->debugAxis);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_ACTIVE_CUTOFFS, "%d,%d,%d", rcSmoothingData->feedforwardCutoffFrequency, BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_ACTIVE_CUTOFFS, "%d,%d,%d", rcSmoothingData->feedforwardCutoffFrequency,
rcSmoothingData->setpointCutoffFrequency, rcSmoothingData->setpointCutoffFrequency,
rcSmoothingData->throttleCutoffFrequency); rcSmoothingData->throttleCutoffFrequency);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingData->averageFrameTimeUs); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_smoothed", "%d", lrintf(rcSmoothingData->smoothedRxRateHz));
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RATES_TYPE, "%d", currentControlRateProfile->rates_type); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RATES_TYPE, "%d", currentControlRateProfile->rates_type);

12
src/main/cli/cli.c
View file

@ -4699,7 +4699,7 @@ static void cliStatus(const char *cmdName, char *cmdline)
// Run status // Run status
const int gyroRate = getTaskDeltaTimeUs(TASK_GYRO) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTimeUs(TASK_GYRO))); const int gyroRate = getTaskDeltaTimeUs(TASK_GYRO) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTimeUs(TASK_GYRO)));
int rxRate = getCurrentRxRefreshRate(); int rxRate = getCurrentRxIntervalUs();
if (rxRate != 0) { if (rxRate != 0) {
rxRate = (int)(1000000.0f / ((float)rxRate)); rxRate = (int)(1000000.0f / ((float)rxRate));
} }
@ -4845,12 +4845,12 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline)
if (rxConfig()->rc_smoothing_mode) { if (rxConfig()->rc_smoothing_mode) {
cliPrintLine("FILTER"); cliPrintLine("FILTER");
if (rcSmoothingAutoCalculate()) { if (rcSmoothingAutoCalculate()) {
const uint16_t avgRxFrameUs = rcSmoothingData->averageFrameTimeUs; const uint16_t smoothedRxRateHz = lrintf(rcSmoothingData->smoothedRxRateHz);
cliPrint("# Detected RX frame rate: "); cliPrint("# Detected Rx frequency: ");
if (avgRxFrameUs == 0) { if (getCurrentRxIntervalUs() == 0) {
cliPrintLine("NO SIGNAL"); cliPrintLine("NO SIGNAL");
} else { } else {
cliPrintLinef("%d.%03dms", avgRxFrameUs / 1000, avgRxFrameUs % 1000); cliPrintLinef("%dHz", smoothedRxRateHz);
} }
} }
cliPrintf("# Active setpoint cutoff: %dhz ", rcSmoothingData->setpointCutoffFrequency); cliPrintf("# Active setpoint cutoff: %dhz ", rcSmoothingData->setpointCutoffFrequency);
@ -4860,7 +4860,7 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline)
cliPrintLine("(auto)"); cliPrintLine("(auto)");
} }
cliPrintf("# Active FF cutoff: %dhz ", rcSmoothingData->feedforwardCutoffFrequency); cliPrintf("# Active FF cutoff: %dhz ", rcSmoothingData->feedforwardCutoffFrequency);
if (rcSmoothingData->ffCutoffSetting) { if (rcSmoothingData->feedforwardCutoffSetting) {
cliPrintLine("(manual)"); cliPrintLine("(manual)");
} else { } else {
cliPrintLine("(auto)"); cliPrintLine("(auto)");

7
src/main/cli/settings.c
View file

@ -979,8 +979,6 @@ const clivalue_t valueTable[] = {
{ "roll_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_ROLL]) }, { "roll_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_ROLL]) },
{ "pitch_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_PITCH]) }, { "pitch_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_PITCH]) },
{ "yaw_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_YAW]) }, { "yaw_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_YAW]) },
{ PARAM_NAME_ANGLE_ROLL_EXPO, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, levelExpo[FD_ROLL]) },
{ PARAM_NAME_ANGLE_PITCH_EXPO, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, levelExpo[FD_PITCH]) },
// PG_SERIAL_CONFIG // PG_SERIAL_CONFIG
{ "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) }, { "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) },
@ -1144,6 +1142,7 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_HORIZON_LIMIT_STICKS, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) }, { PARAM_NAME_HORIZON_LIMIT_STICKS, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) },
{ PARAM_NAME_HORIZON_LIMIT_DEGREES, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_limit_degrees) }, { PARAM_NAME_HORIZON_LIMIT_DEGREES, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_limit_degrees) },
{ PARAM_NAME_HORIZON_IGNORE_STICKS, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_ignore_sticks) }, { PARAM_NAME_HORIZON_IGNORE_STICKS, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_ignore_sticks) },
{ PARAM_NAME_HORIZON_DELAY_MS, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 5000 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_delay_ms) },
#if defined(USE_ABSOLUTE_CONTROL) #if defined(USE_ABSOLUTE_CONTROL)
{ PARAM_NAME_ABS_CONTROL_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) }, { PARAM_NAME_ABS_CONTROL_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) },
@ -1189,8 +1188,8 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_FEEDFORWARD_AVERAGING, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FEEDFORWARD_AVERAGING }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_averaging) }, { PARAM_NAME_FEEDFORWARD_AVERAGING, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FEEDFORWARD_AVERAGING }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_averaging) },
{ PARAM_NAME_FEEDFORWARD_SMOOTH_FACTOR, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 95}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_smooth_factor) }, { PARAM_NAME_FEEDFORWARD_SMOOTH_FACTOR, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 95}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_smooth_factor) },
{ PARAM_NAME_FEEDFORWARD_JITTER_FACTOR, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_jitter_factor) }, { PARAM_NAME_FEEDFORWARD_JITTER_FACTOR, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_jitter_factor) },
{ PARAM_NAME_FEEDFORWARD_BOOST, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_boost) }, { PARAM_NAME_FEEDFORWARD_BOOST, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_boost) },
{ PARAM_NAME_FEEDFORWARD_MAX_RATE_LIMIT, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_max_rate_limit) }, { PARAM_NAME_FEEDFORWARD_MAX_RATE_LIMIT, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 200}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_max_rate_limit) },
#endif #endif
#ifdef USE_DYN_IDLE #ifdef USE_DYN_IDLE

7
src/main/cms/cms_menu_imu.c
View file

@ -435,9 +435,6 @@ static const OSD_Entry cmsx_menuRateProfileEntries[] =
{ "THR LIM TYPE",OME_TAB, NULL, &(OSD_TAB_t) { &rateProfile.throttle_limit_type, THROTTLE_LIMIT_TYPE_COUNT - 1, osdTableThrottleLimitType} }, { "THR LIM TYPE",OME_TAB, NULL, &(OSD_TAB_t) { &rateProfile.throttle_limit_type, THROTTLE_LIMIT_TYPE_COUNT - 1, osdTableThrottleLimitType} },
{ "THR LIM %", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25, 100, 1} }, { "THR LIM %", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25, 100, 1} },
{ "ROLL LVL EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.levelExpo[FD_ROLL], 0, 100, 1, 10 } },
{ "PITCH LVL EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.levelExpo[FD_PITCH], 0, 100, 1, 10 } },
{ "BACK", OME_Back, NULL, NULL }, { "BACK", OME_Back, NULL, NULL },
{ NULL, OME_END, NULL, NULL} { NULL, OME_END, NULL, NULL}
}; };
@ -519,6 +516,7 @@ static CMS_Menu cmsx_menuLaunchControl = {
static uint8_t cmsx_angleP; static uint8_t cmsx_angleP;
static uint8_t cmsx_angleFF; static uint8_t cmsx_angleFF;
static uint8_t cmsx_angleLimit; static uint8_t cmsx_angleLimit;
static uint8_t cmsx_angleEarthRef;
static uint8_t cmsx_horizonStrength; static uint8_t cmsx_horizonStrength;
static uint8_t cmsx_horizonLimitSticks; static uint8_t cmsx_horizonLimitSticks;
@ -567,6 +565,7 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
cmsx_angleP = pidProfile->pid[PID_LEVEL].P; cmsx_angleP = pidProfile->pid[PID_LEVEL].P;
cmsx_angleFF = pidProfile->pid[PID_LEVEL].F; cmsx_angleFF = pidProfile->pid[PID_LEVEL].F;
cmsx_angleLimit = pidProfile->angle_limit; cmsx_angleLimit = pidProfile->angle_limit;
cmsx_angleEarthRef = pidProfile->angle_earth_ref;
cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I; cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I;
cmsx_horizonLimitSticks = pidProfile->pid[PID_LEVEL].D; cmsx_horizonLimitSticks = pidProfile->pid[PID_LEVEL].D;
@ -621,6 +620,7 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
pidProfile->pid[PID_LEVEL].P = cmsx_angleP; pidProfile->pid[PID_LEVEL].P = cmsx_angleP;
pidProfile->pid[PID_LEVEL].F = cmsx_angleFF; pidProfile->pid[PID_LEVEL].F = cmsx_angleFF;
pidProfile->angle_limit = cmsx_angleLimit; pidProfile->angle_limit = cmsx_angleLimit;
pidProfile->angle_earth_ref = cmsx_angleEarthRef;
pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength; pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength;
pidProfile->pid[PID_LEVEL].D = cmsx_horizonLimitSticks; pidProfile->pid[PID_LEVEL].D = cmsx_horizonLimitSticks;
@ -678,6 +678,7 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
{ "ANGLE P", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleP, 0, 200, 1 } }, { "ANGLE P", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleP, 0, 200, 1 } },
{ "ANGLE FF", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleFF, 0, 200, 1 } }, { "ANGLE FF", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleFF, 0, 200, 1 } },
{ "ANGLE LIMIT", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleLimit, 10, 90, 1 } }, { "ANGLE LIMIT", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleLimit, 10, 90, 1 } },
{ "ANGLE E_REF", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleEarthRef, 0, 100, 1 } },
{ "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 100, 1 } }, { "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 100, 1 } },
{ "HORZN LIM_STK", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonLimitSticks, 10, 200, 1 } }, { "HORZN LIM_STK", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonLimitSticks, 10, 200, 1 } },

4
src/main/fc/controlrate_profile.c
View file

@ -37,7 +37,7 @@
controlRateConfig_t *currentControlRateProfile; controlRateConfig_t *currentControlRateProfile;
PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 5); PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 6);
void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
{ {
@ -62,8 +62,6 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
.rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX, .rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
.profileName = { 0 }, .profileName = { 0 },
.quickRatesRcExpo = 0, .quickRatesRcExpo = 0,
.levelExpo[FD_ROLL] = 33,
.levelExpo[FD_PITCH] = 33,
); );
} }
} }

1
src/main/fc/controlrate_profile.h
View file

@ -60,7 +60,6 @@ typedef struct controlRateConfig_s {
uint16_t rate_limit[3]; // Sets the maximum rate for the axes uint16_t rate_limit[3]; // Sets the maximum rate for the axes
char profileName[MAX_RATE_PROFILE_NAME_LENGTH + 1]; // Descriptive name for rate profile char profileName[MAX_RATE_PROFILE_NAME_LENGTH + 1]; // Descriptive name for rate profile
uint8_t quickRatesRcExpo; // Sets expo on rc command for quick rates uint8_t quickRatesRcExpo; // Sets expo on rc command for quick rates
uint8_t levelExpo[2]; // roll/pitch level mode expo
} controlRateConfig_t; } controlRateConfig_t;
PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles);

3
src/main/fc/parameter_names.h
View file

@ -126,8 +126,6 @@
#define PARAM_NAME_POSITION_ALTITUDE_D_LPF "altitude_d_lpf" #define PARAM_NAME_POSITION_ALTITUDE_D_LPF "altitude_d_lpf"
#define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward" #define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward"
#define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms" #define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms"
#define PARAM_NAME_ANGLE_ROLL_EXPO "angle_roll_expo"
#define PARAM_NAME_ANGLE_PITCH_EXPO "angle_pitch_expo"
#define PARAM_NAME_ANGLE_LIMIT "angle_limit" #define PARAM_NAME_ANGLE_LIMIT "angle_limit"
#define PARAM_NAME_ANGLE_P_GAIN "angle_p_gain" #define PARAM_NAME_ANGLE_P_GAIN "angle_p_gain"
#define PARAM_NAME_ANGLE_EARTH_REF "angle_earth_ref" #define PARAM_NAME_ANGLE_EARTH_REF "angle_earth_ref"
@ -136,6 +134,7 @@
#define PARAM_NAME_HORIZON_LIMIT_DEGREES "horizon_limit_degrees" #define PARAM_NAME_HORIZON_LIMIT_DEGREES "horizon_limit_degrees"
#define PARAM_NAME_HORIZON_LIMIT_STICKS "horizon_limit_sticks" #define PARAM_NAME_HORIZON_LIMIT_STICKS "horizon_limit_sticks"
#define PARAM_NAME_HORIZON_IGNORE_STICKS "horizon_ignore_sticks" #define PARAM_NAME_HORIZON_IGNORE_STICKS "horizon_ignore_sticks"
#define PARAM_NAME_HORIZON_DELAY_MS "horizon_delay_ms"
#ifdef USE_GPS #ifdef USE_GPS
#define PARAM_NAME_GPS_PROVIDER "gps_provider" #define PARAM_NAME_GPS_PROVIDER "gps_provider"

517
src/main/fc/rc.c
View file

@ -42,8 +42,8 @@
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/feedforward.h"
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#include "flight/pid.h"
#include "flight/pid_init.h" #include "flight/pid_init.h"
#include "pg/rx.h" #include "pg/rx.h"
@ -57,24 +57,21 @@
typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs); typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs);
// note that rcCommand[] is an external float
#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 rawSetpoint[XYZ_AXIS_COUNT];
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; // deflection range -1 to 1
static bool reverseMotors = false; static bool reverseMotors = false;
static applyRatesFn *applyRates; static applyRatesFn *applyRates;
static uint16_t currentRxRefreshRate;
static uint16_t currentRxIntervalUs; // packet interval in microseconds
static float currentRxRateHz; // packet rate in hertz
static bool isRxDataNew = false; static bool isRxDataNew = false;
static bool isRxRateValid = false; static bool isRxIntervalValid = false;
static float rcCommandDivider = 500.0f; static float rcCommandDivider = 500.0f;
static float rcCommandYawDivider = 500.0f; static float rcCommandYawDivider = 500.0f;
static FAST_DATA_ZERO_INIT bool newRxDataForFF;
enum { enum {
ROLL_FLAG = 1 << ROLL, ROLL_FLAG = 1 << ROLL,
PITCH_FLAG = 1 << PITCH, PITCH_FLAG = 1 << PITCH,
@ -82,32 +79,32 @@ enum {
THROTTLE_FLAG = 1 << THROTTLE, THROTTLE_FLAG = 1 << THROTTLE,
}; };
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_FEEDFORWARD
#define RC_SMOOTHING_CUTOFF_MIN_HZ 15 // Minimum rc smoothing cutoff frequency static float feedforwardSmoothed[3];
#define RC_SMOOTHING_FILTER_STARTUP_DELAY_MS 5000 // Time to wait after power to let the PID loop stabilize before starting average frame rate calculation static float feedforwardRaw[3];
#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 // Number of rx frame rate samples to average during initial training typedef struct laggedMovingAverageCombined_s {
#define RC_SMOOTHING_FILTER_RETRAINING_SAMPLES 20 // Number of rx frame rate samples to average during frame rate changes laggedMovingAverage_t filter;
#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 1000 // Additional time to wait after receiving first valid rx frame before initial training starts float buf[4];
#define RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS 2000 // Guard time to wait after retraining to prevent retraining again too quickly } laggedMovingAverageCombined_t;
#define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT 20 // Look for samples varying this much from the current detected frame rate to initiate retraining laggedMovingAverageCombined_t feedforwardDeltaAvg[XYZ_AXIS_COUNT];
#define RC_SMOOTHING_FEEDFORWARD_INITIAL_HZ 100 // The value to use for "auto" when interpolated feedforward is enabled
float getFeedforward(int axis)
{
#ifdef USE_RC_SMOOTHING_FILTER
return feedforwardSmoothed[axis];
#else
return feedforwardRaw[axis];
#endif
}
#endif // USE_FEEDFORWARD
#ifdef USE_RC_SMOOTHING_FILTER
static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData; static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
static float rcDeflectionSmoothed[3]; static float rcDeflectionSmoothed[3];
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
#define RC_RX_RATE_MIN_US 950 // 0.950ms to fit 1kHz without an issue #define RX_INTERVAL_MIN_US 950 // 0.950ms to fit 1kHz without an issue
#define RC_RX_RATE_MAX_US 65500 // 65.5ms or 15.26hz #define RX_INTERVAL_MAX_US 65500 // 65.5ms or 15.26hz
bool getShouldUpdateFeedforward(void)
// only used in pid.c, when feedforward is enabled, to initiate a new FF value
{
const bool updateFf = newRxDataForFF;
if (newRxDataForFF == true){
newRxDataForFF = false;
}
return updateFf;
}
float getSetpointRate(int axis) float getSetpointRate(int axis)
{ {
@ -118,6 +115,12 @@ float getSetpointRate(int axis)
#endif #endif
} }
static float maxRcRate[3];
float getMaxRcRate(int axis)
{
return maxRcRate[axis];
}
float getRcDeflection(int axis) float getRcDeflection(int axis)
{ {
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
@ -137,23 +140,6 @@ float getRcDeflectionAbs(int axis)
return rcDeflectionAbs[axis]; return rcDeflectionAbs[axis];
} }
#ifdef USE_FEEDFORWARD
float getRawSetpoint(int axis)
{
return rawSetpoint[axis];
}
float getRcCommandDelta(int axis)
{
return rcCommandDelta[axis];
}
bool getRxRateValid(void)
{
return isRxRateValid;
}
#endif
#define THROTTLE_LOOKUP_LENGTH 12 #define THROTTLE_LOOKUP_LENGTH 12
static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
@ -164,8 +150,9 @@ static int16_t rcLookupThrottle(int32_t tmp)
return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;
} }
#define SETPOINT_RATE_LIMIT 1998 #define SETPOINT_RATE_LIMIT_MIN -1998.0f
STATIC_ASSERT(CONTROL_RATE_CONFIG_RATE_LIMIT_MAX <= SETPOINT_RATE_LIMIT, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX_too_large); #define SETPOINT_RATE_LIMIT_MAX 1998.0f
STATIC_ASSERT(CONTROL_RATE_CONFIG_RATE_LIMIT_MAX <= (uint16_t)SETPOINT_RATE_LIMIT_MAX, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX_too_large);
#define RC_RATE_INCREMENTAL 14.54f #define RC_RATE_INCREMENTAL 14.54f
@ -206,7 +193,7 @@ float applyKissRates(const int axis, float rcCommandf, const float rcCommandfAbs
float kissRpyUseRates = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); float kissRpyUseRates = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
float kissRcCommandf = (power3(rcCommandf) * rcCurvef + rcCommandf * (1 - rcCurvef)) * (currentControlRateProfile->rcRates[axis] / 1000.0f); float kissRcCommandf = (power3(rcCommandf) * rcCurvef + rcCommandf * (1 - rcCurvef)) * (currentControlRateProfile->rcRates[axis] / 1000.0f);
float kissAngle = constrainf(((2000.0f * kissRpyUseRates) * kissRcCommandf), -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); float kissAngle = constrainf(((2000.0f * kissRpyUseRates) * kissRcCommandf), SETPOINT_RATE_LIMIT_MIN, SETPOINT_RATE_LIMIT_MAX);
return kissAngle; return kissAngle;
} }
@ -237,21 +224,16 @@ float applyQuickRates(const int axis, float rcCommandf, const float rcCommandfAb
if (currentControlRateProfile->quickRatesRcExpo) { if (currentControlRateProfile->quickRatesRcExpo) {
curve = power3(rcCommandf) * expof + rcCommandf * (1 - expof); curve = power3(rcCommandf) * expof + rcCommandf * (1 - expof);
superFactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * superFactorConfig), 0.01f, 1.00f)); superFactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * superFactorConfig), 0.01f, 1.00f));
angleRate = constrainf(curve * rcRate * superFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); angleRate = constrainf(curve * rcRate * superFactor, SETPOINT_RATE_LIMIT_MIN, SETPOINT_RATE_LIMIT_MAX);
} else { } else {
curve = power3(rcCommandfAbs) * expof + rcCommandfAbs * (1 - expof); curve = power3(rcCommandfAbs) * expof + rcCommandfAbs * (1 - expof);
superFactor = 1.0f / (constrainf(1.0f - (curve * superFactorConfig), 0.01f, 1.00f)); superFactor = 1.0f / (constrainf(1.0f - (curve * superFactorConfig), 0.01f, 1.00f));
angleRate = constrainf(rcCommandf * rcRate * superFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); angleRate = constrainf(rcCommandf * rcRate * superFactor, SETPOINT_RATE_LIMIT_MIN, SETPOINT_RATE_LIMIT_MAX);
} }
return angleRate; return angleRate;
} }
float applyCurve(int axis, float deflection)
{
return applyRates(axis, deflection, fabsf(deflection));
}
static void scaleRawSetpointToFpvCamAngle(void) static void scaleRawSetpointToFpvCamAngle(void)
{ {
//recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
@ -267,8 +249,8 @@ static void scaleRawSetpointToFpvCamAngle(void)
float roll = rawSetpoint[ROLL]; float roll = rawSetpoint[ROLL];
float yaw = rawSetpoint[YAW]; float yaw = rawSetpoint[YAW];
rawSetpoint[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f); rawSetpoint[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, SETPOINT_RATE_LIMIT_MIN, SETPOINT_RATE_LIMIT_MAX);
rawSetpoint[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f); rawSetpoint[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, SETPOINT_RATE_LIMIT_MIN, SETPOINT_RATE_LIMIT_MAX);
} }
void updateRcRefreshRate(timeUs_t currentTimeUs) void updateRcRefreshRate(timeUs_t currentTimeUs)
@ -279,116 +261,91 @@ void updateRcRefreshRate(timeUs_t currentTimeUs)
timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs); timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs);
if (!frameDeltaUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) { if (!frameDeltaUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
frameDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol frameDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs);
} }
DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX)); DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX));
DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX)); DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX));
lastRxTimeUs = currentTimeUs; lastRxTimeUs = currentTimeUs;
isRxRateValid = (frameDeltaUs >= RC_RX_RATE_MIN_US && frameDeltaUs <= RC_RX_RATE_MAX_US); currentRxIntervalUs = constrain(frameDeltaUs, RX_INTERVAL_MIN_US, RX_INTERVAL_MAX_US);
currentRxRefreshRate = constrain(frameDeltaUs, RC_RX_RATE_MIN_US, RC_RX_RATE_MAX_US); isRxIntervalValid = frameDeltaUs == currentRxIntervalUs;
currentRxRateHz = 1e6f / currentRxIntervalUs; // cannot be zero due to preceding constraint
DEBUG_SET(DEBUG_RX_TIMING, 2, isRxIntervalValid);
DEBUG_SET(DEBUG_RX_TIMING, 3, MIN(currentRxIntervalUs / 10, INT16_MAX));
} }
uint16_t getCurrentRxRefreshRate(void) uint16_t getCurrentRxIntervalUs(void)
{ {
return currentRxRefreshRate; return currentRxIntervalUs;
} }
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
// 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));
float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)); // link frequency
cutoff = cutoff * cutoffFactor;
return lrintf(cutoff);
} else {
return 0;
}
}
// Initialize or update the filters base on either the manually selected cutoff, or // Initialize or update the filters base on either the manually selected cutoff, or
// the auto-calculated cutoff frequency based on detected rx frame rate. // the auto-calculated cutoff frequency based on detected rx frame rate.
FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData) FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
{ {
const float dT = targetPidLooptime * 1e-6f; // in auto mode, calculate the RC smoothing cutoff from the smoothed Rx link frequency
uint16_t oldCutoff = smoothingData->setpointCutoffFrequency; const uint16_t oldSetpointCutoff = smoothingData->setpointCutoffFrequency;
const uint16_t oldFeedforwardCutoff = smoothingData->feedforwardCutoffFrequency;
const uint16_t minCutoffHz = 15; // don't let any RC smoothing filter cutoff go below 15Hz
if (smoothingData->setpointCutoffSetting == 0) { if (smoothingData->setpointCutoffSetting == 0) {
smoothingData->setpointCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint)); smoothingData->setpointCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint));
} }
if (smoothingData->throttleCutoffSetting == 0) { if (smoothingData->throttleCutoffSetting == 0) {
smoothingData->throttleCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle)); smoothingData->throttleCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorThrottle));
} }
// initialize or update the Setpoint filter if (smoothingData->feedforwardCutoffSetting == 0) {
if ((smoothingData->setpointCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) { smoothingData->feedforwardCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorFeedforward));
}
const float dT = targetPidLooptime * 1e-6f;
if ((smoothingData->setpointCutoffFrequency != oldSetpointCutoff) || !smoothingData->filterInitialized) {
// note that cutoff frequencies are integers, filter cutoffs won't re-calculate until there is > 1hz variation from previous cutoff
// initialize or update the setpoint cutoff based filters
const float setpointCutoffFrequency = smoothingData->setpointCutoffFrequency;
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
if (i < THROTTLE) { // Throttle handled by smoothing rcCommand if (i < THROTTLE) {
if (!smoothingData->filterInitialized) { if (!smoothingData->filterInitialized) {
pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT)); pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT));
} else { } else {
pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT)); pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT));
} }
} else { } else {
const float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency;
if (!smoothingData->filterInitialized) { if (!smoothingData->filterInitialized) {
pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT)); pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(throttleCutoffFrequency, dT));
} else { } else {
pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT)); pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3FilterGain(throttleCutoffFrequency, dT));
} }
} }
} }
// initialize or update the RC Deflection filter
// initialize or update the Level filter
for (int i = FD_ROLL; i < FD_YAW; i++) { for (int i = FD_ROLL; i < FD_YAW; i++) {
if (!smoothingData->filterInitialized) { if (!smoothingData->filterInitialized) {
pt3FilterInit(&smoothingData->filterDeflection[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT)); pt3FilterInit(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT));
} else { } else {
pt3FilterUpdateCutoff(&smoothingData->filterDeflection[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT)); pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT));
}
}
}
// initialize or update the Feedforward filter
if ((smoothingData->feedforwardCutoffFrequency != oldFeedforwardCutoff) || !smoothingData->filterInitialized) {
for (int i = FD_ROLL; i <= FD_YAW; i++) {
const float feedforwardCutoffFrequency = smoothingData->feedforwardCutoffFrequency;
if (!smoothingData->filterInitialized) {
pt3FilterInit(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT));
} else {
pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT));
} }
} }
} }
// update or initialize the FF filter DEBUG_SET(DEBUG_RC_SMOOTHING, 1, smoothingData->setpointCutoffFrequency);
oldCutoff = smoothingData->feedforwardCutoffFrequency; DEBUG_SET(DEBUG_RC_SMOOTHING, 2, smoothingData->feedforwardCutoffFrequency);
if (rcSmoothingData.ffCutoffSetting == 0) {
smoothingData->feedforwardCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
}
if (!smoothingData->filterInitialized) {
pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis);
} else if (smoothingData->feedforwardCutoffFrequency != oldCutoff) {
pidUpdateFeedforwardLpf(smoothingData->feedforwardCutoffFrequency);
}
}
FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(rcSmoothingFilter_t *smoothingData)
{
smoothingData->training.sum = 0;
smoothingData->training.count = 0;
smoothingData->training.min = UINT16_MAX;
smoothingData->training.max = 0;
}
// Accumulate the rx frame time samples. Once we've collected enough samples calculate the
// average and return true.
static FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothingData, int rxFrameTimeUs)
{
smoothingData->training.sum += rxFrameTimeUs;
smoothingData->training.count++;
smoothingData->training.max = MAX(smoothingData->training.max, rxFrameTimeUs);
smoothingData->training.min = MIN(smoothingData->training.min, rxFrameTimeUs);
// if we've collected enough samples then calculate the average and reset the accumulation
const int sampleLimit = (rcSmoothingData.filterInitialized) ? RC_SMOOTHING_FILTER_RETRAINING_SAMPLES : RC_SMOOTHING_FILTER_TRAINING_SAMPLES;
if (smoothingData->training.count >= sampleLimit) {
smoothingData->training.sum = smoothingData->training.sum - smoothingData->training.min - smoothingData->training.max; // Throw out high and low samples
smoothingData->averageFrameTimeUs = lrintf(smoothingData->training.sum / (smoothingData->training.count - 2));
rcSmoothingResetAccumulation(smoothingData);
return true;
}
return false;
} }
// Determine if we need to caclulate filter cutoffs. If not then we can avoid // Determine if we need to caclulate filter cutoffs. If not then we can avoid
@ -396,7 +353,7 @@ static FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothing
FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void) FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
{ {
// if any rc smoothing cutoff is 0 (auto) then we need to calculate cutoffs // if any rc smoothing cutoff is 0 (auto) then we need to calculate cutoffs
if ((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.ffCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) { if ((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.feedforwardCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) {
return true; return true;
} }
return false; return false;
@ -406,35 +363,30 @@ static FAST_CODE void processRcSmoothingFilter(void)
{ {
static FAST_DATA_ZERO_INIT float rxDataToSmooth[4]; static FAST_DATA_ZERO_INIT float rxDataToSmooth[4];
static FAST_DATA_ZERO_INIT bool initialized; static FAST_DATA_ZERO_INIT bool initialized;
static FAST_DATA_ZERO_INIT timeMs_t validRxFrameTimeMs;
static FAST_DATA_ZERO_INIT bool calculateCutoffs; static FAST_DATA_ZERO_INIT bool calculateCutoffs;
// first call initialization // first call initialization
if (!initialized) { if (!initialized) {
initialized = true; initialized = true;
rcSmoothingData.filterInitialized = false; rcSmoothingData.filterInitialized = false;
rcSmoothingData.averageFrameTimeUs = 0; rcSmoothingData.smoothedRxRateHz = 0.0f;
rcSmoothingData.autoSmoothnessFactorSetpoint = rxConfig()->rc_smoothing_auto_factor_rpy; rcSmoothingData.sampleCount = 0;
rcSmoothingData.autoSmoothnessFactorThrottle = rxConfig()->rc_smoothing_auto_factor_throttle;
rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis; rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
rcSmoothingData.autoSmoothnessFactorSetpoint = 1.5f / (1.0f + (rxConfig()->rc_smoothing_auto_factor_rpy / 10.0f));
rcSmoothingData.autoSmoothnessFactorFeedforward = 1.5f / (1.0f + (rxConfig()->rc_smoothing_auto_factor_rpy / 10.0f));
rcSmoothingData.autoSmoothnessFactorThrottle = 1.5f / (1.0f + (rxConfig()->rc_smoothing_auto_factor_throttle / 10.0f));
rcSmoothingData.setpointCutoffSetting = rxConfig()->rc_smoothing_setpoint_cutoff; rcSmoothingData.setpointCutoffSetting = rxConfig()->rc_smoothing_setpoint_cutoff;
rcSmoothingData.throttleCutoffSetting = rxConfig()->rc_smoothing_throttle_cutoff; rcSmoothingData.throttleCutoffSetting = rxConfig()->rc_smoothing_throttle_cutoff;
rcSmoothingData.ffCutoffSetting = rxConfig()->rc_smoothing_feedforward_cutoff; rcSmoothingData.feedforwardCutoffSetting = rxConfig()->rc_smoothing_feedforward_cutoff;
rcSmoothingResetAccumulation(&rcSmoothingData);
rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting; rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting;
rcSmoothingData.feedforwardCutoffFrequency = rcSmoothingData.feedforwardCutoffSetting;
rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting; 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.feedforwardCutoffFrequency = rcSmoothingData.ffCutoffSetting;
}
if (rxConfig()->rc_smoothing_mode) { if (rxConfig()->rc_smoothing_mode) {
calculateCutoffs = rcSmoothingAutoCalculate(); calculateCutoffs = rcSmoothingAutoCalculate();
// if we don't need to calculate cutoffs dynamically then the filters can be initialized now // if we don't need to calculate cutoffs dynamically then the filters can be initialized now
if (!calculateCutoffs) { if (!calculateCutoffs) {
rcSmoothingSetFilterCutoffs(&rcSmoothingData); rcSmoothingSetFilterCutoffs(&rcSmoothingData);
@ -444,64 +396,51 @@ static FAST_CODE void processRcSmoothingFilter(void)
} }
if (isRxDataNew) { if (isRxDataNew) {
// for auto calculated filters we need to examine each rx frame interval
if (calculateCutoffs) { if (calculateCutoffs) {
// for auto calculated filters, calculate the link interval and update the RC smoothing filters at regular intervals
// this is more efficient than monitoring for significant changes and making comparisons to decide whether to update the filter
const timeMs_t currentTimeMs = millis(); const timeMs_t currentTimeMs = millis();
int sampleState = 0; int sampleState = 0;
const bool ready = (currentTimeMs > 1000) && (targetPidLooptime > 0);
if (ready) { // skip during FC initialization
// Wait 1000ms after power to let the PID loop stabilize before starting average frame rate calculation
if (rxIsReceivingSignal() && isRxIntervalValid) {
// If the filter cutoffs in auto mode, and we have good rx data, then determine the average rx frame rate static uint16_t previousRxIntervalUs;
// and use that to calculate the filter cutoff frequencies if (abs(currentRxIntervalUs - previousRxIntervalUs) < (previousRxIntervalUs - (previousRxIntervalUs / 8))) {
if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization // exclude large steps, eg after dropouts or telemetry
if (rxIsReceivingSignal() && isRxRateValid) { // by using interval here, we catch a dropout/telemetry where the inteval increases by 100%, but accept
// the return to normal value, which is only 50% different from the 100% interval of a single drop, and 66% of a return after a double drop.
// set the guard time expiration if it's not set static float prevRxRateHz;
if (validRxFrameTimeMs == 0) { // smooth the current Rx link frequency estimates
validRxFrameTimeMs = currentTimeMs + (rcSmoothingData.filterInitialized ? RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS : RC_SMOOTHING_FILTER_TRAINING_DELAY_MS); const float kF = 0.1f; // first order lowpass smoothing filter coefficient
} else { const float smoothedRxRateHz = prevRxRateHz + kF * (currentRxRateHz - prevRxRateHz);
sampleState = 1; prevRxRateHz = smoothedRxRateHz;
}
// recalculate cutoffs every 3 acceptable samples
// if the guard time has expired then process the rx frame time if (rcSmoothingData.sampleCount) {
if (currentTimeMs > validRxFrameTimeMs) { rcSmoothingData.sampleCount --;
sampleState = 2; sampleState = 1;
bool accumulateSample = true; } else {
rcSmoothingData.smoothedRxRateHz = smoothedRxRateHz;
// During initial training process all samples. rcSmoothingSetFilterCutoffs(&rcSmoothingData);
// During retraining check samples to determine if they vary by more than the limit percentage. rcSmoothingData.filterInitialized = true;
if (rcSmoothingData.filterInitialized) { rcSmoothingData.sampleCount = 3;
const float percentChange = (abs(currentRxRefreshRate - rcSmoothingData.averageFrameTimeUs) / (float)rcSmoothingData.averageFrameTimeUs) * 100; sampleState = 2;
if (percentChange < RC_SMOOTHING_RX_RATE_CHANGE_PERCENT) {
// We received a sample that wasn't more than the limit percent so reset the accumulation
// During retraining we need a contiguous block of samples that are all significantly different than the current average
rcSmoothingResetAccumulation(&rcSmoothingData);
accumulateSample = false;
}
} }
// accumlate the sample into the average
if (accumulateSample) {
if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) {
// 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;
}
}
} }
previousRxIntervalUs = currentRxIntervalUs;
} else { } else {
// we have either stopped receiving rx samples (failsafe?) or the sample time is unreasonable so reset the accumulation // either we stopped receiving rx samples (failsafe?) or the sample interval is unreasonable
rcSmoothingResetAccumulation(&rcSmoothingData); // require a full re-evaluation period after signal is restored
rcSmoothingData.sampleCount = 0;
sampleState = 4;
} }
} }
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxIntervalUs / 10);
// rx frame rate training blackbox debugging DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.sampleCount);
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxRefreshRate); // log each rx frame interval DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.smoothedRxRateHz); // value used by filters
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.training.count); // log the training step count DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); // guard time = 1, guard time expired = 2
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.averageFrameTimeUs);// the current calculated average
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); // indicates whether guard time is active
} }
// Get new values to be smoothed // Get new values to be smoothed
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
@ -514,48 +453,175 @@ static FAST_CODE void processRcSmoothingFilter(void)
} }
} }
if (rcSmoothingData.filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) { DEBUG_SET(DEBUG_RC_SMOOTHING, 0, rcSmoothingData.smoothedRxRateHz);
// after training has completed then log the raw rc channel and the calculated DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.sampleCount);
// average rx frame rate that was used to calculate the automatic filter cutoffs
DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs);
}
// each pid loop, apply the last received channel value to the filter, if initialised - thanks @klutvott // 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++) { for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
float *dst = i == THROTTLE ? &rcCommand[i] : &setpointRate[i]; float *dst = i == THROTTLE ? &rcCommand[i] : &setpointRate[i];
if (rcSmoothingData.filterInitialized) { if (rcSmoothingData.filterInitialized) {
*dst = pt3FilterApply(&rcSmoothingData.filter[i], rxDataToSmooth[i]); *dst = pt3FilterApply(&rcSmoothingData.filterSetpoint[i], rxDataToSmooth[i]);
} else { } else {
// If filter isn't initialized yet, as in smoothing off, use the actual unsmoothed rx channel data // If filter isn't initialized yet, as in smoothing off, use the actual unsmoothed rx channel data
*dst = rxDataToSmooth[i]; *dst = rxDataToSmooth[i];
} }
} }
// for ANGLE and HORIZON, smooth rcDeflection on pitch and roll to avoid setpoint steps
bool smoothingNeeded = (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && rcSmoothingData.filterInitialized;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
if (smoothingNeeded && axis < FD_YAW) { // Feedforward smoothing
rcDeflectionSmoothed[axis] = pt3FilterApply(&rcSmoothingData.filterDeflection[axis], rcDeflection[axis]); feedforwardSmoothed[axis] = pt3FilterApply(&rcSmoothingData.filterFeedforward[axis], feedforwardRaw[axis]);
// Horizon mode smoothing of rcDeflection on pitch and roll to provide a smooth angle element
const bool smoothRcDeflection = FLIGHT_MODE(HORIZON_MODE) && rcSmoothingData.filterInitialized;
if (smoothRcDeflection && axis < FD_YAW) {
rcDeflectionSmoothed[axis] = pt3FilterApply(&rcSmoothingData.filterRcDeflection[axis], rcDeflection[axis]);
} else { } else {
rcDeflectionSmoothed[axis] = rcDeflection[axis]; rcDeflectionSmoothed[axis] = rcDeflection[axis];
} }
} }
} }
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
NOINLINE void initAveraging(uint16_t feedforwardAveraging)
{
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
laggedMovingAverageInit(&feedforwardDeltaAvg[i].filter, feedforwardAveraging + 1, (float *)&feedforwardDeltaAvg[i].buf[0]);
}
}
FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, int axis)
{
const float rxInterval = currentRxIntervalUs * 1e-6f; // seconds
float rxRate = currentRxRateHz;
static float prevRxInterval;
static float prevRcCommand[3];
static float prevRcCommandDeltaAbs[3]; // for duplicate interpolation
static float prevSetpoint[3]; // equals raw unless interpolated
static float prevSetpointSpeed[3];
static float prevAcceleration[3]; // for duplicate interpolation
static bool prevDuplicatePacket[3]; // to identify multiple identical packets
static uint16_t feedforwardAveraging = 0;
if (feedforwardAveraging != pid->feedforwardAveraging) {
feedforwardAveraging = pid->feedforwardAveraging;
initAveraging(feedforwardAveraging);
}
const float rcCommandDeltaAbs = fabsf(rcCommand[axis] - prevRcCommand[axis]);
prevRcCommand[axis] = rcCommand[axis];
float setpoint = rawSetpoint[axis];
float setpointSpeed = (setpoint - prevSetpoint[axis]);
prevSetpoint[axis] = setpoint;
float setpointAcceleration = 0.0f;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD, 0, lrintf(setpoint * 10.0f)); // un-smoothed final feedforward
}
// attenuators
float zeroTheAcceleration = 1.0f;
float jitterAttenuator = 1.0f;
if (pid->feedforwardJitterFactor) {
if (rcCommandDeltaAbs < pid->feedforwardJitterFactor) {
jitterAttenuator = MAX(1.0f - (rcCommandDeltaAbs + prevRcCommandDeltaAbs[axis]) * pid->feedforwardJitterFactorInv, 0.0f);
// note that feedforwardJitterFactorInv includes a divide by 2 to average the two previous rcCommandDeltaAbs values
jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator;
}
}
prevRcCommandDeltaAbs[axis] = rcCommandDeltaAbs;
// interpolate setpoint if necessary
if (rcCommandDeltaAbs) {
// movement!
if (prevDuplicatePacket[axis] == true) {
rxRate = 1.0f / (rxInterval + prevRxInterval);
zeroTheAcceleration = 0.0f;
// don't add acceleration, empirically seems better on FrSky
}
setpointSpeed *= rxRate;
prevDuplicatePacket[axis] = false;
} else {
// no movement!
if (prevDuplicatePacket[axis] == false) {
// first duplicate after movement
setpointSpeed = prevSetpointSpeed[axis];
if (fabsf(setpoint) < 0.95f * maxRcRate[axis]) {
setpointSpeed += prevAcceleration[axis];
}
zeroTheAcceleration = 0.0f; // force acceleration to zero
} else {
// second and subsequent duplicates after movement should be zeroed
setpointSpeed = 0.0f;
prevSetpointSpeed[axis] = 0.0f;
zeroTheAcceleration = 0.0f; // force acceleration to zero
}
prevDuplicatePacket[axis] = true;
}
prevRxInterval = rxInterval;
// smooth the setpointSpeed value
setpointSpeed = prevSetpointSpeed[axis] + pid->feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
// calculate acceleration and attenuate
setpointAcceleration = (setpointSpeed - prevSetpointSpeed[axis]) * rxRate * 0.01f;
prevSetpointSpeed[axis] = setpointSpeed;
// smooth the acceleration element (effectively a second order filter) and apply jitter reduction
setpointAcceleration = prevAcceleration[axis] + pid->feedforwardSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
prevAcceleration[axis] = setpointAcceleration * zeroTheAcceleration;
setpointAcceleration = setpointAcceleration * pid->feedforwardBoostFactor * jitterAttenuator * zeroTheAcceleration;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD, 1, lrintf(setpointSpeed * 0.1f)); // base feedforward without acceleration
}
float feedforward = setpointSpeed + setpointAcceleration;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD, 2, lrintf(feedforward * 0.1f));
// un-smoothed feedforward including acceleration but before limiting, transition, averaging, and jitter reduction
}
// apply feedforward transition
const bool useTransition = (pid->feedforwardTransition != 0.0f) && (rcDeflectionAbs[axis] < pid->feedforwardTransition);
if (useTransition) {
feedforward *= rcDeflectionAbs[axis] * pid->feedforwardTransitionInv;
}
// apply averaging
if (feedforwardAveraging) {
feedforward = laggedMovingAverageUpdate(&feedforwardDeltaAvg[axis].filter, feedforward);
}
// apply jitter reduction
feedforward *= jitterAttenuator;
// apply max rate limiting
if (pid->feedforwardMaxRateLimit && axis < FD_YAW) {
if (feedforward * setpoint > 0.0f) { // in same direction
const float limit = (maxRcRate[axis] - fabsf(setpoint)) * pid->feedforwardMaxRateLimit;
feedforward = (limit > 0.0f) ? constrainf(feedforward, -limit, limit) : 0.0f;
}
}
feedforwardRaw[axis] = feedforward;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(feedforwardRaw[axis] * 0.1f)); // un-smoothed final feedforward
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 0, lrintf(jitterAttenuator * 100.0f)); // un-smoothed final feedforward
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 1, lrintf(maxRcRate[axis])); // un-smoothed final feedforward
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 2, lrintf(setpoint)); // un-smoothed final feedforward
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 3, lrintf(feedforwardRaw[axis])); // un-smoothed final feedforward
}
}
FAST_CODE void processRcCommand(void) FAST_CODE void processRcCommand(void)
{ {
if (isRxDataNew) { if (isRxDataNew) {
newRxDataForFF = true;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
#ifdef USE_FEEDFORWARD
isDuplicate[axis] = (oldRcCommand[axis] == rcCommand[axis]);
rcCommandDelta[axis] = (rcCommand[axis] - oldRcCommand[axis]);
oldRcCommand[axis] = rcCommand[axis];
#endif
float angleRate; float angleRate;
#ifdef USE_GPS_RESCUE #ifdef USE_GPS_RESCUE
@ -582,12 +648,17 @@ FAST_CODE void processRcCommand(void)
rcDeflectionAbs[axis] = rcCommandfAbs; rcDeflectionAbs[axis] = rcCommandfAbs;
angleRate = applyRates(axis, rcCommandf, rcCommandfAbs); angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
} }
rawSetpoint[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]); rawSetpoint[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]);
DEBUG_SET(DEBUG_ANGLERATE, axis, lrintf(angleRate)); DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
#ifdef USE_FEEDFORWARD
calculateFeedforward(&pidRuntime, axis);
#endif // USE_FEEDFORWARD
} }
// adjust raw setpoint steps to camera angle (mixing Roll and Yaw) // adjust unfiltered setpoint steps to camera angle (mixing Roll and Yaw)
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) { if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleRawSetpointToFpvCamAngle(); scaleRawSetpointToFpvCamAngle();
} }
@ -726,8 +797,16 @@ void initRcProcessing(void)
break; break;
} }
for (int i = 0; i < 3; i++) {
maxRcRate[i] = applyRates(i, 1.0f, 1.0f);
#ifdef USE_FEEDFORWARD
feedforwardSmoothed[i] = 0.0f;
feedforwardRaw[i] = 0.0f;
#endif // USE_FEEDFORWARD
}
#ifdef USE_YAW_SPIN_RECOVERY #ifdef USE_YAW_SPIN_RECOVERY
const int maxYawRate = (int)applyRates(FD_YAW, 1.0f, 1.0f); const int maxYawRate = (int)maxRcRate[FD_YAW];
initYawSpinRecovery(maxYawRate); initYawSpinRecovery(maxYawRate);
#endif #endif
} }

10
src/main/fc/rc.h
View file

@ -41,10 +41,10 @@ bool isMotorsReversed(void);
rcSmoothingFilter_t *getRcSmoothingData(void); rcSmoothingFilter_t *getRcSmoothingData(void);
bool rcSmoothingAutoCalculate(void); bool rcSmoothingAutoCalculate(void);
bool rcSmoothingInitializationComplete(void); bool rcSmoothingInitializationComplete(void);
float getRawSetpoint(int axis);
float getRcCommandDelta(int axis); float getMaxRcRate(int axis);
float applyCurve(int axis, float deflection); float getFeedforward(int axis);
bool getShouldUpdateFeedforward();
void updateRcRefreshRate(timeUs_t currentTimeUs); void updateRcRefreshRate(timeUs_t currentTimeUs);
uint16_t getCurrentRxRefreshRate(void); uint16_t getCurrentRxIntervalUs(void);
bool getRxRateValid(void); bool getRxRateValid(void);

27
src/main/fc/rc_controls.h
View file

@ -84,28 +84,27 @@ typedef enum {
extern float rcCommand[4]; extern float rcCommand[4];
typedef struct rcSmoothingFilterTraining_s {
float sum;
int count;
uint16_t min;
uint16_t max;
} rcSmoothingFilterTraining_t;
typedef struct rcSmoothingFilter_s { typedef struct rcSmoothingFilter_s {
bool filterInitialized; bool filterInitialized;
pt3Filter_t filter[4]; pt3Filter_t filterSetpoint[4];
pt3Filter_t filterDeflection[2]; pt3Filter_t filterRcDeflection[2];
pt3Filter_t filterFeedforward[3];
uint8_t setpointCutoffSetting; uint8_t setpointCutoffSetting;
uint8_t throttleCutoffSetting; uint8_t throttleCutoffSetting;
uint8_t feedforwardCutoffSetting;
uint16_t setpointCutoffFrequency; uint16_t setpointCutoffFrequency;
uint16_t throttleCutoffFrequency; uint16_t throttleCutoffFrequency;
uint8_t ffCutoffSetting;
uint16_t feedforwardCutoffFrequency; uint16_t feedforwardCutoffFrequency;
int averageFrameTimeUs;
rcSmoothingFilterTraining_t training; float smoothedRxRateHz;
uint8_t sampleCount;
uint8_t debugAxis; uint8_t debugAxis;
uint8_t autoSmoothnessFactorSetpoint;
uint8_t autoSmoothnessFactorThrottle; float autoSmoothnessFactorSetpoint;
float autoSmoothnessFactorFeedforward;
float autoSmoothnessFactorThrottle;
} rcSmoothingFilter_t; } rcSmoothingFilter_t;
typedef struct rcControlsConfig_s { typedef struct rcControlsConfig_s {

240
src/main/flight/feedforward.c
View file

@ -1,240 +0,0 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <math.h>
#include "platform.h"
#ifdef USE_FEEDFORWARD
#include "build/debug.h"
#include "common/maths.h"
#include "fc/rc.h"
#include "fc/runtime_config.h"
#include "flight/pid.h"
#include "feedforward.h"
static float setpointDelta[XYZ_AXIS_COUNT];
static float prevSetpoint[XYZ_AXIS_COUNT];
static float prevSetpointSpeed[XYZ_AXIS_COUNT];
static float prevAcceleration[XYZ_AXIS_COUNT];
static uint8_t duplicateCount[XYZ_AXIS_COUNT];
static uint8_t averagingCount;
static float feedforwardMaxRateLimit[XYZ_AXIS_COUNT];
static float feedforwardMaxRate[XYZ_AXIS_COUNT];
typedef struct laggedMovingAverageCombined_s {
laggedMovingAverage_t filter;
float buf[4];
} laggedMovingAverageCombined_t;
laggedMovingAverageCombined_t setpointDeltaAvg[XYZ_AXIS_COUNT];
uint8_t getFeedforwardDuplicateCount(int axis){
return duplicateCount[axis];
}
void feedforwardInit(const pidProfile_t *pidProfile)
{
const float feedforwardMaxRateScale = pidProfile->feedforward_max_rate_limit * 0.01f;
averagingCount = pidProfile->feedforward_averaging + 1;
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
feedforwardMaxRate[i] = applyCurve(i, 1.0f);
feedforwardMaxRateLimit[i] = feedforwardMaxRate[i] * feedforwardMaxRateScale;
laggedMovingAverageInit(&setpointDeltaAvg[i].filter, averagingCount, (float *)&setpointDeltaAvg[i].buf[0]);
}
}
FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging, const float setpoint, bool rawSetpointIsSmoothed)
{
const float feedforwardBoostFactor = pidGetFeedforwardBoostFactor();
if (rawSetpointIsSmoothed) {
// simple feedforward with boost on pre-smoothed data that changes smoothly each PID loop
const float setpointSpeed = (setpoint - prevSetpoint[axis]);
prevSetpoint[axis] = setpoint;
float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
prevSetpointSpeed[axis] = setpointSpeed;
setpointAcceleration *= feedforwardBoostFactor;
setpointDelta[axis] = (setpointSpeed + setpointAcceleration);
} else {
if (newRcFrame) {
// calculate feedforward in steps, when new RC packet arrives, then upsammple the resulting feedforward to PID loop rate
const float feedforwardTransitionFactor = pidGetFeedforwardTransitionFactor();
const float feedforwardSmoothFactor = pidGetFeedforwardSmoothFactor();
// good values : 25 for 111hz FrSky, 30 for 150hz, 50 for 250hz, 65 for 500hz links
const float feedforwardJitterFactor = pidGetFeedforwardJitterFactor();
// 7 is default, 5 for faster links with smaller steps and for racing, 10-12 for 150hz freestyle
const float rxInterval = getCurrentRxRefreshRate() * 1e-6f; // 0.0066 for 150hz RC Link.
const float rxRate = 1.0f / rxInterval; // eg 150 for a 150Hz RC link
const float absSetpointPercent = fabsf(setpoint) / feedforwardMaxRate[axis];
float rcCommandDelta = getRcCommandDelta(axis);
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(rcCommandDelta * 100.0f));
// rcCommand packet difference = value of 100 if 1000 RC steps
DEBUG_SET(DEBUG_FEEDFORWARD, 0, lrintf(setpoint));
// un-smoothed in blackbox
}
// calculate setpoint speed
float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention
float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
float setpointAcceleration = 0.0f;
rcCommandDelta = fabsf(rcCommandDelta);
if (rcCommandDelta) {
// we have movement and should calculate feedforward
// jitter attenuator falls below 1 when rcCommandDelta falls below jitter threshold
float jitterAttenuator = 1.0f;
if (feedforwardJitterFactor) {
if (rcCommandDelta < feedforwardJitterFactor) {
jitterAttenuator = MAX(1.0f - (rcCommandDelta / feedforwardJitterFactor), 0.0f);
jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator;
}
}
// duplicateCount indicates number of prior duplicate/s, 1 means one only duplicate prior to this packet
// reduce setpoint speed by half after a single duplicate or a third after two. Any more are forced to zero.
// needed because while sticks are moving, the next valid step up will be proportionally bigger
// and stops excessive feedforward where steps are at intervals, eg when the OpenTx ADC filter is active
// downside is that for truly held sticks, the first feedforward step won't be as big as it should be
if (duplicateCount[axis]) {
setpointSpeed /= duplicateCount[axis] + 1;
}
// first order type smoothing for setpoint speed noise reduction
setpointSpeed = prevSetpointSpeed[axis] + feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
// calculate acceleration from smoothed setpoint speed
setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
// use rxRate to normalise acceleration to nominal RC packet interval of 100hz
// without this, we would get less boost than we should at higher Rx rates
// note rxRate updates with every new packet (though not every time data changes), hence
// if no Rx packets are received for a period, boost amount is correctly attenuated in proportion to the delay
setpointAcceleration *= rxRate * 0.01f;
// first order acceleration smoothing (with smoothed input this is effectively second order all up)
setpointAcceleration = prevAcceleration[axis] + feedforwardSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
// jitter reduction to reduce acceleration spikes at low rcCommandDelta values
// no effect for rcCommandDelta values above jitter threshold (zero delay)
// does not attenuate the basic feedforward amount, but this is small anyway at centre due to expo
setpointAcceleration *= jitterAttenuator;
if (!FLIGHT_MODE(ANGLE_MODE)) {
if (absSetpointPercent > 0.95f && absSetpointSpeed < 3.0f * absPrevSetpointSpeed) {
// approaching max stick position so zero out feedforward to minimise overshoot
setpointSpeed = 0.0f;
setpointAcceleration = 0.0f;
}
}
prevSetpointSpeed[axis] = setpointSpeed;
prevAcceleration[axis] = setpointAcceleration;
setpointAcceleration *= feedforwardBoostFactor;
// add attenuated boost to base feedforward and apply jitter attenuation
setpointDelta[axis] = (setpointSpeed + setpointAcceleration) * pidGetDT() * jitterAttenuator;
//reset counter
duplicateCount[axis] = 0;
} else {
// no movement
if (duplicateCount[axis]) {
// increment duplicate count to max of 2
duplicateCount[axis] += (duplicateCount[axis] < 2) ? 1 : 0;
// second or subsequent duplicate, or duplicate when held at max stick or centre position.
// force feedforward to zero
setpointDelta[axis] = 0.0f;
// zero speed and acceleration for correct smoothing of next good packet
setpointSpeed = 0.0f;
prevSetpointSpeed[axis] = 0.0f;
prevAcceleration[axis] = 0.0f;
} else {
// first duplicate; hold feedforward and previous static values, as if we just never got anything
duplicateCount[axis] = 1;
}
}
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD, 1, lrintf(setpointSpeed * pidGetDT() * 100.0f)); // setpoint speed after smoothing
DEBUG_SET(DEBUG_FEEDFORWARD, 2, lrintf(setpointAcceleration * pidGetDT() * 100.0f)); // boost amount after smoothing
// debug 0 is interpolated setpoint, above
// debug 3 is rcCommand delta, above
}
prevSetpoint[axis] = setpoint;
// apply averaging, if enabled - include zero values in averaging
if (feedforwardAveraging) {
setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDelta[axis]);
}
// apply feedforward transition
setpointDelta[axis] *= feedforwardTransitionFactor > 0 ? MIN(1.0f, getRcDeflectionAbs(axis) * feedforwardTransitionFactor) : 1.0f;
}
}
return setpointDelta[axis]; // the value used by the PID code
}
FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint)
{
switch (axis) {
case FD_ROLL:
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 0, lrintf(value));
break;
case FD_PITCH:
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 1, lrintf(value));
break;
}
if (value * currentPidSetpoint > 0.0f) {
if (fabsf(currentPidSetpoint) <= feedforwardMaxRateLimit[axis]) {
value = constrainf(value, (-feedforwardMaxRateLimit[axis] - currentPidSetpoint) * Kp, (feedforwardMaxRateLimit[axis] - currentPidSetpoint) * Kp);
} else {
value = 0;
}
}
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 2, lrintf(value));
}
return value;
}
bool shouldApplyFeedforwardLimits(int axis)
{
return axis < FD_YAW && !FLIGHT_MODE(ANGLE_MODE) && feedforwardMaxRateLimit[axis] != 0.0f;
}
#endif

32
src/main/flight/feedforward.h
View file

@ -1,32 +0,0 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#include "common/axis.h"
#include "flight/pid.h"
uint8_t getFeedforwardDuplicateCount(int axis);
void feedforwardInit(const pidProfile_t *pidProfile);
float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging, const float setpoint, bool rawSetpointIsSmoothed);
float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint);
bool shouldApplyFeedforwardLimits(int axis);

254
src/main/flight/pid.c
View file

@ -50,7 +50,6 @@
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
#include "flight/feedforward.h"
#include "io/gps.h" #include "io/gps.h"
@ -130,7 +129,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
[PID_ROLL] = PID_ROLL_DEFAULT, [PID_ROLL] = PID_ROLL_DEFAULT,
[PID_PITCH] = PID_PITCH_DEFAULT, [PID_PITCH] = PID_PITCH_DEFAULT,
[PID_YAW] = PID_YAW_DEFAULT, [PID_YAW] = PID_YAW_DEFAULT,
[PID_LEVEL] = { 50, 50, 75, 50 }, [PID_LEVEL] = { 50, 75, 75, 50 },
[PID_MAG] = { 40, 0, 0, 0 }, [PID_MAG] = { 40, 0, 0, 0 },
}, },
.pidSumLimit = PIDSUM_LIMIT, .pidSumLimit = PIDSUM_LIMIT,
@ -228,6 +227,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.tpa_breakpoint = 1350, .tpa_breakpoint = 1350,
.angle_feedforward_smoothing_ms = 80, .angle_feedforward_smoothing_ms = 80,
.angle_earth_ref = 100, .angle_earth_ref = 100,
.horizon_delay_ms = 500, // 500ms time constant on any increase in horizon strength
); );
#ifndef USE_D_MIN #ifndef USE_D_MIN
@ -265,28 +265,6 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState)
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
#ifdef USE_FEEDFORWARD
float pidGetFeedforwardTransitionFactor(void)
{
return pidRuntime.feedforwardTransitionFactor;
}
float pidGetFeedforwardSmoothFactor(void)
{
return pidRuntime.feedforwardSmoothFactor;
}
float pidGetFeedforwardJitterFactor(void)
{
return pidRuntime.feedforwardJitterFactor;
}
float pidGetFeedforwardBoostFactor(void)
{
return pidRuntime.feedforwardBoostFactor;
}
#endif
void pidResetIterm(void) void pidResetIterm(void)
{ {
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
@ -368,99 +346,50 @@ float pidApplyThrustLinearization(float motorOutput)
#endif #endif
#if defined(USE_ACC) #if defined(USE_ACC)
// calculate the stick deflection while applying level mode expo // Calculate strength of horizon leveling; 0 = none, 1.0 = most leveling
static float getAngleModeStickInputRaw(uint8_t axis)
{
const float stickDeflection = getRcDeflectionRaw(axis);
if (axis < FD_YAW) {
const float expof = currentControlRateProfile->levelExpo[axis] / 100.0f;
return power3(stickDeflection) * expof + stickDeflection * (1 - expof);
} else {
return stickDeflection;
}
}
// calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
STATIC_UNIT_TESTED FAST_CODE_NOINLINE float calcHorizonLevelStrength(void) STATIC_UNIT_TESTED FAST_CODE_NOINLINE float calcHorizonLevelStrength(void)
{ {
const float currentInclination = MAX(abs(attitude.values.roll), abs(attitude.values.pitch)) / 10.0f; const float currentInclination = MAX(abs(attitude.values.roll), abs(attitude.values.pitch)) * 0.1f;
// 0 when level, 90 when vertical, 180 when inverted (degrees): // 0 when level, 90 when vertical, 180 when inverted (degrees):
float horizonLevelStrength = MAX((pidRuntime.horizonLimitDegrees - currentInclination) / pidRuntime.horizonLimitDegrees, 0.0f); float absMaxStickDeflection = MAX(fabsf(getRcDeflection(FD_ROLL)), fabsf(getRcDeflection(FD_PITCH)));
// 1.0 when attitude is 'flat', 0 when angle is equal to, or greater than, horizonLimitDegrees // 0-1, smoothed if RC smoothing is enabled
if (!pidRuntime.horizonIgnoreSticks) { float horizonLevelStrength = MAX((pidRuntime.horizonLimitDegrees - currentInclination) * pidRuntime.horizonLimitDegreesInv, 0.0f);
// horizonIgnoreSticks: 0 = default; levelling attenuated by both attitude and sticks; // 1.0 when attitude is 'flat', 0 when angle is equal to, or greater than, horizonLimitDegrees
// 1 = level attenuation only by attitude horizonLevelStrength *= MAX((pidRuntime.horizonLimitSticks - absMaxStickDeflection) * pidRuntime.horizonLimitSticksInv, pidRuntime.horizonIgnoreSticks);
const float absMaxStickDeflection = MAX(fabsf(getRcDeflection(FD_ROLL)), fabsf(getRcDeflection(FD_PITCH))); // 0-1, smoothed if RC smoothing is enabled // use the value of horizonIgnoreSticks to enable/disable this effect.
const float horizonStickAttenuation = MAX((pidRuntime.horizonLimitSticks - absMaxStickDeflection) / pidRuntime.horizonLimitSticks, 0.0f); // value should be 1.0 at center stick, 0.0 at max stick deflection:
// 1.0 at center stick, 0.0 at max stick deflection: horizonLevelStrength *= pidRuntime.horizonGain;
horizonLevelStrength *= horizonStickAttenuation * pidRuntime.horizonGain;
if (pidRuntime.horizonDelayMs) {
const float horizonLevelStrengthSmoothed = pt1FilterApply(&pidRuntime.horizonSmoothingPt1, horizonLevelStrength);
horizonLevelStrength = MIN(horizonLevelStrength, horizonLevelStrengthSmoothed);
} }
return horizonLevelStrength; return horizonLevelStrength;
// 1 means full levelling, 0 means none
} }
// Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow. // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
// The impact is possibly slightly slower performance on F7/H7 but they have more than enough // The impact is possibly slightly slower performance on F7/H7 but they have more than enough
// processing power that it should be a non-issue. // processing power that it should be a non-issue.
STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim,
float rawSetpoint, float horizonLevelStrength, bool newRcFrame) float currentPidSetpoint, float horizonLevelStrength)
{ {
// Applies only to axes that are in Angle mode
// We now use Acro Rates, transformed into the range +/- 1, to provide setpoints
const float angleLimit = pidProfile->angle_limit; const float angleLimit = pidProfile->angle_limit;
// ** angle loop feedforward
float angleFeedforward = 0.0f; float angleFeedforward = 0.0f;
#ifdef USE_FEEDFORWARD #ifdef USE_FEEDFORWARD
const float rxRateHz = 1e6f / getCurrentRxRefreshRate(); angleFeedforward = angleLimit * getFeedforward(axis) * pidRuntime.angleFeedforwardGain * pidRuntime.maxRcRateInv[axis];
const float rcCommandDeltaAbs = fabsf(getRcCommandDelta(axis)); // angle feedforward must be heavily filtered, at the PID loop rate, with limited user control over time constant
// it MUST be very delayed to avoid early overshoot and being too aggressive
if (newRcFrame){
// this runs at Rx link rate
const float angleTargetRaw = angleLimit * getAngleModeStickInputRaw(axis);
float angleFeedforwardInput = 0.0f;
if (rcCommandDeltaAbs) {
// there is movement, so calculate a Delta value to get feedforward
pidRuntime.angleTargetDelta[axis] = (angleTargetRaw - pidRuntime.angleTargetPrevious[axis]);
angleFeedforwardInput = pidRuntime.angleTargetDelta[axis];
pidRuntime.angleDuplicateCount[axis] = 0;
pidRuntime.angleTargetPrevious[axis] = angleTargetRaw;
// no need for averaging or PT1 smoothing because of the strong PT3 smoothing of the final value
} else { // it's a duplicate
// TO DO - interpolate duplicates in rc.c for frsky so that incoming steps don't have them
pidRuntime.angleDuplicateCount[axis] += 1;
const float rcDeflectionAbs = getRcDeflectionAbs(axis);
if (pidRuntime.angleDuplicateCount[axis] == 1 && rcDeflectionAbs < 0.97f) {
// on first duplicate, unless we just hit max deflection, reduce glitch by interpolation
angleFeedforwardInput = pidRuntime.angleTargetDelta[axis];
} else {
// force feedforward to zero
pidRuntime.angleDuplicateCount[axis] = 2;
pidRuntime.angleTargetDelta[axis] = 0.0f;
angleFeedforwardInput = 0.0f;
}
}
// jitter attenuation copied from feedforward.c
// TO DO move jitter algorithm to rc.c and use same code here and in feedforward.c
const float feedforwardJitterFactor = pidRuntime.feedforwardJitterFactor;
float jitterAttenuator = 1.0f;
if (feedforwardJitterFactor && rcCommandDeltaAbs < feedforwardJitterFactor) {
jitterAttenuator = 1.0f - (rcCommandDeltaAbs / feedforwardJitterFactor);
jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator;
}
angleFeedforwardInput *= jitterAttenuator * rxRateHz;
pidRuntime.angleFeedforward[axis] = angleFeedforwardInput; // feedforward element in degrees
}
angleFeedforward = pidRuntime.angleFeedforward[axis] * pidRuntime.angleFeedforwardGain;
// filter angle feedforward, heavily, at the PID loop rate, providing user control over time constant
angleFeedforward = pt3FilterApply(&pidRuntime.angleFeedforwardPt3[axis], angleFeedforward); angleFeedforward = pt3FilterApply(&pidRuntime.angleFeedforwardPt3[axis], angleFeedforward);
#endif // USE_FEEDFORWARD #endif
float angleTarget = angleLimit * currentPidSetpoint * pidRuntime.maxRcRateInv[axis];
// use acro rates for the angle target in both horizon and angle modes, converted to -1 to +1 range using maxRate
// calculate error angle and limit the angle to the max inclination
// stick input is from rcCommand, is smoothed, includes level expo, and is in range [-1.0, 1.0]
float angleTarget = angleLimit * getAngleModeStickInputRaw(axis);
#ifdef USE_GPS_RESCUE #ifdef USE_GPS_RESCUE
angleTarget += gpsRescueAngle[axis] / 100.0f; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch angleTarget += gpsRescueAngle[axis] / 100.0f; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch
#endif #endif
@ -468,25 +397,23 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
const float errorAngle = angleTarget - currentAngle; const float errorAngle = angleTarget - currentAngle;
float angleRate = errorAngle * pidRuntime.angleGain + angleFeedforward; float angleRate = errorAngle * pidRuntime.angleGain + angleFeedforward;
// optionally, minimise cross-axis wobble due to faster yaw responses than roll or pitch, and make co-ordinated yaw turns // minimise cross-axis wobble due to faster yaw responses than roll or pitch, and make co-ordinated yaw turns
// by compensating for the effect of yaw on roll while pitched, and on pitch while rolled // by compensating for the effect of yaw on roll while pitched, and on pitch while rolled
float axisCoordination = pidRuntime.angleYawSetpoint; // earthRef code here takes about 76 cycles, if conditional on angleEarthRef it takes about 100. sin_approx costs most of those cycles.
if (pidRuntime.angleEarthRef) { float sinAngle = sin_approx(DEGREES_TO_RADIANS(pidRuntime.angleTarget[axis == FD_ROLL ? FD_PITCH : FD_ROLL]));
const float sinAngle = sin_approx(DEGREES_TO_RADIANS(pidRuntime.angleTarget[axis == FD_ROLL ? FD_PITCH : FD_ROLL])); sinAngle *= (axis == FD_ROLL) ? -1.0f : 1.0f; // must be negative for Roll
pidRuntime.angleTarget[axis] = angleTarget; // store target for alternate axis to current axis, for use in preceding calculation angleRate += pidRuntime.angleYawSetpoint * sinAngle * pidRuntime.angleEarthRef;
axisCoordination *= (axis == FD_ROLL) ? -sinAngle : sinAngle; // must be negative for Roll pidRuntime.angleTarget[axis] = angleTarget; // set target for alternate axis to current axis, for use in preceding calculation
angleRate += axisCoordination * pidRuntime.angleEarthRef;
}
// smooth final angle rate output to clean up attitude signal steps (500hz), GPS steps (10 or 100hz), RC steps etc // smooth final angle rate output to clean up attitude signal steps (500hz), GPS steps (10 or 100hz), RC steps etc
// this filter runs at ATTITUDE_CUTOFF_HZ, currently 50hz, so GPS roll may be a bit steppy // this filter runs at ATTITUDE_CUTOFF_HZ, currently 50hz, so GPS roll may be a bit steppy
angleRate = pt3FilterApply(&pidRuntime.attitudeFilter[axis], angleRate); angleRate = pt3FilterApply(&pidRuntime.attitudeFilter[axis], angleRate);
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) { if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) {
rawSetpoint = angleRate; currentPidSetpoint = angleRate;
} else { } else {
// can only be HORIZON mode - crossfade Angle rate and Acro rate // can only be HORIZON mode - crossfade Angle rate and Acro rate
rawSetpoint = rawSetpoint * (1.0f - horizonLevelStrength) + angleRate * horizonLevelStrength; currentPidSetpoint = currentPidSetpoint * (1.0f - horizonLevelStrength) + angleRate * horizonLevelStrength;
} }
//logging //logging
@ -497,13 +424,13 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
DEBUG_SET(DEBUG_ANGLE_MODE, 3, lrintf(currentAngle * 10.0f)); // angle returned DEBUG_SET(DEBUG_ANGLE_MODE, 3, lrintf(currentAngle * 10.0f)); // angle returned
DEBUG_SET(DEBUG_ANGLE_TARGET, 0, lrintf(angleTarget * 10.0f)); DEBUG_SET(DEBUG_ANGLE_TARGET, 0, lrintf(angleTarget * 10.0f));
DEBUG_SET(DEBUG_ANGLE_TARGET, 1, lrintf(axisCoordination * 10.0f)); DEBUG_SET(DEBUG_ANGLE_TARGET, 1, lrintf(sinAngle * 10.0f)); // modification factor from earthRef
// debug ANGLE_TARGET 2 is yaw attenuation // debug ANGLE_TARGET 2 is yaw attenuation
DEBUG_SET(DEBUG_ANGLE_TARGET, 3, lrintf(currentAngle * 10.0f)); // angle returned DEBUG_SET(DEBUG_ANGLE_TARGET, 3, lrintf(currentAngle * 10.0f)); // angle returned
} }
DEBUG_SET(DEBUG_CURRENT_ANGLE, axis, lrintf(currentAngle * 10.0f)); // current angle DEBUG_SET(DEBUG_CURRENT_ANGLE, axis, lrintf(currentAngle * 10.0f)); // current angle
return rawSetpoint; return currentPidSetpoint;
} }
static FAST_CODE_NOINLINE void handleCrashRecovery( static FAST_CODE_NOINLINE void handleCrashRecovery(
@ -711,23 +638,6 @@ STATIC_UNIT_TESTED void rotateItermAndAxisError(void)
} }
} }
#ifdef USE_RC_SMOOTHING_FILTER
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.feedforwardLpfInitialized) {
ret = pt3FilterApply(&pidRuntime.feedforwardPt3[axis], pidSetpointDelta);
if (axis == pidRuntime.rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f));
}
}
return ret;
}
#endif // USE_RC_SMOOTHING_FILTER
#if defined(USE_ITERM_RELAX) #if defined(USE_ITERM_RELAX)
#if defined(USE_ABSOLUTE_CONTROL) #if defined(USE_ABSOLUTE_CONTROL)
STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate) STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate)
@ -981,43 +891,36 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
rpmFilterUpdate(); rpmFilterUpdate();
#endif #endif
const bool newRcFrame = getShouldUpdateFeedforward();
// ----------PID controller---------- // ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
float currentPidSetpoint = getSetpointRate(axis); float currentPidSetpoint = getSetpointRate(axis);
float rawSetpoint = getRawSetpoint(axis);
bool rawSetpointIsSmoothed = false;
if (pidRuntime.maxVelocity[axis]) { if (pidRuntime.maxVelocity[axis]) {
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint); currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
} }
// Yaw control is GYRO based, direct sticks control is applied to rate PID // Yaw control is GYRO based, direct sticks control is applied to rate PID
// When Race Mode is active PITCH control is also GYRO based in level or horizon mode // When Race Mode is active PITCH control is also GYRO based in level or horizon mode
#if defined(USE_ACC) #if defined(USE_ACC)
// earth reference yaw by attenuating yaw rate at higher angles pidRuntime.axisInAngleMode[axis] = false;
if (axis == FD_YAW && pidRuntime.angleEarthRef) { if (axis < FD_YAW) {
pidRuntime.angleYawSetpoint = currentPidSetpoint; if (levelMode == LEVEL_MODE_RP || (levelMode == LEVEL_MODE_R && axis == FD_ROLL)) {
if (levelMode == LEVEL_MODE_RP) { pidRuntime.axisInAngleMode[axis] = true;
float maxAngleTargetAbs = fmaxf( fabsf(pidRuntime.angleTarget[FD_ROLL]), fabsf(pidRuntime.angleTarget[FD_PITCH]) ); currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint, horizonLevelStrength);
maxAngleTargetAbs *= pidRuntime.angleEarthRef; }
if (FLIGHT_MODE(HORIZON_MODE)) { } else { // yaw axis only
// reduce yaw compensation when Horizon uses less levelling if (levelMode == LEVEL_MODE_RP) {
maxAngleTargetAbs *= horizonLevelStrength; // if earth referencing is requested, attenuate yaw axis setpoint when pitched or rolled
} // and send yawSetpoint to Angle code to modulate pitch and roll
float attenuateYawSetpoint = cos_approx(DEGREES_TO_RADIANS(maxAngleTargetAbs)); // code cost is 107 cycles when earthRef enabled, 20 otherwise, nearly all in cos_approx
currentPidSetpoint *= attenuateYawSetpoint; if (pidRuntime.angleEarthRef) {
rawSetpoint *= attenuateYawSetpoint; pidRuntime.angleYawSetpoint = currentPidSetpoint;
DEBUG_SET(DEBUG_ANGLE_TARGET, 2, lrintf(attenuateYawSetpoint * 100.0f)); // yaw attenuation float maxAngleTargetAbs = pidRuntime.angleEarthRef * fmaxf( fabsf(pidRuntime.angleTarget[FD_ROLL]), fabsf(pidRuntime.angleTarget[FD_PITCH]) );
maxAngleTargetAbs *= (FLIGHT_MODE(HORIZON_MODE)) ? horizonLevelStrength : 1.0f;
// reduce compensation whenever Horizon uses less levelling
currentPidSetpoint *= cos_approx(DEGREES_TO_RADIANS(maxAngleTargetAbs));
DEBUG_SET(DEBUG_ANGLE_TARGET, 2, currentPidSetpoint); // yaw setpoint after attenuation
}
} }
}
if ((levelMode == LEVEL_MODE_R && axis == FD_ROLL)
|| (levelMode == LEVEL_MODE_RP && (axis == FD_ROLL || axis == FD_PITCH)) ) {
rawSetpoint = pidLevel(axis, pidProfile, angleTrim, rawSetpoint, horizonLevelStrength, newRcFrame);
currentPidSetpoint = rawSetpoint;
rawSetpointIsSmoothed = true;
// levelled axes are already smoothed; feedforward should be calculated each PID loop
DEBUG_SET(DEBUG_ATTITUDE, axis - FD_ROLL + 2, currentPidSetpoint);
} }
#endif #endif
@ -1096,17 +999,32 @@ const bool newRcFrame = getShouldUpdateFeedforward();
const float iTermChange = (Ki + pidRuntime.itermAccelerator) * dynCi * pidRuntime.dT * itermErrorRate; const float iTermChange = (Ki + pidRuntime.itermAccelerator) * dynCi * pidRuntime.dT * itermErrorRate;
pidData[axis].I = constrainf(previousIterm + iTermChange, -pidRuntime.itermLimit, pidRuntime.itermLimit); pidData[axis].I = constrainf(previousIterm + iTermChange, -pidRuntime.itermLimit, pidRuntime.itermLimit);
// -----calculate pidSetpointDelta
float pidSetpointDelta = 0;
#ifdef USE_FEEDFORWARD
pidSetpointDelta = feedforwardApply(axis, newRcFrame, pidRuntime.feedforwardAveraging, rawSetpoint, rawSetpointIsSmoothed);
#endif
pidRuntime.previousPidSetpoint[axis] = currentPidSetpoint;
// -----calculate D component // -----calculate D component
float pidSetpointDelta = 0;
#ifdef USE_FEEDFORWARD
if (FLIGHT_MODE(ANGLE_MODE) && pidRuntime.axisInAngleMode[axis]) {
// this axis is fully under self-levelling control
// it will already have stick based feedforward applied in the input to their angle setpoint
// a simple setpoint Delta can be used to for PID feedforward element for motor lag on these axes
// however RC steps come in, via angle setpoint
// and setpoint RC smoothing must have a cutoff half normal to remove those steps completely
// the RC stepping does not come in via the feedforward, which is very well smoothed already
// if uncommented, and the forcing to zero is removed, the two following lines will restore PID feedforward to angle mode axes
// but for now let's see how we go without it (which was the case before 4.5 anyway)
// pidSetpointDelta = currentPidSetpoint - pidRuntime.previousPidSetpoint[axis];
// pidSetpointDelta *= pidRuntime.pidFrequency * pidRuntime.angleFeedforwardGain;
pidSetpointDelta = 0.0f;
} else {
// the axis is operating as a normal acro axis, so use normal feedforard from rc.c
pidSetpointDelta = getFeedforward(axis);
}
#endif
pidRuntime.previousPidSetpoint[axis] = currentPidSetpoint; // this is the value sent to blackbox, and used for Dmin setpoint
// disable D if launch control is active // disable D if launch control is active
if ((pidRuntime.pidCoefficient[axis].Kd > 0) && !launchControlActive) { if ((pidRuntime.pidCoefficient[axis].Kd > 0) && !launchControlActive) {
// Divide rate change by dT to get differential (ie dr/dt). // Divide rate change by dT to get differential (ie dr/dt).
// dT is fixed and calculated from the target PID loop time // dT is fixed and calculated from the target PID loop time
// This is done to avoid DTerm spikes that occur with dynamically // This is done to avoid DTerm spikes that occur with dynamically
@ -1171,18 +1089,8 @@ const bool newRcFrame = getShouldUpdateFeedforward();
// no feedforward in launch control // no feedforward in launch control
float feedforwardGain = launchControlActive ? 0.0f : pidRuntime.pidCoefficient[axis].Kf; float feedforwardGain = launchControlActive ? 0.0f : pidRuntime.pidCoefficient[axis].Kf;
if (feedforwardGain > 0) { if (feedforwardGain > 0) {
// transition now calculated in feedforward.c when new RC data arrives float feedForward = feedforwardGain * pidSetpointDelta;
float feedForward = feedforwardGain * pidSetpointDelta * pidRuntime.pidFrequency;
#ifdef USE_FEEDFORWARD
pidData[axis].F = shouldApplyFeedforwardLimits(axis) ?
applyFeedforwardLimit(axis, feedForward, pidRuntime.pidCoefficient[axis].Kp, currentPidSetpoint) : feedForward;
#else
pidData[axis].F = feedForward; pidData[axis].F = feedForward;
#endif
#ifdef USE_RC_SMOOTHING_FILTER
pidData[axis].F = applyRcSmoothingFeedforwardFilter(axis, pidData[axis].F);
#endif // USE_RC_SMOOTHING_FILTER
} else { } else {
pidData[axis].F = 0; pidData[axis].F = 0;
} }

37
src/main/flight/pid.h
View file

@ -236,6 +236,7 @@ typedef struct pidProfile_s {
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
uint8_t angle_feedforward_smoothing_ms; // Smoothing factor for angle feedforward as time constant in milliseconds uint8_t angle_feedforward_smoothing_ms; // Smoothing factor for angle feedforward as time constant in milliseconds
uint8_t angle_earth_ref; // Control amount of "co-ordination" from yaw into roll while pitched forward in angle mode uint8_t angle_earth_ref; // Control amount of "co-ordination" from yaw into roll while pitched forward in angle mode
uint16_t horizon_delay_ms; // delay when Horizon Strength increases, 50 = 500ms time constant
} pidProfile_t; } pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles); PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@ -300,8 +301,10 @@ typedef struct pidRuntime_s {
float angleFeedforwardGain; float angleFeedforwardGain;
float horizonGain; float horizonGain;
float horizonLimitSticks; float horizonLimitSticks;
float horizonLimitSticksInv;
float horizonLimitDegrees; float horizonLimitDegrees;
uint8_t horizonIgnoreSticks; float horizonLimitDegreesInv;
float horizonIgnoreSticks;
float maxVelocity[XYZ_AXIS_COUNT]; float maxVelocity[XYZ_AXIS_COUNT];
float itermWindupPointInv; float itermWindupPointInv;
bool inCrashRecoveryMode; bool inCrashRecoveryMode;
@ -349,13 +352,6 @@ typedef struct pidRuntime_s {
pt1Filter_t airmodeThrottleLpf2; pt1Filter_t airmodeThrottleLpf2;
#endif #endif
#ifdef USE_RC_SMOOTHING_FILTER
pt3Filter_t feedforwardPt3[XYZ_AXIS_COUNT];
bool feedforwardLpfInitialized;
uint8_t rcSmoothingDebugAxis;
uint8_t rcSmoothingFilterType;
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_ACRO_TRAINER #ifdef USE_ACRO_TRAINER
float acroTrainerAngleLimit; float acroTrainerAngleLimit;
float acroTrainerLookaheadTime; float acroTrainerLookaheadTime;
@ -393,23 +389,26 @@ typedef struct pidRuntime_s {
#endif #endif
#ifdef USE_FEEDFORWARD #ifdef USE_FEEDFORWARD
float feedforwardTransitionFactor;
feedforwardAveraging_t feedforwardAveraging; feedforwardAveraging_t feedforwardAveraging;
float feedforwardSmoothFactor; float feedforwardSmoothFactor;
float feedforwardJitterFactor; uint8_t feedforwardJitterFactor;
float feedforwardJitterFactorInv;
float feedforwardBoostFactor; float feedforwardBoostFactor;
float angleFeedforward[XYZ_AXIS_COUNT]; float feedforwardTransition;
float angleTargetPrevious[XYZ_AXIS_COUNT]; float feedforwardTransitionInv;
float angleTargetDelta[XYZ_AXIS_COUNT]; uint8_t feedforwardMaxRateLimit;
uint8_t angleDuplicateCount[XYZ_AXIS_COUNT]; pt3Filter_t angleFeedforwardPt3[XYZ_AXIS_COUNT];
#endif #endif
#ifdef USE_ACC #ifdef USE_ACC
pt3Filter_t attitudeFilter[2]; // Only for ROLL and PITCH pt3Filter_t attitudeFilter[2]; // Only for ROLL and PITCH
pt3Filter_t angleFeedforwardPt3[XYZ_AXIS_COUNT]; pt1Filter_t horizonSmoothingPt1;
uint16_t horizonDelayMs;
float angleYawSetpoint; float angleYawSetpoint;
float angleEarthRef; float angleEarthRef;
float angleTarget[2]; float angleTarget[2];
bool axisInAngleMode[3];
float maxRcRateInv[2];
#endif #endif
} pidRuntime_t; } pidRuntime_t;
@ -456,16 +455,14 @@ void applyItermRelax(const int axis, const float iterm,
void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate); void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate);
void rotateItermAndAxisError(); void rotateItermAndAxisError();
float pidLevel(int axis, const pidProfile_t *pidProfile, float pidLevel(int axis, const pidProfile_t *pidProfile,
const rollAndPitchTrims_t *angleTrim, float rawSetpoint, float horizonLevelStrength, bool newRcFrame); const rollAndPitchTrims_t *angleTrim, float rawSetpoint, float horizonLevelStrength);
float calcHorizonLevelStrength(void); float calcHorizonLevelStrength(void);
#endif #endif
void dynLpfDTermUpdate(float throttle); void dynLpfDTermUpdate(float throttle);
void pidSetItermReset(bool enabled); void pidSetItermReset(bool enabled);
float pidGetPreviousSetpoint(int axis); float pidGetPreviousSetpoint(int axis);
float pidGetDT(); float pidGetDT();
float pidGetPidFrequency(); float pidGetPidFrequency();
float pidGetFeedforwardBoostFactor();
float pidGetFeedforwardSmoothFactor();
float pidGetFeedforwardJitterFactor();
float pidGetFeedforwardTransitionFactor();
float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo); float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);

57
src/main/flight/pid_init.c
View file

@ -35,8 +35,8 @@
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/rc.h"
#include "flight/feedforward.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
@ -239,11 +239,19 @@ void pidInitFilters(const pidProfile_t *pidProfile)
const float k = pt3FilterGain(ATTITUDE_CUTOFF_HZ, pidRuntime.dT); const float k = pt3FilterGain(ATTITUDE_CUTOFF_HZ, pidRuntime.dT);
const float angleCutoffHz = 1000.0f / (2.0f * M_PIf * pidProfile->angle_feedforward_smoothing_ms); // default of 80ms -> 2.0Hz, 160ms -> 1.0Hz, approximately const float angleCutoffHz = 1000.0f / (2.0f * M_PIf * pidProfile->angle_feedforward_smoothing_ms); // default of 80ms -> 2.0Hz, 160ms -> 1.0Hz, approximately
const float k2 = pt3FilterGain(angleCutoffHz, pidRuntime.dT); const float k2 = pt3FilterGain(angleCutoffHz, pidRuntime.dT);
pidRuntime.horizonDelayMs = pidProfile->horizon_delay_ms;
if (pidRuntime.horizonDelayMs) {
const float horizonSmoothingHz = 1e3f / (2.0f * M_PIf * pidProfile->horizon_delay_ms); // default of 500ms means 0.318Hz
const float kHorizon = pt1FilterGain(horizonSmoothingHz, pidRuntime.dT);
pt1FilterInit(&pidRuntime.horizonSmoothingPt1, kHorizon);
}
for (int axis = 0; axis < 2; axis++) { // ROLL and PITCH only for (int axis = 0; axis < 2; axis++) { // ROLL and PITCH only
pt3FilterInit(&pidRuntime.attitudeFilter[axis], k); pt3FilterInit(&pidRuntime.attitudeFilter[axis], k);
pt3FilterInit(&pidRuntime.angleFeedforwardPt3[axis], k2); pt3FilterInit(&pidRuntime.angleFeedforwardPt3[axis], k2);
pidRuntime.maxRcRateInv[axis] = 1.0f / getMaxRcRate(axis);
} }
pidRuntime.angleYawSetpoint = 0.0f;
#endif #endif
pt2FilterInit(&pidRuntime.antiGravityLpf, pt2FilterGain(pidProfile->anti_gravity_cutoff_hz, pidRuntime.dT)); pt2FilterInit(&pidRuntime.antiGravityLpf, pt2FilterGain(pidProfile->anti_gravity_cutoff_hz, pidRuntime.dT));
@ -259,28 +267,6 @@ void pidInit(const pidProfile_t *pidProfile)
#endif #endif
} }
#ifdef USE_RC_SMOOTHING_FILTER
void pidInitFeedforwardLpf(uint16_t filterCutoff, uint8_t debugAxis)
{
pidRuntime.rcSmoothingDebugAxis = debugAxis;
if (filterCutoff > 0) {
pidRuntime.feedforwardLpfInitialized = true;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt3FilterInit(&pidRuntime.feedforwardPt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
}
}
}
void pidUpdateFeedforwardLpf(uint16_t filterCutoff)
{
if (filterCutoff > 0) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt3FilterUpdateCutoff(&pidRuntime.feedforwardPt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
}
}
}
#endif // USE_RC_SMOOTHING_FILTER
void pidInitConfig(const pidProfile_t *pidProfile) void pidInitConfig(const pidProfile_t *pidProfile)
{ {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -300,9 +286,13 @@ void pidInitConfig(const pidProfile_t *pidProfile)
pidRuntime.angleEarthRef = pidProfile->angle_earth_ref / 100.0f; pidRuntime.angleEarthRef = pidProfile->angle_earth_ref / 100.0f;
pidRuntime.horizonGain = MIN(pidProfile->pid[PID_LEVEL].I / 100.0f, 1.0f); pidRuntime.horizonGain = MIN(pidProfile->pid[PID_LEVEL].I / 100.0f, 1.0f);
pidRuntime.horizonIgnoreSticks = (pidProfile->horizon_ignore_sticks) ? 1.0f : 0.0f;
pidRuntime.horizonLimitSticks = pidProfile->pid[PID_LEVEL].D / 100.0f; pidRuntime.horizonLimitSticks = pidProfile->pid[PID_LEVEL].D / 100.0f;
pidRuntime.horizonIgnoreSticks = pidProfile->horizon_ignore_sticks; pidRuntime.horizonLimitSticksInv = (pidProfile->pid[PID_LEVEL].D) ? 1.0f / pidRuntime.horizonLimitSticks : 1.0f;
pidRuntime.horizonLimitDegrees = (float)pidProfile->horizon_limit_degrees; pidRuntime.horizonLimitDegrees = (float)pidProfile->horizon_limit_degrees;
pidRuntime.horizonLimitDegreesInv = (pidProfile->horizon_limit_degrees) ? 1.0f / pidRuntime.horizonLimitDegrees : 1.0f;
pidRuntime.horizonDelayMs = pidProfile->horizon_delay_ms;
pidRuntime.maxVelocity[FD_ROLL] = pidRuntime.maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * pidRuntime.dT; pidRuntime.maxVelocity[FD_ROLL] = pidRuntime.maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * pidRuntime.dT;
pidRuntime.maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT; pidRuntime.maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT;
@ -421,20 +411,15 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#endif #endif
#ifdef USE_FEEDFORWARD #ifdef USE_FEEDFORWARD
if (pidProfile->feedforward_transition == 0) { pidRuntime.feedforwardTransition = pidProfile->feedforward_transition / 100.0f;
pidRuntime.feedforwardTransitionFactor = 0; pidRuntime.feedforwardTransitionInv = (pidProfile->feedforward_transition == 0) ? 0.0f : 100.0f / pidProfile->feedforward_transition;
} else {
pidRuntime.feedforwardTransitionFactor = 100.0f / pidProfile->feedforward_transition;
}
pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging; pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging;
pidRuntime.feedforwardSmoothFactor = 1.0f; pidRuntime.feedforwardSmoothFactor = 1.0f - (0.01f * pidProfile->feedforward_smooth_factor);
if (pidProfile->feedforward_smooth_factor) {
pidRuntime.feedforwardSmoothFactor = 1.0f - ((float)pidProfile->feedforward_smooth_factor) / 100.0f;
}
pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor; pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor;
pidRuntime.feedforwardBoostFactor = (float)pidProfile->feedforward_boost / 10.0f; pidRuntime.feedforwardJitterFactorInv = 1.0f / (2.0f * pidProfile->feedforward_jitter_factor);
// the extra division by 2 is to average the sum of the two previous rcCommandAbs values
feedforwardInit(pidProfile); pidRuntime.feedforwardBoostFactor = 0.1f * pidProfile->feedforward_boost;
pidRuntime.feedforwardMaxRateLimit = pidProfile->feedforward_max_rate_limit;
#endif #endif
pidRuntime.levelRaceMode = pidProfile->level_race_mode; pidRuntime.levelRaceMode = pidProfile->level_race_mode;

2
src/main/flight/pid_init.h
View file

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

4
src/test/unit/cli_unittest.cc
View file

@ -310,7 +310,7 @@ uint8_t __config_end = 0x10;
uint16_t averageSystemLoadPercent = 0; uint16_t averageSystemLoadPercent = 0;
timeDelta_t getTaskDeltaTimeUs(taskId_e){ return 0; } timeDelta_t getTaskDeltaTimeUs(taskId_e){ return 0; }
uint16_t currentRxRefreshRate = 9000; uint16_t currentRxIntervalUs = 9000;
armingDisableFlags_e getArmingDisableFlags(void) { return ARMING_DISABLED_NO_GYRO; } armingDisableFlags_e getArmingDisableFlags(void) { return ARMING_DISABLED_NO_GYRO; }
const char *armingDisableFlagNames[]= { const char *armingDisableFlagNames[]= {
@ -368,6 +368,6 @@ bool isModeActivationConditionConfigured(const modeActivationCondition_t *, cons
void delay(uint32_t) {} void delay(uint32_t) {}
displayPort_t *osdGetDisplayPort(osdDisplayPortDevice_e *) { return NULL; } displayPort_t *osdGetDisplayPort(osdDisplayPortDevice_e *) { return NULL; }
mcuTypeId_e getMcuTypeId(void) { return MCU_TYPE_UNKNOWN; } mcuTypeId_e getMcuTypeId(void) { return MCU_TYPE_UNKNOWN; }
uint16_t getCurrentRxRefreshRate(void) { return 0; } uint16_t getCurrentRxIntervalUs(void) { return 0; }
uint16_t getAverageSystemLoadPercent(void) { return 0; } uint16_t getAverageSystemLoadPercent(void) { return 0; }
} }

187
src/test/unit/pid_unittest.cc
View file

@ -30,8 +30,8 @@ float simulatedPrevSetpointRate[3] = { 0,0,0 };
float simulatedRcDeflection[3] = { 0,0,0 }; float simulatedRcDeflection[3] = { 0,0,0 };
float simulatedRcCommandDelta[3] = { 1,1,1 }; float simulatedRcCommandDelta[3] = { 1,1,1 };
float simulatedRawSetpoint[3] = { 0,0,0 }; float simulatedRawSetpoint[3] = { 0,0,0 };
uint16_t simulatedCurrentRxRefreshRate = 10000; float simulatedMaxRate[3] = { 670,670,670 };
uint8_t simulatedDuplicateCount = 0; float simulatedFeedforward[3] = { 0,0,0 };
float simulatedMotorMixRange = 0.0f; float simulatedMotorMixRange = 0.0f;
int16_t debug[DEBUG16_VALUE_COUNT]; int16_t debug[DEBUG16_VALUE_COUNT];
@ -88,38 +88,20 @@ extern "C" {
void systemBeep(bool) { } void systemBeep(bool) { }
bool gyroOverflowDetected(void) { return false; } bool gyroOverflowDetected(void) { return false; }
float getRcDeflection(int axis) { return simulatedRcDeflection[axis]; } float getRcDeflection(int axis) { return simulatedRcDeflection[axis]; }
float getRcCommandDelta(int axis) { return simulatedRcCommandDelta[axis]; }
float getRcDeflectionRaw(int axis) { return simulatedRcDeflection[axis]; } float getRcDeflectionRaw(int axis) { return simulatedRcDeflection[axis]; }
float getRawSetpoint(int axis) { return simulatedRawSetpoint[axis]; } float getRawSetpoint(int axis) { return simulatedRawSetpoint[axis]; }
uint16_t getCurrentRxRefreshRate(void) { return simulatedCurrentRxRefreshRate; } float getFeedforward(int axis) {
uint8_t getFeedforwardDuplicateCount(void) { return simulatedDuplicateCount; } return simulatedSetpointRate[axis] - simulatedPrevSetpointRate[axis];
}
void beeperConfirmationBeeps(uint8_t) { } void beeperConfirmationBeeps(uint8_t) { }
bool isLaunchControlActive(void) {return unitLaunchControlActive; } bool isLaunchControlActive(void) {return unitLaunchControlActive; }
void disarm(flightLogDisarmReason_e) { } void disarm(flightLogDisarmReason_e) { }
float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint) float getMaxRcRate(int axis)
{ {
UNUSED(axis); UNUSED(axis);
UNUSED(Kp); float maxRate = simulatedMaxRate[axis];
UNUSED(currentPidSetpoint); return maxRate;
return value;
} }
void feedforwardInit(const pidProfile_t) { }
float feedforwardApply(int axis, bool newRcFrame, feedforwardAveraging_t feedforwardAveraging, const float setpoint)
{
UNUSED(newRcFrame);
UNUSED(feedforwardAveraging);
UNUSED(setpoint);
const float feedforwardTransitionFactor = pidGetFeedforwardTransitionFactor();
float setpointDelta = simulatedSetpointRate[axis] - simulatedPrevSetpointRate[axis];
setpointDelta *= feedforwardTransitionFactor > 0 ? MIN(1.0f, getRcDeflectionAbs(axis) * feedforwardTransitionFactor) : 1;
return setpointDelta;
}
bool shouldApplyFeedforwardLimits(int axis)
{
UNUSED(axis);
return true;
}
bool getShouldUpdateFeedforward() { return true; }
void initRcProcessing(void) { } void initRcProcessing(void) { }
} }
@ -217,9 +199,6 @@ void resetTest(void)
pidInit(pidProfile); pidInit(pidProfile);
loadControlRateProfile(); loadControlRateProfile();
currentControlRateProfile->levelExpo[FD_ROLL] = 0;
currentControlRateProfile->levelExpo[FD_PITCH] = 0;
// Run pidloop for a while after reset // Run pidloop for a while after reset
for (int loop = 0; loop < 20; loop++) { for (int loop = 0; loop < 20; loop++) {
pidController(pidProfile, currentTestTime()); pidController(pidProfile, currentTestTime());
@ -389,54 +368,56 @@ TEST(pidControllerTest, testPidLevel)
// Test Angle mode response // Test Angle mode response
enableFlightMode(ANGLE_MODE); enableFlightMode(ANGLE_MODE);
float currentPidSetpoint = 30; float currentPidSetpointRoll = 0;
float currentPidSetpointPitch = 0;
float calculatedAngleSetpoint = 0;
rollAndPitchTrims_t angleTrim = { { 0, 0 } }; rollAndPitchTrims_t angleTrim = { { 0, 0 } };
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true); calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(0, currentPidSetpoint); EXPECT_FLOAT_EQ(0, calculatedAngleSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true); calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(0, currentPidSetpoint); EXPECT_FLOAT_EQ(0, calculatedAngleSetpoint);
currentPidSetpointRoll = 200;
calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(51.456356, calculatedAngleSetpoint);
currentPidSetpointPitch = -200;
calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(-51.456356, calculatedAngleSetpoint);
currentPidSetpointRoll = 400;
calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(128.94597, calculatedAngleSetpoint);
currentPidSetpointPitch = -400;
calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(-128.94597, calculatedAngleSetpoint);
// Test attitude response // Test attitude response
setStickPosition(FD_ROLL, 1.0f);
setStickPosition(FD_PITCH, -1.0f);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(174.39539, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(-174.39539, currentPidSetpoint);
setStickPosition(FD_ROLL, -0.5f);
setStickPosition(FD_PITCH, 0.5f);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(4.0751495, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
EXPECT_FLOAT_EQ(-4.0751495, currentPidSetpoint);
attitude.values.roll = -275; attitude.values.roll = -275;
attitude.values.pitch = 275; attitude.values.pitch = 275;
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true); calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(-19.130268, currentPidSetpoint); EXPECT_FLOAT_EQ(242.76686, calculatedAngleSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true); calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(19.130268, currentPidSetpoint); EXPECT_FLOAT_EQ(-242.76686, calculatedAngleSetpoint);
// Disable ANGLE_MODE // Disable ANGLE_MODE
disableFlightMode(ANGLE_MODE); disableFlightMode(ANGLE_MODE);
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true); calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true); EXPECT_FLOAT_EQ(393.44571, calculatedAngleSetpoint);
EXPECT_FLOAT_EQ(14.693689, currentPidSetpoint); calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
EXPECT_FLOAT_EQ(-392.88422, calculatedAngleSetpoint);
// Test level mode expo // Test level mode expo
enableFlightMode(ANGLE_MODE); enableFlightMode(ANGLE_MODE);
attitude.values.roll = 0; attitude.values.roll = 0;
attitude.values.pitch = 0; attitude.values.pitch = 0;
setStickPosition(FD_ROLL, 0.5f); currentPidSetpointRoll = 400;
setStickPosition(FD_PITCH, -0.5f); currentPidSetpointPitch = -400;
currentControlRateProfile->levelExpo[FD_ROLL] = 50; // need to set some rates type and some expo here ??? HELP !!
currentControlRateProfile->levelExpo[FD_PITCH] = 26; calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true); EXPECT_FLOAT_EQ(231.55479, calculatedAngleSetpoint);
EXPECT_FLOAT_EQ(45.520832, currentPidSetpoint); calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true); EXPECT_FLOAT_EQ(-231.55479, calculatedAngleSetpoint);
EXPECT_FLOAT_EQ(-61.216412, currentPidSetpoint);
} }
@ -452,21 +433,20 @@ TEST(pidControllerTest, testPidHorizon)
setStickPosition(FD_PITCH, -0.76f); setStickPosition(FD_PITCH, -0.76f);
EXPECT_FLOAT_EQ(0.0f, calcHorizonLevelStrength()); EXPECT_FLOAT_EQ(0.0f, calcHorizonLevelStrength());
// Expect full rate output on full stick // Return sticks to center, should expect some levelling, but will be delayed
// Test zero stick response
setStickPosition(FD_ROLL, 0); setStickPosition(FD_ROLL, 0);
setStickPosition(FD_PITCH, 0); setStickPosition(FD_PITCH, 0);
EXPECT_FLOAT_EQ(0.5f, calcHorizonLevelStrength()); EXPECT_FLOAT_EQ(0.0078740157, calcHorizonLevelStrength());
// Test small stick response when flat // Test small stick response when flat, considering delay
setStickPosition(FD_ROLL, 0.1f); setStickPosition(FD_ROLL, 0.1f);
setStickPosition(FD_PITCH, -0.1f); setStickPosition(FD_PITCH, -0.1f);
EXPECT_NEAR(0.4333f, calcHorizonLevelStrength(), calculateTolerance(0.434)); EXPECT_NEAR(0.01457f, calcHorizonLevelStrength(), calculateTolerance(0.01457));
// Test larger stick response when flat // Test larger stick response when flat
setStickPosition(FD_ROLL, 0.5f); setStickPosition(FD_ROLL, 0.5f);
setStickPosition(FD_PITCH, -0.5f); setStickPosition(FD_PITCH, -0.5f);
EXPECT_NEAR(0.166f, calcHorizonLevelStrength(), calculateTolerance(0.166)); EXPECT_NEAR(0.0166, calcHorizonLevelStrength(), calculateTolerance(0.0166));
// set attitude of craft to 90 degrees // set attitude of craft to 90 degrees
attitude.values.roll = 900; attitude.values.roll = 900;
@ -476,17 +456,17 @@ TEST(pidControllerTest, testPidHorizon)
setStickPosition(FD_ROLL, 0); setStickPosition(FD_ROLL, 0);
setStickPosition(FD_PITCH, 0); setStickPosition(FD_PITCH, 0);
// with gain of 50, and max angle of 135 deg, strength = 0.5 * (135-90) / 90 ie 0.5 * 45/136 or 0.5 * 0.333 = 0.166 // with gain of 50, and max angle of 135 deg, strength = 0.5 * (135-90) / 90 ie 0.5 * 45/136 or 0.5 * 0.333 = 0.166
EXPECT_NEAR(0.166f, calcHorizonLevelStrength(), calculateTolerance(0.166)); EXPECT_NEAR(0.0193f, calcHorizonLevelStrength(), calculateTolerance(0.0193));
// Test small stick response at 90 degrees // Test small stick response at 90 degrees
setStickPosition(FD_ROLL, 0.1f); setStickPosition(FD_ROLL, 0.1f);
setStickPosition(FD_PITCH, -0.1f); setStickPosition(FD_PITCH, -0.1f);
EXPECT_NEAR(0.144f, calcHorizonLevelStrength(), calculateTolerance(0.144)); EXPECT_NEAR(0.0213f, calcHorizonLevelStrength(), calculateTolerance(0.0213));
// Test larger stick response at 90 degrees // Test larger stick response at 90 degrees
setStickPosition(FD_ROLL, 0.5f); setStickPosition(FD_ROLL, 0.5f);
setStickPosition(FD_PITCH, -0.5f); setStickPosition(FD_PITCH, -0.5f);
EXPECT_NEAR(0.055f, calcHorizonLevelStrength(), calculateTolerance(0.055)); EXPECT_NEAR(0.0218f, calcHorizonLevelStrength(), calculateTolerance(0.0218));
// set attitude of craft to 120 degrees, inside limit of 135 // set attitude of craft to 120 degrees, inside limit of 135
attitude.values.roll = 1200; attitude.values.roll = 1200;
@ -495,18 +475,18 @@ TEST(pidControllerTest, testPidHorizon)
// Test centered sticks at 120 degrees // Test centered sticks at 120 degrees
setStickPosition(FD_ROLL, 0); setStickPosition(FD_ROLL, 0);
setStickPosition(FD_PITCH, 0); setStickPosition(FD_PITCH, 0);
EXPECT_NEAR(0.055f, calcHorizonLevelStrength(), calculateTolerance(0.055)); EXPECT_NEAR(0.0224f, calcHorizonLevelStrength(), calculateTolerance(0.0224));
// Test small stick response at 120 degrees // Test small stick response at 120 degrees
setStickPosition(FD_ROLL, 0.1f); setStickPosition(FD_ROLL, 0.1f);
setStickPosition(FD_PITCH, -0.1f); setStickPosition(FD_PITCH, -0.1f);
EXPECT_NEAR(0.048f, calcHorizonLevelStrength(), calculateTolerance(0.048)); EXPECT_NEAR(0.0228f, calcHorizonLevelStrength(), calculateTolerance(0.0228));
// Test larger stick response at 120 degrees // Test larger stick response at 120 degrees
setStickPosition(FD_ROLL, 0.5f); setStickPosition(FD_ROLL, 0.5f);
setStickPosition(FD_PITCH, -0.5f); setStickPosition(FD_PITCH, -0.5f);
EXPECT_NEAR(0.018f, calcHorizonLevelStrength(), calculateTolerance(0.018)); EXPECT_NEAR(0.018f, calcHorizonLevelStrength(), calculateTolerance(0.018));
// set attitude of craft to 1500 degrees, outside limit of 135 // set attitude of craft to 1500 degrees, outside limit of 135
attitude.values.roll = 1500; attitude.values.roll = 1500;
attitude.values.pitch = 1500; attitude.values.pitch = 1500;
@ -619,6 +599,8 @@ TEST(pidControllerTest, testCrashRecoveryMode)
} }
TEST(pidControllerTest, testFeedForward) TEST(pidControllerTest, testFeedForward)
// NOTE: THIS DOES NOT TEST THE FEEDFORWARD CALCULATIONS, which are now in rc.c, and return setpointDelta
// This test only validates the feedforward value calculated in pid.c for a given simulated setpointDelta
{ {
resetTest(); resetTest();
ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(ARMED);
@ -628,36 +610,67 @@ TEST(pidControllerTest, testFeedForward)
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F); EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F);
// Match the stick to gyro to stop error // Move the sticks fully
setStickPosition(FD_ROLL, 1.0f); setStickPosition(FD_ROLL, 1.0f);
setStickPosition(FD_PITCH, -1.0f); setStickPosition(FD_PITCH, -1.0f);
setStickPosition(FD_YAW, -1.0f); setStickPosition(FD_YAW, -1.0f);
pidController(pidProfile, currentTestTime()); pidController(pidProfile, currentTestTime());
EXPECT_NEAR(2232.78, pidData[FD_ROLL].F, calculateTolerance(2232.78)); EXPECT_NEAR(17.86, pidData[FD_ROLL].F, calculateTolerance(17.86));
EXPECT_NEAR(-2061.03, pidData[FD_PITCH].F, calculateTolerance(-2061.03)); EXPECT_NEAR(-16.49, pidData[FD_PITCH].F, calculateTolerance(-16.49));
EXPECT_NEAR(-2061.03, pidData[FD_YAW].F, calculateTolerance(-2061.03)); EXPECT_NEAR(-16.49, pidData[FD_YAW].F, calculateTolerance(-16.49));
// Match the stick to gyro to stop error // Bring sticks back to half way
setStickPosition(FD_ROLL, 0.5f); setStickPosition(FD_ROLL, 0.5f);
setStickPosition(FD_PITCH, -0.5f); setStickPosition(FD_PITCH, -0.5f);
setStickPosition(FD_YAW, -0.5f); setStickPosition(FD_YAW, -0.5f);
pidController(pidProfile, currentTestTime()); pidController(pidProfile, currentTestTime());
EXPECT_NEAR(-558.20, pidData[FD_ROLL].F, calculateTolerance(-558.20)); EXPECT_NEAR(-8.93, pidData[FD_ROLL].F, calculateTolerance(-8.93));
EXPECT_NEAR(515.26, pidData[FD_PITCH].F, calculateTolerance(515.26)); EXPECT_NEAR(8.24, pidData[FD_PITCH].F, calculateTolerance(8.24));
EXPECT_NEAR(515.26, pidData[FD_YAW].F, calculateTolerance(515.26)); EXPECT_NEAR(8.24, pidData[FD_YAW].F, calculateTolerance(8.24));
// Bring sticks back to two tenths out
setStickPosition(FD_ROLL, 0.2f);
setStickPosition(FD_PITCH, -0.2f);
setStickPosition(FD_YAW, -0.2f);
pidController(pidProfile, currentTestTime());
EXPECT_NEAR(-5.36, pidData[FD_ROLL].F, calculateTolerance(-5.36));
EXPECT_NEAR(4.95, pidData[FD_PITCH].F, calculateTolerance(4.95));
EXPECT_NEAR(4.95, pidData[FD_YAW].F, calculateTolerance(4.95));
// Bring sticks back to one tenth out, to give a difference of 0.1
setStickPosition(FD_ROLL, 0.1f);
setStickPosition(FD_PITCH, -0.1f);
setStickPosition(FD_YAW, -0.1f);
pidController(pidProfile, currentTestTime());
EXPECT_NEAR(-1.79, pidData[FD_ROLL].F, calculateTolerance(-1.79));
EXPECT_NEAR(1.65, pidData[FD_PITCH].F, calculateTolerance(1.65));
EXPECT_NEAR(1.65, pidData[FD_YAW].F, calculateTolerance(1.65));
// Center the sticks, giving the same delta value as before, should return the same feedforward
setStickPosition(FD_ROLL, 0.0f); setStickPosition(FD_ROLL, 0.0f);
setStickPosition(FD_PITCH, 0.0f); setStickPosition(FD_PITCH, 0.0f);
setStickPosition(FD_YAW, 0.0f); setStickPosition(FD_YAW, 0.0f);
for (int loop = 0; loop <= 15; loop++) { pidController(pidProfile, currentTestTime());
gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL];
pidController(pidProfile, currentTestTime()); EXPECT_NEAR(-1.79, pidData[FD_ROLL].F, calculateTolerance(-1.79));
} EXPECT_NEAR(1.65, pidData[FD_PITCH].F, calculateTolerance(1.65));
EXPECT_NEAR(1.65, pidData[FD_YAW].F, calculateTolerance(1.65));
// Keep centered
setStickPosition(FD_ROLL, 0.0f);
setStickPosition(FD_PITCH, 0.0f);
setStickPosition(FD_YAW, 0.0f);
pidController(pidProfile, currentTestTime());
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F); EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);