diff --git a/make/source.mk b/make/source.mk
index 59da75e8ff..04de19c25d 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -90,6 +90,7 @@ COMMON_SRC = \
flight/gps_rescue.c \
flight/gyroanalyse.c \
flight/imu.c \
+ flight/interpolated_setpoint.c \
flight/mixer.c \
flight/mixer_tricopter.c \
flight/pid.c \
diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index 148e1e9031..f426db5729 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -93,4 +93,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"BARO",
"GPS_RESCUE_THROTTLE_PID",
"DYN_IDLE",
+ "FF_LIMIT",
+ "FF_INTERPOLATED",
};
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 0d1c4222a2..71fc0c1b46 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -109,6 +109,8 @@ typedef enum {
DEBUG_BARO,
DEBUG_GPS_RESCUE_THROTTLE_PID,
DEBUG_DYN_IDLE,
+ DEBUG_FF_LIMIT,
+ DEBUG_FF_INTERPOLATED,
DEBUG_COUNT
} debugType_e;
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 509c45e4af..bcd3da3c44 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -1060,6 +1060,13 @@ const clivalue_t valueTable[] = {
#ifdef USE_AIRMODE_LPF
{ "transient_throttle_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, transient_throttle_limit) },
#endif
+#ifdef USE_INTERPOLATED_SP
+ { "ff_interpolate_sp", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_sp) },
+ { "ff_spread", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 50}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_spread) },
+ { "ff_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_max_rate_limit) },
+ { "ff_lookahead_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 255}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_lookahead_limit) },
+#endif
+ { "ff_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) },
#ifdef USE_DYN_IDLE
{ "idle_min_rpm", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_min_rpm) },
diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c
index e9e30692b8..b3f407462a 100644
--- a/src/main/fc/rc.c
+++ b/src/main/fc/rc.c
@@ -53,6 +53,12 @@
typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs);
+#ifdef USE_INTERPOLATED_SP
+// Setpoint in degrees/sec before RC-Smoothing is applied
+static float rawSetpoint[XYZ_AXIS_COUNT];
+// Stick deflection [-1.0, 1.0] before RC-Smoothing is applied
+static float rawDeflection[XYZ_AXIS_COUNT];
+#endif
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
static float throttlePIDAttenuation;
static bool reverseMotors = false;
@@ -60,6 +66,7 @@ static applyRatesFn *applyRates;
uint16_t currentRxRefreshRate;
FAST_RAM_ZERO_INIT uint8_t interpolationChannels;
+static FAST_RAM_ZERO_INIT uint32_t rcFrameNumber;
enum {
ROLL_FLAG = 1 << ROLL,
@@ -82,6 +89,11 @@ enum {
static FAST_RAM_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
#endif // USE_RC_SMOOTHING_FILTER
+uint32_t getRcFrameNumber()
+{
+ return rcFrameNumber;
+}
+
float getSetpointRate(int axis)
{
return setpointRate[axis];
@@ -102,6 +114,18 @@ float getThrottlePIDAttenuation(void)
return throttlePIDAttenuation;
}
+#ifdef USE_INTERPOLATED_SP
+float getRawSetpoint(int axis)
+{
+ return rawSetpoint[axis];
+}
+
+float getRawDeflection(int axis)
+{
+ return rawDeflection[axis];
+}
+#endif
+
#define THROTTLE_LOOKUP_LENGTH 12
static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
@@ -152,13 +176,18 @@ float applyKissRates(const int axis, float rcCommandf, const float rcCommandfAbs
{
const float rcCurvef = currentControlRateProfile->rcExpo[axis] / 100.0f;
- float kissRpyUseRates = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
+ float kissRpyUseRates = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
float kissRcCommandf = (power3(rcCommandf) * rcCurvef + rcCommandf * (1 - rcCurvef)) * (currentControlRateProfile->rcRates[axis] / 1000.0f);
float kissAngle = constrainf(((2000.0f * kissRpyUseRates) * kissRcCommandf), -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
return kissAngle;
}
+float applyCurve(int axis, float deflection)
+{
+ return applyRates(axis, deflection, fabsf(deflection));
+}
+
static void calculateSetpointRate(int axis)
{
float angleRate;
@@ -561,10 +590,25 @@ FAST_CODE void processRcCommand(void)
{
uint8_t updatedChannel;
+ if (isRXDataNew) {
+ rcFrameNumber++;
+ }
+
if (isRXDataNew && pidAntiGravityEnabled()) {
checkForThrottleErrorResetState(currentRxRefreshRate);
}
+#ifdef USE_INTERPOLATED_SP
+ if (isRXDataNew) {
+ for (int i = FD_ROLL; i <= FD_YAW; i++) {
+ const float rcCommandf = rcCommand[i] / 500.0f;
+ const float rcCommandfAbs = fabsf(rcCommandf);
+ rawSetpoint[i] = applyRates(i, rcCommandf, rcCommandfAbs);
+ rawDeflection[i] = rcCommandf;
+ }
+ }
+#endif
+
switch (rxConfig()->rc_smoothing_type) {
#ifdef USE_RC_SMOOTHING_FILTER
case RC_SMOOTHING_TYPE_FILTER:
diff --git a/src/main/fc/rc.h b/src/main/fc/rc.h
index e8a7bcc0f2..8278bab2f7 100644
--- a/src/main/fc/rc.h
+++ b/src/main/fc/rc.h
@@ -45,3 +45,7 @@ bool rcSmoothingIsEnabled(void);
rcSmoothingFilter_t *getRcSmoothingData(void);
bool rcSmoothingAutoCalculate(void);
bool rcSmoothingInitializationComplete(void);
+float getRawSetpoint(int axis);
+float getRawDeflection(int axis);
+float applyCurve(int axis, float deflection);
+uint32_t getRcFrameNumber();
diff --git a/src/main/flight/interpolated_setpoint.c b/src/main/flight/interpolated_setpoint.c
new file mode 100644
index 0000000000..4f1af50fe8
--- /dev/null
+++ b/src/main/flight/interpolated_setpoint.c
@@ -0,0 +1,151 @@
+/*
+ * 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 "platform.h"
+
+#ifdef USE_INTERPOLATED_SP
+#include
+
+#include "build/debug.h"
+#include "common/maths.h"
+#include "fc/rc.h"
+#include "flight/interpolated_setpoint.h"
+
+static float projectedSetpoint[XYZ_AXIS_COUNT];
+static float prevSetpointSpeed[XYZ_AXIS_COUNT];
+static float prevRawSetpoint[XYZ_AXIS_COUNT];
+static float prevRawDeflection[XYZ_AXIS_COUNT];
+static uint16_t interpolationSteps[XYZ_AXIS_COUNT];
+static float setpointChangePerIteration[XYZ_AXIS_COUNT];
+static float deflectionChangePerIteration[XYZ_AXIS_COUNT];
+static float setpointReservoir[XYZ_AXIS_COUNT];
+static float deflectionReservoir[XYZ_AXIS_COUNT];
+
+// Configuration
+static float ffLookaheadLimit;
+static float ffSpread;
+static float ffMaxRateLimit[XYZ_AXIS_COUNT];
+static float ffMaxRate[XYZ_AXIS_COUNT];
+
+void interpolatedSpInit(const pidProfile_t *pidProfile) {
+ const float ffMaxRateScale = pidProfile->ff_max_rate_limit * 0.01f;
+ ffLookaheadLimit = pidProfile->ff_lookahead_limit * 0.0001f;
+ ffSpread = pidProfile->ff_spread;
+ for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
+ ffMaxRate[i] = applyCurve(i, 1.0f);
+ ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale;
+ }
+}
+
+FAST_CODE_NOINLINE float interpolatedSpApply(int axis, float pidFrequency, bool newRcFrame) {
+ const float rawSetpoint = getRawSetpoint(axis);
+ const float rawDeflection = getRawDeflection(axis);
+
+ float pidSetpointDelta = 0.0f;
+ static int iterationsSinceLastUpdate[XYZ_AXIS_COUNT];
+ if (newRcFrame) {
+
+ setpointReservoir[axis] -= iterationsSinceLastUpdate[axis] * setpointChangePerIteration[axis];
+ deflectionReservoir[axis] -= iterationsSinceLastUpdate[axis] * deflectionChangePerIteration[axis];
+ iterationsSinceLastUpdate[axis] = 0;
+
+ // get the number of interpolation steps either dynamically based on RX refresh rate
+ // or manually based on ffSpread configuration property
+ if (ffSpread) {
+ interpolationSteps[axis] = (uint16_t) ((ffSpread + 1.0f) * 0.001f * pidFrequency);
+ } else {
+ interpolationSteps[axis] = (uint16_t) ((currentRxRefreshRate + 1000) * pidFrequency * 1e-6f + 0.5f);
+ }
+
+ // interpolate stick deflection
+ deflectionReservoir[axis] += rawDeflection - prevRawDeflection[axis];
+ deflectionChangePerIteration[axis] = deflectionReservoir[axis] / interpolationSteps[axis];
+ const float projectedStickPos =
+ rawDeflection + deflectionChangePerIteration[axis] * pidFrequency * ffLookaheadLimit;
+ projectedSetpoint[axis] = applyCurve(axis, projectedStickPos);
+ prevRawDeflection[axis] = rawDeflection;
+
+ // apply linear interpolation on setpoint
+ setpointReservoir[axis] += rawSetpoint - prevRawSetpoint[axis];
+ const float ffBoostFactor = pidGetFfBoostFactor();
+ if (ffBoostFactor != 0.0f) {
+ const float speed = rawSetpoint - prevRawSetpoint[axis];
+ if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || fabsf(3.0f * speed) > fabsf(prevSetpointSpeed[axis])) {
+ const float setpointAcc = speed - prevSetpointSpeed[axis];
+ setpointReservoir[axis] += ffBoostFactor * setpointAcc;
+ }
+ prevSetpointSpeed[axis] = speed;
+ }
+
+ setpointChangePerIteration[axis] = setpointReservoir[axis] / interpolationSteps[axis];
+
+ prevRawSetpoint[axis] = rawSetpoint;
+
+ if (axis == FD_ROLL) {
+ DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, rawDeflection * 100);
+ DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, projectedStickPos * 100);
+ DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, projectedSetpoint[axis]);
+ }
+ }
+
+ if (iterationsSinceLastUpdate[axis] < interpolationSteps[axis]) {
+ iterationsSinceLastUpdate[axis]++;
+ pidSetpointDelta = setpointChangePerIteration[axis];
+ }
+
+ return pidSetpointDelta;
+}
+
+FAST_CODE_NOINLINE float applyFFLimit(int axis, float value, float Kp, float currentPidSetpoint) {
+ if (axis == FD_ROLL) {
+ DEBUG_SET(DEBUG_FF_LIMIT, 0, value);
+ }
+
+ if (ffLookaheadLimit) {
+ const float limit = fabsf((projectedSetpoint[axis] - prevRawSetpoint[axis]) * Kp);
+ value = constrainf(value, -limit, limit);
+ if (axis == FD_ROLL) {
+ DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, projectedSetpoint[axis]);
+ }
+ }
+ if (axis == FD_ROLL) {
+ DEBUG_SET(DEBUG_FF_LIMIT, 1, value);
+ }
+
+ if (ffMaxRateLimit[axis]) {
+ if (fabsf(currentPidSetpoint) <= ffMaxRateLimit[axis]) {
+ value = constrainf(value, (-ffMaxRateLimit[axis] - currentPidSetpoint) * Kp, (ffMaxRateLimit[axis] - currentPidSetpoint) * Kp);
+ } else {
+ value = 0;
+ }
+ }
+ if (axis == FD_ROLL) {
+ DEBUG_SET(DEBUG_FF_LIMIT, 2, value);
+ }
+ return value;
+}
+
+bool shouldApplyFFLimits(int axis)
+{
+ return ffLookaheadLimit != 0.0f || ffMaxRateLimit[axis] != 0.0f;
+}
+
+
+#endif
diff --git a/src/main/flight/interpolated_setpoint.h b/src/main/flight/interpolated_setpoint.h
new file mode 100644
index 0000000000..f9f7a4a97a
--- /dev/null
+++ b/src/main/flight/interpolated_setpoint.h
@@ -0,0 +1,31 @@
+/*
+ * 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"
+
+void interpolatedSpInit(const pidProfile_t *pidProfile);
+float interpolatedSpApply(int axis, float pidFrequency, bool newRcFrame);
+float applyFFLimit(int axis, float value, float Kp, float currentPidSetpoint);
+bool shouldApplyFFLimits(int axis);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 817ce52b3a..97464593bc 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -49,6 +49,7 @@
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/rpm_filter.h"
+#include "flight/interpolated_setpoint.h"
#include "io/gps.h"
@@ -211,6 +212,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
.idle_p = 50,
.idle_pid_limit = 200,
.idle_max_increase = 150,
+ .ff_interpolate_sp = 0,
+ .ff_spread = 0,
+ .ff_max_rate_limit = 0,
+ .ff_lookahead_limit = 0,
+ .ff_boost = 15,
);
#ifndef USE_D_MIN
pidProfile->pid[PID_ROLL].D = 30;
@@ -285,6 +291,7 @@ static FAST_RAM_ZERO_INIT float acLimit;
static FAST_RAM_ZERO_INIT float acErrorLimit;
static FAST_RAM_ZERO_INIT float acCutoff;
static FAST_RAM_ZERO_INIT pt1Filter_t acLpf[XYZ_AXIS_COUNT];
+static FAST_RAM_ZERO_INIT float oldSetpointCorrection[XYZ_AXIS_COUNT];
#endif
#if defined(USE_D_MIN)
@@ -307,6 +314,14 @@ static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2;
static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
+static FAST_RAM_ZERO_INIT float ffBoostFactor;
+
+float pidGetFfBoostFactor()
+{
+ return ffBoostFactor;
+}
+
+
void pidInitFilters(const pidProfile_t *pidProfile)
{
STATIC_ASSERT(FD_YAW == 2, FD_YAW_incorrect); // ensure yaw axis is 2
@@ -443,6 +458,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
#endif
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
+
+ ffBoostFactor = (float)pidProfile->ff_boost / 10.0f;
}
#ifdef USE_RC_SMOOTHING_FILTER
@@ -565,6 +582,10 @@ static FAST_RAM_ZERO_INIT float dMinGyroGain;
static FAST_RAM_ZERO_INIT float dMinSetpointGain;
#endif
+#ifdef USE_INTERPOLATED_SP
+static FAST_RAM_ZERO_INIT bool ffFromInterpolatedSetpoint;
+#endif
+
void pidInitConfig(const pidProfile_t *pidProfile)
{
if (pidProfile->feedForwardTransition == 0) {
@@ -708,6 +729,10 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#if defined(USE_AIRMODE_LPF)
airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
#endif
+#ifdef USE_INTERPOLATED_SP
+ ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp;
+ interpolatedSpInit(pidProfile);
+#endif
}
void pidInit(const pidProfile_t *pidProfile)
@@ -1212,6 +1237,9 @@ static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
{
static float previousGyroRateDterm[XYZ_AXIS_COUNT];
+#ifdef USE_INTERPOLATED_SP
+ static FAST_RAM_ZERO_INIT uint32_t lastFrameNumber;
+#endif
#if defined(USE_ACC)
static timeUs_t levelModeStartTimeUs = 0;
@@ -1286,6 +1314,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
rpmFilterUpdate();
#endif
+#ifdef USE_INTERPOLATED_SP
+ bool newRcFrame = false;
+ if (lastFrameNumber != getRcFrameNumber()) {
+ lastFrameNumber = getRcFrameNumber();
+ newRcFrame = true;
+ }
+#endif
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
@@ -1336,6 +1371,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
const float previousIterm = pidData[axis].I;
float itermErrorRate = errorRate;
+ float uncorrectedSetpoint = currentPidSetpoint;
#if defined(USE_ITERM_RELAX)
if (!launchControlActive && !inCrashRecoveryMode) {
@@ -1343,6 +1379,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
errorRate = currentPidSetpoint - gyroRate;
}
#endif
+#ifdef USE_ABSOLUTE_CONTROL
+ float setpointCorrection = currentPidSetpoint - uncorrectedSetpoint;
+#endif
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
// 2-DOF PID controller with optional filter on derivative term.
@@ -1365,9 +1404,18 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// -----calculate pidSetpointDelta
float pidSetpointDelta = 0;
+#ifdef USE_INTERPOLATED_SP
+ if (ffFromInterpolatedSetpoint) {
+ pidSetpointDelta = interpolatedSpApply(axis, pidFrequency, newRcFrame);
+ } else {
+ pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
+ }
+#else
pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
+#endif
previousPidSetpoint[axis] = currentPidSetpoint;
+
#ifdef USE_RC_SMOOTHING_FILTER
pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
#endif // USE_RC_SMOOTHING_FILTER
@@ -1416,12 +1464,25 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
previousGyroRateDterm[axis] = gyroRateDterm[axis];
// -----calculate feedforward component
+#ifdef USE_ABSOLUTE_CONTROL
+ // include abs control correction in FF
+ pidSetpointDelta += setpointCorrection - oldSetpointCorrection[axis];
+ oldSetpointCorrection[axis] = setpointCorrection;
+#endif
+
// Only enable feedforward for rate mode and if launch control is inactive
const float feedforwardGain = (flightModeFlags || launchControlActive) ? 0.0f : pidCoefficient[axis].Kf;
if (feedforwardGain > 0) {
// no transition if feedForwardTransition == 0
float transition = feedForwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * feedForwardTransition) : 1;
- pidData[axis].F = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
+ float feedForward = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
+
+#ifdef USE_INTERPOLATED_SP
+ pidData[axis].F = shouldApplyFFLimits(axis) ?
+ applyFFLimit(axis, feedForward, pidCoefficient[axis].Kp, currentPidSetpoint) : feedForward;
+#else
+ pidData[axis].F = feedForward;
+#endif
} else {
pidData[axis].F = 0;
}
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 3dbfdf575d..bbf0159aac 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -170,6 +170,7 @@ typedef struct pidProfile_s {
uint8_t motor_output_limit; // Upper limit of the motor output (percent)
int8_t auto_profile_cell_count; // Cell count for this profile to be used with if auto PID profile switching is used
uint8_t transient_throttle_limit; // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise
+ uint8_t ff_boost; // amount of high-pass filtered FF to add to FF, 100 means 100% added
char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile
uint8_t idle_min_rpm; // minimum motor speed enforced by integrating p controller
@@ -178,7 +179,10 @@ typedef struct pidProfile_s {
uint8_t idle_pid_limit; // max P
uint8_t idle_max_increase; // max integrated correction
-
+ uint8_t ff_interpolate_sp; // Calculate FF from interpolated setpoint
+ uint8_t ff_spread; // Spread ff out over at least min spread ms
+ uint8_t ff_max_rate_limit; // Maximum setpoint rate percentage for FF
+ uint8_t ff_lookahead_limit; // FF stick extrapolation lookahead period in ms
} pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -255,4 +259,4 @@ void pidSetItermReset(bool enabled);
float pidGetPreviousSetpoint(int axis);
float pidGetDT();
float pidGetPidFrequency();
-
+float pidGetFfBoostFactor();
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index 0d3b9af2e6..103fd804fb 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -612,7 +612,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce
// battery alerts
sbufWriteU8(dst, (uint8_t)getBatteryState());
-
+
sbufWriteU16(dst, getBatteryVoltage()); // in 0.01V steps
break;
}
diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h
index 067a27ff65..d613565a14 100644
--- a/src/main/target/common_pre.h
+++ b/src/main/target/common_pre.h
@@ -334,4 +334,5 @@
#define USE_PERSISTENT_STATS
#define USE_PROFILE_NAMES
#define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol
+#define USE_INTERPOLATED_SP
#endif
diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc
index dbee030720..deda21f501 100644
--- a/src/test/unit/pid_unittest.cc
+++ b/src/test/unit/pid_unittest.cc
@@ -80,6 +80,12 @@ extern "C" {
void beeperConfirmationBeeps(uint8_t) { }
bool isLaunchControlActive(void) {return unitLaunchControlActive; }
void disarm(void) { }
+ float applyFFLimit(int axis, float value, float Kp, float currentPidSetpoint) {
+ UNUSED(axis);
+ UNUSED(Kp);
+ UNUSED(currentPidSetpoint);
+ return value;
+ }
}
pidProfile_t *pidProfile;