smoothing fix level mode
This commit is contained in:
parent
86aa5cc84e
commit
4ef02928e0
|
@ -5018,7 +5018,7 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline)
|
|||
cliPrintLine("(auto)");
|
||||
}
|
||||
cliPrintf("# Active throttle cutoff: %dhz ", rcSmoothingData->throttleCutoffFrequency);
|
||||
if (rcSmoothingData->ffCutoffSetting) {
|
||||
if (rcSmoothingData->throttleCutoffSetting) {
|
||||
cliPrintLine("(manual)");
|
||||
} else {
|
||||
cliPrintLine("(auto)");
|
||||
|
|
|
@ -64,6 +64,7 @@ float rcCommandDelta[XYZ_AXIS_COUNT];
|
|||
#endif
|
||||
static float rawSetpoint[XYZ_AXIS_COUNT];
|
||||
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
|
||||
static float rcDeflectionSmoothed[3];
|
||||
static float throttlePIDAttenuation;
|
||||
static bool reverseMotors = false;
|
||||
static applyRatesFn *applyRates;
|
||||
|
@ -106,14 +107,21 @@ bool getShouldUpdateFeedforward()
|
|||
}
|
||||
|
||||
float getSetpointRate(int axis)
|
||||
// only used in pid.c to provide setpointRate for the crash recovery function
|
||||
{
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
return setpointRate[axis];
|
||||
#else
|
||||
return rawSetpoint[axis];
|
||||
#endif
|
||||
}
|
||||
|
||||
float getRcDeflection(int axis)
|
||||
{
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
return rcDeflectionSmoothed[axis];
|
||||
#else
|
||||
return rcDeflection[axis];
|
||||
#endif
|
||||
}
|
||||
|
||||
float getRcDeflectionAbs(int axis)
|
||||
|
@ -336,7 +344,6 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
|
|||
smoothingData->throttleCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle);
|
||||
}
|
||||
|
||||
|
||||
// initialize or update the Setpoint filter
|
||||
if ((smoothingData->setpointCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
|
||||
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
|
||||
|
@ -354,6 +361,15 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
// initialize or update the Level filter
|
||||
for (int i = FD_ROLL; i < FD_YAW; i++) {
|
||||
if (!smoothingData->filterInitialized) {
|
||||
pt3FilterInit(&smoothingData->filterDeflection[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
|
||||
} else {
|
||||
pt3FilterUpdateCutoff(&smoothingData->filterDeflection[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// update or initialize the FF filter
|
||||
|
@ -537,6 +553,17 @@ static FAST_CODE void processRcSmoothingFilter(void)
|
|||
*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]);
|
||||
} else {
|
||||
rcDeflectionSmoothed[axis] = rcDeflection[axis];
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
|
|
|
@ -95,6 +95,7 @@ typedef struct rcSmoothingFilterTraining_s {
|
|||
typedef struct rcSmoothingFilter_s {
|
||||
bool filterInitialized;
|
||||
pt3Filter_t filter[4];
|
||||
pt3Filter_t filterDeflection[2];
|
||||
uint8_t setpointCutoffSetting;
|
||||
uint8_t throttleCutoffSetting;
|
||||
uint16_t setpointCutoffFrequency;
|
||||
|
|
Loading…
Reference in New Issue