Skip to content
New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Jump to bottom

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

Merged
merged 19 commits into from
Apr 23, 2023
Merged

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

Show file tree
Hide file tree
Changes from 15 commits
Commits
Show all changes
19 commits
Select commit Hold shift + click to select a range
83498df
Refactor Feedforward Angle and RC Smoothing
ctzsnooze Jun 13, 2021
839b149
update rc_smoothing at regular intervals
ctzsnooze Apr 3, 2023
92e136f
add Earth Ref to OSD, update pid and rate PG
ctzsnooze Apr 4, 2023
1bf01ea
Initialise filters correctly
ctzsnooze Apr 5, 2023
0cfd7fd
refactoring to improve performance
ctzsnooze Apr 7, 2023
96bbe43
Save 24 cycles in Horizon calculations, other optimisations
ctzsnooze Apr 8, 2023
5072713
save 25 cycles and 330 bytes in rc_smoothing
ctzsnooze Apr 9, 2023
dd8fec3
feedforward max rate improvements
ctzsnooze Apr 10, 2023
d46e819
typo fix
ctzsnooze Apr 10, 2023
2edb7e8
Karatebrot's review suggestions part one
ctzsnooze Apr 15, 2023
722c6ee
Karatebrot's excellent suggestions part 2
ctzsnooze Apr 15, 2023
6881830
more efficient if we calculate inverse at init time
ctzsnooze Apr 15, 2023
0f15efb
Horizon delay, to ease it in when returning sticks to centre
ctzsnooze Apr 16, 2023
42de0eb
fix unit tests after horizon changes
ctzsnooze Apr 17, 2023
d715571
horizon_delay_ms, default 500
ctzsnooze Apr 21, 2023
b70aef8
fix unit test for feedforward from setpointDelta
ctzsnooze Apr 22, 2023
b01a6d2
Final optimisations - thanks @Karatebrot for your advice
ctzsnooze Apr 23, 2023
f847f66
increase horizon level strength default to 75 now we have the delay
ctzsnooze Apr 23, 2023
b17cd84
restore Makefile value which allowed local make test on mac
ctzsnooze Apr 23, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
1 change: 0 additions & 1 deletion make/source.mk
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,6 @@ COMMON_SRC = \
flight/gps_rescue.c \
flight/dyn_notch_filter.c \
flight/imu.c \
flight/feedforward.c \
flight/mixer.c \
flight/mixer_init.c \
flight/mixer_tricopter.c \
Expand Down
12 changes: 6 additions & 6 deletions src/main/blackbox/blackbox.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_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_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
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_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, "%d", currentPidProfile->rateAccelLimit);
Expand Down Expand Up @@ -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_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_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_ACTIVE_CUTOFFS, "%d,%d,%d", rcSmoothingData->feedforwardCutoffFrequency,
rcSmoothingData->setpointCutoffFrequency,
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
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RATES_TYPE, "%d", currentControlRateProfile->rates_type);

Expand Down
12 changes: 6 additions & 6 deletions src/main/cli/cli.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4699,7 +4699,7 @@ static void cliStatus(const char *cmdName, char *cmdline)
// Run status

const int gyroRate = getTaskDeltaTimeUs(TASK_GYRO) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTimeUs(TASK_GYRO)));
int rxRate = getCurrentRxRefreshRate();
int rxRate = getCurrentRxIntervalUs();
if (rxRate != 0) {
rxRate = (int)(1000000.0f / ((float)rxRate));
}
Expand Down Expand Up @@ -4845,12 +4845,12 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline)
if (rxConfig()->rc_smoothing_mode) {
cliPrintLine("FILTER");
if (rcSmoothingAutoCalculate()) {
const uint16_t avgRxFrameUs = rcSmoothingData->averageFrameTimeUs;
cliPrint("# Detected RX frame rate: ");
if (avgRxFrameUs == 0) {
const uint16_t smoothedRxRateHz = lrintf(rcSmoothingData->smoothedRxRateHz);
cliPrint("# Detected Rx frequency: ");
if (getCurrentRxIntervalUs() == 0) {
cliPrintLine("NO SIGNAL");
} else {
cliPrintLinef("%d.%03dms", avgRxFrameUs / 1000, avgRxFrameUs % 1000);
cliPrintLinef("%dHz", smoothedRxRateHz);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
cliPrintLinef("%dHz", smoothedRxRateHz);
cliPrintLinef("%.0fHz", rcSmoothingData->smoothedRxRateHz);

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Very cool, much better! Didn't know about that method.

Copy link
Member Author

@ctzsnooze ctzsnooze Apr 21, 2023 •
edited

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hmm @KarateBrot I get this error, for now am leaving it as it was. Let me know how to proceed.

./src/main/cli/cli.c:4852:56: error: implicit conversion from 'float' to 'doubl' when passing argument to function [-Werror=double-promotion]
 4852 |                 cliPrintLinef("%.0fHz", rcSmoothingData->smoothedRxRateHz);

}
}
cliPrintf("# Active setpoint cutoff: %dhz ", rcSmoothingData->setpointCutoffFrequency);
Expand All @@ -4860,7 +4860,7 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline)
cliPrintLine("(auto)");
}
cliPrintf("# Active FF cutoff: %dhz ", rcSmoothingData->feedforwardCutoffFrequency);
if (rcSmoothingData->ffCutoffSetting) {
if (rcSmoothingData->feedforwardCutoffSetting) {
cliPrintLine("(manual)");
} else {
cliPrintLine("(auto)");
Expand Down
7 changes: 3 additions & 4 deletions src/main/cli/settings.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]) },
{ "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]) },
{ 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
{ "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) },
Expand Down Expand Up @@ -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_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_DELAY_MS, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 5000 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_delay_ms) },

#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) },
Expand Down Expand Up @@ -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_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_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_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, 200}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_max_rate_limit) },
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

