About Thrust compensation in MultiCopter Position Control

Hi everyone, I’m a beginner reading PX4 source code, and there is a confusing thing in the multicopter position control code mc_pos_control_main.cpp

 if (_control_mode.flag_control_altitude_enabled) {
 	/* thrust compensation for altitude only control modes */
 	float att_comp;

 	if (_R(2, 2) > TILT_COS_MAX) {
 		att_comp = 1.0f / _R(2, 2);

 	} else if (_R(2, 2) > 0.0f) {
 		att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
 		saturation_z = true;

 	} else {
 		att_comp = 1.0f;
 		saturation_z = true;
 	}

 	thrust_sp(2) *= att_comp;
 }

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~

if (_control_mode.flag_control_velocity_enabled) {
	...
} else {
	/* thrust compensation for altitude only control mode */
	float att_comp;

	if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
	    att_comp = 1.0f / PX4_R(_att.R, 2, 2);

	} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
	        att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
	        saturation_z = true;

	} else {
		att_comp = 1.0f;
		saturation_z = true;
	        }

		thrust_sp(2) *= att_comp;
	}

@franke

Take a look at the at the control flags (_control_mode in mc_pos_control). In the current code they’re enabled or disabled independently.

flag_control_altitude_enabled
flag_control_velocity_enabled
flag_control_position_enabled
etc

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 */

}

I see what you mean, perhaps @tumbili could provide more detail. It happens to be the 1 year anniversary of this change. :sparkles:

Great! Hope @tumbili can give some detail and comment. Maybe there are some engineering considerations in this modification.

@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.:slight_smile:
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.

2 Likes

That make sense. Thanks for the explanation!