From 9b5cae2134616a2d9b0a7b3e15718d372aa94b25 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Nov 2013 19:44:38 +1100 Subject: [PATCH] APM_Control: reduce roll compensation in pitch controller by cos(pitch) This reduces the use of the pitch compensation when in a steep climb or dive Pair-Programmed-With: Paul Riseborough --- libraries/APM_Control/AP_PitchController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 95e4b40eed..7b0dbaca30 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -218,7 +218,7 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv // don't do turn coordination handling when at very high pitch angles rate_offset = 0; } else { - rate_offset = fabsf(ToDeg((GRAVITY_MSS / max(aspeed , float(aparm.airspeed_min))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff; + rate_offset = cosf(_ahrs.pitch)*fabsf(ToDeg((GRAVITY_MSS / max(aspeed , float(aparm.airspeed_min))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff; } if (inverted) { rate_offset = -rate_offset;