mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-14 20:10:18 +03:00
RC smoothing retraining update - adds full support for CRSF
Adds in flight monitoring of the rx frame rate and adapts the filters if the frame rate changes. Primarily to add support for Crossfire with its ability to switch from 150hz to 50hz (and back) under some circumstances. Will work with any protocol - not CRSF specific. So if future receivers add the ability to switch frame rates dynamically the logic should support them. If the current rx frame rate is more than +-20% from the previously detected rate, then the process will retrain for the next 50 samples as long as the rate continues to be outside the 20% tolerance. Once 50 samples are collected the new frame rate is updated and the filter cutoffs are adjusted. Only filters set with their cutoffs = 0 (auto) will be adjusted. There is a 2 second guard time after a successful update before retraining can start again to prevent rapid switching back and forth. The logic is optimized to not perform any training if the filters are set to manual cutoffs. So there is an opportunity for advanced users to choose specific cutoffs and reduce the PID loop load slightly. However this is not recommended for Crossfire or other protocols that might change their rx frame rate. Updated the output of the `rc_smoothing_info` cli command to match the revised logic.
This commit is contained in:
Bruce Luckcuck
parent
6acbed6d6e
commit
1982c94780
11 changed files with 207 additions and 91 deletions
|
|
@ -1333,6 +1333,7 @@ static bool blackboxWriteSysinfo(void)
|
|||
rxConfig()->rc_smoothing_derivative_type);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE),
|
||||
rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE));
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME));
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -73,4 +73,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
"ACRO_TRAINER",
|
||||
"RC_SMOOTHING",
|
||||
"RX_SIGNAL_LOSS",
|
||||
"RC_SMOOTHING_RATE",
|
||||
};
|
||||
|
|
|
|||
|
|
@ -91,6 +91,7 @@ typedef enum {
|
|||
DEBUG_ACRO_TRAINER,
|
||||
DEBUG_RC_SMOOTHING,
|
||||
DEBUG_RX_SIGNAL_LOSS,
|
||||
DEBUG_RC_SMOOTHING_RATE,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -56,6 +56,11 @@ void pt1FilterInit(pt1Filter_t *filter, float k)
|
|||
filter->k = k;
|
||||
}
|
||||
|
||||
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k)
|
||||
{
|
||||
filter->k = k;
|
||||
}
|
||||
|
||||
FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input)
|
||||
{
|
||||
filter->state = filter->state + filter->k * (input - filter->state);
|
||||
|
|
@ -167,6 +172,11 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
|
|||
filter->y2 = y2;
|
||||
}
|
||||
|
||||
FAST_CODE void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
|
||||
{
|
||||
biquadFilterUpdate(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
|
||||
}
|
||||
|
||||
/* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
|
||||
FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -67,6 +67,7 @@ float nullFilterApply(filter_t *filter, float input);
|
|||
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
|
||||
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
|
||||
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
|
||||
void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
|
||||
|
||||
float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
|
||||
float biquadFilterApply(biquadFilter_t *filter, float input);
|
||||
|
|
@ -77,6 +78,7 @@ float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input);
|
|||
|
||||
float pt1FilterGain(uint16_t f_cut, float dT);
|
||||
void pt1FilterInit(pt1Filter_t *filter, float k);
|
||||
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
|
||||
float pt1FilterApply(pt1Filter_t *filter, float input);
|
||||
|
||||
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
|
||||
|
|
|
|||
|
|
@ -70,14 +70,24 @@ enum {
|
|||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
#define RC_SMOOTHING_IDENTITY_FREQUENCY 80 // Used in the formula to convert a BIQUAD cutoff frequency to PT1
|
||||
#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_DELAY_MS 1000 // Additional time to wait after receiving first valid rx frame before training starts
|
||||
#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50
|
||||
#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 // Number of rx frame rate samples to average
|
||||
#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_RX_RATE_MIN_US 5000 // 5ms or 200hz
|
||||
#define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz
|
||||
|
||||
static FAST_RAM_ZERO_INIT uint16_t rcSmoothingFilterCutoffFrequency;
|
||||
static FAST_RAM_ZERO_INIT uint16_t rcSmoothingDerivativeCutoffFrequency;
|
||||
static FAST_RAM_ZERO_INIT int rcSmoothingAvgRxFrameTimeUs;
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t rcSmoothingFilterPt1[4];
|
||||
static FAST_RAM_ZERO_INIT biquadFilter_t rcSmoothingFilterBiquad[4];
|
||||
static FAST_RAM_ZERO_INIT float rcSmoothingFrameTimeSum;
|
||||
static FAST_RAM_ZERO_INIT int rcSmoothingFrameCount;
|
||||
static FAST_RAM_ZERO_INIT uint16_t rcSmoothingMinFrameInterval;
|
||||
static FAST_RAM_ZERO_INIT uint16_t rcSmoothingMaxFrameInterval;
|
||||
static FAST_RAM_ZERO_INIT bool rcSmoothingFilterInitialized;
|
||||
|
||||
static FAST_RAM_ZERO_INIT uint16_t defaultInputCutoffFrequency;
|
||||
static FAST_RAM_ZERO_INIT uint16_t defaultDerivativeCutoffFrequency;
|
||||
static FAST_RAM_ZERO_INIT uint16_t filterCutoffFrequency;
|
||||
static FAST_RAM_ZERO_INIT uint16_t derivativeCutoffFrequency;
|
||||
static FAST_RAM_ZERO_INIT uint16_t calculatedFrameTimeAverageUs;
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
float getSetpointRate(int axis)
|
||||
|
|
@ -263,10 +273,10 @@ FAST_CODE uint8_t processRcInterpolation(void)
|
|||
}
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1)
|
||||
FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1)
|
||||
{
|
||||
if (avgRxFrameTime > 0) {
|
||||
float cutoff = (1 / avgRxFrameTime) / 2; // calculate the nyquist frequency
|
||||
if (avgRxFrameTimeUs > 0) {
|
||||
float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)) / 2; // calculate the nyquist frequency
|
||||
cutoff = cutoff * 0.90f; // Use 90% of the calculated nyquist frequency
|
||||
if (pt1) {
|
||||
cutoff = sq(cutoff) / RC_SMOOTHING_IDENTITY_FREQUENCY; // convert to a cutoff for pt1 that has similar characteristics
|
||||
|
|
@ -277,25 +287,112 @@ int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1)
|
|||
}
|
||||
}
|
||||
|
||||
FAST_CODE bool rcSmoothingRxRateValid(int currentRxRefreshRate)
|
||||
{
|
||||
return (currentRxRefreshRate >= RC_SMOOTHING_RX_RATE_MIN_US && currentRxRefreshRate <= RC_SMOOTHING_RX_RATE_MAX_US);
|
||||
}
|
||||
|
||||
FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(void)
|
||||
{
|
||||
const float dT = targetPidLooptime * 1e-6f;
|
||||
uint16_t oldCutoff = rcSmoothingFilterCutoffFrequency;
|
||||
if (rxConfig()->rc_smoothing_input_cutoff == 0) {
|
||||
rcSmoothingFilterCutoffFrequency = calcRcSmoothingCutoff(rcSmoothingAvgRxFrameTimeUs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1));
|
||||
}
|
||||
if ((rcSmoothingFilterCutoffFrequency != oldCutoff) || !rcSmoothingFilterInitialized) {
|
||||
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
|
||||
if ((1 << i) & interpolationChannels) {
|
||||
switch (rxConfig()->rc_smoothing_input_type) {
|
||||
case RC_SMOOTHING_INPUT_PT1:
|
||||
if (!rcSmoothingFilterInitialized) {
|
||||
pt1FilterInit(&rcSmoothingFilterPt1[i], pt1FilterGain(rcSmoothingFilterCutoffFrequency, dT));
|
||||
} else {
|
||||
pt1FilterUpdateCutoff(&rcSmoothingFilterPt1[i], pt1FilterGain(rcSmoothingFilterCutoffFrequency, dT));
|
||||
}
|
||||
break;
|
||||
case RC_SMOOTHING_INPUT_BIQUAD:
|
||||
default:
|
||||
if (!rcSmoothingFilterInitialized) {
|
||||
biquadFilterInitLPF(&rcSmoothingFilterBiquad[i], rcSmoothingFilterCutoffFrequency, targetPidLooptime);
|
||||
} else {
|
||||
biquadFilterUpdateLPF(&rcSmoothingFilterBiquad[i], rcSmoothingFilterCutoffFrequency, targetPidLooptime);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
oldCutoff = rcSmoothingDerivativeCutoffFrequency;
|
||||
if ((rxConfig()->rc_smoothing_derivative_cutoff == 0) && (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF)) {
|
||||
rcSmoothingDerivativeCutoffFrequency = calcRcSmoothingCutoff(rcSmoothingAvgRxFrameTimeUs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1));
|
||||
}
|
||||
if ((rcSmoothingDerivativeCutoffFrequency != oldCutoff) || !rcSmoothingFilterInitialized) {
|
||||
if (!rcSmoothingFilterInitialized) {
|
||||
pidInitSetpointDerivativeLpf(rcSmoothingDerivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type);
|
||||
} else {
|
||||
pidUpdateSetpointDerivativeLpf(rcSmoothingDerivativeCutoffFrequency);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(void)
|
||||
{
|
||||
rcSmoothingFrameTimeSum = 0;
|
||||
rcSmoothingFrameCount = 0;
|
||||
rcSmoothingMinFrameInterval = UINT16_MAX;
|
||||
rcSmoothingMaxFrameInterval = 0;
|
||||
}
|
||||
|
||||
FAST_CODE bool rcSmoothingAccumulateSample(int rxFrameTimeUs)
|
||||
{
|
||||
rcSmoothingFrameTimeSum += rxFrameTimeUs;
|
||||
rcSmoothingFrameCount++;
|
||||
rcSmoothingMaxFrameInterval = MAX(rcSmoothingMaxFrameInterval, rxFrameTimeUs);
|
||||
rcSmoothingMinFrameInterval = MIN(rcSmoothingMinFrameInterval, rxFrameTimeUs);
|
||||
if (rcSmoothingFrameCount >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) {
|
||||
rcSmoothingFrameTimeSum = rcSmoothingFrameTimeSum - rcSmoothingMinFrameInterval - rcSmoothingMaxFrameInterval; // Throw out high and low samples
|
||||
rcSmoothingAvgRxFrameTimeUs = lrintf(rcSmoothingFrameTimeSum / (rcSmoothingFrameCount - 2));
|
||||
rcSmoothingResetAccumulation();
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
|
||||
{
|
||||
bool ret = false;
|
||||
if (rxConfig()->rc_smoothing_input_cutoff == 0) {
|
||||
ret = true;
|
||||
}
|
||||
if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) {
|
||||
if (rxConfig()->rc_smoothing_derivative_cutoff == 0) {
|
||||
ret = true;
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
FAST_CODE uint8_t processRcSmoothingFilter(void)
|
||||
{
|
||||
uint8_t updatedChannel = 0;
|
||||
|
||||
static FAST_RAM_ZERO_INIT float lastRxData[4];
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t rcCommandFilterPt1[4];
|
||||
static FAST_RAM_ZERO_INIT biquadFilter_t rcCommandFilterBiquad[4];
|
||||
static FAST_RAM_ZERO_INIT bool initialized;
|
||||
static FAST_RAM_ZERO_INIT bool filterInitialized;
|
||||
static FAST_RAM_ZERO_INIT float rxFrameTimeSum;
|
||||
static FAST_RAM_ZERO_INIT int rxFrameCount;
|
||||
static FAST_RAM uint16_t minRxFrameInterval = UINT16_MAX;
|
||||
static FAST_RAM_ZERO_INIT uint16_t maxRxFrameInterval;
|
||||
static FAST_RAM_ZERO_INIT timeMs_t validRxFrameTimeMs;
|
||||
static FAST_RAM_ZERO_INIT bool calculateCutoffs;
|
||||
|
||||
if (!initialized) {
|
||||
initialized = true;
|
||||
filterCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff;
|
||||
derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff;
|
||||
rcSmoothingResetAccumulation();
|
||||
rcSmoothingFilterCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff;
|
||||
if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) {
|
||||
rcSmoothingDerivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff;
|
||||
}
|
||||
calculateCutoffs = rcSmoothingAutoCalculate();
|
||||
if (!calculateCutoffs) {
|
||||
rcSmoothingSetFilterCutoffs();
|
||||
rcSmoothingFilterInitialized = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (isRXDataNew) {
|
||||
|
|
@ -304,80 +401,68 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
|
|||
lastRxData[i] = rcCommand[i];
|
||||
}
|
||||
}
|
||||
if (calculateCutoffs) {
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
int sampleState = 0;
|
||||
|
||||
// If the filter cutoffs are set to auto and we have good rx data, then determine the average rx frame rate
|
||||
// and use that to calculate the filter cutoff frequencies
|
||||
if (!filterInitialized) {
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
if (rxIsReceivingSignal() && (targetPidLooptime > 0) && (currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS)) {
|
||||
if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization
|
||||
if (rxIsReceivingSignal() && rcSmoothingRxRateValid(currentRxRefreshRate)) {
|
||||
if (validRxFrameTimeMs == 0) {
|
||||
validRxFrameTimeMs = currentTimeMs;
|
||||
} else if ((currentTimeMs - validRxFrameTimeMs) > RC_SMOOTHING_FILTER_TRAINING_DELAY_MS) {
|
||||
rxFrameTimeSum += currentRxRefreshRate;
|
||||
rxFrameCount++;
|
||||
maxRxFrameInterval = MAX(maxRxFrameInterval, currentRxRefreshRate);
|
||||
minRxFrameInterval = MIN(minRxFrameInterval, currentRxRefreshRate);
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 0, rxFrameCount); // log the step count during training
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 3, currentRxRefreshRate); // log each frame interval during training
|
||||
if (rxFrameCount >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) {
|
||||
rxFrameTimeSum = rxFrameTimeSum - minRxFrameInterval - maxRxFrameInterval; // Throw out high and low samples
|
||||
calculatedFrameTimeAverageUs = lrintf(rxFrameTimeSum / (rxFrameCount - 2));
|
||||
const float avgRxFrameTime = (rxFrameTimeSum / (rxFrameCount - 2)) * 1e-6f;
|
||||
|
||||
defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTime, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1));
|
||||
filterCutoffFrequency = (filterCutoffFrequency == 0) ? defaultInputCutoffFrequency : filterCutoffFrequency;
|
||||
|
||||
if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) {
|
||||
derivativeCutoffFrequency = 0;
|
||||
validRxFrameTimeMs = currentTimeMs + (rcSmoothingFilterInitialized ? RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS : RC_SMOOTHING_FILTER_TRAINING_DELAY_MS);
|
||||
} else {
|
||||
defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTime, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1));
|
||||
derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultDerivativeCutoffFrequency : derivativeCutoffFrequency;
|
||||
sampleState = 1;
|
||||
}
|
||||
|
||||
const float dT = targetPidLooptime * 1e-6f;
|
||||
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
|
||||
if ((1 << i) & interpolationChannels) {
|
||||
switch (rxConfig()->rc_smoothing_input_type) {
|
||||
case RC_SMOOTHING_INPUT_PT1:
|
||||
pt1FilterInit(&rcCommandFilterPt1[i], pt1FilterGain(filterCutoffFrequency, dT));
|
||||
break;
|
||||
case RC_SMOOTHING_INPUT_BIQUAD:
|
||||
default:
|
||||
biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime);
|
||||
break;
|
||||
if (currentTimeMs > validRxFrameTimeMs) {
|
||||
sampleState = 2;
|
||||
bool accumulateSample = true;
|
||||
if (rcSmoothingFilterInitialized) {
|
||||
const float percentChange = (ABS(currentRxRefreshRate - rcSmoothingAvgRxFrameTimeUs) / (float)rcSmoothingAvgRxFrameTimeUs) * 100;
|
||||
if (percentChange < RC_SMOOTHING_RX_RATE_CHANGE_PERCENT) {
|
||||
rcSmoothingResetAccumulation();
|
||||
accumulateSample = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type);
|
||||
filterInitialized = true;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
rxFrameTimeSum = 0;
|
||||
rxFrameCount = 0;
|
||||
if (accumulateSample) {
|
||||
if (rcSmoothingAccumulateSample(currentRxRefreshRate)) {
|
||||
rcSmoothingSetFilterCutoffs();
|
||||
rcSmoothingFilterInitialized = true;
|
||||
validRxFrameTimeMs = 0;
|
||||
minRxFrameInterval = UINT16_MAX;
|
||||
maxRxFrameInterval = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
validRxFrameTimeMs = 0;
|
||||
rcSmoothingResetAccumulation();
|
||||
}
|
||||
}
|
||||
if (debugMode == DEBUG_RC_SMOOTHING_RATE) {
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxRefreshRate); // log each rx frame interval
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingFrameCount); // log the training step count
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingAvgRxFrameTimeUs);
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) {
|
||||
if (rcSmoothingFilterInitialized && (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, 0, lrintf(lastRxData[rxConfig()->rc_smoothing_debug_axis]));
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 3, calculatedFrameTimeAverageUs);
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingAvgRxFrameTimeUs);
|
||||
}
|
||||
|
||||
for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
|
||||
if ((1 << updatedChannel) & interpolationChannels) {
|
||||
if (filterInitialized) {
|
||||
if (rcSmoothingFilterInitialized) {
|
||||
switch (rxConfig()->rc_smoothing_input_type) {
|
||||
case RC_SMOOTHING_INPUT_PT1:
|
||||
rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilterPt1[updatedChannel], lastRxData[updatedChannel]);
|
||||
rcCommand[updatedChannel] = pt1FilterApply(&rcSmoothingFilterPt1[updatedChannel], lastRxData[updatedChannel]);
|
||||
break;
|
||||
case RC_SMOOTHING_INPUT_BIQUAD:
|
||||
default:
|
||||
rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]);
|
||||
rcCommand[updatedChannel] = biquadFilterApplyDF1(&rcSmoothingFilterBiquad[updatedChannel], lastRxData[updatedChannel]);
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
|
|
@ -594,16 +679,12 @@ void initRcProcessing(void)
|
|||
int rcSmoothingGetValue(int whichValue)
|
||||
{
|
||||
switch (whichValue) {
|
||||
case RC_SMOOTHING_VALUE_INPUT_AUTO:
|
||||
return defaultInputCutoffFrequency;
|
||||
case RC_SMOOTHING_VALUE_INPUT_ACTIVE:
|
||||
return filterCutoffFrequency;
|
||||
case RC_SMOOTHING_VALUE_DERIVATIVE_AUTO:
|
||||
return defaultDerivativeCutoffFrequency;
|
||||
return rcSmoothingFilterCutoffFrequency;
|
||||
case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE:
|
||||
return derivativeCutoffFrequency;
|
||||
return rcSmoothingDerivativeCutoffFrequency;
|
||||
case RC_SMOOTHING_VALUE_AVERAGE_FRAME:
|
||||
return calculatedFrameTimeAverageUs;
|
||||
return rcSmoothingAvgRxFrameTimeUs;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -40,3 +40,4 @@ void resetYawAxis(void);
|
|||
void initRcProcessing(void);
|
||||
bool isMotorsReversed(void);
|
||||
int rcSmoothingGetValue(int whichValue);
|
||||
bool rcSmoothingAutoCalculate(void);
|
||||
|
|
|
|||
|
|
@ -77,9 +77,7 @@ typedef enum {
|
|||
} rcSmoothingDerivativeFilter_e;
|
||||
|
||||
typedef enum {
|
||||
RC_SMOOTHING_VALUE_INPUT_AUTO,
|
||||
RC_SMOOTHING_VALUE_INPUT_ACTIVE,
|
||||
RC_SMOOTHING_VALUE_DERIVATIVE_AUTO,
|
||||
RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE,
|
||||
RC_SMOOTHING_VALUE_AVERAGE_FRAME
|
||||
} rcSmoothingInfoType_e;
|
||||
|
|
|
|||
|
|
@ -325,6 +325,22 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
|
||||
{
|
||||
if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
switch (rcSmoothingFilterType) {
|
||||
case RC_SMOOTHING_DERIVATIVE_PT1:
|
||||
pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
|
||||
break;
|
||||
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
|
||||
biquadFilterUpdateLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
typedef struct pidCoefficient_s {
|
||||
|
|
@ -921,7 +937,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
|||
pidSetpointDelta = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
|
||||
break;
|
||||
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
|
||||
pidSetpointDelta = biquadFilterApply(&setpointDerivativeBiquad[axis], pidSetpointDelta);
|
||||
pidSetpointDelta = biquadFilterApplyDF1(&setpointDerivativeBiquad[axis], pidSetpointDelta);
|
||||
break;
|
||||
}
|
||||
if (axis == rcSmoothingDebugAxis) {
|
||||
|
|
|
|||
|
|
@ -183,3 +183,4 @@ bool crashRecoveryModeActive(void);
|
|||
void pidAcroTrainerInit(void);
|
||||
void pidSetAcroTrainerState(bool newState);
|
||||
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
|
||||
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
|
||||
|
|
|
|||
|
|
@ -3662,20 +3662,24 @@ static void cliRcSmoothing(char *cmdline)
|
|||
if (rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_FILTER) {
|
||||
cliPrintLine("FILTER");
|
||||
uint16_t avgRxFrameMs = rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME);
|
||||
if (rcSmoothingAutoCalculate()) {
|
||||
cliPrint("# Detected RX frame rate: ");
|
||||
if (avgRxFrameMs == 0) {
|
||||
cliPrintLine("NO SIGNAL");
|
||||
} else {
|
||||
cliPrintLinef("%d.%dms", avgRxFrameMs / 1000, avgRxFrameMs % 1000);
|
||||
}
|
||||
cliPrintLinef("# Auto input cutoff: %dhz", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_AUTO));
|
||||
}
|
||||
cliPrint("# Input filter type: ");
|
||||
cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rxConfig()->rc_smoothing_input_type]);
|
||||
cliPrintf("# Active input cutoff: %dhz ", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE));
|
||||
if (rxConfig()->rc_smoothing_input_cutoff == 0) {
|
||||
cliPrintLine("(auto)");
|
||||
} else {
|
||||
cliPrintLine("(manual)");
|
||||
}
|
||||
cliPrintLinef("# Auto derivative cutoff: %dhz", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_AUTO));
|
||||
cliPrint("# Derivative filter type: ");
|
||||
cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rxConfig()->rc_smoothing_derivative_type]);
|
||||
cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE));
|
||||
if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) {
|
||||
cliPrintLine("off)");
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue