Skip to content

Commit

Permalink
Merge branch 'borisbstyle-rcFilter'
Browse files Browse the repository at this point in the history
  • Loading branch information
@hydra
hydra committed Sep 29, 2015
2 parents 2c701bd + 36a3ee4 commit 5c6559d
Show file tree
Hide file tree
Showing 6 changed files with 62 additions and 1 deletion.
1 change: 1 addition & 0 deletions docs/Cli.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,7 @@ Re-apply any new defaults as desired.
| `rssi_scale` | | 1 | 255 | 30 | Master | UINT8 |
| `rssi_ppm_invert` | | 0 | 1 | 0 | Master | UINT8 |
| `input_filtering_mode` | | 0 | 1 | 0 | Master | INT8 |
| `rc_smoothing ` | Interpolation of Rc data during looptimes when there are no new updates. This gives smoother RC input to PID controller and cleaner PIDsum | 0 | 1 | 1 | Master | INT8 |
| `min_throttle` | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. | 0 | 2000 | 1150 | Master | UINT16 |
| `max_throttle` | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. | 0 | 2000 | 1850 | Master | UINT16 |
| `min_command` | This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once. | 0 | 2000 | 1000 | Master | UINT16 |
Expand Down
1 change: 1 addition & 0 deletions src/main/config/config.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
1 change: 1 addition & 0 deletions src/main/io/serial_cli.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 },
Expand Down
42 changes: 42 additions & 0 deletions src/main/mw.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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;
Expand All @@ -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.
Expand Down Expand Up @@ -749,6 +787,10 @@ void loop(void)
}
}

if (masterConfig.rxConfig.rcSmoothing) {
filterRc();
}

annexCode();
#if defined(BARO) || defined(SONAR)
haveProcessedAnnexCodeOnce = true;
Expand Down
15 changes: 14 additions & 1 deletion src/main/rx/rx.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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);

Expand Down Expand Up @@ -168,6 +169,7 @@ void rxInit(rxConfig_t *rxConfig)
}

if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
rxRefreshRate = 20000;
rxPwmInit(&rxRuntimeConfig, &rcReadRawFunc);
}

Expand All @@ -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;
}
Expand Down Expand Up @@ -567,4 +577,7 @@ void updateRSSI(uint32_t currentTime)
}
}

void initRxRefreshRate(uint16_t *rxRefreshRatePtr) {
*rxRefreshRatePtr = rxRefreshRate;
}

3 changes: 3 additions & 0 deletions src/main/rx/rx.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -152,3 +153,5 @@ void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChann

void suspendRxSignal(void);
void resumeRxSignal(void);

void initRxRefreshRate(uint16_t *rxRefreshRatePtr);

0 comments on commit 5c6559d

Please sign in to comment.