added some comments on the new manual attitude setpoint generation

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};