MC Position Controller

crest_wave · May 3, 2020, 3:24pm

Hello All,

I’m trying to understand the MC position controller and have some questions on the diagram below.

Question 1: Could anyone help me on verifying the calculation in the proj box and conversion box? What are the exact equations for those boxes?
Question 2: The body vertical thrust output from the proj box goes directly to the mixer?

Thanks in advance.

JulianOes · May 4, 2020, 8:05am

Let me ping @bresch to help here.

crest_wave · May 12, 2020, 1:39pm

Thank you very much for your help.
Also with a help from antoncl (somehow his post is deleted now), it took me a while, but I think I figured it out for now.

Regarding to the attached controller for the altitude mode, is my understanding below correct?

Only z-component for position-velocity loop is run.
The projz box is bypassed.
MPC_THR_MAX and MPC_THR_MIN limit the thrust setpoint value from the velocity PID loop and the thrust setpoint goes to the mixer.
If the throttle stick is at outside center, the throttle stick input goes to z-component of the velocity PID loop and the position loop is bypassed.
The value of the MPC_THR_HOVER parameter is not used in the altitude mode.

antonerasm · May 12, 2020, 7:58pm

Hey @crest_wave, sorry that my post was deleted. I posted that on an account that I realised I don’t use anymore and deleted the account.
Just for completeness, I’ll briefly mention what I posted before and then answer your current questions.

Deleted post contents:

The equations for the proj box and conversion box can be found in this article: http://www-personal.acfr.usyd.edu.au/spns/cdm/papers/Mellinger.pdf. The code can be found in the thrustToAttitude function in the module mc_pos_control/PositionControl/ControlMath.cpp.
The body vertical thrust goes directly to the mixer, with some saturation checks.

Your current questions:

Yes, only the Z component is run as only altitude is being controlled and not position.
As far as I understand, the conversion box is bypassed as the attitude setpoints come from the controller. But the projz box is run to determine the needed vertical thrust.
Yes, those are the saturation checks.
Yes, only the Z velocity loop is run because the stick controls the speed of ascent/descent.
The value of MPC_THR_HOVER is used in altitude mode. It requires this to hover the vehicle when no throttle stick input is given. It is used in the updateHoverThrust function in the module mc_pos_control/PositionControl/PositionControl.cpp

I hope that makes sense and helps. Let us know if you have any other questions.

crest_wave · May 13, 2020, 4:22pm

Thank you very much for your help and post. I really appreciate it.

I have some more questions.

X/Y component of the position loop is not run, but Z component of the position loop is run, right?

The needed vertical thrust is determined as “_thr_sp” in the script below? If so, for the altitude mode, how “_acc_sp(0)” and “_acc_sp(1)” for “body_z” are obtained?

If the throttle stick is at the center, the z component of the position loop is also run, right?

Question 1:
“_vel_int(2)” in “updateHoverThrust” is calculated before the PID velocity control loop and added to the integral term of the z component of the velocity control loop?
So, if “vel_error.emult(_gain_vel_i) = 0, _vel_int(2) is going to be (hover_thrust_new - _hover_thrust) * (CONSTANTS_ONE_G / hover_thrust_new)?
Also, could you help me to understand how the expression above* is came up?
Note: *: (hover_thrust_new - _hover_thrust) * (CONSTANTS_ONE_G / hover_thrust_new)

<mc_pos_control/PositionControl/PositionControl.cpp>

Question 2:
I’m trying to find the definition of “hte.hover_thrust” in a script. Would you know where it is?
<mc_pos_control/mc_pos_control_main.cpp>

antonerasm · May 13, 2020, 9:32pm

Yes, that is correct. However, if I am not mistaken, in altitude mode the Z component of the position loop is run when the throttle stick input is centred to keep its altitude. Otherwise, the throttle stick input is taken as the Z velocity setpoint of the velocity controller and the X/Y components of the velocity controller is not run. You should be able to clarify this by looking at src/lib/flight_tasks/ManualAltitude (or the ManualAltitudeSmooth and ManualAltitudeSmoothVel alternatives ). Those flight tasks are responsible for setting the setpoints in altitude mode.

