“mc_att_control” is responsible for PX4 multicopter attitude control, this application can be found in “Firmware/src/modules/mc_att_control”. Here is what I understand about this application.
Start
Similar to “mc_pos_control”, this application is started by script “rc.mc_apps” on line 32:
rc.mc_apps
Call Hierarchy
The source code for the application exists in “mc_att_control_main.cpp”, it follows a very similar architecture as “mc_pos_control_main.cpp”. You will find the call hierarchy fmailiar:
- line 992: function “start()” is called by the script
- line 1001: function “task_main_trampoline” is called by “start()”
- line 791: seems like “task_main_trampoline” is only used to access “task_main”
- line 797: function “task_main()” begins, subscribes to a bunch of topics
- line 822: enter while loop, while application is not stopped
- line 857: copy attitude and control state topics
- line 870-944: control attitude (outer loop, control angular error)
- line 882: function “control_attitude(dt)” is called
- line 905-910: “_rates_sp_id” topic is advertised and published
- line 946-981: control attitude rates (inter loop, control angular rate error)
- line 947: function “control_attitude_rates(dt)” is called
- line 962-069: “_actuators_id” topic is advertised and published (which contains _att_control vector)
In summary, the application starts->control attitude->publish and advertise->control attitude rates->publish and advertise.
mc_att_control algorithm flow chart
Other Functions
“mc_att_control” has some small useful functions that are different to the ones found in “mc_pos_control”:
- Poll functions
- vehicle_control_mode_poll(): check for changes in vehicle control mode, looks like vehicle mode is stored in the topic “vehicle_control_mode”
- vehicle_manual_poll(): check for changes in manual inputs, need to investigate what this means
- vehicle_attitude_setpoint_poll(): check for attitude setpoint updates
- vehicle_rates_setpoint_poll(): check for rates setpoint updates
- arming_status_poll(): check for arming status updates
- vehicle_status_poll(): check for vehicle status updates, need to investigate what is vehicle status
- vehicle_motor_limits_poll(): check for vehicle motor limits status, need to investigate what are motor limits
Flip Mode?
I think for the sake of understanding source code, I will try to implement a new mode similar to the APM flip mode. Based on my understanding so far, the following tasks have to be done in order to create a flip mode:
- create a “_v_control_mode.flag_control_flip_enabled” flag
- in task_main, create an if statement that skips control_attitude and execute my custom code
Outcome of this experiment will be recorded in later blogs.
Appendix
What is going on in task_main?
If you see ?? it means I don’t know what this line is doing, please fill in.
- 803-811: subscribe to topics
- 813-814: update parameters
- 816-820: set up poll topic, fd stands for file descriptor
- 822: enter while loop, loop continues while attitude control is required
- 824-840: wait for data
- 842-982: run controller if there is data coming in
- 857: copy attitude and control state data
- 867-875: in rattitude, if pitch or roll is above threshold, no need to control
- 877-944: run control_attitude if attitude control is enabled, otherwise just poll rates setpoint topic
- 946-981: run control_attitude_rates if rate control is enabled, otherwise do nothing
Hello,
First of all I would like to thank you for the effort you put in to breakdown the github codes.
I have found your posts very helpful. I have been going through the mc_att_control.cpp file.
There is this section where the param_find function was used. For eg.
_params_handles.roll_p = param_find(“MC_ROLL_P”);
_params_handles.roll_rate_p = param_find(“MC_ROLLRATE_P”);
_params_handles.roll_rate_i = param_find(“MC_ROLLRATE_I”);
_params_handles.roll_rate_integ_lim = param_find(“MC_RR_INT_LIM”);
_params_handles.roll_rate_d = param_find(“MC_ROLLRATE_D”);
_params_handles.roll_rate_ff = param_find(“MC_ROLLRATE_FF”);
_params_handles.pitch_p = param_find(“MC_PITCH_P”);
_params_handles.pitch_rate_p = param_find(“MC_PITCHRATE_P”);
_params_handles.pitch_rate_i = param_find(“MC_PITCHRATE_I”);
I want to know if you have any idea where the “MC_PITCH_P” and the rest were introduced into the cpp file. I did notice that “MC_PITCH_P” and the rest appeared mc_att_control_params.c but I did not see where this was declared in the cpp file. I appreciate any insight you might have.
Thank you
Hi Chinedu,
Thank you for showing interest in my blog.
The section you described basically updates the parameters. The “MC_PITCH_P” is just a string that is used by the “param_find()” function to search for the corresponding parameter. The “param_find()” function itself is called by “param.c” in “src/modules/systemlib/param”. Hope this helps.
Hello Zihao,
It has given me some clarity. I am currently working on my Master’s which will be designing a controller for a quadcopter and I will be using the pixhawk; hence my interest in the code. I would like to pick your brain from time to time if you aren’t too busy.
Regards,
Chinedu
Hi Chinedu
That sounds great! I am still learning the code myself, maybe I can learn something from you too.
Regards
Zihao
Hello Zihao,
I am looking forward to that. Thank you!
Regards,
Chinedu
I am sure this post has touched all the internet visitors, its really really good post on building up new blog.
Hey Zihao,
I have been able to understand the code more but I still have some hiccups.
Do you mind sending me your email address? I think that will make communication easier.
Thank you.
Happy New Year in advance!
Regards,
Chinedu
Hi Chinedu,
Yeah I will send an email directly to your gmail account.
Happy new year to you as well!
Hey Zihao,
This is going to be a fairly long post but I believe it will be beneficial to those of us that frequent this blog
and those that are yet to come.
I’ve been looking at the code for weeks now and I’ve been able to gain more understanding, however there are
some parts I am hoping you can assist me on.
1. __EXPORT
I have seen this in this instance: extern “C” _EXPORT int mc_att_control_main(int argc, char *agrv[])
Do you have an idea what ‘__EXPORT’ does? And also “C” in the instance I cited. I guess it has to do
with the language C but I don’t know exactly what it means.
2. Mathematical Model
Coming from a control system background, I have been looking through the codes to find a mathematical model
of the quadcopter in order for me to gain better understanding of the relationship between the inputs and outputs
in association with the code. I am asking for some direction to a header or cpp file that may contain such or if you’ve
got an idea of how the math model is represented in the code.
3. Difference between attitude control and control attitude rates controller
I am still trying to figure out the difference between these two and when they are to be used.
4. ASSERT(_control_task == -1)
Is this a way of confirming that the variable _control_task is initialized to the default value of -1?
5. Do you know what the function of _actuator_0_circut_breaker_enabled is?
6. Lastly, the function parameters_update.
I understand how the default values of the different gains are obtained but I do not see how these values
vary in subsequent iterations. I do see that there are subscriptions and copies to topics in later parts of the code
but I can’t see how they fit into the function. Surely this is needed in order to update the parameters. Have you been
able to figure this out?
I know this is much and you are also trying to make sense of the codes, but any assistance will be very helpful. Thank you so much.
Regards,
Chinedu
Hi Chinedu,
All of the answers I provided are based on my understanding:
1. The code is written in c++ as evident from the object oriented codes, but the makefile is written in such a way that it only supports C for some reason (I am not sure about this, but according to the error message when you remove this line, it sounds like the compiler doesn’t recognise c++). The line containing “_EXPORT” makes the compiler understands c++ in a sense.
2. For each airframe there is a motor mixer profile, go to Firmware/src/modules/systemlib/mixer, you can see how PX4 controls each individual motor.
3. This has more to do with control theory, if you set up the feedback loop such that you have an inner loop to control the velocity (which is commanding acceleration), and an outer loop to control the displacement (which is commanding velocity), you will get a better response.
4. Yes, see this: http://www.cplusplus.com/reference/cassert/assert/, basically if _control_task==-1 is false, an error message will be printed.
5. I haven’t used this before, but it looks like if this is set to true, all actuators output will be suppressed (i.e. no actuation)
6. in the task_main while loop, the parameter_update_poll() function will call the parameters_update() function if the parameters are changed, thus updating the gains in the iterations.
Not all of my answers are correct, since they are mostly based on my understanding and assumptions, if you find out new things please update here. Many thanks.
I understand. I will go through what you’ve written and I will update everyone here on any findings I come across.
Thank you.
Does anyone having flow chart of px4 fixed wing position control ?
Hi Guys ,
Any chance one of you have the PID structure of PX4 (ATT.C) i want to develop Simulink simulation and PID tuning for pixhawk .
Thanks
I don’t have the PID structure, but if you understand the mc_att_control completely you should get a good idea of how they implement the PID loop.
Hello !
Thank you for sharing this with everyone. It is very helpful 🙂 I currently don’t understand what is “emult” in the code ?
Thank you for the great article