Some questions about “mc_att_control” hope to get help

[wanggen123]

@DrTon @sanderux

Some questions about “mc_att_control” hope to get help

void
MulticopterAttitudeControl::control_attitude(float dt)
{
vehicle_attitude_setpoint_poll();

_thrust_sp = _v_att_sp.thrust;

/* construct attitude setpoint rotation matrix */
math::Quaternion q_sp(_v_att_sp.q_d[0], _v_att_sp.q_d[1], _v_att_sp.q_d[2], _v_att_sp.q_d[3]);
math::Matrix<3, 3> R_sp = q_sp.to_dcm();

/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Matrix<3, 3> R = q_att.to_dcm();

/* all input data is ready, run controller itself */

/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));

/* axis and sin(angle) of desired rotation */
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);

/* calculate angle error */
float e_R_z_sin = e_R.length();
float e_R_z_cos = R_z * R_sp_z;	

1、The weight of yaw why is that so

   /* calculate weight for yaw control */  
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;

if (e_R_z_sin > 0.0f) {
	/* get axis-angle representation */
	float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
	math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;

2、Why can the error be expressed as such？Can yaw error can be directly modified “e_R(2)”

         e_R = e_R_z_axis * e_R_z_angle;//Why can the error be expressed as such？Can yaw error can be directly modified “e_R(2)”
	/* cross product matrix for e_R_axis */
	math::Matrix<3, 3> e_R_cp;
	e_R_cp.zero();
	e_R_cp(0, 1) = -e_R_z_axis(2);
	e_R_cp(0, 2) = e_R_z_axis(1);
	e_R_cp(1, 0) = e_R_z_axis(2);
	e_R_cp(1, 2) = -e_R_z_axis(0);
	e_R_cp(2, 0) = -e_R_z_axis(1);
	e_R_cp(2, 1) = e_R_z_axis(0);

	/* rotation matrix for roll/pitch only rotation */
	R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));

} else {
	/* zero roll/pitch rotation */
	R_rp = R;
}

/* R_rp and R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;

3、for large thrust vector rotations how to calculate？？？Is it convenient to provide some documentation?

if (e_R_z_cos < 0.0f) {
	/* for large thrust vector rotations use another rotation method:
	 * calculate angle and axis for R -> R_sp rotation directly */
	math::Quaternion q_error;
	q_error.from_dcm(R.transposed() * R_sp);
	math::Vector<3> e_R_d = q_error(0) >= 0.0f ? q_error.imag()  * 2.0f : -q_error.imag() * 2.0f;

	/* use fusion of Z axis based rotation and direct rotation */
	float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
	e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
}

/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);

thanks，Looking forward to receiving your reply。

[sanderux]

@wanggen123 Please try not to use issues for asking questions. The place to ask questions would be the forum http://discuss.px4.io/ or the slack channel http://slack.px4.io/

I'm not a specialist on MC, i hope others can help you out. It would most likely help if you make clear what it is you are trying to achieve