The application responsible for multicopter position control exists in “Firmware/src/modules/mc_pos_control”. I will describe what I understand about this application as much as I can in this blog.

Start

The application is started by the script “rc.mc_apps” in “Firmware/ROMFS/px4fmu_common/init.d”. The script itself is called by the main starting script “rcS” at line 761. Since “rc.mc_apps” is a very short script, I will paste it here for convenience:

rc.mc_apps

rc.mc_apps

As can be seen, the “mc_pos_control” application is started by the script at line 34. Interestingly, there are three attitude and position estimator group, the first one is INAV (inertial navigation); second one is LPE (local position estimator); thrid one is EKF (extended Kalman filter). The script sets “SYS_MC_EST_GROUP” 0 by default, (in “Firmware/src/modules/systemlib/system_params.c” at line 107), hence the INAV estimator group will be used.

Call Hierarchy

Here is an overview of how the function “mc_pos_control_main.cpp” flows, all functions have “MulticopterPositionControl::” prefix:

  • line 2095: function “start()” is called when the application is started
  • line 2104: function “task_main_trampoline()” is called by “start()”
  • line 714: seems like “task_main_trampoline()” is only used to access the function “:task_main()”
  • line 1184: function “task_main()” begins, subscribes a bunch of topics, enter a while loop on line 1233
    • line 1344: function “control_manual()” is called by function “task_main()” if in manual modes
      • line 797: function “control_manual()” starts
    • line 1349: function “control_offboard()” is called by function “task_main()” if in offboard modes
      • line 896: function “control_offboard()” starts
    • line 1354: function “control_auto()” is called by function”task_main()” if in auto modes
      • line 975″ function “control_auto()” starts
  • line 1577-1582: function “task_main()” publishes and advertises “vehicle_global_velocity_setpoint”*
  • line 1934-1940: function “task_main()” publishes and advertises “vehicle_local_position_setpoint”
  • line 2076-2081: function “task_main()” publishes and advertises “_attitude_setpoint_id”*

*No mathematical manipulations on these topics.

To sum it up, this application starts->calculate setpoints based on mode->publish and advertises. Just like the example application, but much much more complex.
mc_pos_control algorithm flow chart

mc_pos_control algorithm flow chart

Other Functions
A quick look at the function declaration reveals some other useful smaller functions in this application:
  • poll_subscriptions(): called every loop to update topics
  • update_ref(): called every loop to update position reference
  • reset_pos_sp(): reset position setpoint to current position, called when position controller is disabled, or when landed, or on arming etc.
  • reset_alt_sp(): reset altitude setpoint to current altitude, called in similar fashion as “reset_pos_sp”
  • limit_pos_sp_offset(): if position setpoint is too far from current position, this function will scale the setpoint so that it is closer

Interestingly, “control_update” and “select_alt” are declared but never used in this application, need further investigation.

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.

  • 1187-1201: subscribe to topics
  • 1204: update parameters
  • 1207: make sure you are telling PX4 vehicle is not armed
  • 1210: orb_copy vehicle status if orb subscriptions are updated
  • 1212-1216: initialize some boolean variables
  • 1218: hrt_abstime is a high-resolution timer object (src/drivers/drv_hrt.h), set t_prev =0, used on 1253
  • 1220-1221: ??
  • 1224-1225: ??
  • 1228-1231: set up poll topic, fd stands for file descriptor
  • 1233: enter while loop, while _task_should_exit is false, this will be set false when the destructor is called
    • 1234-1250: getting data and parameters
    • 1252: hrt_abstime again, this time it is setting t = hrt_absolute time(), an absolute time after system startup
    • 1253: calculate dt, is equal to a millionth of a loop run time, or 0 for the first loop, declared on 1218
    • 1259-1267: reset all setpoints as the vehicle gets armed
    • 1269-1275: VTOL stuff
    • 1278: change was_armed to the arming state now, will be used on 1259 in the next loop
    • 1285-1316:
      • 1287-1298: put current local xyz position into _pos
      • 1300-1311: put current local xyz velocity into _vel
      • 1313-1315: calculate velocity derivative error using object _vel_x_deriv.update(negative _vel), put into _vel_err_d
    • 1320-1326: if flight mode does not require position/altitude hold, set the corresponding flags to false
    • 1328-1942: if you need any position control, run these
      • 1342-1355: control source switch, can be manual, offboard or auto
      • 1357-1364: vtol stuff
      • 1366-1940: ??
    • 1942-1953: if you don’t need position or altitude control, skip 1328-1942, and reset all setpoints, and store last velocity
    • 1955-2056: this part of the code generate attitude setpoint from manual controls
      • 1958-1962: reset yaw setpoints to current position if needed
      • 1964-1984: if on the ground, yaw is untouched
      • 1986-1990: control roll and pitch directly if no aiding velocity controller is active
      • 1992-2001: control throttle directly if no climb rate controller is active
      • 2005-2049: ??
      • 2051-2055: copy quaternion setpoint to attitude setpoint topic
    • 2057-2060: if manual control and attitude control are not enabled, skip 1955-2056, and reset yaw setpoints
    • 2065-2082: publish attitude setpoints
  • 2092: while loop ends