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

use floats for rc interpolation 

do not filter setpoint data
This commit is contained in:
rav 2017-05-04 00:16:46 +02:00
parent ec0373d7c7
commit 33043d79dc
4 changed files with 32 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
src/main/fc/fc_rc.c
View File

@ -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;
}

2
src/main/fc/rc_controls.c
View File

@ -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

2
src/main/fc/rc_controls.h
View File

@ -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.

11
src/main/flight/pid.c
View File

@ -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;
}