Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

Use RC Smoothing initialized settings rather than current PG values

Browse source 
Prevents possibility of changing paramaters after initializing affecting runtime operation of RC smoothing. Parameter values are loaded during initialization instead of relying on the current PG values.
This commit is contained in:
Bruce Luckcuck 2019-05-15 08:21:15 -04:00
parent a8e9dd94e8
commit 3cee0c99cc
5 changed files with 56 additions and 52 deletions
Download patch file Download diff file Expand all files Collapse all files

24
src/main/blackbox/blackbox.c
View file

@ -1225,6 +1225,10 @@ static bool blackboxWriteSysinfo(void)
char buf[FORMATTED_DATE_TIME_BUFSIZE];
#ifdef USE_RC_SMOOTHING_FILTER
rcSmoothingFilter_t *rcSmoothingData = getRcSmoothingData();
#endif
const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile);
switch (xmitState.headerIndex) {
BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s", "Cleanflight");
@ -1377,16 +1381,16 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
#ifdef USE_RC_SMOOTHING_FILTER
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_type", "%d", rxConfig()->rc_smoothing_type);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_debug_axis", "%d", rxConfig()->rc_smoothing_debug_axis);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rxConfig()->rc_smoothing_input_cutoff,
rxConfig()->rc_smoothing_derivative_cutoff);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor", "%d", rxConfig()->rc_smoothing_auto_factor);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rxConfig()->rc_smoothing_input_type,
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));
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_type", "%d", rcSmoothingData->inputFilterType);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_debug_axis", "%d", rcSmoothingData->debugAxis);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rcSmoothingData->inputCutoffSetting,
rcSmoothingData->derivativeCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor", "%d", rcSmoothingData->autoSmoothnessFactor);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rcSmoothingData->inputFilterType,
rcSmoothingData->derivativeFilterType);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingData->inputCutoffFrequency,
rcSmoothingData->derivativeCutoffFrequency);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingData->averageFrameTimeUs);
#endif // USE_RC_SMOOTHING_FILTER

17
src/main/cli/cli.c
View file

