rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Remove Aux filter // more accurate cycleTime // Code cleanup 

Change datatypes
This commit is contained in:
borisbstyle 2015-08-20 21:59:07 +02:00
parent b69bab33b2
commit ff23cab2a8
3 changed files with 26 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
src/main/mw.c
View File

@ -93,8 +93,6 @@ enum {
/* IBat monitoring interval (in microseconds) - 6 default looptimes */
#define IBATINTERVAL (6 * 3500)
#define LOOP_DEADBAND 400 // Dead band for loop to modify to rcInterpolationFactor in RC Filtering for unstable looptimes
uint32_t currentTime = 0;
uint32_t previousTime = 0;
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
@ -109,7 +107,7 @@ 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;
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
@ -685,92 +683,28 @@ void processRx(void)
}
void getArmingChannel(modeActivationCondition_t *modeActivationConditions, uint8_t *armingChannel) {
for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
*armingChannel = modeActivationCondition->auxChannelIndex + NON_AUX_CHANNEL_COUNT;
break;
}
}
}
void filterRc(void){
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t loop[5] = { 0, 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor, loopAvg;
static uint32_t rxRefreshRate;
static int16_t lastAux, deltaAux; // last arming AUX position and delta for arming AUX
static uint8_t auxChannelToFilter; // AUX channel used for arming needs filtering when used
static int loopCount;
static int16_t factor, rcInterpolationFactor;
static filterStatePt1_t filteredCycleTimeState;
uint16_t rxRefreshRate, filteredCycleTime;
// Set RC refresh rate for sampling and channels to filter
if (!rxRefreshRate) {
if (feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) {
rxRefreshRate = 20000;
initRxRefreshRate(&rxRefreshRate);
// AUX Channels to filter to replace PPM/PWM averaging
getArmingChannel(currentProfile->modeActivationConditions,&auxChannelToFilter);
filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1);
rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
}
// TODO Are there more different refresh rates?
else {
switch (masterConfig.rxConfig.serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
rxRefreshRate = 22000;
break;
case SERIALRX_SPEKTRUM2048:
rxRefreshRate = 11000;
break;
case SERIALRX_SBUS:
rxRefreshRate = 11000;
break;
default:
rxRefreshRate = 10000;
break;
}
}
rcInterpolationFactor = 1; // Initial Factor before looptime average is calculated
}
// Averaging of cycleTime for more precise sampling
loop[loopCount] = cycleTime;
loopCount++;
// Start recalculating new rcInterpolationFactor every 5 loop iterations
if (loopCount > 4) {
uint16_t tmp = (loop[0] + loop[1] + loop[2] + loop[3] + loop[4]) / 5;
// Jitter tolerance to prevent rcInterpolationFactor jump too much
if (tmp > (loopAvg + LOOP_DEADBAND) || tmp < (loopAvg - LOOP_DEADBAND)) {
loopAvg = tmp;
rcInterpolationFactor = rxRefreshRate / loopAvg + 1;
}
loopCount = 0;
}
if (isRXdataNew) {
if (isRXDataNew) {
for (int channel=0; channel < 4; channel++) {
deltaRC[channel] = rcData[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcData[channel];
}
// Read AUX channel (arm/disarm guard enhancement)
if (auxChannelToFilter) {
deltaAux = rcData[auxChannelToFilter] - (lastAux - deltaAux * factor/rcInterpolationFactor);
lastAux = rcData[auxChannelToFilter];
}
isRXdataNew = false;
isRXDataNew = false;
factor = rcInterpolationFactor - 1;
}
else {
} else {
factor--;
}
@ -779,14 +713,7 @@ void filterRc(void){
for (int channel=0; channel < 4; channel++) {
rcData[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
}
// Interpolate steps of Aux
if (auxChannelToFilter) {
rcData[auxChannelToFilter] = lastAux - deltaAux * factor/rcInterpolationFactor;
}
}
else {
} else {
factor = 0;
}
}
@ -802,7 +729,7 @@ void loop(void)
if (shouldProcessRx(currentTime)) {
processRx();
isRXdataNew = true;
isRXDataNew = true;
#ifdef BARO
// the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.

13
src/main/rx/rx.c
View File

@ -91,6 +91,7 @@ static uint16_t nullReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channe
}
static rcReadRawDataPtr rcReadRawFunc = nullReadRawRC;
static uint16_t rxRefreshRate;
void serialRxInit(rxConfig_t *rxConfig);
@ -161,6 +162,7 @@ void rxInit(rxConfig_t *rxConfig)
}
if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
rxRefreshRate = 20000;
rxPwmInit(&rxRuntimeConfig, &rcReadRawFunc);
}
@ -173,20 +175,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;
}
@ -545,4 +555,7 @@ void updateRSSI(uint32_t currentTime)
}
}
void initRxRefreshRate(uint16_t *rxRefreshRatePtr) {
*rxRefreshRatePtr = rxRefreshRate;
}

1
src/main/rx/rx.h
View File

@ -143,3 +143,4 @@ uint8_t serialRxFrameStatus(rxConfig_t *rxConfig);
void updateRSSI(uint32_t currentTime);
void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChannelRangeConfiguration);
void initRxRefreshRate(uint16_t *rxRefreshRatePtr);