“_thr_sp” is a vector in the inertial (earth) frame, containing the required forces in the X, Y, and Z directions necessary to achieve the desired velocity setpoints. The magnitude of this vector is the needed vertical thrust in the body frame. It is calculated in src/modules/mc_pos_control/PositionControl/ControlMath.cpp in the function thrustToAttitude by the line: att_sp.thrust_body[2] = -thr_sp.length();.

The “_acc_sp(0)” and “_acc_sp(1)” variables are obtained by the flight task (such as src/lib/flight_tasks/ManualAltitude). See the file src/lib/flight_tasks/ManualAltitude/FlightTaskManualAltitude.cpp and the function _updateSetpoints, where the acceleration X/Y setpoints are generated.

Yes, that is correct.

Yes, the required hover thrust is calculated and added to the “_vel_int(2)” term. Let us take an example: The initial value of “_hover_thrust” and “_vel_int(2)” is zero when the module is first loaded. Then, the value stored in the parameter MPC_THR_HOVER is loaded as the hover thrust. In this case “_vel_int(2) += (MPC_THR_HOVER - 0) * (CONSTANTS_ONE_G / MPC_THR_HOVER) = CONSTANTS_ONE_G”. This makes sense as the hover thrust should overcome gravity. Also, this value is an acceleration, because the output of the velocity controller is the input to the “_accelerationControl”. I am assuming that the “updateHoverThrust” function updates it in this way to be compatible with the “mc_hover_thrust_estimator” module that will update the hover thrust estimate.

Yes, that is the hover thrust estimate. I assume it comes from the “mc_hover_thrust_estimator” module. However, I have not specifically worked with that before.

I hope this helps and clears up some things. Feel free to post any other questions, I’ll help where I can (:

crest_wave · May 14, 2020, 12:44pm

Thank you very much for your help. I really appreciate your detailed explanation.
It’s getting clear, but few more questions.

Question
“_acc_sp(0)” and “_acc_sp(1)” in “body_z” calculation below have the negative sign in the script below. Is that because the body_z points in a direction opposite to the desired destination in the NED coordinates? Also, I’m wondering why “CONSTANTS_ONE_G” in “body_z” calculation is used instead of “-_acc_sp(2)”.

antonerasm:

Yes, the required hover thrust is calculated and added to the “_vel_int(2)” term. Let us take an example: The initial value of “_hover_thrust” and “_vel_int(2)” is zero when the module is first loaded. Then, the value stored in the parameter MPC_THR_HOVER is loaded as the hover thrust. In this case “_vel_int(2) += (MPC_THR_HOVER - 0) * (CONSTANTS_ONE_G / MPC_THR_HOVER) = CONSTANTS_ONE_G”. This makes sense as the hover thrust should overcome gravity. Also, this value is an acceleration, because the output of the velocity controller is the input to the “_accelerationControl”. I am assuming that the “updateHoverThrust” function updates it in this way to be compatible with the “mc_hover_thrust_estimator” module that will update the hover thrust estimate.

Case 1: For MPC_USE_HTE = 0
Question
If MPC_USE_HTE = 0, “updateHoverThrust” is not run and “_vel_int(2) ± (hover_thrust_new - _hover_thrust) * (CONSTANTS_ONE_G / hover_thrust_new)” is not added to “_vel_int”, right?

Case 2: For MPC_USE_HTE = 1
Question
Looking at the scripts below, it seems that “_hover_thrust” is equal to “MPC_THR_HOVER” (_param_mpc_thr_hover) at first, not “hover_thrust_new”. Please correct me if I’m wrong.
If so, “hover_thrust_new” comes from the “mc_hover_thrust_estimator” module?
I can’t follow from “hte.hover_thrust” in a script below to “mc_hover_thrust_estimator” module. Any help would be appreciated.

bresch · May 18, 2020, 6:42am

Hi @crest_wave ,

Currently, he hover thrust estimator (HTE) is only active when at least the z controller is active (altitude, position and auto modes, not acro and stabilized). This is why the hover thrust is always set at boot using MPC_THR_HOVER and then updated with the estimate if available.

HTE publishes its value here:

github.com

PX4/Firmware/blob/e670d5f1e2fa5a9ac99bceb7c29985885aaa7deb/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp#L165-L176


void MulticopterHoverThrustEstimator::publishStatus(ZeroOrderHoverThrustEkf::status &status)
{
	hover_thrust_estimate_s status_msg{};
	status_msg.hover_thrust = status.hover_thrust;
	status_msg.hover_thrust_var = status.hover_thrust_var;
	status_msg.accel_innov = status.innov;
	status_msg.accel_innov_var = status.innov_var;
	status_msg.accel_innov_test_ratio = status.innov_test_ratio;
	status_msg.accel_noise_var = status.accel_noise_var;
	status_msg.timestamp = hrt_absolute_time();
	_hover_thrust_ekf_pub.publish(status_msg);
}

The position controller subscribes to it and gets the value here:

github.com

PX4/Firmware/blob/e670d5f1e2fa5a9ac99bceb7c29985885aaa7deb/src/modules/mc_pos_control/mc_pos_control_main.cpp#L439


		vehicle_attitude_s att;

		if (_att_sub.copy(&att) && PX4_ISFINITE(att.q[0])) {
			_states.yaw = Eulerf(Quatf(att.q)).psi();
		}
	}

	if (_param_mpc_use_hte.get()) {
		hover_thrust_estimate_s hte;

		if (_hover_thrust_estimate_sub.update(&hte)) {
			_control.updateHoverThrust(hte.hover_thrust);
		}
	}
}

