Fixes for roll/pitch feedforward

apps/fixedwing_att_control/fixedwing_att_control_att.c

@@ -146,8 +146,12 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
 
 	/* Pitch (P) */
-	float pitch_sp_rollcompensation = att_sp->pitch_body + p.pitch_roll_compensation_p * att_sp->roll_body;
-	rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0);
+
+	/* 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);
+	/* 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