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]; char buf[FORMATTED_DATE_TIME_BUFSIZE];
#ifdef USE_RC_SMOOTHING_FILTER
rcSmoothingFilter_t *rcSmoothingData = getRcSmoothingData();
#endif
const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile); const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile);
switch (xmitState.headerIndex) { switch (xmitState.headerIndex) {
BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s", "Cleanflight"); BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s", "Cleanflight");
@ -1377,16 +1381,16 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures); BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_type", "%d", rxConfig()->rc_smoothing_type); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_type", "%d", rcSmoothingData->inputFilterType);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_debug_axis", "%d", rxConfig()->rc_smoothing_debug_axis); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_debug_axis", "%d", rcSmoothingData->debugAxis);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rxConfig()->rc_smoothing_input_cutoff, BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rcSmoothingData->inputCutoffSetting,
rxConfig()->rc_smoothing_derivative_cutoff); rcSmoothingData->derivativeCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor", "%d", rxConfig()->rc_smoothing_auto_factor); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor", "%d", rcSmoothingData->autoSmoothnessFactor);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rxConfig()->rc_smoothing_input_type, BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rcSmoothingData->inputFilterType,
rxConfig()->rc_smoothing_derivative_type); rcSmoothingData->derivativeFilterType);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE), BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingData->inputCutoffFrequency,
rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE)); rcSmoothingData->derivativeCutoffFrequency);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME)); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingData->averageFrameTimeUs);
#endif // USE_RC_SMOOTHING_FILTER #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) static void cliRcSmoothing(char *cmdline)
{ {
UNUSED(cmdline); UNUSED(cmdline);
rcSmoothingFilter_t *rcSmoothingData = getRcSmoothingData();
cliPrint("# RC Smoothing Type: "); cliPrint("# RC Smoothing Type: ");
if (rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_FILTER) { if (rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_FILTER) {
cliPrintLine("FILTER"); cliPrintLine("FILTER");
uint16_t avgRxFrameMs = rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME); uint16_t avgRxFrameMs = rcSmoothingData->averageFrameTimeUs;
if (rcSmoothingAutoCalculate()) { if (rcSmoothingAutoCalculate()) {
cliPrint("# Detected RX frame rate: "); cliPrint("# Detected RX frame rate: ");
if (avgRxFrameMs == 0) { if (avgRxFrameMs == 0) {
@ -4435,20 +4436,20 @@ static void cliRcSmoothing(char *cmdline)
} }
} }
cliPrint("# Input filter type: "); cliPrint("# Input filter type: ");
cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rxConfig()->rc_smoothing_input_type]); cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rcSmoothingData->inputFilterType]);
cliPrintf("# Active input cutoff: %dhz ", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE)); cliPrintf("# Active input cutoff: %dhz ", rcSmoothingData->inputCutoffFrequency);
if (rxConfig()->rc_smoothing_input_cutoff == 0) { if (rcSmoothingData->inputCutoffSetting == 0) {
cliPrintLine("(auto)"); cliPrintLine("(auto)");
} else { } else {
cliPrintLine("(manual)"); cliPrintLine("(manual)");
} }
cliPrint("# Derivative filter type: "); cliPrint("# Derivative filter type: ");
cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rxConfig()->rc_smoothing_derivative_type]); cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rcSmoothingData->derivativeFilterType]);
cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE)); cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingData->derivativeCutoffFrequency);
if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) { if (rcSmoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_OFF) {
cliPrintLine("off)"); cliPrintLine("off)");
} else { } else {
if (rxConfig()->rc_smoothing_derivative_cutoff == 0) { if (rcSmoothingData->derivativeCutoffSetting == 0) {
cliPrintLine("auto)"); cliPrintLine("auto)");
} else { } else {
cliPrintLine("manual)"); 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 #ifdef USE_RC_SMOOTHING_FILTER
// Determine a cutoff frequency based on filter type and the calculated // Determine a cutoff frequency based on filter type and the calculated
// average rx frame time // 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) { 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 float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)) / 2; // calculate the nyquist frequency
cutoff = cutoff * cutoffFactor; cutoff = cutoff * cutoffFactor;
@ -316,15 +316,15 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
const float dT = targetPidLooptime * 1e-6f; const float dT = targetPidLooptime * 1e-6f;
uint16_t oldCutoff = smoothingData->inputCutoffFrequency; uint16_t oldCutoff = smoothingData->inputCutoffFrequency;
if (rxConfig()->rc_smoothing_input_cutoff == 0) { if (smoothingData->inputCutoffSetting == 0) {
smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->inputFilterType == RC_SMOOTHING_INPUT_PT1), smoothingData->autoSmoothnessFactor);
} }
// initialize or update the input filter // initialize or update the input filter
if ((smoothingData->inputCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) { if ((smoothingData->inputCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
if ((1 << i) & interpolationChannels) { // only update channels specified by rc_interp_ch 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: case RC_SMOOTHING_INPUT_PT1:
if (!smoothingData->filterInitialized) { if (!smoothingData->filterInitialized) {
@ -349,12 +349,12 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
// update or initialize the derivative filter // update or initialize the derivative filter
oldCutoff = smoothingData->derivativeCutoffFrequency; oldCutoff = smoothingData->derivativeCutoffFrequency;
if ((rxConfig()->rc_smoothing_derivative_cutoff == 0) && (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF)) { if ((smoothingData->derivativeCutoffSetting == 0) && (smoothingData->derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_PT1), smoothingData->autoSmoothnessFactor);
} }
if (!smoothingData->filterInitialized) { 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) { } else if (smoothingData->derivativeCutoffFrequency != oldCutoff) {
pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency); pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency);
} }
@ -395,13 +395,13 @@ FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
bool ret = false; bool ret = false;
// if the input cutoff is 0 (auto) then we need to calculate cutoffs // 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; ret = true;
} }
// if the derivative type isn't OFF and the cutoff is 0 then we need to calculate // 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 (rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) {
if (rxConfig()->rc_smoothing_derivative_cutoff == 0) { if (rcSmoothingData.derivativeCutoffSetting == 0) {
ret = true; ret = true;
} }
} }
@ -421,12 +421,18 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
initialized = true; initialized = true;
rcSmoothingData.filterInitialized = false; rcSmoothingData.filterInitialized = false;
rcSmoothingData.averageFrameTimeUs = 0; 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); rcSmoothingResetAccumulation(&rcSmoothingData);
rcSmoothingData.inputCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff; rcSmoothingData.inputCutoffFrequency = rcSmoothingData.inputCutoffSetting;
if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) { if (rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) {
rcSmoothingData.derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff; rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting;
} }
calculateCutoffs = rcSmoothingAutoCalculate(); calculateCutoffs = rcSmoothingAutoCalculate();
@ -511,7 +517,7 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
if (rcSmoothingData.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 // 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 // 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); 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++) { for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value
if (rcSmoothingData.filterInitialized) { if (rcSmoothingData.filterInitialized) {
switch (rxConfig()->rc_smoothing_input_type) { switch (rcSmoothingData.inputFilterType) {
case RC_SMOOTHING_INPUT_PT1: case RC_SMOOTHING_INPUT_PT1:
rcCommand[updatedChannel] = pt1FilterApply((pt1Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]); rcCommand[updatedChannel] = pt1FilterApply((pt1Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
break; break;
@ -749,18 +755,9 @@ bool rcSmoothingIsEnabled(void)
} }
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
int rcSmoothingGetValue(int whichValue) rcSmoothingFilter_t *getRcSmoothingData(void)
{ {
switch (whichValue) { return &rcSmoothingData;
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;
}
} }
bool rcSmoothingInitializationComplete(void) { bool rcSmoothingInitializationComplete(void) {

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

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

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

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