Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

fix feedforward when fpv camera angle set

Browse source
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
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)); return applyRates(axis, deflection, fabsf(deflection));
} }
static void scaleSetpointToFpvCamAngle(void) static void scaleRawSetpointToFpvCamAngle(void)
{ {
//recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0; static uint8_t lastFpvCamAngleDegrees = 0;
@ -249,10 +249,10 @@ static void scaleSetpointToFpvCamAngle(void)
sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD); sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
} }
float roll = setpointRate[ROLL]; float roll = rawSetpoint[ROLL];
float yaw = setpointRate[YAW]; float yaw = rawSetpoint[YAW];
setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f); rawSetpoint[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); rawSetpoint[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f);
} }
#define THROTTLE_BUFFER_MAX 20 #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]); rawSetpoint[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]);
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate); DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
} }
// adjust raw setpoint steps to camera angle (mixing Roll and Yaw)
// adjust un-filtered setpoint steps 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)) {
scaleSetpointToFpvCamAngle(); scaleRawSetpointToFpvCamAngle();
} }
} }