diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-25 16:42:31 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-25 16:42:31 +0100 |
commit | 20a29bff9923b5fd82c4f0484808e0b5538016bf (patch) | |
tree | 2ea595398d28ecec424f18a7caecc2feda66dd50 /apps/fixedwing_att_control/fixedwing_att_control_att.c | |
parent | 346d93b2710e5b55c203723188e40d2bad2f9d85 (diff) | |
download | px4-firmware-20a29bff9923b5fd82c4f0484808e0b5538016bf.tar.gz px4-firmware-20a29bff9923b5fd82c4f0484808e0b5538016bf.tar.bz2 px4-firmware-20a29bff9923b5fd82c4f0484808e0b5538016bf.zip |
Fixes for roll/pitch feedforward
Diffstat (limited to 'apps/fixedwing_att_control/fixedwing_att_control_att.c')
-rw-r--r-- | apps/fixedwing_att_control/fixedwing_att_control_att.c | 8 |
1 files 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 334b95226..045627700 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 |