Position and attitud controllers diagram

PX4 Autopilot
1

Hi everyone,

For my master’s project I need to create the Simulink model for the position and attitude controllers of the PX4 firmware, so I can perform some analyses on its performances in simulation. I see that they are very dependents of the flight mode but for the moment I am just looking for a general diagram of the controller structure (Inner/Outer loop P, PID). I have review the mc_att_control_main.cpp and I have identified the following block diagram.

PID-PX4.PNG847×334 25.8 KB

I see that position control use a similar controller but I do not understand yet how it is connected to the attitude controller.

I would appreciate if someone could confirm the identification I have done and suggest documentation about this problem so I can better understand the code.

Thanks.

7 Likes
2

Hi, you diagram looks pretty good. The feed-forward path is not used for multirotors and it would require a derivative term with it. I think it’s mainly used for helicopter attitude control. Maybe you could use better symbols for the integrator and the derivative, at first I had the saturation confused with an integrator.
The position controller looks pretty much the same, with an outer position loop feeding the inner velocity loop.
The output of the position controller is an attitude and a thrust setpoint which is the input for the attitude controller.

1 Like
3

Hi, I think that the input of the D block should be the error (theta-dot_ref - theta_dot) and not theta-dot directly (same input as inner-loop P and I). Am I wrong ?

4

I really think that angle rate signal is used for derivative component of PID.
Here there is a section of the mc_att_control_main.cpp code where I believe PID is implemented :

/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;

_att_control = rates_p_scaled.emult(rates_err) +
	       _rates_int +
	       rates_d_scaled.emult(_rates_prev - rates) / dt +
	       _params.rate_ff.emult(_rates_sp);

_rates_sp_prev = _rates_sp;
_rates_prev = rates;
5

Ok, then you should have a minus somewhere in the D feedback loop (e.g. in your last sum block)

6

Ah that is true, I forgot to specify it in the diagram.

7

Hello Nestor,

I’ve been digging deep trying to understand how the position and attitude controller is implemented. I’m some how confused. Could you share with me the final control diagram that you’ve got. It will help me to understand the implementation. Thanks a lot.

8

Hi Abel,
I do not have yet a diagram for position control, but it seems to be very similar to the attitude controller.
Here is the attitude controller diagram with the corrections suggested before.

PID-PX4.PNG835×455 34.7 KB

Theta is the angle, you just need to replace it by roll, pitch or yaw angle.
Theta dot is the rate of the angle.
For yaw, there should be also a feed forward gain added to the proportional gain for the angle error.

9

Wow. Thank you so much. You did a nice investigation !

10

So, I have a question about the D. Why it is doing differential with theta_dot, but not the error between setpoint and estimate value ?

11

Yeah, I agree with you, and I changed it myself. But actually D block does a little affect.

12

@yun The D is taken directly on the angular rate (filtered gyro measurement) and not on the angular rate error in order to avoid the so called “derivative kick”. For example, imagine what is the output of the derivative term if the reference is a Heaviside step function…

1 Like
13

image.png821×502 27 KB

Hi,

I am trying to expand this controller diagram to include the sensors and their frequencies. Please see the diagram attached and kindly please let me know , how may I improve it more. Specially the LPE estimator, how can I dig into it more?

14

The final goal is to come up with a diagram like this one, but with LPE and Q-estimator.

image.jpg1255×530 121 KB

15

Hello!!

What is Kff??

16

Very helpful diagram! Where did it come from, or did you make it? I’ve been looking for diagrams to help me understand the attitude control algorithm and haven’t really found anything detailed until now.

17

I think Kff is the feed forward gain:

There is also MC_YAW_FF parameter that controls how much of user input need to feed forward to yaw rate controller. 0 means very slow control, controller will start to move yaw only when sees yaw position error, 1 means very responsive control, but with some overshot, controller will move yaw immediately, always keeping yaw error near zero.

-From the PID Tuning doc.

18

Controller.png1255×601 94.3 KB

With LPE and Q-estimator

2 Likes
19

If anyone can verify the content of the above diagram or tell me how to improve it, that would be great

20

I think there is a constant (thr_hover ) subtracted from the PID on the outer-loop velocity controller (mc_pos_control).