# Control loop architecture **URL:** \ **Category:** PX4 Autopilot\ **Created:** [April 23, 2017, 10:45pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114 "2017-04-23T22:45:53Z")\ **Posts on this page:** 19\ **Page:** 1
**Author:** ![toopazo](https://discuss.px4.io/user_avatar/discuss.px4.io/toopazo/32/3544_2.png) [@toopazo](https://discuss.px4.io/u/toopazo)\ **Post date:** [April 23, 2017, 10:45pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/1 "2017-04-23T22:45:53Z")
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](http://www.aero.psu.edu/avia/) ). Sometimes we use Pixhawk for the flight control. I have been searching in [Library has been merged into the PX4 User Guide](https://dev.px4.io/en/) 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. ![](https://discuss.px4.io/uploads/default/original/2X/f/fd15a899c7096f81680fd2c013a1aad5f2522750.JPG) 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 ---
**Author:** ![Antiheavy](https://discuss.px4.io/user_avatar/discuss.px4.io/antiheavy/32/6398_2.png) [@Antiheavy](https://discuss.px4.io/u/Antiheavy)\ **Post date:** [April 27, 2017, 12:47am UTC](https://discuss.px4.io/t/control-loop-architecture/3114/2 "2017-04-27T00:47:42Z")
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? ---
**Author:** ![wil](https://discuss.px4.io/letter_avatar_proxy/v4/letter/w/59ef9b/32.png) [@wil](https://discuss.px4.io/u/wil)\ **Post date:** [September 22, 2017, 8:47pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/3 "2017-09-22T20:47:15Z")
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. ---
**Author:** ![toopazo](https://discuss.px4.io/user_avatar/discuss.px4.io/toopazo/32/3544_2.png) [@toopazo](https://discuss.px4.io/u/toopazo)\ **Post date:** [October 2, 2017, 3:51pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/4 "2017-10-02T15:51:20Z")
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. ---
**Author:** ![neo](https://discuss.px4.io/letter_avatar_proxy/v4/letter/n/9fc348/32.png) [@neo](https://discuss.px4.io/u/neo)\ **Post date:** [February 10, 2018, 1:10pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/5 "2018-02-10T13:10:11Z")
@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! ---
**Author:** ![alexh](https://discuss.px4.io/user_avatar/discuss.px4.io/alexh/32/3840_2.png) [@alexh](https://discuss.px4.io/u/alexh)\ **Post date:** [February 14, 2018, 5:28pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/6 "2018-02-14T17:28:27Z")
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. ---
**Author:** ![neo](https://discuss.px4.io/letter_avatar_proxy/v4/letter/n/9fc348/32.png) [@neo](https://discuss.px4.io/u/neo)\ **Post date:** [February 18, 2018, 5:30pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/7 "2018-02-18T17:30:20Z")
@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! ---
**Author:** ![alexh](https://discuss.px4.io/user_avatar/discuss.px4.io/alexh/32/3840_2.png) [@alexh](https://discuss.px4.io/u/alexh)\ **Post date:** [February 19, 2018, 12:19pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/8 "2018-02-19T12:19:07Z")
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. ---
**Author:** ![toopazo](https://discuss.px4.io/user_avatar/discuss.px4.io/toopazo/32/3544_2.png) [@toopazo](https://discuss.px4.io/u/toopazo)\ **Post date:** [February 19, 2018, 5:00pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/9 "2018-02-19T17:00:41Z")
Hi @alexh and @neo I was checking again the master branch of file “mc\_att\_control\_main.cpp” And in line 1035 I found ```cpp _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 ```cpp // 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 ---
**Author:** ![alexh](https://discuss.px4.io/user_avatar/discuss.px4.io/alexh/32/3840_2.png) [@alexh](https://discuss.px4.io/u/alexh)\ **Post date:** [February 19, 2018, 6:54pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/10 "2018-02-19T18:54:49Z")
@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](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. ---
**Author:** ![toopazo](https://discuss.px4.io/user_avatar/discuss.px4.io/toopazo/32/3544_2.png) [@toopazo](https://discuss.px4.io/u/toopazo)\ **Post date:** [February 22, 2018, 6:00pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/11 "2018-02-22T18:00:15Z")
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 ? ---
**Author:** ![bresch](https://discuss.px4.io/user_avatar/discuss.px4.io/bresch/32/2343_2.png) [@bresch](https://discuss.px4.io/u/bresch)\ **Post date:** [February 25, 2018, 10:01pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/13 "2018-02-25T22:01:55Z")
@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. ---
**Author:** ![mjtaheri](https://discuss.px4.io/letter_avatar_proxy/v4/letter/m/a698b9/32.png) [@mjtaheri](https://discuss.px4.io/u/mjtaheri)\ **Post date:** [January 23, 2019, 9:41pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/15 "2019-01-23T21:41:35Z")
> [@bresch](#): > > schematic for the multicopter Please let me know when you add the schematic ---
**Author:** ![Kyuhyong\_You](https://discuss.px4.io/user_avatar/discuss.px4.io/kyuhyong_you/32/2210_2.png) [@Kyuhyong\_You](https://discuss.px4.io/u/Kyuhyong_You)\ **Post date:** [January 24, 2019, 2:33am UTC](https://discuss.px4.io/t/control-loop-architecture/3114/16 "2019-01-24T02:33:26Z")
@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. ![](https://discuss.px4.io/uploads/default/original/2X/9/9965a57011cff2344c4155f602191c0f7eac3cfd.png) ---
**Author:** ![mjtaheri](https://discuss.px4.io/letter_avatar_proxy/v4/letter/m/a698b9/32.png) [@mjtaheri](https://discuss.px4.io/u/mjtaheri)\ **Post date:** [January 24, 2019, 4:14pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/17 "2019-01-24T16:14:00Z")
> [@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] ---
**Author:** ![tecdee](https://discuss.px4.io/user_avatar/discuss.px4.io/tecdee/32/4474_2.png) [@tecdee](https://discuss.px4.io/u/tecdee)\ **Post date:** [August 14, 2019, 7:46pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/19 "2019-08-14T19:46:44Z")
I’ve searched for a schematic all around as well. Please add one, it would be of great help. ---
**Author:** ![honordee](https://discuss.px4.io/letter_avatar_proxy/v4/letter/h/76d3ee/32.png) [@honordee](https://discuss.px4.io/u/honordee)\ **Post date:** [December 27, 2019, 10:35am UTC](https://discuss.px4.io/t/control-loop-architecture/3114/20 "2019-12-27T10:35:42Z")
@bresch Can you share us the controller architecture ? For modelling our drone on matlab, I need to px4 attitude controller diagram. ---
**Author:** ![roomez](https://discuss.px4.io/user_avatar/discuss.px4.io/roomez/32/5483_2.png) [@roomez](https://discuss.px4.io/u/roomez)\ **Post date:** [January 27, 2020, 5:41pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/21 "2020-01-27T17:41:25Z")
Where is the source of this figure? ---
**Author:** ![Salma\_Abd-Elaty](https://discuss.px4.io/user_avatar/discuss.px4.io/salma_abd-elaty/32/5744_2.png) [@Salma\_Abd-Elaty](https://discuss.px4.io/u/Salma_Abd-Elaty)\ **Post date:** [March 5, 2020, 5:24pm UTC](https://discuss.px4.io/t/control-loop-architecture/3114/22 "2020-03-05T17:24:48Z")
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?