From 1119ea5a80f14efd271148eb39821d1f433664bf Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 18 May 2016 21:20:38 +0200 Subject: [PATCH] added some comments on the new manual attitude setpoint generation --- .../mc_pos_control/mc_pos_control_main.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index c81dac56d8b8..f07e06d37393 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -2002,19 +2002,32 @@ MulticopterPositionControl::task_main() math::Matrix<3, 3> R_sp; - // construct attitude setpoint rotation matrix + // construct attitude setpoint rotation matrix. modify the setpoints for roll + // and pitch such that they reflect the user's intention even if a yaw error + // (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix + // from the pure euler angle setpoints will lead to unexpected attitude behaviour from + // the user's view as the euler angle sequence uses the yaw setpoint and not the current + // heading of the vehicle. // calculate our current yaw error float yaw_error = _wrap_pi(_att_sp.yaw_body - _yaw); + // compute the vector obtained by rotating a z unit vector by the rotation + // given by the roll and pitch commands of the user math::Vector<3> zB = {0, 0, 1}; math::Matrix<3,3> R_sp_roll_pitch; R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0); math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB; + // transform the vector into a new frame which is rotated around the z axis + // by the current yaw error. this vector defines the desired tilt when we look + // into the direction of the desired heading math::Matrix<3,3> R_yaw_correction; R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error); z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp; + + // compute the quaternion which captures the tilt of the vector z_roll_pitch_sp + // the desired corrected roll and pitch angles can then be computed from the quaternion float angle = acosf(zB * z_roll_pitch_sp); math::Vector<3> tilt_axis = {0,0,1.0f};