diff --git a/docs/en/flight_stack/controller_diagrams.md b/docs/en/flight_stack/controller_diagrams.md
index e822675b2b..1152c6301b 100644
--- a/docs/en/flight_stack/controller_diagrams.md
+++ b/docs/en/flight_stack/controller_diagrams.md
@@ -219,15 +219,20 @@ In order to keep a constant rate, this damping can be compensated using feedforw
 
 ### Turn coordination
 
- The  yaw rate setpoint is generated using the turn coordination constraint in order to minimize lateral acceleration, generated when the aircraft is slipping.
+The  yaw rate setpoint is generated using the turn coordination constraint in order to minimize lateral acceleration, generated when the aircraft is slipping.
 
-$$\dot{\Psi}_{sp} = \frac{g}{V_T} \tan{\phi_{sp}} \cdot 0.6$$
+$$\dot{\Psi}_{sp} = \frac{2g}{V_T}\left(q_0 q_1 + q_2 q_3\right)$$
 
 This also helps to counteract [adverse yaw effects](https://youtu.be/sNV_SDDxuWk) and to damp the [Dutch roll mode](https://en.wikipedia.org/wiki/Dutch_roll) by providing extra directional damping.
 
 To compensate for the non-zero pitch rate that naturally occurs during coordinated turns, a geometry-based feedforward term is added to the pitch-rate command.
 This feedforward term accounts for the aircraft's current attitude and airspeed so that the controller does not need to generate this motion purely through feedback.
 
+$$\dot{\theta}_{sp}^{ff} = \frac{4g(q_0 q_1 + q_2 q_3)^2}{V(1 - 2q_1^2 - 2q_2^2)}$$
+
+To compensate for the non-zero pitch rate that naturally occurs during coordinated turns, a geometry-based feedforward term is added to the pitch-rate command.
+This feedforward term accounts for the aircraft's current attitude and airspeed so that the controller does not need to generate this motion purely through feedback.
+
 $$\dot{\theta}_{sp}^{ff} = \frac{2g(q_0 q_1 + q_2 q_3)^2}{V(1 - 2q_1^2 - 2q_2^2)}$$
 
 ## VTOL Flight Controller
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
index 6ed96842d5..e3fcd86364 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -32,6 +32,8 @@
  ****************************************************************************/
 
 #include "FixedwingAttitudeControl.hpp"
+#include <lib/geo/geo.h>
+
 
 using namespace time_literals;
 using namespace matrix;
@@ -309,7 +311,7 @@ void FixedwingAttitudeControl::Run()
 					const float V = math::max(get_airspeed_constrained(), 0.1f);
 					const float q1 = 2.f * (q_current(0) * q_current(1) + q_current(2) * q_current(3));
 					const float r_tc_ff = CONSTANTS_ONE_G * q1 / V;
-					const float p_tc_ff = q1 * r_tc_ff / (1.f - 2.f * q_current(1) * q_current(1) - 2.f * q_current(2) * q_current(2));
+					const float p_tc_ff = 2.f * q1 * r_tc_ff / (1.f - 2.f * q_current(1) * q_current(1) - 2.f * q_current(2) * q_current(2));
 
 					body_rates_setpoint(1) += p_tc_ff;
 					body_rates_setpoint(2) += r_tc_ff;
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
index da8b4d4b97..4f67ff7631 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
@@ -35,7 +35,6 @@
 
 #include <drivers/drv_hrt.h>
 #include "fw_wheel_controller.h"
-#include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>