From 20a29bff9923b5fd82c4f0484808e0b5538016bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Nov 2012 16:42:31 +0100 Subject: [PATCH] Fixes for roll/pitch feedforward --- apps/fixedwing_att_control/fixedwing_att_control_att.c | 8 ++++++-- 1 file changed, 6 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 334b95226f..0456277007 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.c +++ b/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