From 0c49abbef85598a10689cc65dbd3837d0ddb5ab2 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 8 Dec 2016 07:32:53 +0100 Subject: [PATCH] standard vtol: correctly modify attitude for pusher assist - fix a bug where the wrong rotation order was used to compute the attitude setpoint when using the pusher assist feature Signed-off-by: Roman --- src/modules/vtol_att_control/standard.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 0247a63d73..70a934298d 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -358,13 +358,13 @@ void Standard::update_mc_state() // calculate the desired pitch seen in the heading frame // this value corresponds to the amount the vehicle would try to pitch forward - float pitch_forward = asinf(body_z_sp(0)); + float pitch_forward = atan2f(body_z_sp(0), body_z_sp(2)); // only allow pitching forward up to threshold, the rest of the desired // forward acceleration will be compensated by the pusher if (pitch_forward < -_params_standard.down_pitch_max) { // desired roll angle in heading frame stays the same - float roll_new = -atan2f(body_z_sp(1), body_z_sp(2)); + float roll_new = -asinf(body_z_sp(1)); _pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max)) * _v_att_sp->thrust * _params_standard.forward_thrust_scale; @@ -383,9 +383,9 @@ void Standard::update_mc_state() tilt_new = R_yaw_correction * tilt_new; // now extract roll and pitch setpoints - float pitch = asinf(tilt_new(0)); - float roll = -atan2f(tilt_new(1), tilt_new(2)); - R_sp = matrix::Eulerf(roll, pitch, euler_sp(2)); + _v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2)); + _v_att_sp->roll_body = -asinf(tilt_new(1)); + R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2)); matrix::Quatf q_sp(R_sp); memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d)); }