Merged with coordinated turn effort

1 files changed, 4 insertions, 2 deletions

diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c
index 045627700..a3c7df69d 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_att.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c
@@ -113,6 +113,7 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc
 
 int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
 		const struct vehicle_attitude_s *att,
+		const float speed_body[],
 		struct vehicle_rates_setpoint_s *rates_sp)
 {
 	static int counter = 0;
@@ -148,13 +149,14 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	/* Pitch (P) */
 
 	/* compensate feedforward for loss of lift due to non-horizontal angle of wing */
-	float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(att_sp->roll_body);
+	float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body));
 	/* set pitch plus feedforward roll compensation */
 	rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + pitch_sp_rollcompensation,
 				 att->pitch, 0, 0);
 
 	/* Yaw (from coordinated turn constraint or lateral force) */
-	//TODO
+	rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
+			/ (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[1] * sinf(att->pitch));
 
 	counter++;