void
MulticopterPositionControl::limit_altitude(vehicle_local_position_setpoint_s &setpoint)
{
	if (_vehicle_land_detected.alt_max < 0.0f || !_home_pos.valid_alt || !_local_pos.v_z_valid) {
		// there is no altitude limitation present or the required information not available

Also note that:

The hover thrust estimate isn’t directly affecting the dynamics of the altitude loop as it updates through the updateHoverThrust function that adjusts the integrator to “cancel out” the effect. The estimate is then removing the steady offset of the integrator.
The hover trust estimate is also used in the land detector for better accuracy of the “low thrust”
detection.
The algorithm use for HTE is described here:
https://github.com/PX4/Firmware/blob/e670d5f1e2fa5a9ac99bceb7c29985885aaa7deb/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp#L38-L59

crest_wave · May 18, 2020, 2:28pm

Hi bresch,

Thank you very much for your help. I really appreciate it.
Now, I understand the most of the part. But, I have one more question below.

Could you help me to understand how the updateHoverThrust function “cancel out” the the effect of the thrust estimator addition on the altitude loop?
The updateHoverThrust function adds the _vel_int(2) as below, right? But, how is it “cancel out” the effect?

bresch · May 19, 2020, 9:21am

_hover_thrust is set through setHoverThrust and is also called in updateHoverThrust.

So hover_thrust_new - _hover_thrust is the difference between the last iteration and now.

In other words, in the first iteration, it’s correct that _hover_thrust = MPC_THR_HOVER but that’s not the case during the whole flight, as soon as updateHoverThrust is called, this value is updated as well.

crest_wave · May 19, 2020, 1:59pm

Thank you very much for your explanation.
Now, I understand how the _hover_thrust and hover_thrust_new are updated.
But, still I don’t have a clear understanding how the expression below is came up and how the updateHoverThrust function “cancels out” the the effect of the thrust estimator addition on the altitude loop. I appreciate your help.

crest_wave · June 17, 2020, 11:49am

Hi bresch,

I have another question.
I’m trying to understand the horizontal velocity constrain calculation below.
Does “_vel_sp” come from “FlightTaskManualPosition.cpp”?

In the term, “(_vel_sp-vel_sp_position).xy()”, why is “vel_sp_position” subtracted?

Thank you very much for your help in advance.

lfdiniz · June 17, 2020, 12:50pm

crest_wave,

Firstly _vel_sp receives setpoints generated by the active flight task, but in _positionControl() the velocity setpoint generated by the position controller is summed to this value (ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position)).

As for the second one, take a closer look at constrainXY function in ControlMath.cpp and it may make sense for you.

This function outputs the sum of two vectors (in this case, vel_sp_position + _vel_sp - vel_sp_position, or just _vel_sp) but prioritizing the first one (vel_sp_position, generated by the position controller). The second vector (_vel_sp - vel_sp_position) is the feedforward velocity generated by the active flight task (the “original” _vel_sp).

Hope it helps!

crest_wave · June 18, 2020, 10:37am

Hi Ifdiniz,

Thank you very much for your detailed explanation. I figured it out!
I really appreciate your help!