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

Use RC Smoothing initialized settings rather than current PG values (#8282)

Browse source 
Use RC Smoothing initialized settings rather than current PG values
This commit is contained in:
Michael Keller 2019-05-21 03:14:26 +12:00 • committed by GitHub
commit 7cd030559d
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
10 changed files with 94 additions and 59 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

@ -4453,10 +4453,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) {
@ -4466,20 +4467,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)");

14
src/main/fc/config.c
View file

@ -41,6 +41,7 @@
#include "fc/rc.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
@ -72,6 +73,8 @@
static bool configIsDirty; /* someone indicated that the config is modified and it is not yet saved */
static bool rebootRequired = false; // set if a config change requires a reboot to take effect
pidProfile_t *currentPidProfile;
#ifndef RX_SPI_DEFAULT_PROTOCOL
@ -742,3 +745,14 @@ bool isSystemConfigured(void)
return true;
#endif
}
void setRebootRequired(void)
{
rebootRequired = true;
setArmingDisabled(ARMING_DISABLED_REBOOT_REQUIRED);
}
bool getRebootRequired(void)
{
return rebootRequired;
}

3
src/main/fc/config.h
View file

@ -55,6 +55,7 @@ PG_DECLARE(systemConfig_t, systemConfig);
struct pidProfile_s;
extern struct pidProfile_s *currentPidProfile;
void initEEPROM(void);
void resetEEPROM(void);
bool readEEPROM(void);
@ -86,3 +87,5 @@ void targetConfiguration(void);
void targetValidateConfiguration(void);
bool isSystemConfigured(void);
void setRebootRequired(void);
bool getRebootRequired(void);

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

@ -297,10 +297,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;
@ -327,15 +327,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) {
@ -360,12 +360,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);
}
@ -406,13 +406,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;
}
}
@ -432,12 +432,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();
@ -522,7 +528,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);
}
@ -530,7 +536,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;
@ -764,18 +770,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 {

3
src/main/fc/runtime_config.c
View file

@ -53,8 +53,9 @@ const char *armingDisableFlagNames[]= {
"MSP",
"PARALYZE",
"GPS",
"RESCUE SW",
"RESCUE_SW",
"RPMFILTER",
"REBOOT_REQD",
"ARMSWITCH",
};

3
src/main/fc/runtime_config.h
View file

@ -61,7 +61,8 @@ typedef enum {
ARMING_DISABLED_GPS = (1 << 18),
ARMING_DISABLED_RESC = (1 << 19),
ARMING_DISABLED_RPMFILTER = (1 << 20),
ARMING_DISABLED_ARM_SWITCH = (1 << 21), // Needs to be the last element, since it's always activated if one of the others is active when arming
ARMING_DISABLED_REBOOT_REQUIRED = (1 << 21),
ARMING_DISABLED_ARM_SWITCH = (1 << 22), // Needs to be the last element, since it's always activated if one of the others is active when arming
} armingDisableFlags_e;
#define ARMING_DISABLE_FLAGS_COUNT (LOG2(ARMING_DISABLED_ARM_SWITCH) + 1)

22
src/main/msp/msp.c
View file

@ -241,6 +241,14 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr
}
#endif //USE_SERIAL_4WAY_BLHELI_INTERFACE
static void configRebootUpdateCheckU8(uint8_t *parm, uint8_t value)
{
if (*parm != value) {
setRebootRequired();
}
*parm = value;
}
static void mspRebootFn(serialPort_t *serialPort)
{
UNUSED(serialPort);
@ -862,6 +870,10 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
// 4 bytes, flags
const uint32_t armingDisableFlags = getArmingDisableFlags();
sbufWriteU32(dst, armingDisableFlags);
// config state flags - bits to indicate the state of the configuration, reboot required, etc.
// other flags can be added as needed
sbufWriteU8(dst, (getRebootRequired() << 0));
}
break;
@ -2433,11 +2445,11 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
// Added in MSP API 1.40
rxConfigMutable()->rcInterpolationChannels = sbufReadU8(src);
#if defined(USE_RC_SMOOTHING_FILTER)
rxConfigMutable()->rc_smoothing_type = sbufReadU8(src);
rxConfigMutable()->rc_smoothing_input_cutoff = sbufReadU8(src);
rxConfigMutable()->rc_smoothing_derivative_cutoff = sbufReadU8(src);
rxConfigMutable()->rc_smoothing_input_type = sbufReadU8(src);
rxConfigMutable()->rc_smoothing_derivative_type = sbufReadU8(src);
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_type, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_input_cutoff, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_derivative_cutoff, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_input_type, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_derivative_type, sbufReadU8(src));
#else
sbufReadU8(src);
sbufReadU8(src);