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.

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

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

Merged
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
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion make/source.mk
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
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
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

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
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

@haslinghuis haslinghuis Apr 20, 2023

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
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
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
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
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
Loading