From 33043d79dca87621c387c0d90e0a54c86f6d3df9 Mon Sep 17 00:00:00 2001 From: rav Date: Thu, 4 May 2017 00:16:46 +0200 Subject: [PATCH] use floats for rc interpolation do not filter setpoint data --- src/main/fc/fc_rc.c | 44 +++++++++++++++++++++------------------ src/main/fc/rc_controls.c | 2 +- src/main/fc/rc_controls.h | 2 +- src/main/flight/pid.c | 11 +++++----- 4 files changed, 32 insertions(+), 27 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 3754a4e61..2dd0c7fb5 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -173,11 +173,11 @@ static void scaleRcCommandToFpvCamAngle(void) { void processRcCommand(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 float rcCommandInterp[4] = { 0, 0, 0, 0 }; + static float rcStepSize[4] = { 0, 0, 0, 0 };; + static int16_t rcInterpolationStepCount; static uint16_t currentRxRefreshRate; - const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; + const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT" uint16_t rxRefreshRate; bool readyToCalculateRate = false; uint8_t readyToCalculateRateAxisCnt = 0; @@ -205,35 +205,35 @@ void processRcCommand(void) } if (isRXDataNew) { - rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; - - if (debugMode == DEBUG_RC_INTERPOLATION) { - for(int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis]; - debug[3] = rxRefreshRate; - } + rcInterpolationStepCount = rxRefreshRate / targetPidLooptime; for (int channel=ROLL; channel < interpolationChannels; channel++) { - deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); - lastCommand[channel] = rcCommand[channel]; + rcStepSize[channel] = (rcCommand[channel] - rcCommandInterp[channel]) / (float)rcInterpolationStepCount; } - factor = rcInterpolationFactor - 1; + if (debugMode == DEBUG_RC_INTERPOLATION) { + debug[0] = lrintf(rcCommand[0]); + debug[1] = lrintf(getTaskDeltaTime(TASK_RX) / 1000); + //debug[1] = lrintf(rcCommandInterp[0]); + //debug[1] = lrintf(rcStepSize[0]*100); + } } else { - factor--; + rcInterpolationStepCount--; } // Interpolate steps of rcCommand - if (factor > 0) { + if (rcInterpolationStepCount > 0) { for (int channel=ROLL; channel < interpolationChannels; channel++) { - rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; - readyToCalculateRateAxisCnt = MAX(channel,FD_YAW); // throttle channel doesn't require rate calculation + rcCommandInterp[channel] += rcStepSize[channel]; + rcCommand[channel] = rcCommandInterp[channel]; + readyToCalculateRateAxisCnt = MAX(channel, FD_YAW); // throttle channel doesn't require rate calculation readyToCalculateRate = true; } } else { - factor = 0; + rcInterpolationStepCount = 0; } } else { - factor = 0; // reset factor in case of level modes flip flopping + rcInterpolationStepCount = 0; // reset factor in case of level modes flip flopping } if (readyToCalculateRate || isRXDataNew) { @@ -243,6 +243,10 @@ void processRcCommand(void) for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) calculateSetpointRate(axis); + if (debugMode == DEBUG_RC_INTERPOLATION) { + debug[2] = rcInterpolationStepCount; + debug[3] = setpointRate[0]; + } // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) scaleRcCommandToFpvCamAngle(); @@ -311,7 +315,7 @@ void updateRcCommands(void) const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); const float cosDiff = cos_approx(radDiff); const float sinDiff = sin_approx(radDiff); - const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; + const float rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 74fc3200d..c5a4f62ec 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -65,7 +65,7 @@ static pidProfile_t *pidProfile; // true if arming is done via the sticks (as opposed to a switch) static bool isUsingSticksToArm = true; -int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW +float rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index fe35b88e0..9be4198d2 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -151,7 +151,7 @@ typedef struct modeActivationProfile_s { #define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) -extern int16_t rcCommand[4]; +extern float rcCommand[4]; typedef struct rcControlsConfig_s { uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c397ef900..a36832c30 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -437,13 +437,18 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // -----calculate D component if (axis != FD_YAW) { + // apply filters + float gyroRateD = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate); + gyroRateD = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateD); + float dynC = dtermSetpointWeight; if (pidProfile->setpointRelaxRatio < 100) { dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f); } - const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y + const float rD = dynC * currentPidSetpoint - gyroRateD; // cr - y // Divide rate change by dT to get differential (ie dr/dt) float delta = (rD - previousRateError[axis]) / dT; + previousRateError[axis] = rD; // if crash recovery is on check for a crash @@ -457,10 +462,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an } } - // apply filters - delta = dtermNotchFilterApplyFn(dtermFilterNotch[axis], delta); - delta = dtermLpfApplyFn(dtermFilterLpf[axis], delta); - axisPID_D[axis] = Kd[axis] * delta * tpaFactor; }