From 271bcf9d55d13a39be18207cd62e7cf4764c7cd9 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Mon, 26 Jul 2021 14:53:56 +1000 Subject: [PATCH] fix feedforward when fpv camera angle set --- src/main/fc/rc.c | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 15d988a82..1e889b540 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -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(); } }