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)");
|
cliPrintLine("(auto)");
|
||||||
}
|
}
|
||||||
cliPrintf("# Active throttle cutoff: %dhz ", rcSmoothingData->throttleCutoffFrequency);
|
cliPrintf("# Active throttle cutoff: %dhz ", rcSmoothingData->throttleCutoffFrequency);
|
||||||
if (rcSmoothingData->ffCutoffSetting) {
|
if (rcSmoothingData->throttleCutoffSetting) {
|
||||||
cliPrintLine("(manual)");
|
cliPrintLine("(manual)");
|
||||||
} else {
|
} else {
|
||||||
cliPrintLine("(auto)");
|
cliPrintLine("(auto)");
|
||||||
|
|
|
@ -64,6 +64,7 @@ float rcCommandDelta[XYZ_AXIS_COUNT];
|
||||||
#endif
|
#endif
|
||||||
static float rawSetpoint[XYZ_AXIS_COUNT];
|
static float rawSetpoint[XYZ_AXIS_COUNT];
|
||||||
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
|
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
|
||||||
|
static float rcDeflectionSmoothed[3];
|
||||||
static float throttlePIDAttenuation;
|
static float throttlePIDAttenuation;
|
||||||
static bool reverseMotors = false;
|
static bool reverseMotors = false;
|
||||||
static applyRatesFn *applyRates;
|
static applyRatesFn *applyRates;
|
||||||
|
@ -106,14 +107,21 @@ bool getShouldUpdateFeedforward()
|
||||||
}
|
}
|
||||||
|
|
||||||
float getSetpointRate(int axis)
|
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];
|
return setpointRate[axis];
|
||||||
|
#else
|
||||||
|
return rawSetpoint[axis];
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
float getRcDeflection(int axis)
|
float getRcDeflection(int axis)
|
||||||
{
|
{
|
||||||
|
#ifdef USE_RC_SMOOTHING_FILTER
|
||||||
|
return rcDeflectionSmoothed[axis];
|
||||||
|
#else
|
||||||
return rcDeflection[axis];
|
return rcDeflection[axis];
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
float getRcDeflectionAbs(int axis)
|
float getRcDeflectionAbs(int axis)
|
||||||
|
@ -336,7 +344,6 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
|
||||||
smoothingData->throttleCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle);
|
smoothingData->throttleCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// initialize or update the Setpoint filter
|
// initialize or update the Setpoint filter
|
||||||
if ((smoothingData->setpointCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
|
if ((smoothingData->setpointCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
|
||||||
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
|
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
|
// update or initialize the FF filter
|
||||||
|
@ -537,6 +553,17 @@ static FAST_CODE void processRcSmoothingFilter(void)
|
||||||
*dst = rxDataToSmooth[i];
|
*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
|
#endif // USE_RC_SMOOTHING_FILTER
|
||||||
|
|
||||||
|
|
|
@ -95,6 +95,7 @@ typedef struct rcSmoothingFilterTraining_s {
|
||||||
typedef struct rcSmoothingFilter_s {
|
typedef struct rcSmoothingFilter_s {
|
||||||
bool filterInitialized;
|
bool filterInitialized;
|
||||||
pt3Filter_t filter[4];
|
pt3Filter_t filter[4];
|
||||||
|
pt3Filter_t filterDeflection[2];
|
||||||
uint8_t setpointCutoffSetting;
|
uint8_t setpointCutoffSetting;
|
||||||
uint8_t throttleCutoffSetting;
|
uint8_t throttleCutoffSetting;
|
||||||
uint16_t setpointCutoffFrequency;
|
uint16_t setpointCutoffFrequency;
|
||||||
|
|
Loading…
Reference in New Issue