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?
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.
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.
If the throttle stick is at the center, the z component of the position loop is also run, right?
“_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)
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 = -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 (:
Thank you very much for your help. I really appreciate your detailed explanation.
It’s getting clear, but few more questions.
“_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)”.
Case 1: For MPC_USE_HTE = 0
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
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.
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:
The position controller subscribes to it and gets the value here:
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”
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?
_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.
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.
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).