Control loop architecture

[toopazo]

Hi everyone.

I’m currently working in an aerospace Lab at PennState University. We work on control algorithms for autonomous flight vehicles ( Air Vehicle Intelligence and Autonomy Lab- PSU Aerospace Engineering ). Sometimes we use Pixhawk for the flight control.

I have been searching in Redirecting to latest version of documentation and the web in general for a complete control loop architecture of the px4 without much success. So, I started looking at the source code, specially the mc_att_control_main.cpp and mc_pos_control_main.cpp files, and I came up with this.

So, I know that there are many scale factors not being shown. But, is this the control loop architecture implemented in the px4 source code for a quadcopter in stable flight?

Saludos

[Antiheavy]

Very cool! I’ve been looking for a version of this for PX4 fixed wing for a while. Anyone have one laying around? Or up for making one?

[wil]

Nice been looking for a diagram like that. From reading through the source the control algorithms are from this paper,

Mellinger, Daniel, and Vijay Kumar. “Minimum snap trajectory generation and control for quadrotors.” Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011.

Any idea how close those algorithms are implemented in PX4? I’d like to use that as a benchmark for our own development.

[toopazo]

I remember reading in the source code the same comments about that paper.
Apparently they “wanted to” use that algorithm. But is not currently implemented.

[neo]

@Paul_Riseborough @LorenzMeier Can you please confirm or comment on the control architecture diagram posted by the OP here? That would be of great help.
Thanks!

[alexh]

Just be careful that the inner controllers (rate controllers) for position and angle are implemented with a PI+D structure, which means that the derivative component is not the related to the angular rates errors e(t), but to the plant output itself y(t).

I would say something like

u(t) = kp e(t) + ki int e(t) dt - kd d(y(t))/dt

Moreover, I would say that angular rates are controlled using a full PID controller.

[neo]

@alexh
“inner controllers (rate controllers) for position and angle are implemented with a PI+D structure”
and
“Moreover, I would say that angular rates are controlled using a full PID controller”

statements seem contradictory to me. Can you please elaborate?
Thanks!

[alexh]

Hello, @neo

The original diagram shows that angular rates controllers are PD controllers, however, gains like MC_PITCHRATE_I suggest that they use a PID structures.

Although I called it PID controllers, they are implemented using a PI+D structure, as I stated above.

Hope that it looks clearer for you.

[toopazo]

Hi @alexh and @neo

I was checking again the master branch of file “mc_att_control_main.cpp”

And in line 1035 I found

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

You were right that I missed the integral term, but I found something different to what you said. Because the integral term is defined in a weird way.

Line 1070

    // Perform the integration using a first order method and do not propaate the result if out of range or invalid
    float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;

So, in a nutshell to me it seems like

• Proportional element defined as usual
• Pgain * Perror
• Second element (defined as such in lines 1071 - 1074) is quite Sui generis
• Ierror + Igain * Perror
• Third element is again defined as usual (although derivatives are low-pass filtered)
• Dgain * Derror
• Fourth element is FF of rate set point
• FFgain * sp_rates

[alexh]

@toopazo

Although it may not seem obvious at first sight, it is coherent with control theory (see, e.g., here http://www.emba.uvm.edu/~gmirchan/classes/EE275/Handouts_Ed4/Ch07(4e)Handouts/Ch7(4)Handouts_4e.pdf). The code implements a “Backward Rectangular Integrator” or “First order method”, as stated by the comments.

From a theoretical point of view, let ui(k) be the integral term and e(k) the rate error at instant k, respectively, (represent by rate_i and rates_err in the code), let Ui(z) and E(z) be their z transforms, if implemented as you mentioned (the recursive definition that you called a “weird way”), we can apply the z transform to the formula

ui(k) = ui(k-1) + ki * dt * e(k)

to find out its transfer function

Ui(z)/E(z) = ki * dt * / (1 - z^(-1))

and that’s the same expression that is presented in the document that I mentioned.

[toopazo]

So, in line with what you just said that there is nothing weird with the Integral gain.
Do we all agree then that the implementation is a PID + FF scheme ?

[bresch]

@toopazo Yes, the rectangular integration is correctly implemented.
Some further facts that might be useful for you:

• The integral part has a saturation defined by a parameter which is 0.3 by default.
• The output of the controller is saturated between -1 and +1.
• The derivative is computed from the feedback (as said previously) using the finite difference method (FDM), filtered by a 2nd order low-pass filter.

P.S.: I’ll try to add a schematic for the multicopter attitude/rate controller in the dev guide as soon as possible.

[mjtaheri]

@bresch

schematic for the multicopter

Please let me know when you add the schematic

[Kyuhyong_You]

@mjtaheri, @toopazo
This is what I understood about the entire position/attitude control schematics of PX4 so far.
Please correct me if anything is wrong or missing.

[mjtaheri]

@Kyuhyong_You

s wrong

As @alexh mentioned PID is compromised of PI+D [quote=“alexh, post:6, topic:3114”]
for position and angle are implemented with a PI+D structure, which means that the derivative component is not the related to the angular rates errors e(t)
[/quote]

[tecdee]

I’ve searched for a schematic all around as well. Please add one, it would be of great help.

[honordee]

@bresch Can you share us the controller architecture ? For modelling our drone on matlab, I need to px4 attitude controller diagram.

[roomez]

Where is the source of this figure?

[Salma_Abd-Elaty]

Hi, I hope you made a progress in your project
I just want to make sure if you have validated your control architecture as we are trying to implement it in Simulink to tune the controller in a simulation… Are there other issues than the IP+D problems in the position and attitude loops?