requires update in configurator ?
Only marked this one to keep review clean

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks, yes, not essential to change configurator, since I think very few people need above 150.

#endif

#ifdef USE_DYN_IDLE
Expand Down
7 changes: 4 additions & 3 deletions src/main/cms/cms_menu_imu.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 %", 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 },
{ NULL, OME_END, NULL, NULL}
};
Expand Down Expand Up @@ -519,6 +516,7 @@ static CMS_Menu cmsx_menuLaunchControl = {
static uint8_t cmsx_angleP;
static uint8_t cmsx_angleFF;
static uint8_t cmsx_angleLimit;
static uint8_t cmsx_angleEarthRef;

static uint8_t cmsx_horizonStrength;
static uint8_t cmsx_horizonLimitSticks;
Expand Down Expand Up @@ -567,6 +565,7 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
cmsx_angleP = pidProfile->pid[PID_LEVEL].P;
cmsx_angleFF = pidProfile->pid[PID_LEVEL].F;
cmsx_angleLimit = pidProfile->angle_limit;
cmsx_angleEarthRef = pidProfile->angle_earth_ref;

cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I;
cmsx_horizonLimitSticks = pidProfile->pid[PID_LEVEL].D;
Expand Down Expand Up @@ -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].F = cmsx_angleFF;
pidProfile->angle_limit = cmsx_angleLimit;
pidProfile->angle_earth_ref = cmsx_angleEarthRef;

pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength;
pidProfile->pid[PID_LEVEL].D = cmsx_horizonLimitSticks;
Expand Down Expand Up @@ -678,6 +678,7 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
{ "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 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 LIM_STK", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonLimitSticks, 10, 200, 1 } },
Expand Down
4 changes: 1 addition & 3 deletions src/main/fc/controlrate_profile.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@

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)
{
Expand All @@ -62,8 +62,6 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
.rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
.profileName = { 0 },
.quickRatesRcExpo = 0,
.levelExpo[FD_ROLL] = 33,
.levelExpo[FD_PITCH] = 33,
);
}
}
Expand Down
1 change: 0 additions & 1 deletion src/main/fc/controlrate_profile.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,6 @@ typedef struct controlRateConfig_s {
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
uint8_t quickRatesRcExpo; // Sets expo on rc command for quick rates
uint8_t levelExpo[2]; // roll/pitch level mode expo
} controlRateConfig_t;

PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles);
Expand Down
3 changes: 1 addition & 2 deletions src/main/fc/parameter_names.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,8 +126,6 @@
#define PARAM_NAME_POSITION_ALTITUDE_D_LPF "altitude_d_lpf"
#define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward"
#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_P_GAIN "angle_p_gain"
#define PARAM_NAME_ANGLE_EARTH_REF "angle_earth_ref"
Expand All @@ -136,6 +134,7 @@
#define PARAM_NAME_HORIZON_LIMIT_DEGREES "horizon_limit_degrees"
#define PARAM_NAME_HORIZON_LIMIT_STICKS "horizon_limit_sticks"
#define PARAM_NAME_HORIZON_IGNORE_STICKS "horizon_ignore_sticks"
#define PARAM_NAME_HORIZON_DELAY_MS "horizon_delay_ms"

#ifdef USE_GPS
#define PARAM_NAME_GPS_PROVIDER "gps_provider"
Expand Down