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

fix feedforward when fpv camera angle set

This commit is contained in:
ctzsnooze 2021-07-26 14:53:56 +10:00
parent 864cf3f3b4
commit 271bcf9d55
1 changed files with 8 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/main/fc/rc.c
View File

@ -236,7 +236,7 @@ float applyCurve(int axis, float deflection)
return applyRates(axis, deflection, fabsf(deflection));
}
static void scaleSetpointToFpvCamAngle(void)
static void scaleRawSetpointToFpvCamAngle(void)
{
//recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0;
@ -249,10 +249,10 @@ static void scaleSetpointToFpvCamAngle(void)
sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
}
float roll = setpointRate[ROLL];
float yaw = setpointRate[YAW];
setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f);
setpointRate[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f);
float roll = rawSetpoint[ROLL];
float yaw = rawSetpoint[YAW];
rawSetpoint[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f);
rawSetpoint[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f);
}
#define THROTTLE_BUFFER_MAX 20
@ -587,11 +587,10 @@ FAST_CODE void processRcCommand(void)
}
rawSetpoint[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]);
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
}
// adjust un-filtered setpoint steps to camera angle (mixing Roll and Yaw)
}
// adjust raw setpoint steps to camera angle (mixing Roll and Yaw)
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleSetpointToFpvCamAngle();
scaleRawSetpointToFpvCamAngle();
}
}