Merge pull request #6194 from etracer65/rc_smoothing_retraining
RC smoothing retraining update - adds full support for CRSF
This commit is contained in:
Michael Keller
GitHub
commit
95dcce8471
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -72,5 +72,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
"ITERM_RELAX",
|
||||
"ACRO_TRAINER",
|
||||
"RC_SMOOTHING",
|
||||
"RX_SIGNAL_LOSS",
|
||||
"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);
|
||||
|
|
|
|||
|
|
@ -68,16 +68,16 @@ 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_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_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 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;
|
||||
static FAST_RAM_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
float getSetpointRate(int axis)
|
||||
|
|
@ -263,11 +263,14 @@ FAST_CODE uint8_t processRcInterpolation(void)
|
|||
}
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1)
|
||||
// Determine a cutoff frequency based on filter type and the calculated
|
||||
// average rx frame time
|
||||
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,107 +280,231 @@ int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1)
|
|||
}
|
||||
}
|
||||
|
||||
// Preforms a reasonableness check on the rx frame time to avoid bad data
|
||||
// skewing the average.
|
||||
FAST_CODE bool rcSmoothingRxRateValid(int currentRxRefreshRate)
|
||||
{
|
||||
return (currentRxRefreshRate >= RC_SMOOTHING_RX_RATE_MIN_US && currentRxRefreshRate <= RC_SMOOTHING_RX_RATE_MAX_US);
|
||||
}
|
||||
|
||||
// Initialize or update the filters base on either the manually selected cutoff, or
|
||||
// the auto-calculated cutoff frequency based on detected rx frame rate.
|
||||
FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
|
||||
{
|
||||
const float dT = targetPidLooptime * 1e-6f;
|
||||
uint16_t oldCutoff = smoothingData->inputCutoffFrequency;
|
||||
|
||||
if (rxConfig()->rc_smoothing_input_cutoff == 0) {
|
||||
smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1));
|
||||
}
|
||||
|
||||
// initialize or update the input filter
|
||||
if ((smoothingData->inputCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
|
||||
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
|
||||
if ((1 << i) & interpolationChannels) { // only update channels specified by rc_interp_ch
|
||||
switch (rxConfig()->rc_smoothing_input_type) {
|
||||
|
||||
case RC_SMOOTHING_INPUT_PT1:
|
||||
if (!smoothingData->filterInitialized) {
|
||||
pt1FilterInit((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT));
|
||||
} else {
|
||||
pt1FilterUpdateCutoff((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT));
|
||||
}
|
||||
break;
|
||||
|
||||
case RC_SMOOTHING_INPUT_BIQUAD:
|
||||
default:
|
||||
if (!smoothingData->filterInitialized) {
|
||||
biquadFilterInitLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime);
|
||||
} else {
|
||||
biquadFilterUpdateLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// update or initialize the derivative filter
|
||||
oldCutoff = smoothingData->derivativeCutoffFrequency;
|
||||
if ((rxConfig()->rc_smoothing_derivative_cutoff == 0) && (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF)) {
|
||||
smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1));
|
||||
}
|
||||
|
||||
if (!smoothingData->filterInitialized) {
|
||||
pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type);
|
||||
} else if (smoothingData->derivativeCutoffFrequency != oldCutoff) {
|
||||
pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency);
|
||||
}
|
||||
}
|
||||
|
||||
FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(rcSmoothingFilter_t *smoothingData)
|
||||
{
|
||||
smoothingData->training.sum = 0;
|
||||
smoothingData->training.count = 0;
|
||||
smoothingData->training.min = UINT16_MAX;
|
||||
smoothingData->training.max = 0;
|
||||
}
|
||||
|
||||
// Accumulate the rx frame time samples. Once we've collected enough samples calculate the
|
||||
// average and return true.
|
||||
FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothingData, int rxFrameTimeUs)
|
||||
{
|
||||
smoothingData->training.sum += rxFrameTimeUs;
|
||||
smoothingData->training.count++;
|
||||
smoothingData->training.max = MAX(smoothingData->training.max, rxFrameTimeUs);
|
||||
smoothingData->training.min = MIN(smoothingData->training.min, rxFrameTimeUs);
|
||||
|
||||
// if we've collected enough samples then calculate the average and reset the accumulation
|
||||
if (smoothingData->training.count >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) {
|
||||
smoothingData->training.sum = smoothingData->training.sum - smoothingData->training.min - smoothingData->training.max; // Throw out high and low samples
|
||||
smoothingData->averageFrameTimeUs = lrintf(smoothingData->training.sum / (smoothingData->training.count - 2));
|
||||
rcSmoothingResetAccumulation(smoothingData);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Determine if we need to caclulate filter cutoffs. If not then we can avoid
|
||||
// examining the rx frame times completely
|
||||
FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
|
||||
{
|
||||
bool ret = false;
|
||||
|
||||
// if the input cutoff is 0 (auto) then we need to calculate cutoffs
|
||||
if (rxConfig()->rc_smoothing_input_cutoff == 0) {
|
||||
ret = true;
|
||||
}
|
||||
|
||||
// if the derivative type isn't OFF and the cutoff is 0 then we need to calculate
|
||||
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;
|
||||
|
||||
// first call initialization
|
||||
if (!initialized) {
|
||||
initialized = true;
|
||||
filterCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff;
|
||||
derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff;
|
||||
rcSmoothingData.filterInitialized = false;
|
||||
rcSmoothingData.averageFrameTimeUs = 0;
|
||||
rcSmoothingResetAccumulation(&rcSmoothingData);
|
||||
|
||||
rcSmoothingData.inputCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff;
|
||||
|
||||
if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) {
|
||||
rcSmoothingData.derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff;
|
||||
}
|
||||
|
||||
calculateCutoffs = rcSmoothingAutoCalculate();
|
||||
|
||||
// if we don't need to calculate cutoffs dynamically then the filters can be initialized now
|
||||
if (!calculateCutoffs) {
|
||||
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
|
||||
rcSmoothingData.filterInitialized = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (isRXDataNew) {
|
||||
|
||||
// store the new raw channel values
|
||||
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
|
||||
if ((1 << i) & interpolationChannels) {
|
||||
lastRxData[i] = rcCommand[i];
|
||||
}
|
||||
}
|
||||
// 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) {
|
||||
|
||||
// for dynamically calculated filters we need to examine each rx frame interval
|
||||
if (calculateCutoffs) {
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
if (rxIsReceivingSignal() && (targetPidLooptime > 0) && (currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS)) {
|
||||
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;
|
||||
int sampleState = 0;
|
||||
|
||||
defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTime, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1));
|
||||
filterCutoffFrequency = (filterCutoffFrequency == 0) ? defaultInputCutoffFrequency : filterCutoffFrequency;
|
||||
// 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 ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization
|
||||
if (rxIsReceivingSignal() && rcSmoothingRxRateValid(currentRxRefreshRate)) {
|
||||
|
||||
if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) {
|
||||
derivativeCutoffFrequency = 0;
|
||||
} else {
|
||||
defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTime, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1));
|
||||
derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultDerivativeCutoffFrequency : derivativeCutoffFrequency;
|
||||
}
|
||||
// set the guard time expiration if it's not set
|
||||
if (validRxFrameTimeMs == 0) {
|
||||
validRxFrameTimeMs = currentTimeMs + (rcSmoothingData.filterInitialized ? RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS : RC_SMOOTHING_FILTER_TRAINING_DELAY_MS);
|
||||
} else {
|
||||
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 the guard time has expired then process the rx frame time
|
||||
if (currentTimeMs > validRxFrameTimeMs) {
|
||||
sampleState = 2;
|
||||
bool accumulateSample = true;
|
||||
|
||||
// During initial training process all samples.
|
||||
// During retraining check samples to determine if they vary by more than the limit percentage.
|
||||
if (rcSmoothingData.filterInitialized) {
|
||||
const float percentChange = (ABS(currentRxRefreshRate - rcSmoothingData.averageFrameTimeUs) / (float)rcSmoothingData.averageFrameTimeUs) * 100;
|
||||
if (percentChange < RC_SMOOTHING_RX_RATE_CHANGE_PERCENT) {
|
||||
// We received a sample that wasn't more than the limit percent so reset the accumulation
|
||||
// During retraining we need a contiguous block of samples that are all significantly different than the current average
|
||||
rcSmoothingResetAccumulation(&rcSmoothingData);
|
||||
accumulateSample = false;
|
||||
}
|
||||
}
|
||||
pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type);
|
||||
filterInitialized = true;
|
||||
|
||||
// accumlate the sample into the average
|
||||
if (accumulateSample) {
|
||||
if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) {
|
||||
// the required number of samples were collected so set the filter cutoffs
|
||||
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
|
||||
rcSmoothingData.filterInitialized = true;
|
||||
validRxFrameTimeMs = 0;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
} else {
|
||||
// we have either stopped receiving rx samples (failsafe?) or the sample time is unreasonable so reset the accumulation
|
||||
validRxFrameTimeMs = 0;
|
||||
rcSmoothingResetAccumulation(&rcSmoothingData);
|
||||
}
|
||||
} else {
|
||||
rxFrameTimeSum = 0;
|
||||
rxFrameCount = 0;
|
||||
validRxFrameTimeMs = 0;
|
||||
minRxFrameInterval = UINT16_MAX;
|
||||
maxRxFrameInterval = 0;
|
||||
}
|
||||
|
||||
// rx frame rate training blackbox debugging
|
||||
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, rcSmoothingData.training.count); // log the training step count
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.averageFrameTimeUs);// the current calculated average
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); // indicates whether guard time is active
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) {
|
||||
if (rcSmoothingData.filterInitialized && (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, rcSmoothingData.averageFrameTimeUs);
|
||||
}
|
||||
|
||||
// each pid loop continue to apply the last received channel value to the filter
|
||||
for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
|
||||
if ((1 << updatedChannel) & interpolationChannels) {
|
||||
if (filterInitialized) {
|
||||
if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value
|
||||
if (rcSmoothingData.filterInitialized) {
|
||||
switch (rxConfig()->rc_smoothing_input_type) {
|
||||
case RC_SMOOTHING_INPUT_PT1:
|
||||
rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilterPt1[updatedChannel], lastRxData[updatedChannel]);
|
||||
rcCommand[updatedChannel] = pt1FilterApply((pt1Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
|
||||
break;
|
||||
|
||||
case RC_SMOOTHING_INPUT_BIQUAD:
|
||||
default:
|
||||
rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]);
|
||||
rcCommand[updatedChannel] = biquadFilterApplyDF1((biquadFilter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
|
|
@ -594,16 +721,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 rcSmoothingData.inputCutoffFrequency;
|
||||
case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE:
|
||||
return derivativeCutoffFrequency;
|
||||
return rcSmoothingData.derivativeCutoffFrequency;
|
||||
case RC_SMOOTHING_VALUE_AVERAGE_FRAME:
|
||||
return calculatedFrameTimeAverageUs;
|
||||
return rcSmoothingData.averageFrameTimeUs;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -40,3 +40,4 @@ void resetYawAxis(void);
|
|||
void initRcProcessing(void);
|
||||
bool isMotorsReversed(void);
|
||||
int rcSmoothingGetValue(int whichValue);
|
||||
bool rcSmoothingAutoCalculate(void);
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "common/filter.h"
|
||||
#include "pg/pg.h"
|
||||
|
||||
typedef enum rc_alias {
|
||||
|
|
@ -77,9 +78,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;
|
||||
|
|
@ -108,6 +107,27 @@ typedef enum {
|
|||
|
||||
extern float rcCommand[4];
|
||||
|
||||
typedef struct rcSmoothingFilterTraining_s {
|
||||
float sum;
|
||||
int count;
|
||||
uint16_t min;
|
||||
uint16_t max;
|
||||
} rcSmoothingFilterTraining_t;
|
||||
|
||||
typedef union rcSmoothingFilterTypes_u {
|
||||
pt1Filter_t pt1Filter;
|
||||
biquadFilter_t biquadFilter;
|
||||
} rcSmoothingFilterTypes_t;
|
||||
|
||||
typedef struct rcSmoothingFilter_s {
|
||||
bool filterInitialized;
|
||||
rcSmoothingFilterTypes_t filter[4];
|
||||
uint16_t inputCutoffFrequency;
|
||||
uint16_t derivativeCutoffFrequency;
|
||||
int averageFrameTimeUs;
|
||||
rcSmoothingFilterTraining_t training;
|
||||
} rcSmoothingFilter_t;
|
||||
|
||||
typedef struct rcControlsConfig_s {
|
||||
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
|
||||
uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
cliPrint("# Detected RX frame rate: ");
|
||||
if (avgRxFrameMs == 0) {
|
||||
cliPrintLine("NO SIGNAL");
|
||||
} else {
|
||||
cliPrintLinef("%d.%dms", avgRxFrameMs / 1000, avgRxFrameMs % 1000);
|
||||
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…
Reference in New Issue