It seem that whether xy velocity is controlled or not, the vertical thrust compensation is always executed.
While in an older version of the PX4 source code (2014) that a friend showed me(below), it is executed when xy velocity is not controlled, which makes more sense to me, is there a justification of the present(above) code?
Thanks~
Hi @dagar , thanks for the comment.
My point is, it is just confusing to me that they can be enabled of disabled independently. Like in the comment of the code /* thrust compensation for altitude only control mode */
I think this compensation is necessary when xy position and velocity is not controlled and only altitude control is enabled, and if boty xy position and altitude control are engaged, the thrust doesn’t need compensation since the set point thrust from pid can be obtained through attitude posing.
/* calculate attitude setpoint from thrust vector */
if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {
/* desired body_z axis = -normalize(thrust_vector) */
math::Vector<3> body_x;
math::Vector<3> body_y;
math::Vector<3> body_z;
if (thrust_abs > SIGMA) {
body_z = -thrust_sp / thrust_abs;
} else {
/* no thrust, set Z axis to safe value */
body_z.zero();
body_z(2) = 1.0f;
}
/* vector of desired yaw direction in XY plane, rotated by PI/2 */
math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
if (fabsf(body_z(2)) > SIGMA) {
/* desired body_x axis, orthogonal to body_z */
body_x = y_C % body_z;
/* keep nose to front while inverted upside down */
if (body_z(2) < 0.0f) {
body_x = -body_x;
}
body_x.normalize();
} else {
/* desired thrust is in XY plane, set X downside to construct correct matrix,
* but yaw component will not be used actually */
body_x.zero();
body_x(2) = 1.0f;
}
/* desired body_y axis */
body_y = body_z % body_x;
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
R(i, 0) = body_x(i);
R(i, 1) = body_y(i);
R(i, 2) = body_z(i);
}
/* copy quaternion setpoint to attitude setpoint topic */
matrix::Quatf q_sp = R;
memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d));
_att_sp.q_d_valid = true;
/* calculate euler angles, for logging only, must not be used for control */
matrix::Eulerf euler = R;
_att_sp.roll_body = euler(0);
_att_sp.pitch_body = euler(1);
/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
}
@frank Hi Frank! Thanks for spotting this, looks like a mistake. I don’t see a reason for tilt compensation when velocity is controlled. Let me know if you want to do the pull request, otherwise I can do it quickly.
@tumbili Hi Tumbili, I think it is better you do the pull request, I am still learning.
Do you have any idea why this is not causing a problem in controlling the drones?
@frank I fixed the issue and master should now be ok. Thanks for your efforts!
I guess you can look at the bug as a disturbance entering the system. Depending on how well the controller reacts against it might be hard to see the effect.