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

Merge branch 'rcFilter' of https://github.com/borisbstyle/cleanflight into borisbstyle-rcFilter

Browse source
This commit is contained in:
Dominic Clifton 2015-09-29 10:36:41 +01:00
commit 36a3ee4e59
6 changed files with 62 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/config/config.c
View file

@ -420,6 +420,7 @@ static void resetConf(void)
masterConfig.rxConfig.rssi_channel = 0;
masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
masterConfig.rxConfig.rssi_ppm_invert = 0;
masterConfig.rxConfig.rcSmoothing = 1;
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);

1
src/main/io/serial_cli.c
View file

@ -325,6 +325,7 @@ const clivalue_t valueTable[] = {
{ "rssi_channel", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_channel, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT },
{ "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, RSSI_SCALE_MIN, RSSI_SCALE_MAX },
{ "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_ppm_invert, 0, 1 },
{ "rc_smoothing", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rcSmoothing, 0, 1 },
{ "input_filtering_mode", VAR_INT8 | MASTER_VALUE, &masterConfig.inputFilteringMode, 0, 1 },
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX },

42
src/main/mw.c
View file

@ -107,6 +107,8 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m
extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
static bool isRXDataNew;
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
@ -684,6 +686,41 @@ void processRx(void)
}
void filterRc(void){
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor;
static filterStatePt1_t filteredCycleTimeState;
uint16_t rxRefreshRate, filteredCycleTime;
// Set RC refresh rate for sampling and channels to filter
initRxRefreshRate(&rxRefreshRate);
filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1);
rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
if (isRXDataNew) {
for (int channel=0; channel < 4; channel++) {
deltaRC[channel] = rcData[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcData[channel];
}
isRXDataNew = false;
factor = rcInterpolationFactor - 1;
} else {
factor--;
}
// Interpolate steps of rcData
if (factor > 0) {
for (int channel=0; channel < 4; channel++) {
rcData[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
}
} else {
factor = 0;
}
}
void loop(void)
{
static uint32_t loopTime;
@ -695,6 +732,7 @@ void loop(void)
if (shouldProcessRx(currentTime)) {
processRx();
isRXDataNew = true;
#ifdef BARO
// the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
@ -749,6 +787,10 @@ void loop(void)
}
}
if (masterConfig.rxConfig.rcSmoothing) {
filterRc();
}
annexCode();
#if defined(BARO) || defined(SONAR)
haveProcessedAnnexCodeOnce = true;

15
src/main/rx/rx.c
View file

@ -79,7 +79,7 @@ static uint8_t skipRxSamples = 0;
int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
#define PPM_AND_PWM_SAMPLE_COUNT 4
#define PPM_AND_PWM_SAMPLE_COUNT 3
#define DELAY_50_HZ (1000000 / 50)
#define DELAY_10_HZ (1000000 / 10)
@ -99,6 +99,7 @@ static uint16_t nullReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channe
}
static rcReadRawDataPtr rcReadRawFunc = nullReadRawRC;
static uint16_t rxRefreshRate;
void serialRxInit(rxConfig_t *rxConfig);
@ -168,6 +169,7 @@ void rxInit(rxConfig_t *rxConfig)
}
if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
rxRefreshRate = 20000;
rxPwmInit(&rxRuntimeConfig, &rcReadRawFunc);
}
@ -180,20 +182,28 @@ void serialRxInit(rxConfig_t *rxConfig)
bool enabled = false;
switch (rxConfig->serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
rxRefreshRate = 22000;
enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_SPEKTRUM2048:
rxRefreshRate = 11000;
enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_SBUS:
rxRefreshRate = 11000;
enabled = sbusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_SUMD:
rxRefreshRate = 11000;
enabled = sumdInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_SUMH:
rxRefreshRate = 11000;
enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
case SERIALRX_XBUS_MODE_B:
case SERIALRX_XBUS_MODE_B_RJ01:
rxRefreshRate = 11000;
enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
break;
}
@ -567,4 +577,7 @@ void updateRSSI(uint32_t currentTime)
}
}
void initRxRefreshRate(uint16_t *rxRefreshRatePtr) {
*rxRefreshRatePtr = rxRefreshRate;
}

3
src/main/rx/rx.h
View file

@ -114,6 +114,7 @@ typedef struct rxConfig_s {
uint8_t rssi_channel;
uint8_t rssi_scale;
uint8_t rssi_ppm_invert;
uint8_t rcSmoothing; // Enable/Disable RC filtering
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end
@ -152,3 +153,5 @@ void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChann
void suspendRxSignal(void);
void resumeRxSignal(void);
void initRxRefreshRate(uint16_t *rxRefreshRatePtr);