Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
Code Issues Wiki Activity

Code tidy of fc_rc.c

Browse source
This commit is contained in:
Martin Budden 2017-09-21 05:20:39 +01:00
parent c612280c1a
commit 39613d15e5
1 changed files with 21 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

34
src/main/fc/fc_rc.c
View file

@ -156,24 +156,27 @@ static void scaleRcCommandToFpvCamAngle(void)
#define THROTTLE_BUFFER_MAX 20 #define THROTTLE_BUFFER_MAX 20
#define THROTTLE_DELTA_MS 100 #define THROTTLE_DELTA_MS 100
static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) static void checkForThrottleErrorResetState(uint16_t rxRefreshRate)
{ {
static int index; static int index;
static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX]; static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX];
const int rxRefreshRateMs = rxRefreshRate / 1000; const int rxRefreshRateMs = rxRefreshRate / 1000;
const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX); const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX);
const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold; const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold;
rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE]; rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE];
if (index >= indexMax) if (index >= indexMax) {
index = 0; index = 0;
}
const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
if (ABS(rcCommandSpeed) > throttleVelocityThreshold) if (ABS(rcCommandSpeed) > throttleVelocityThreshold) {
pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain)); pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
else } else {
pidSetItermAccelerator(1.0f); pidSetItermAccelerator(1.0f);
}
} }
void processRcCommand(void) void processRcCommand(void)
@ -182,10 +185,6 @@ void processRcCommand(void)
static float rcStepSize[4] = { 0, 0, 0, 0 }; static float rcStepSize[4] = { 0, 0, 0, 0 };
static int16_t rcInterpolationStepCount; static int16_t rcInterpolationStepCount;
static uint16_t currentRxRefreshRate; static uint16_t currentRxRefreshRate;
const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT"
uint16_t rxRefreshRate;
bool readyToCalculateRate = false;
uint8_t readyToCalculateRateAxisCnt = 0;
if (isRXDataNew) { if (isRXDataNew) {
currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000); currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
@ -194,6 +193,11 @@ void processRcCommand(void)
} }
} }
const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT"
uint16_t rxRefreshRate;
bool readyToCalculateRate = false;
uint8_t readyToCalculateRateAxisCnt = 0;
if (rxConfig()->rcInterpolation) { if (rxConfig()->rcInterpolation) {
// Set RC refresh rate for sampling and channels to filter // Set RC refresh rate for sampling and channels to filter
switch (rxConfig()->rcInterpolation) { switch (rxConfig()->rcInterpolation) {
@ -240,19 +244,22 @@ void processRcCommand(void)
} }
if (readyToCalculateRate || isRXDataNew) { if (readyToCalculateRate || isRXDataNew) {
if (isRXDataNew) if (isRXDataNew) {
readyToCalculateRateAxisCnt = FD_YAW; readyToCalculateRateAxisCnt = FD_YAW;
}
for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) {
calculateSetpointRate(axis); calculateSetpointRate(axis);
}
if (debugMode == DEBUG_RC_INTERPOLATION) { if (debugMode == DEBUG_RC_INTERPOLATION) {
debug[2] = rcInterpolationStepCount; debug[2] = rcInterpolationStepCount;
debug[3] = setpointRate[0]; debug[3] = setpointRate[0];
} }
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw) // Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleRcCommandToFpvCamAngle(); scaleRcCommandToFpvCamAngle();
}
isRXDataNew = false; isRXDataNew = false;
} }
@ -330,7 +337,8 @@ void updateRcCommands(void)
} }
} }
void resetYawAxis(void) { void resetYawAxis(void)
{
rcCommand[YAW] = 0; rcCommand[YAW] = 0;
setpointRate[YAW] = 0; setpointRate[YAW] = 0;
} }