mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 20:35:33 +03:00
Merge pull request #10777 from ctzsnooze/move-feedforward-transtion-calculation
This commit is contained in:
Michael Keller
GitHub
commit
7a8b40a670
12 changed files with 72 additions and 45 deletions
|
|
@ -1384,16 +1384,16 @@ static bool blackboxWriteSysinfo(void)
|
|||
#ifdef USE_INTEGRATED_YAW_CONTROL
|
||||
BLACKBOX_PRINT_HEADER_LINE("use_integrated_yaw", "%d", currentPidProfile->use_integrated_yaw);
|
||||
#endif
|
||||
BLACKBOX_PRINT_HEADER_LINE("ff_transition", "%d", currentPidProfile->feedforwardTransition);
|
||||
BLACKBOX_PRINT_HEADER_LINE("ff_weight", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].F,
|
||||
currentPidProfile->pid[PID_PITCH].F,
|
||||
currentPidProfile->pid[PID_YAW].F);
|
||||
#ifdef USE_FEEDFORWARD
|
||||
BLACKBOX_PRINT_HEADER_LINE("ff_transition", "%d", currentPidProfile->feedforward_transition);
|
||||
BLACKBOX_PRINT_HEADER_LINE("ff_averaging", "%d", currentPidProfile->feedforward_averaging);
|
||||
BLACKBOX_PRINT_HEADER_LINE("ff_max_rate_limit", "%d", currentPidProfile->feedforward_max_rate_limit);
|
||||
BLACKBOX_PRINT_HEADER_LINE("ff_smooth_factor", "%d", currentPidProfile->feedforward_smooth_factor);
|
||||
BLACKBOX_PRINT_HEADER_LINE("ff_jitter_factor", "%d", currentPidProfile->feedforward_jitter_factor);
|
||||
BLACKBOX_PRINT_HEADER_LINE("ff_boost", "%d", currentPidProfile->feedforward_boost);
|
||||
BLACKBOX_PRINT_HEADER_LINE("ff_max_rate_limit", "%d", currentPidProfile->feedforward_max_rate_limit);
|
||||
#endif
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit);
|
||||
|
|
|
|||
|
|
@ -1052,7 +1052,6 @@ const clivalue_t valueTable[] = {
|
|||
{ "anti_gravity_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ANTI_GRAVITY_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, antiGravityMode) },
|
||||
{ "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) },
|
||||
{ "anti_gravity_gain", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) },
|
||||
{ "feedforward_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforwardTransition) },
|
||||
{ "acc_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) },
|
||||
{ "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) },
|
||||
{ "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) },
|
||||
|
|
@ -1151,12 +1150,13 @@ const clivalue_t valueTable[] = {
|
|||
#endif
|
||||
|
||||
#ifdef USE_FEEDFORWARD
|
||||
{ "feedforward_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_transition) },
|
||||
{ "feedforward_averaging", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FEEDFORWARD_AVERAGING }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_averaging) },
|
||||
{ "feedforward_smoothing", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_smooth_factor) },
|
||||
{ "feedforward_jitter_reduction", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_jitter_factor) },
|
||||
{ "feedforward_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_boost) },
|
||||
{ "feedforward_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_max_rate_limit) },
|
||||
#endif
|
||||
{ "feedforward_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_boost) },
|
||||
|
||||
#ifdef USE_DYN_IDLE
|
||||
{ "dyn_idle_min_rpm", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_idle_min_rpm) },
|
||||
|
|
|
|||
|
|
@ -489,8 +489,6 @@ static CMS_Menu cmsx_menuLaunchControl = {
|
|||
};
|
||||
#endif
|
||||
|
||||
static uint8_t cmsx_feedforwardTransition;
|
||||
static uint8_t cmsx_feedforward_boost;
|
||||
static uint8_t cmsx_angleStrength;
|
||||
static uint8_t cmsx_horizonStrength;
|
||||
static uint8_t cmsx_horizonTransition;
|
||||
|
|
@ -517,6 +515,8 @@ static uint8_t cmsx_iterm_relax_cutoff;
|
|||
#endif
|
||||
|
||||
#ifdef USE_FEEDFORWARD
|
||||
static uint8_t cmsx_feedforward_transition;
|
||||
static uint8_t cmsx_feedforward_boost;
|
||||
static uint8_t cmsx_feedforward_averaging;
|
||||
static uint8_t cmsx_feedforward_smooth_factor;
|
||||
static uint8_t cmsx_feedforward_jitter_factor;
|
||||
|
|
@ -530,9 +530,6 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
|
|||
|
||||
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
|
||||
|
||||
cmsx_feedforwardTransition = pidProfile->feedforwardTransition;
|
||||
cmsx_feedforward_boost = pidProfile->feedforward_boost;
|
||||
|
||||
cmsx_angleStrength = pidProfile->pid[PID_LEVEL].P;
|
||||
cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I;
|
||||
cmsx_horizonTransition = pidProfile->pid[PID_LEVEL].D;
|
||||
|
|
@ -560,7 +557,9 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
|
|||
#endif
|
||||
|
||||
#ifdef USE_FEEDFORWARD
|
||||
cmsx_feedforward_transition = pidProfile->feedforward_transition;
|
||||
cmsx_feedforward_averaging = pidProfile->feedforward_averaging;
|
||||
cmsx_feedforward_boost = pidProfile->feedforward_boost;
|
||||
cmsx_feedforward_smooth_factor = pidProfile->feedforward_smooth_factor;
|
||||
cmsx_feedforward_jitter_factor = pidProfile->feedforward_jitter_factor;
|
||||
#endif
|
||||
|
|
@ -577,9 +576,7 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
|
|||
UNUSED(self);
|
||||
|
||||
pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
|
||||
pidProfile->feedforwardTransition = cmsx_feedforwardTransition;
|
||||
pidInitConfig(currentPidProfile);
|
||||
pidProfile->feedforward_boost = cmsx_feedforward_boost;
|
||||
|
||||
pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength;
|
||||
pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength;
|
||||
|
|
@ -608,7 +605,9 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
|
|||
#endif
|
||||
|
||||
#ifdef USE_FEEDFORWARD
|
||||
pidProfile->feedforward_transition = cmsx_feedforward_transition;
|
||||
pidProfile->feedforward_averaging = cmsx_feedforward_averaging;
|
||||
pidProfile->feedforward_boost = cmsx_feedforward_boost;
|
||||
pidProfile->feedforward_smooth_factor = cmsx_feedforward_smooth_factor;
|
||||
pidProfile->feedforward_jitter_factor = cmsx_feedforward_jitter_factor;
|
||||
#endif
|
||||
|
|
@ -624,13 +623,13 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
|
|||
static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
|
||||
{ "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 },
|
||||
|
||||
{ "FF TRANSITION", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_feedforwardTransition, 0, 100, 1, 10 }, 0 },
|
||||
#ifdef USE_FEEDFORWARD
|
||||
{ "FF TRANSITION", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_feedforward_transition, 0, 100, 1, 10 }, 0 },
|
||||
{ "FF AVERAGING", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_feedforward_averaging, 4, lookupTableFeedforwardAveraging}, 0 },
|
||||
{ "FF SMOOTHNESS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_feedforward_smooth_factor, 0, 75, 1 } , 0 },
|
||||
{ "FF JITTER", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_feedforward_jitter_factor, 0, 20, 1 } , 0 },
|
||||
#endif
|
||||
{ "FF BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_feedforward_boost, 0, 50, 1 } , 0 },
|
||||
#endif
|
||||
{ "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 },
|
||||
{ "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 },
|
||||
{ "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 },
|
||||
|
|
|
|||
|
|
@ -416,11 +416,13 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a
|
|||
currentPidProfile->pid[PID_YAW].F = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue);
|
||||
break;
|
||||
#if defined(USE_FEEDFORWARD)
|
||||
case ADJUSTMENT_FEEDFORWARD_TRANSITION:
|
||||
newValue = constrain(currentPidProfile->feedforwardTransition + delta, 1, 100); // FIXME magic numbers repeated in cli.c
|
||||
currentPidProfile->feedforwardTransition = newValue;
|
||||
newValue = constrain(currentPidProfile->feedforward_transition + delta, 1, 100); // FIXME magic numbers repeated in cli.c
|
||||
currentPidProfile->feedforward_transition = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
newValue = -1;
|
||||
break;
|
||||
|
|
@ -577,11 +579,13 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus
|
|||
currentPidProfile->pid[PID_YAW].F = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue);
|
||||
break;
|
||||
#if defined(USE_FEEDFORWARD)
|
||||
case ADJUSTMENT_FEEDFORWARD_TRANSITION:
|
||||
newValue = constrain(value, 1, 100); // FIXME magic numbers repeated in cli.c
|
||||
currentPidProfile->feedforwardTransition = newValue;
|
||||
currentPidProfile->feedforward_transition = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
newValue = -1;
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -71,15 +71,16 @@ FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforward
|
|||
float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
|
||||
float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
|
||||
float setpointAcceleration = 0.0f;
|
||||
const float feedforwardTransitionFactor = pidGetFeedforwardTransitionFactor();
|
||||
const float feedforwardSmoothFactor = pidGetFeedforwardSmoothFactor();
|
||||
const float feedforwardJitterFactor = pidGetFeedforwardJitterFactor();
|
||||
float feedforward;
|
||||
|
||||
// calculate an attenuator from average of two most recent rcCommand deltas vs jitter threshold
|
||||
if (axis == FD_ROLL) {
|
||||
DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(rcCommandDelta * 100.0f)); // rcCommand packet difference = steps of 50 mean 2000 RC steps
|
||||
}
|
||||
rcCommandDelta = fabsf(rcCommandDelta);
|
||||
// calculate the jitter attenuator from average of two most recent abs rcCommand deltas vs jitter threshold
|
||||
float jitterAttenuator = 1.0f;
|
||||
if (feedforwardJitterFactor) {
|
||||
if (rcCommandDelta < feedforwardJitterFactor) {
|
||||
|
|
@ -161,6 +162,10 @@ FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforward
|
|||
} else {
|
||||
setpointDelta[axis] = feedforward;
|
||||
}
|
||||
|
||||
// apply feedforward transition
|
||||
setpointDelta[axis] *= feedforwardTransitionFactor > 0 ? MIN(1.0f, getRcDeflectionAbs(axis) * feedforwardTransitionFactor) : 1.0f;
|
||||
|
||||
}
|
||||
return setpointDelta[axis];
|
||||
}
|
||||
|
|
|
|||
|
|
@ -87,7 +87,7 @@ FAST_DATA_ZERO_INIT float throttleBoost;
|
|||
pt1Filter_t throttleLpf;
|
||||
#endif
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 3);
|
||||
|
||||
#if defined(STM32F1)
|
||||
#define PID_PROCESS_DENOM_DEFAULT 8
|
||||
|
|
@ -141,7 +141,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
.itermWindupPointPercent = 100,
|
||||
.pidAtMinThrottle = PID_STABILISATION_ON,
|
||||
.levelAngleLimit = 55,
|
||||
.feedforwardTransition = 0,
|
||||
.feedforward_transition = 0,
|
||||
.yawRateAccelLimit = 0,
|
||||
.rateAccelLimit = 0,
|
||||
.itermThrottleThreshold = 250,
|
||||
|
|
@ -256,12 +256,12 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState)
|
|||
|
||||
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
|
||||
|
||||
float pidGetFeedforwardBoostFactor()
|
||||
#ifdef USE_FEEDFORWARD
|
||||
float pidGetFeedforwardTransitionFactor()
|
||||
{
|
||||
return pidRuntime.feedforwardBoostFactor;
|
||||
return pidRuntime.feedforwardTransitionFactor;
|
||||
}
|
||||
|
||||
#ifdef USE_FEEDFORWARD
|
||||
float pidGetFeedforwardSmoothFactor()
|
||||
{
|
||||
return pidRuntime.feedforwardSmoothFactor;
|
||||
|
|
@ -271,6 +271,11 @@ float pidGetFeedforwardJitterFactor()
|
|||
{
|
||||
return pidRuntime.feedforwardJitterFactor;
|
||||
}
|
||||
|
||||
float pidGetFeedforwardBoostFactor()
|
||||
{
|
||||
return pidRuntime.feedforwardBoostFactor;
|
||||
}
|
||||
#endif
|
||||
|
||||
void pidResetIterm(void)
|
||||
|
|
@ -1105,9 +1110,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
|
|||
if (feedforwardGain > 0) {
|
||||
// halve feedforward in Level mode since stick sensitivity is weaker by about half
|
||||
feedforwardGain *= FLIGHT_MODE(ANGLE_MODE) ? 0.5f : 1.0f;
|
||||
// no transition if feedforwardTransition == 0
|
||||
float transition = pidRuntime.feedforwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * pidRuntime.feedforwardTransition) : 1;
|
||||
float feedForward = feedforwardGain * transition * pidSetpointDelta * pidRuntime.pidFrequency;
|
||||
// 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) ?
|
||||
|
|
|
|||
|
|
@ -161,7 +161,6 @@ typedef struct pidProfile_s {
|
|||
uint16_t crash_delay; // ms
|
||||
uint8_t crash_recovery_angle; // degrees
|
||||
uint8_t crash_recovery_rate; // degree/second
|
||||
uint8_t feedforwardTransition; // Feedforward attenuation around centre sticks
|
||||
uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase
|
||||
uint16_t itermLimit;
|
||||
uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz
|
||||
|
|
@ -197,7 +196,6 @@ 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 feedforward_boost; // amount of setpoint acceleration to add to feedforward, 10 means 100% added
|
||||
char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile
|
||||
|
||||
uint8_t dyn_idle_min_rpm; // minimum motor speed enforced by the dynamic idle controller
|
||||
|
|
@ -206,10 +204,13 @@ typedef struct pidProfile_s {
|
|||
uint8_t dyn_idle_d_gain; // D gain for corrections around rapid changes in rpm
|
||||
uint8_t dyn_idle_max_increase; // limit on maximum possible increase in motor idle drive during active control
|
||||
|
||||
uint8_t feedforward_transition; // Feedforward attenuation around centre sticks
|
||||
uint8_t feedforward_averaging; // Number of packets to average when averaging is on
|
||||
uint8_t feedforward_max_rate_limit; // Maximum setpoint rate percentage for feedforward
|
||||
uint8_t feedforward_smooth_factor; // Amount of lowpass type smoothing for feedforward steps
|
||||
uint8_t feedforward_jitter_factor; // Number of RC steps below which to attenuate feedforward
|
||||
uint8_t feedforward_boost; // amount of setpoint acceleration to add to feedforward, 10 means 100% added
|
||||
uint8_t feedforward_max_rate_limit; // Maximum setpoint rate percentage for feedforward
|
||||
|
||||
uint8_t dyn_lpf_curve_expo; // set the curve for dynamic dterm lowpass filter
|
||||
uint8_t level_race_mode; // NFE race mode - when true pitch setpoint calcualtion is gyro based in level mode
|
||||
uint8_t vbat_sag_compensation; // Reduce motor output by this percentage of the maximum compensation amount
|
||||
|
|
@ -284,10 +285,8 @@ typedef struct pidRuntime_s {
|
|||
float antiGravityOsdCutoff;
|
||||
float antiGravityThrottleHpf;
|
||||
float antiGravityPBoost;
|
||||
float feedforwardBoostFactor;
|
||||
float itermAccelerator;
|
||||
uint16_t itermAcceleratorGain;
|
||||
float feedforwardTransition;
|
||||
pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
|
||||
float levelGain;
|
||||
float horizonGain;
|
||||
|
|
@ -385,9 +384,11 @@ typedef struct pidRuntime_s {
|
|||
#endif
|
||||
|
||||
#ifdef USE_FEEDFORWARD
|
||||
float feedforwardTransitionFactor;
|
||||
feedforwardAveraging_t feedforwardAveraging;
|
||||
float feedforwardSmoothFactor;
|
||||
float feedforwardJitterFactor;
|
||||
float feedforwardBoostFactor;
|
||||
#endif
|
||||
} pidRuntime_t;
|
||||
|
||||
|
|
@ -443,4 +444,5 @@ float pidGetPidFrequency();
|
|||
float pidGetFeedforwardBoostFactor();
|
||||
float pidGetFeedforwardSmoothFactor();
|
||||
float pidGetFeedforwardJitterFactor();
|
||||
float pidGetFeedforwardTransitionFactor();
|
||||
float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);
|
||||
|
|
|
|||
|
|
@ -238,8 +238,6 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
|
||||
pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT));
|
||||
pt1FilterInit(&pidRuntime.antiGravitySmoothLpf, pt1FilterGain(ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF, pidRuntime.dT));
|
||||
|
||||
pidRuntime.feedforwardBoostFactor = (float)pidProfile->feedforward_boost / 10.0f;
|
||||
}
|
||||
|
||||
void pidInit(const pidProfile_t *pidProfile)
|
||||
|
|
@ -276,11 +274,6 @@ void pidUpdateFeedforwardLpf(uint16_t filterCutoff)
|
|||
|
||||
void pidInitConfig(const pidProfile_t *pidProfile)
|
||||
{
|
||||
if (pidProfile->feedforwardTransition == 0) {
|
||||
pidRuntime.feedforwardTransition = 0;
|
||||
} else {
|
||||
pidRuntime.feedforwardTransition = 100.0f / pidProfile->feedforwardTransition;
|
||||
}
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
pidRuntime.pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
|
||||
pidRuntime.pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
|
||||
|
|
@ -422,12 +415,18 @@ 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.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.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor;
|
||||
pidRuntime.feedforwardBoostFactor = (float)pidProfile->feedforward_boost / 10.0f;
|
||||
feedforwardInit(pidProfile);
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -1823,7 +1823,11 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit
|
||||
sbufWriteU8(dst, 0); // reserved
|
||||
sbufWriteU8(dst, 0); // was vbatPidCompensation
|
||||
sbufWriteU8(dst, currentPidProfile->feedforwardTransition);
|
||||
#if defined(USE_FEEDFORWARD)
|
||||
sbufWriteU8(dst, currentPidProfile->feedforward_transition);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
#endif
|
||||
sbufWriteU8(dst, 0); // was low byte of currentPidProfile->dtermSetpointWeight
|
||||
sbufWriteU8(dst, 0); // reserved
|
||||
sbufWriteU8(dst, 0); // reserved
|
||||
|
|
@ -1902,11 +1906,12 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
#if defined(USE_FEEDFORWARD)
|
||||
sbufWriteU8(dst, currentPidProfile->feedforward_averaging);
|
||||
sbufWriteU8(dst, currentPidProfile->feedforward_smooth_factor);
|
||||
sbufWriteU8(dst, currentPidProfile->feedforward_boost);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
sbufWriteU8(dst, 0);
|
||||
sbufWriteU8(dst, 0);
|
||||
#endif
|
||||
sbufWriteU8(dst, currentPidProfile->feedforward_boost);
|
||||
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
|
||||
sbufWriteU8(dst, currentPidProfile->vbat_sag_compensation);
|
||||
#else
|
||||
|
|
@ -2708,7 +2713,11 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
sbufReadU16(src); // was pidProfile.yaw_p_limit
|
||||
sbufReadU8(src); // reserved
|
||||
sbufReadU8(src); // was vbatPidCompensation
|
||||
currentPidProfile->feedforwardTransition = sbufReadU8(src);
|
||||
#if defined(USE_FEEDFORWARD)
|
||||
currentPidProfile->feedforward_transition = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
#endif
|
||||
sbufReadU8(src); // was low byte of currentPidProfile->dtermSetpointWeight
|
||||
sbufReadU8(src); // reserved
|
||||
sbufReadU8(src); // reserved
|
||||
|
|
@ -2805,11 +2814,13 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
#if defined(USE_FEEDFORWARD)
|
||||
currentPidProfile->feedforward_averaging = sbufReadU8(src);
|
||||
currentPidProfile->feedforward_smooth_factor = sbufReadU8(src);
|
||||
currentPidProfile->feedforward_boost = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
sbufReadU8(src);
|
||||
sbufReadU8(src);
|
||||
#endif
|
||||
currentPidProfile->feedforward_boost = sbufReadU8(src);
|
||||
|
||||
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
|
||||
currentPidProfile->vbat_sag_compensation = sbufReadU8(src);
|
||||
#else
|
||||
|
|
|
|||
|
|
@ -131,7 +131,7 @@ void targetConfiguration(void)
|
|||
pidProfile->dterm_notch_hz = 0;
|
||||
pidProfile->pid[PID_PITCH].F = 100;
|
||||
pidProfile->pid[PID_ROLL].F = 100;
|
||||
pidProfile->feedforwardTransition = 0;
|
||||
pidProfile->feedforward_transition = 0;
|
||||
|
||||
/* Anti-Gravity */
|
||||
pidProfile->itermThrottleThreshold = 500;
|
||||
|
|
|
|||
|
|
@ -88,7 +88,7 @@ void targetConfiguration(void)
|
|||
|
||||
pidProfile->pid[PID_PITCH].F = 200;
|
||||
pidProfile->pid[PID_ROLL].F = 200;
|
||||
pidProfile->feedforwardTransition = 50;
|
||||
pidProfile->feedforward_transition = 50;
|
||||
}
|
||||
|
||||
for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) {
|
||||
|
|
|
|||
|
|
@ -101,7 +101,10 @@ extern "C" {
|
|||
{
|
||||
UNUSED(newRcFrame);
|
||||
UNUSED(feedforwardAveraging);
|
||||
return simulatedSetpointRate[axis] - simulatedPrevSetpointRate[axis];
|
||||
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)
|
||||
{
|
||||
|
|
@ -139,7 +142,7 @@ void setDefaultTestSettings(void) {
|
|||
pidProfile->itermWindupPointPercent = 50;
|
||||
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
|
||||
pidProfile->levelAngleLimit = 55;
|
||||
pidProfile->feedforwardTransition = 100;
|
||||
pidProfile->feedforward_transition = 100;
|
||||
pidProfile->yawRateAccelLimit = 100;
|
||||
pidProfile->rateAccelLimit = 0;
|
||||
pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue