diff --git a/make/source.mk b/make/source.mk
index 65434a6c8b..ce574bc12d 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -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 \
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index f73db9505c..ce4aba886d 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -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);
@@ -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);
diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c
index 3d35095e7e..5de1cf1cd9 100644
--- a/src/main/cli/cli.c
+++ b/src/main/cli/cli.c
@@ -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));
}
@@ -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);
}
}
cliPrintf("# Active setpoint cutoff: %dhz ", rcSmoothingData->setpointCutoffFrequency);
@@ -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)");
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 9bf8161590..87972e0ae7 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -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) },
@@ -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) },
@@ -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) },
#endif
#ifdef USE_DYN_IDLE
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 819be7aeb0..a88fddd29d 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -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}
};
@@ -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;
@@ -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;
@@ -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;
@@ -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 } },
diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c
index 24f2e4c16c..0add5622b5 100644
--- a/src/main/fc/controlrate_profile.c
+++ b/src/main/fc/controlrate_profile.c
@@ -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)
{
@@ -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,
);
}
}
diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h
index f1ad7bb1b5..6a2a05d415 100644
--- a/src/main/fc/controlrate_profile.h
+++ b/src/main/fc/controlrate_profile.h
@@ -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);
diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h
index 0396c34278..e789ed7bfe 100644
--- a/src/main/fc/parameter_names.h
+++ b/src/main/fc/parameter_names.h
@@ -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"
@@ -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"
diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c
index 11fed528ee..a523d89d68 100644
--- a/src/main/fc/rc.c
+++ b/src/main/fc/rc.c
@@ -42,8 +42,8 @@
#include "flight/failsafe.h"
#include "flight/imu.h"
-#include "flight/feedforward.h"
#include "flight/gps_rescue.h"
+#include "flight/pid.h"
#include "flight/pid_init.h"
#include "pg/rx.h"
@@ -57,24 +57,21 @@
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 setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
+static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; // deflection range -1 to 1
static bool reverseMotors = false;
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 isRxRateValid = false;
+static bool isRxIntervalValid = false;
static float rcCommandDivider = 500.0f;
static float rcCommandYawDivider = 500.0f;
-static FAST_DATA_ZERO_INIT bool newRxDataForFF;
-
enum {
ROLL_FLAG = 1 << ROLL,
PITCH_FLAG = 1 << PITCH,
@@ -82,32 +79,32 @@ enum {
THROTTLE_FLAG = 1 << THROTTLE,
};
-#ifdef USE_RC_SMOOTHING_FILTER
-#define RC_SMOOTHING_CUTOFF_MIN_HZ 15 // Minimum rc smoothing cutoff frequency
-#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
-#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 // Number of rx frame rate samples to average during initial training
-#define RC_SMOOTHING_FILTER_RETRAINING_SAMPLES 20 // Number of rx frame rate samples to average during frame rate changes
-#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 1000 // Additional time to wait after receiving first valid rx frame before initial training starts
-#define RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS 2000 // Guard time to wait after retraining to prevent retraining again too quickly
-#define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT 20 // Look for samples varying this much from the current detected frame rate to initiate retraining
-#define RC_SMOOTHING_FEEDFORWARD_INITIAL_HZ 100 // The value to use for "auto" when interpolated feedforward is enabled
+#ifdef USE_FEEDFORWARD
+static float feedforwardSmoothed[3];
+static float feedforwardRaw[3];
+typedef struct laggedMovingAverageCombined_s {
+ laggedMovingAverage_t filter;
+ float buf[4];
+} laggedMovingAverageCombined_t;
+laggedMovingAverageCombined_t feedforwardDeltaAvg[XYZ_AXIS_COUNT];
+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 float rcDeflectionSmoothed[3];
#endif // USE_RC_SMOOTHING_FILTER
-#define RC_RX_RATE_MIN_US 950 // 0.950ms to fit 1kHz without an issue
-#define RC_RX_RATE_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;
-}
+#define RX_INTERVAL_MIN_US 950 // 0.950ms to fit 1kHz without an issue
+#define RX_INTERVAL_MAX_US 65500 // 65.5ms or 15.26hz
float getSetpointRate(int axis)
{
@@ -118,6 +115,12 @@ float getSetpointRate(int axis)
#endif
}
+static float maxRcRate[3];
+float getMaxRcRate(int axis)
+{
+ return maxRcRate[axis];
+}
+
float getRcDeflection(int axis)
{
#ifdef USE_RC_SMOOTHING_FILTER
@@ -137,23 +140,6 @@ float getRcDeflectionAbs(int 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
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;
}
-#define SETPOINT_RATE_LIMIT 1998
-STATIC_ASSERT(CONTROL_RATE_CONFIG_RATE_LIMIT_MAX <= SETPOINT_RATE_LIMIT, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX_too_large);
+#define SETPOINT_RATE_LIMIT_MIN -1998.0f
+#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
@@ -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 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;
}
@@ -237,21 +224,16 @@ float applyQuickRates(const int axis, float rcCommandf, const float rcCommandfAb
if (currentControlRateProfile->quickRatesRcExpo) {
curve = power3(rcCommandf) * expof + rcCommandf * (1 - expof);
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 {
curve = power3(rcCommandfAbs) * expof + rcCommandfAbs * (1 - expof);
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;
}
-float applyCurve(int axis, float deflection)
-{
- return applyRates(axis, deflection, fabsf(deflection));
-}
-
static void scaleRawSetpointToFpvCamAngle(void)
{
//recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
@@ -267,8 +249,8 @@ static void scaleRawSetpointToFpvCamAngle(void)
float roll = rawSetpoint[ROLL];
float yaw = rawSetpoint[YAW];
- rawSetpoint[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f);
- rawSetpoint[YAW] = constrainf(yaw * cosFactor + roll * 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_MIN, SETPOINT_RATE_LIMIT_MAX);
}
void updateRcRefreshRate(timeUs_t currentTimeUs)
@@ -279,116 +261,91 @@ void updateRcRefreshRate(timeUs_t currentTimeUs)
timeDelta_t frameDeltaUs = rxGetFrameDelta(&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, 1, MIN(frameAgeUs / 10, INT16_MAX));
lastRxTimeUs = currentTimeUs;
- isRxRateValid = (frameDeltaUs >= RC_RX_RATE_MIN_US && frameDeltaUs <= RC_RX_RATE_MAX_US);
- currentRxRefreshRate = constrain(frameDeltaUs, RC_RX_RATE_MIN_US, RC_RX_RATE_MAX_US);
+ currentRxIntervalUs = constrain(frameDeltaUs, RX_INTERVAL_MIN_US, RX_INTERVAL_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
-// 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
// the auto-calculated cutoff frequency based on detected rx frame rate.
FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
{
- const float dT = targetPidLooptime * 1e-6f;
- uint16_t oldCutoff = smoothingData->setpointCutoffFrequency;
-
+ // in auto mode, calculate the RC smoothing cutoff from the smoothed Rx link frequency
+ 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) {
- 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) {
- 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->setpointCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
+ if (smoothingData->feedforwardCutoffSetting == 0) {
+ 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++) {
- if (i < THROTTLE) { // Throttle handled by smoothing rcCommand
+ if (i < THROTTLE) {
if (!smoothingData->filterInitialized) {
- pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
+ pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT));
} else {
- pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
+ pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT));
}
} else {
+ const float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency;
if (!smoothingData->filterInitialized) {
- pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
+ pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(throttleCutoffFrequency, dT));
} else {
- pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
+ pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3FilterGain(throttleCutoffFrequency, dT));
}
}
}
-
- // initialize or update the Level filter
+ // initialize or update the RC Deflection filter
for (int i = FD_ROLL; i < FD_YAW; i++) {
if (!smoothingData->filterInitialized) {
- pt3FilterInit(&smoothingData->filterDeflection[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
+ pt3FilterInit(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT));
} 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
- oldCutoff = 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;
+ DEBUG_SET(DEBUG_RC_SMOOTHING, 1, smoothingData->setpointCutoffFrequency);
+ DEBUG_SET(DEBUG_RC_SMOOTHING, 2, smoothingData->feedforwardCutoffFrequency);
}
// 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)
{
// 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 false;
@@ -406,35 +363,30 @@ static FAST_CODE void processRcSmoothingFilter(void)
{
static FAST_DATA_ZERO_INIT float rxDataToSmooth[4];
static FAST_DATA_ZERO_INIT bool initialized;
- static FAST_DATA_ZERO_INIT timeMs_t validRxFrameTimeMs;
static FAST_DATA_ZERO_INIT bool calculateCutoffs;
// first call initialization
if (!initialized) {
initialized = true;
rcSmoothingData.filterInitialized = false;
- rcSmoothingData.averageFrameTimeUs = 0;
- rcSmoothingData.autoSmoothnessFactorSetpoint = rxConfig()->rc_smoothing_auto_factor_rpy;
- rcSmoothingData.autoSmoothnessFactorThrottle = rxConfig()->rc_smoothing_auto_factor_throttle;
+ rcSmoothingData.smoothedRxRateHz = 0.0f;
+ rcSmoothingData.sampleCount = 0;
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.throttleCutoffSetting = rxConfig()->rc_smoothing_throttle_cutoff;
- rcSmoothingData.ffCutoffSetting = rxConfig()->rc_smoothing_feedforward_cutoff;
- rcSmoothingResetAccumulation(&rcSmoothingData);
+ rcSmoothingData.feedforwardCutoffSetting = rxConfig()->rc_smoothing_feedforward_cutoff;
+
rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting;
+ rcSmoothingData.feedforwardCutoffFrequency = rcSmoothingData.feedforwardCutoffSetting;
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) {
calculateCutoffs = rcSmoothingAutoCalculate();
-
// if we don't need to calculate cutoffs dynamically then the filters can be initialized now
if (!calculateCutoffs) {
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
@@ -444,64 +396,51 @@ static FAST_CODE void processRcSmoothingFilter(void)
}
if (isRxDataNew) {
- // for auto calculated filters we need to examine each rx frame interval
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();
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
- // and use that to calculate the filter cutoff frequencies
- if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization
- if (rxIsReceivingSignal() && isRxRateValid) {
-
- // set the guard time expiration if it's not set
- if (validRxFrameTimeMs == 0) {
- validRxFrameTimeMs = currentTimeMs + (rcSmoothingData.filterInitialized ? RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS : RC_SMOOTHING_FILTER_TRAINING_DELAY_MS);
- } else {
- sampleState = 1;
- }
-
- // if the guard time has expired then process the rx frame time
- if (currentTimeMs > validRxFrameTimeMs) {
- sampleState = 2;
- bool accumulateSample = true;
-
- // During initial training process all samples.
- // During retraining check samples to determine if they vary by more than the limit percentage.
- if (rcSmoothingData.filterInitialized) {
- const float percentChange = (abs(currentRxRefreshRate - rcSmoothingData.averageFrameTimeUs) / (float)rcSmoothingData.averageFrameTimeUs) * 100;
- 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;
- }
+ static uint16_t previousRxIntervalUs;
+ if (abs(currentRxIntervalUs - previousRxIntervalUs) < (previousRxIntervalUs - (previousRxIntervalUs / 8))) {
+ // exclude large steps, eg after dropouts or telemetry
+ // 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.
+ static float prevRxRateHz;
+ // smooth the current Rx link frequency estimates
+ const float kF = 0.1f; // first order lowpass smoothing filter coefficient
+ const float smoothedRxRateHz = prevRxRateHz + kF * (currentRxRateHz - prevRxRateHz);
+ prevRxRateHz = smoothedRxRateHz;
+
+ // recalculate cutoffs every 3 acceptable samples
+ if (rcSmoothingData.sampleCount) {
+ rcSmoothingData.sampleCount --;
+ sampleState = 1;
+ } else {
+ rcSmoothingData.smoothedRxRateHz = smoothedRxRateHz;
+ rcSmoothingSetFilterCutoffs(&rcSmoothingData);
+ rcSmoothingData.filterInitialized = true;
+ rcSmoothingData.sampleCount = 3;
+ sampleState = 2;
}
-
- // 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 {
- // we have either stopped receiving rx samples (failsafe?) or the sample time is unreasonable so reset the accumulation
- rcSmoothingResetAccumulation(&rcSmoothingData);
+ // either we stopped receiving rx samples (failsafe?) or the sample interval is unreasonable
+ // require a full re-evaluation period after signal is restored
+ rcSmoothingData.sampleCount = 0;
+ sampleState = 4;
}
}
-
- // rx frame rate training blackbox debugging
- DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxRefreshRate); // log each rx frame interval
- DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.training.count); // log the training step count
- 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
+ DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxIntervalUs / 10);
+ DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.sampleCount);
+ DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.smoothedRxRateHz); // value used by filters
+ DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); // guard time = 1, guard time expired = 2
}
// Get new values to be smoothed
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)) {
- // after training has completed then log the raw rc channel and the calculated
- // average rx frame rate that was used to calculate the automatic filter cutoffs
- DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs);
- }
+ DEBUG_SET(DEBUG_RC_SMOOTHING, 0, rcSmoothingData.smoothedRxRateHz);
+ DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.sampleCount);
// each pid loop, apply the last received channel value to the filter, if initialised - thanks @klutvott
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
float *dst = i == THROTTLE ? &rcCommand[i] : &setpointRate[i];
if (rcSmoothingData.filterInitialized) {
- *dst = pt3FilterApply(&rcSmoothingData.filter[i], rxDataToSmooth[i]);
+ *dst = pt3FilterApply(&rcSmoothingData.filterSetpoint[i], rxDataToSmooth[i]);
} else {
// If filter isn't initialized yet, as in smoothing off, use the actual unsmoothed rx channel data
*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++) {
- if (smoothingNeeded && axis < FD_YAW) {
- rcDeflectionSmoothed[axis] = pt3FilterApply(&rcSmoothingData.filterDeflection[axis], rcDeflection[axis]);
+ // Feedforward smoothing
+ 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 {
rcDeflectionSmoothed[axis] = rcDeflection[axis];
}
}
-
}
#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)
{
if (isRxDataNew) {
- newRxDataForFF = true;
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;
#ifdef USE_GPS_RESCUE
@@ -582,12 +648,17 @@ FAST_CODE void processRcCommand(void)
rcDeflectionAbs[axis] = rcCommandfAbs;
angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
-
}
+
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)) {
scaleRawSetpointToFpvCamAngle();
}
@@ -726,8 +797,16 @@ void initRcProcessing(void)
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
- const int maxYawRate = (int)applyRates(FD_YAW, 1.0f, 1.0f);
+ const int maxYawRate = (int)maxRcRate[FD_YAW];
initYawSpinRecovery(maxYawRate);
#endif
}
diff --git a/src/main/fc/rc.h b/src/main/fc/rc.h
index 7969fa6dcb..4b5e5c5e46 100644
--- a/src/main/fc/rc.h
+++ b/src/main/fc/rc.h
@@ -41,10 +41,10 @@ bool isMotorsReversed(void);
rcSmoothingFilter_t *getRcSmoothingData(void);
bool rcSmoothingAutoCalculate(void);
bool rcSmoothingInitializationComplete(void);
-float getRawSetpoint(int axis);
-float getRcCommandDelta(int axis);
-float applyCurve(int axis, float deflection);
-bool getShouldUpdateFeedforward();
+
+float getMaxRcRate(int axis);
+float getFeedforward(int axis);
+
void updateRcRefreshRate(timeUs_t currentTimeUs);
-uint16_t getCurrentRxRefreshRate(void);
+uint16_t getCurrentRxIntervalUs(void);
bool getRxRateValid(void);
diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h
index 74f7565987..e0bc36e64e 100644
--- a/src/main/fc/rc_controls.h
+++ b/src/main/fc/rc_controls.h
@@ -84,28 +84,27 @@ typedef enum {
extern float rcCommand[4];
-typedef struct rcSmoothingFilterTraining_s {
- float sum;
- int count;
- uint16_t min;
- uint16_t max;
-} rcSmoothingFilterTraining_t;
-
typedef struct rcSmoothingFilter_s {
bool filterInitialized;
- pt3Filter_t filter[4];
- pt3Filter_t filterDeflection[2];
+ pt3Filter_t filterSetpoint[4];
+ pt3Filter_t filterRcDeflection[2];
+ pt3Filter_t filterFeedforward[3];
+
uint8_t setpointCutoffSetting;
uint8_t throttleCutoffSetting;
+ uint8_t feedforwardCutoffSetting;
+
uint16_t setpointCutoffFrequency;
uint16_t throttleCutoffFrequency;
- uint8_t ffCutoffSetting;
uint16_t feedforwardCutoffFrequency;
- int averageFrameTimeUs;
- rcSmoothingFilterTraining_t training;
+
+ float smoothedRxRateHz;
+ uint8_t sampleCount;
uint8_t debugAxis;
- uint8_t autoSmoothnessFactorSetpoint;
- uint8_t autoSmoothnessFactorThrottle;
+
+ float autoSmoothnessFactorSetpoint;
+ float autoSmoothnessFactorFeedforward;
+ float autoSmoothnessFactorThrottle;
} rcSmoothingFilter_t;
typedef struct rcControlsConfig_s {
diff --git a/src/main/flight/feedforward.c b/src/main/flight/feedforward.c
deleted file mode 100644
index 28d1aabfd0..0000000000
--- a/src/main/flight/feedforward.c
+++ /dev/null
@@ -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 .
- */
-
-#include
-#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
diff --git a/src/main/flight/feedforward.h b/src/main/flight/feedforward.h
deleted file mode 100644
index 009410f082..0000000000
--- a/src/main/flight/feedforward.h
+++ /dev/null
@@ -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 .
- */
-
-#pragma once
-
-#include
-
-#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);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index c232aeae7d..86eca7936e 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -50,7 +50,6 @@
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/rpm_filter.h"
-#include "flight/feedforward.h"
#include "io/gps.h"
@@ -130,7 +129,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
[PID_ROLL] = PID_ROLL_DEFAULT,
[PID_PITCH] = PID_PITCH_DEFAULT,
[PID_YAW] = PID_YAW_DEFAULT,
- [PID_LEVEL] = { 50, 50, 75, 50 },
+ [PID_LEVEL] = { 50, 75, 75, 50 },
[PID_MAG] = { 40, 0, 0, 0 },
},
.pidSumLimit = PIDSUM_LIMIT,
@@ -228,6 +227,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.tpa_breakpoint = 1350,
.angle_feedforward_smoothing_ms = 80,
.angle_earth_ref = 100,
+ .horizon_delay_ms = 500, // 500ms time constant on any increase in horizon strength
);
#ifndef USE_D_MIN
@@ -265,28 +265,6 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState)
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)
{
for (int axis = 0; axis < 3; axis++) {
@@ -368,99 +346,50 @@ float pidApplyThrustLinearization(float motorOutput)
#endif
#if defined(USE_ACC)
-// calculate the stick deflection while applying level mode expo
-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
+// Calculate strength of horizon leveling; 0 = none, 1.0 = most leveling
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):
- float horizonLevelStrength = MAX((pidRuntime.horizonLimitDegrees - currentInclination) / pidRuntime.horizonLimitDegrees, 0.0f);
- // 1.0 when attitude is 'flat', 0 when angle is equal to, or greater than, horizonLimitDegrees
+ float absMaxStickDeflection = MAX(fabsf(getRcDeflection(FD_ROLL)), fabsf(getRcDeflection(FD_PITCH)));
+ // 0-1, smoothed if RC smoothing is enabled
- if (!pidRuntime.horizonIgnoreSticks) {
- // horizonIgnoreSticks: 0 = default; levelling attenuated by both attitude and sticks;
- // 1 = level attenuation only by attitude
- const float absMaxStickDeflection = MAX(fabsf(getRcDeflection(FD_ROLL)), fabsf(getRcDeflection(FD_PITCH))); // 0-1, smoothed if RC smoothing is enabled
- const float horizonStickAttenuation = MAX((pidRuntime.horizonLimitSticks - absMaxStickDeflection) / pidRuntime.horizonLimitSticks, 0.0f);
- // 1.0 at center stick, 0.0 at max stick deflection:
- horizonLevelStrength *= horizonStickAttenuation * pidRuntime.horizonGain;
+ float horizonLevelStrength = MAX((pidRuntime.horizonLimitDegrees - currentInclination) * pidRuntime.horizonLimitDegreesInv, 0.0f);
+ // 1.0 when attitude is 'flat', 0 when angle is equal to, or greater than, horizonLimitDegrees
+ horizonLevelStrength *= MAX((pidRuntime.horizonLimitSticks - absMaxStickDeflection) * pidRuntime.horizonLimitSticksInv, pidRuntime.horizonIgnoreSticks);
+ // use the value of horizonIgnoreSticks to enable/disable this effect.
+ // value should be 1.0 at center stick, 0.0 at max stick deflection:
+ horizonLevelStrength *= pidRuntime.horizonGain;
+
+ if (pidRuntime.horizonDelayMs) {
+ const float horizonLevelStrengthSmoothed = pt1FilterApply(&pidRuntime.horizonSmoothingPt1, horizonLevelStrength);
+ horizonLevelStrength = MIN(horizonLevelStrength, horizonLevelStrengthSmoothed);
}
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.
// 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.
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;
-
- // ** angle loop feedforward
float angleFeedforward = 0.0f;
+
#ifdef USE_FEEDFORWARD
- const float rxRateHz = 1e6f / getCurrentRxRefreshRate();
- const float rcCommandDeltaAbs = fabsf(getRcCommandDelta(axis));
-
- 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 = angleLimit * getFeedforward(axis) * pidRuntime.angleFeedforwardGain * pidRuntime.maxRcRateInv[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
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
angleTarget += gpsRescueAngle[axis] / 100.0f; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch
#endif
@@ -468,25 +397,23 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
const float errorAngle = angleTarget - currentAngle;
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
- float axisCoordination = pidRuntime.angleYawSetpoint;
- if (pidRuntime.angleEarthRef) {
- const float sinAngle = sin_approx(DEGREES_TO_RADIANS(pidRuntime.angleTarget[axis == FD_ROLL ? FD_PITCH : FD_ROLL]));
- pidRuntime.angleTarget[axis] = angleTarget; // store target for alternate axis to current axis, for use in preceding calculation
- axisCoordination *= (axis == FD_ROLL) ? -sinAngle : sinAngle; // must be negative for Roll
- angleRate += axisCoordination * pidRuntime.angleEarthRef;
- }
+ // earthRef code here takes about 76 cycles, if conditional on angleEarthRef it takes about 100. sin_approx costs most of those cycles.
+ 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
+ angleRate += pidRuntime.angleYawSetpoint * sinAngle * pidRuntime.angleEarthRef;
+ pidRuntime.angleTarget[axis] = angleTarget; // set target for alternate axis to current axis, for use in preceding calculation
// 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
angleRate = pt3FilterApply(&pidRuntime.attitudeFilter[axis], angleRate);
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) {
- rawSetpoint = angleRate;
+ currentPidSetpoint = angleRate;
} else {
// 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
@@ -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_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_SET(DEBUG_ANGLE_TARGET, 3, lrintf(currentAngle * 10.0f)); // angle returned
}
DEBUG_SET(DEBUG_CURRENT_ANGLE, axis, lrintf(currentAngle * 10.0f)); // current angle
- return rawSetpoint;
+ return currentPidSetpoint;
}
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_ABSOLUTE_CONTROL)
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();
#endif
-const bool newRcFrame = getShouldUpdateFeedforward();
-
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
float currentPidSetpoint = getSetpointRate(axis);
- float rawSetpoint = getRawSetpoint(axis);
- bool rawSetpointIsSmoothed = false;
if (pidRuntime.maxVelocity[axis]) {
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
}
// 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
#if defined(USE_ACC)
- // earth reference yaw by attenuating yaw rate at higher angles
- if (axis == FD_YAW && pidRuntime.angleEarthRef) {
- pidRuntime.angleYawSetpoint = currentPidSetpoint;
- if (levelMode == LEVEL_MODE_RP) {
- float maxAngleTargetAbs = fmaxf( fabsf(pidRuntime.angleTarget[FD_ROLL]), fabsf(pidRuntime.angleTarget[FD_PITCH]) );
- maxAngleTargetAbs *= pidRuntime.angleEarthRef;
- if (FLIGHT_MODE(HORIZON_MODE)) {
- // reduce yaw compensation when Horizon uses less levelling
- maxAngleTargetAbs *= horizonLevelStrength;
- }
- float attenuateYawSetpoint = cos_approx(DEGREES_TO_RADIANS(maxAngleTargetAbs));
- currentPidSetpoint *= attenuateYawSetpoint;
- rawSetpoint *= attenuateYawSetpoint;
- DEBUG_SET(DEBUG_ANGLE_TARGET, 2, lrintf(attenuateYawSetpoint * 100.0f)); // yaw attenuation
+ pidRuntime.axisInAngleMode[axis] = false;
+ if (axis < FD_YAW) {
+ if (levelMode == LEVEL_MODE_RP || (levelMode == LEVEL_MODE_R && axis == FD_ROLL)) {
+ pidRuntime.axisInAngleMode[axis] = true;
+ currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint, horizonLevelStrength);
+ }
+ } else { // yaw axis only
+ if (levelMode == LEVEL_MODE_RP) {
+ // if earth referencing is requested, attenuate yaw axis setpoint when pitched or rolled
+ // and send yawSetpoint to Angle code to modulate pitch and roll
+ // code cost is 107 cycles when earthRef enabled, 20 otherwise, nearly all in cos_approx
+ if (pidRuntime.angleEarthRef) {
+ pidRuntime.angleYawSetpoint = currentPidSetpoint;
+ 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
@@ -1096,17 +999,32 @@ const bool newRcFrame = getShouldUpdateFeedforward();
const float iTermChange = (Ki + pidRuntime.itermAccelerator) * dynCi * pidRuntime.dT * itermErrorRate;
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
+
+ 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
if ((pidRuntime.pidCoefficient[axis].Kd > 0) && !launchControlActive) {
-
// Divide rate change by dT to get differential (ie dr/dt).
// dT is fixed and calculated from the target PID loop time
// This is done to avoid DTerm spikes that occur with dynamically
@@ -1171,18 +1089,8 @@ const bool newRcFrame = getShouldUpdateFeedforward();
// no feedforward in launch control
float feedforwardGain = launchControlActive ? 0.0f : pidRuntime.pidCoefficient[axis].Kf;
if (feedforwardGain > 0) {
- // transition now calculated in feedforward.c when new RC data arrives
- float feedForward = feedforwardGain * pidSetpointDelta * pidRuntime.pidFrequency;
-
-#ifdef USE_FEEDFORWARD
- pidData[axis].F = shouldApplyFeedforwardLimits(axis) ?
- applyFeedforwardLimit(axis, feedForward, pidRuntime.pidCoefficient[axis].Kp, currentPidSetpoint) : feedForward;
-#else
+ float feedForward = feedforwardGain * pidSetpointDelta;
pidData[axis].F = feedForward;
-#endif
-#ifdef USE_RC_SMOOTHING_FILTER
- pidData[axis].F = applyRcSmoothingFeedforwardFilter(axis, pidData[axis].F);
-#endif // USE_RC_SMOOTHING_FILTER
} else {
pidData[axis].F = 0;
}
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 5d1a930b84..5511245361 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -236,6 +236,7 @@ typedef struct pidProfile_s {
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_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;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -300,8 +301,10 @@ typedef struct pidRuntime_s {
float angleFeedforwardGain;
float horizonGain;
float horizonLimitSticks;
+ float horizonLimitSticksInv;
float horizonLimitDegrees;
- uint8_t horizonIgnoreSticks;
+ float horizonLimitDegreesInv;
+ float horizonIgnoreSticks;
float maxVelocity[XYZ_AXIS_COUNT];
float itermWindupPointInv;
bool inCrashRecoveryMode;
@@ -349,13 +352,6 @@ typedef struct pidRuntime_s {
pt1Filter_t airmodeThrottleLpf2;
#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
float acroTrainerAngleLimit;
float acroTrainerLookaheadTime;
@@ -393,23 +389,26 @@ typedef struct pidRuntime_s {
#endif
#ifdef USE_FEEDFORWARD
- float feedforwardTransitionFactor;
feedforwardAveraging_t feedforwardAveraging;
float feedforwardSmoothFactor;
- float feedforwardJitterFactor;
+ uint8_t feedforwardJitterFactor;
+ float feedforwardJitterFactorInv;
float feedforwardBoostFactor;
- float angleFeedforward[XYZ_AXIS_COUNT];
- float angleTargetPrevious[XYZ_AXIS_COUNT];
- float angleTargetDelta[XYZ_AXIS_COUNT];
- uint8_t angleDuplicateCount[XYZ_AXIS_COUNT];
+ float feedforwardTransition;
+ float feedforwardTransitionInv;
+ uint8_t feedforwardMaxRateLimit;
+ pt3Filter_t angleFeedforwardPt3[XYZ_AXIS_COUNT];
#endif
#ifdef USE_ACC
pt3Filter_t attitudeFilter[2]; // Only for ROLL and PITCH
- pt3Filter_t angleFeedforwardPt3[XYZ_AXIS_COUNT];
+ pt1Filter_t horizonSmoothingPt1;
+ uint16_t horizonDelayMs;
float angleYawSetpoint;
float angleEarthRef;
float angleTarget[2];
+ bool axisInAngleMode[3];
+ float maxRcRateInv[2];
#endif
} 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 rotateItermAndAxisError();
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);
#endif
+
void dynLpfDTermUpdate(float throttle);
void pidSetItermReset(bool enabled);
float pidGetPreviousSetpoint(int axis);
float pidGetDT();
float pidGetPidFrequency();
-float pidGetFeedforwardBoostFactor();
-float pidGetFeedforwardSmoothFactor();
-float pidGetFeedforwardJitterFactor();
-float pidGetFeedforwardTransitionFactor();
+
float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);
diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c
index 8924319815..5327431190 100644
--- a/src/main/flight/pid_init.c
+++ b/src/main/flight/pid_init.c
@@ -35,8 +35,8 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
+#include "fc/rc.h"
-#include "flight/feedforward.h"
#include "flight/pid.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 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);
+ 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
pt3FilterInit(&pidRuntime.attitudeFilter[axis], k);
pt3FilterInit(&pidRuntime.angleFeedforwardPt3[axis], k2);
+ pidRuntime.maxRcRateInv[axis] = 1.0f / getMaxRcRate(axis);
}
+ pidRuntime.angleYawSetpoint = 0.0f;
#endif
pt2FilterInit(&pidRuntime.antiGravityLpf, pt2FilterGain(pidProfile->anti_gravity_cutoff_hz, pidRuntime.dT));
@@ -259,28 +267,6 @@ void pidInit(const pidProfile_t *pidProfile)
#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)
{
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.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.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.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_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT;
@@ -421,20 +411,15 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#endif
#ifdef USE_FEEDFORWARD
- if (pidProfile->feedforward_transition == 0) {
- pidRuntime.feedforwardTransitionFactor = 0;
- } else {
- pidRuntime.feedforwardTransitionFactor = 100.0f / pidProfile->feedforward_transition;
- }
+ pidRuntime.feedforwardTransition = pidProfile->feedforward_transition / 100.0f;
+ pidRuntime.feedforwardTransitionInv = (pidProfile->feedforward_transition == 0) ? 0.0f : 100.0f / pidProfile->feedforward_transition;
pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging;
- pidRuntime.feedforwardSmoothFactor = 1.0f;
- if (pidProfile->feedforward_smooth_factor) {
- pidRuntime.feedforwardSmoothFactor = 1.0f - ((float)pidProfile->feedforward_smooth_factor) / 100.0f;
- }
+ pidRuntime.feedforwardSmoothFactor = 1.0f - (0.01f * pidProfile->feedforward_smooth_factor);
pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor;
- pidRuntime.feedforwardBoostFactor = (float)pidProfile->feedforward_boost / 10.0f;
-
- feedforwardInit(pidProfile);
+ 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
+ pidRuntime.feedforwardBoostFactor = 0.1f * pidProfile->feedforward_boost;
+ pidRuntime.feedforwardMaxRateLimit = pidProfile->feedforward_max_rate_limit;
#endif
pidRuntime.levelRaceMode = pidProfile->level_race_mode;
diff --git a/src/main/flight/pid_init.h b/src/main/flight/pid_init.h
index ebd265ff74..3471e9212b 100644
--- a/src/main/flight/pid_init.h
+++ b/src/main/flight/pid_init.h
@@ -24,6 +24,4 @@ void pidInit(const pidProfile_t *pidProfile);
void pidInitFilters(const pidProfile_t *pidProfile);
void pidInitConfig(const pidProfile_t *pidProfile);
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);
diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc
index 853278c206..a81a176a3c 100644
--- a/src/test/unit/cli_unittest.cc
+++ b/src/test/unit/cli_unittest.cc
@@ -310,7 +310,7 @@ uint8_t __config_end = 0x10;
uint16_t averageSystemLoadPercent = 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; }
const char *armingDisableFlagNames[]= {
@@ -368,6 +368,6 @@ bool isModeActivationConditionConfigured(const modeActivationCondition_t *, cons
void delay(uint32_t) {}
displayPort_t *osdGetDisplayPort(osdDisplayPortDevice_e *) { return NULL; }
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; }
}
diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc
index e313f3a89b..05942b6729 100644
--- a/src/test/unit/pid_unittest.cc
+++ b/src/test/unit/pid_unittest.cc
@@ -30,8 +30,8 @@ float simulatedPrevSetpointRate[3] = { 0,0,0 };
float simulatedRcDeflection[3] = { 0,0,0 };
float simulatedRcCommandDelta[3] = { 1,1,1 };
float simulatedRawSetpoint[3] = { 0,0,0 };
-uint16_t simulatedCurrentRxRefreshRate = 10000;
-uint8_t simulatedDuplicateCount = 0;
+float simulatedMaxRate[3] = { 670,670,670 };
+float simulatedFeedforward[3] = { 0,0,0 };
float simulatedMotorMixRange = 0.0f;
int16_t debug[DEBUG16_VALUE_COUNT];
@@ -88,38 +88,20 @@ extern "C" {
void systemBeep(bool) { }
bool gyroOverflowDetected(void) { return false; }
float getRcDeflection(int axis) { return simulatedRcDeflection[axis]; }
- float getRcCommandDelta(int axis) { return simulatedRcCommandDelta[axis]; }
float getRcDeflectionRaw(int axis) { return simulatedRcDeflection[axis]; }
float getRawSetpoint(int axis) { return simulatedRawSetpoint[axis]; }
- uint16_t getCurrentRxRefreshRate(void) { return simulatedCurrentRxRefreshRate; }
- uint8_t getFeedforwardDuplicateCount(void) { return simulatedDuplicateCount; }
+ float getFeedforward(int axis) {
+ return simulatedSetpointRate[axis] - simulatedPrevSetpointRate[axis];
+ }
void beeperConfirmationBeeps(uint8_t) { }
bool isLaunchControlActive(void) {return unitLaunchControlActive; }
void disarm(flightLogDisarmReason_e) { }
- float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint)
+ float getMaxRcRate(int axis)
{
UNUSED(axis);
- UNUSED(Kp);
- UNUSED(currentPidSetpoint);
- return value;
+ float maxRate = simulatedMaxRate[axis];
+ return maxRate;
}
- 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) { }
}
@@ -217,9 +199,6 @@ void resetTest(void)
pidInit(pidProfile);
loadControlRateProfile();
- currentControlRateProfile->levelExpo[FD_ROLL] = 0;
- currentControlRateProfile->levelExpo[FD_PITCH] = 0;
-
// Run pidloop for a while after reset
for (int loop = 0; loop < 20; loop++) {
pidController(pidProfile, currentTestTime());
@@ -389,54 +368,56 @@ TEST(pidControllerTest, testPidLevel)
// Test Angle mode response
enableFlightMode(ANGLE_MODE);
- float currentPidSetpoint = 30;
+ float currentPidSetpointRoll = 0;
+ float currentPidSetpointPitch = 0;
+ float calculatedAngleSetpoint = 0;
rollAndPitchTrims_t angleTrim = { { 0, 0 } };
- currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
- EXPECT_FLOAT_EQ(0, currentPidSetpoint);
- currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
- EXPECT_FLOAT_EQ(0, currentPidSetpoint);
+ calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
+ EXPECT_FLOAT_EQ(0, calculatedAngleSetpoint);
+ calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
+ 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
- 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.pitch = 275;
- currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
- EXPECT_FLOAT_EQ(-19.130268, currentPidSetpoint);
- currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
- EXPECT_FLOAT_EQ(19.130268, currentPidSetpoint);
+ calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
+ EXPECT_FLOAT_EQ(242.76686, calculatedAngleSetpoint);
+ calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
+ EXPECT_FLOAT_EQ(-242.76686, calculatedAngleSetpoint);
// Disable ANGLE_MODE
disableFlightMode(ANGLE_MODE);
- currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
- currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
- EXPECT_FLOAT_EQ(14.693689, currentPidSetpoint);
+ calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
+ EXPECT_FLOAT_EQ(393.44571, calculatedAngleSetpoint);
+ calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
+ EXPECT_FLOAT_EQ(-392.88422, calculatedAngleSetpoint);
// Test level mode expo
enableFlightMode(ANGLE_MODE);
attitude.values.roll = 0;
attitude.values.pitch = 0;
- setStickPosition(FD_ROLL, 0.5f);
- setStickPosition(FD_PITCH, -0.5f);
- currentControlRateProfile->levelExpo[FD_ROLL] = 50;
- currentControlRateProfile->levelExpo[FD_PITCH] = 26;
- currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
- EXPECT_FLOAT_EQ(45.520832, currentPidSetpoint);
- currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength(), true);
- EXPECT_FLOAT_EQ(-61.216412, currentPidSetpoint);
+ currentPidSetpointRoll = 400;
+ currentPidSetpointPitch = -400;
+ // need to set some rates type and some expo here ??? HELP !!
+ calculatedAngleSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpointRoll, calcHorizonLevelStrength());
+ EXPECT_FLOAT_EQ(231.55479, calculatedAngleSetpoint);
+ calculatedAngleSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpointPitch, calcHorizonLevelStrength());
+ EXPECT_FLOAT_EQ(-231.55479, calculatedAngleSetpoint);
}
@@ -452,21 +433,20 @@ TEST(pidControllerTest, testPidHorizon)
setStickPosition(FD_PITCH, -0.76f);
EXPECT_FLOAT_EQ(0.0f, calcHorizonLevelStrength());
- // Expect full rate output on full stick
- // Test zero stick response
+ // Return sticks to center, should expect some levelling, but will be delayed
setStickPosition(FD_ROLL, 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_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
setStickPosition(FD_ROLL, 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
attitude.values.roll = 900;
@@ -476,17 +456,17 @@ TEST(pidControllerTest, testPidHorizon)
setStickPosition(FD_ROLL, 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
- EXPECT_NEAR(0.166f, calcHorizonLevelStrength(), calculateTolerance(0.166));
+ EXPECT_NEAR(0.0193f, calcHorizonLevelStrength(), calculateTolerance(0.0193));
// Test small stick response at 90 degrees
setStickPosition(FD_ROLL, 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
setStickPosition(FD_ROLL, 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
attitude.values.roll = 1200;
@@ -495,18 +475,18 @@ TEST(pidControllerTest, testPidHorizon)
// Test centered sticks at 120 degrees
setStickPosition(FD_ROLL, 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
setStickPosition(FD_ROLL, 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
setStickPosition(FD_ROLL, 0.5f);
setStickPosition(FD_PITCH, -0.5f);
EXPECT_NEAR(0.018f, calcHorizonLevelStrength(), calculateTolerance(0.018));
-
+
// set attitude of craft to 1500 degrees, outside limit of 135
attitude.values.roll = 1500;
attitude.values.pitch = 1500;
@@ -619,6 +599,8 @@ TEST(pidControllerTest, testCrashRecoveryMode)
}
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();
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_YAW].F);
- // Match the stick to gyro to stop error
+ // Move the sticks fully
setStickPosition(FD_ROLL, 1.0f);
setStickPosition(FD_PITCH, -1.0f);
setStickPosition(FD_YAW, -1.0f);
pidController(pidProfile, currentTestTime());
- EXPECT_NEAR(2232.78, pidData[FD_ROLL].F, calculateTolerance(2232.78));
- EXPECT_NEAR(-2061.03, pidData[FD_PITCH].F, calculateTolerance(-2061.03));
- EXPECT_NEAR(-2061.03, pidData[FD_YAW].F, calculateTolerance(-2061.03));
+ EXPECT_NEAR(17.86, pidData[FD_ROLL].F, calculateTolerance(17.86));
+ EXPECT_NEAR(-16.49, pidData[FD_PITCH].F, calculateTolerance(-16.49));
+ 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_PITCH, -0.5f);
setStickPosition(FD_YAW, -0.5f);
pidController(pidProfile, currentTestTime());
- EXPECT_NEAR(-558.20, pidData[FD_ROLL].F, calculateTolerance(-558.20));
- EXPECT_NEAR(515.26, pidData[FD_PITCH].F, calculateTolerance(515.26));
- EXPECT_NEAR(515.26, pidData[FD_YAW].F, calculateTolerance(515.26));
+ EXPECT_NEAR(-8.93, pidData[FD_ROLL].F, calculateTolerance(-8.93));
+ EXPECT_NEAR(8.24, pidData[FD_PITCH].F, calculateTolerance(8.24));
+ 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_PITCH, 0.0f);
setStickPosition(FD_YAW, 0.0f);
- for (int loop = 0; loop <= 15; loop++) {
- gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL];
- pidController(pidProfile, currentTestTime());
- }
+ 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_PITCH].F);