@ -4422,10 +4422,11 @@ static void cliVersion(char *cmdline)
static void cliRcSmoothing(char *cmdline)
{
UNUSED(cmdline);
rcSmoothingFilter_t *rcSmoothingData = getRcSmoothingData();
cliPrint("# RC Smoothing Type: ");
if (rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_FILTER) {
cliPrintLine("FILTER");
uint16_t avgRxFrameMs = rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME);
uint16_t avgRxFrameMs = rcSmoothingData->averageFrameTimeUs;
if (rcSmoothingAutoCalculate()) {
cliPrint("# Detected RX frame rate: ");
if (avgRxFrameMs == 0) {
@ -4435,20 +4436,20 @@ static void cliRcSmoothing(char *cmdline)
}
}
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) {
cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rcSmoothingData->inputFilterType]);
cliPrintf("# Active input cutoff: %dhz ", rcSmoothingData->inputCutoffFrequency);
if (rcSmoothingData->inputCutoffSetting == 0) {
cliPrintLine("(auto)");
} else {
cliPrintLine("(manual)");
}
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) {
cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rcSmoothingData->derivativeFilterType]);
cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingData->derivativeCutoffFrequency);
if (rcSmoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_OFF) {
cliPrintLine("off)");
} else {
if (rxConfig()->rc_smoothing_derivative_cutoff == 0) {
if (rcSmoothingData->derivativeCutoffSetting == 0) {
cliPrintLine("auto)");
} else {
cliPrintLine("manual)");

51
src/main/fc/rc.c
View file

@ -286,10 +286,10 @@ FAST_CODE uint8_t processRcInterpolation(void)
#ifdef USE_RC_SMOOTHING_FILTER
// Determine a cutoff frequency based on filter type and the calculated
// average rx frame time
FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1)
FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1, uint8_t autoSmoothnessFactor)
{
if (avgRxFrameTimeUs > 0) {
const float cutoffFactor = (100 - rxConfig()->rc_smoothing_auto_factor) / 100.0f;
const float cutoffFactor = (100 - autoSmoothnessFactor) / 100.0f;
float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)) / 2; // calculate the nyquist frequency
cutoff = cutoff * cutoffFactor;
@ -316,15 +316,15 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
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));
if (smoothingData->inputCutoffSetting == 0) {
smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->inputFilterType == RC_SMOOTHING_INPUT_PT1), smoothingData->autoSmoothnessFactor);
}
// 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) {
switch (smoothingData->inputFilterType) {
case RC_SMOOTHING_INPUT_PT1:
if (!smoothingData->filterInitialized) {
@ -349,12 +349,12 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
// 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->derivativeCutoffSetting == 0) && (smoothingData->derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_PT1), smoothingData->autoSmoothnessFactor);
}
if (!smoothingData->filterInitialized) {
pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type);
pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, smoothingData->debugAxis, smoothingData->derivativeFilterType);
} else if (smoothingData->derivativeCutoffFrequency != oldCutoff) {
pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency);
}
@ -395,13 +395,13 @@ 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) {
if (rcSmoothingData.inputCutoffSetting == 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) {
if (rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) {
if (rcSmoothingData.derivativeCutoffSetting == 0) {
ret = true;
}
}
@ -421,12 +421,18 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
initialized = true;
rcSmoothingData.filterInitialized = false;
rcSmoothingData.averageFrameTimeUs = 0;
rcSmoothingData.autoSmoothnessFactor = rxConfig()->rc_smoothing_auto_factor;
rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
rcSmoothingData.inputFilterType = rxConfig()->rc_smoothing_input_type;
rcSmoothingData.inputCutoffSetting = rxConfig()->rc_smoothing_input_cutoff;
rcSmoothingData.derivativeFilterType = rxConfig()->rc_smoothing_derivative_type;
rcSmoothingData.derivativeCutoffSetting = rxConfig()->rc_smoothing_derivative_cutoff;
rcSmoothingResetAccumulation(&rcSmoothingData);
rcSmoothingData.inputCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff;
rcSmoothingData.inputCutoffFrequency = rcSmoothingData.inputCutoffSetting;
if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) {
rcSmoothingData.derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff;
if (rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) {
rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting;
}
calculateCutoffs = rcSmoothingAutoCalculate();
@ -511,7 +517,7 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
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, 0, lrintf(lastRxData[rcSmoothingData.debugAxis]));
DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs);
}
@ -519,7 +525,7 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value
if (rcSmoothingData.filterInitialized) {
switch (rxConfig()->rc_smoothing_input_type) {
switch (rcSmoothingData.inputFilterType) {
case RC_SMOOTHING_INPUT_PT1:
rcCommand[updatedChannel] = pt1FilterApply((pt1Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
break;
@ -749,18 +755,9 @@ bool rcSmoothingIsEnabled(void)
}
#ifdef USE_RC_SMOOTHING_FILTER
int rcSmoothingGetValue(int whichValue)
rcSmoothingFilter_t *getRcSmoothingData(void)
{
switch (whichValue) {
case RC_SMOOTHING_VALUE_INPUT_ACTIVE:
return rcSmoothingData.inputCutoffFrequency;
case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE:
return rcSmoothingData.derivativeCutoffFrequency;
case RC_SMOOTHING_VALUE_AVERAGE_FRAME:
return rcSmoothingData.averageFrameTimeUs;
default:
return 0;
}
return &rcSmoothingData;
}
bool rcSmoothingInitializationComplete(void) {

4
src/main/fc/rc.h
View file

@ -20,6 +20,8 @@
#pragma once
#include "fc/rc_controls.h"
typedef enum {
INTERPOLATION_CHANNELS_RP,
INTERPOLATION_CHANNELS_RPY,
@ -40,6 +42,6 @@ void resetYawAxis(void);
void initRcProcessing(void);
bool isMotorsReversed(void);
bool rcSmoothingIsEnabled(void);
int rcSmoothingGetValue(int whichValue);
rcSmoothingFilter_t *getRcSmoothingData(void);
bool rcSmoothingAutoCalculate(void);
bool rcSmoothingInitializationComplete(void);

12
src/main/fc/rc_controls.h
View file

@ -77,12 +77,6 @@ typedef enum {
RC_SMOOTHING_DERIVATIVE_BIQUAD
} rcSmoothingDerivativeFilter_e;
typedef enum {
RC_SMOOTHING_VALUE_INPUT_ACTIVE,
RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE,
RC_SMOOTHING_VALUE_AVERAGE_FRAME
} rcSmoothingInfoType_e;
#define ROL_LO (1 << (2 * ROLL))
#define ROL_CE (3 << (2 * ROLL))
#define ROL_HI (2 << (2 * ROLL))
@ -125,10 +119,16 @@ typedef union rcSmoothingFilterTypes_u {
typedef struct rcSmoothingFilter_s {
bool filterInitialized;
rcSmoothingFilterTypes_t filter[4];
rcSmoothingInputFilter_e inputFilterType;
uint8_t inputCutoffSetting;
uint16_t inputCutoffFrequency;
rcSmoothingDerivativeFilter_e derivativeFilterType;
uint8_t derivativeCutoffSetting;
uint16_t derivativeCutoffFrequency;
int averageFrameTimeUs;
rcSmoothingFilterTraining_t training;
uint8_t debugAxis;
uint8_t autoSmoothnessFactor;
} rcSmoothingFilter_t;
typedef struct rcControlsConfig_s {