Summary(TL;DR)

I go through how ‘actuator_control’ topic gets published(incl. Gimbal case, distinguished by ‘group_mlx’ parameter) by Mavlink Reciever in the core. Show how the PX4 codebase uses ‘submodule’ architecture to use external Mavlink C++ library.

1. Where this file is At:

This file is located at: “\Firmware\src\modules\mavlink\mavlink_receiver.cpp”

So it is basically a part of the ‘module’ Mavlink, in the PX4 firmware.

2. The Code that Processes set_actuator_control:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 void MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t * msg) { mavlink_set_actuator_control_target_t set_actuator_control_target; mavlink_msg_set_actuator_control_target_decode (msg, & set_actuator_control_target); bool values_finite = PX4_ISFINITE(set_actuator_control_target.controls[ 0 ]) & & PX4_ISFINITE(set_actuator_control_target.controls[ 1 ]) & & PX4_ISFINITE(set_actuator_control_target.controls[ 2 ]) & & PX4_ISFINITE(set_actuator_control_target.controls[ 3 ]) & & PX4_ISFINITE(set_actuator_control_target.controls[ 4 ]) & & PX4_ISFINITE(set_actuator_control_target.controls[ 5 ]) & & PX4_ISFINITE(set_actuator_control_target.controls[ 6 ]) & & PX4_ISFINITE(set_actuator_control_target.controls[ 7 ]); if ((mavlink_system.sysid = = set_actuator_control_target.target_system | | set_actuator_control_target.target_system = = 0 ) & & (mavlink_system.compid = = set_actuator_control_target.target_component | | set_actuator_control_target.target_component = = 0 ) & & values_finite) { #if defined (ENABLE_LOCKSTEP_SCHEDULER) PX4_ERR( “SET_ACTUATOR_CONTROL_TARGET not supported with lockstep enabled” ); PX4_ERR( “Please disable lockstep for actuator offboard control:” ); https://dev.px4.io/master/en/simulation/#disable-lockstep-simulation” PX4_ERR(); return ; #endif /* Ignore all setpoints except when controlling the gimbal(group_mlx==2) as we are setting raw actuators here */ bool ignore_setpoints = bool (set_actuator_control_target.group_mlx ! = 2 ); offboard_control_mode_s offboard_control_mode{}; offboard_control_mode.ignore_thrust = ignore_setpoints; offboard_control_mode.ignore_attitude = ignore_setpoints; offboard_control_mode.ignore_bodyrate_x = ignore_setpoints; offboard_control_mode.ignore_bodyrate_y = ignore_setpoints; offboard_control_mode.ignore_bodyrate_z = ignore_setpoints; offboard_control_mode.ignore_position = ignore_setpoints; offboard_control_mode.ignore_velocity = ignore_setpoints; offboard_control_mode.ignore_acceleration_force = ignore_setpoints; offboard_control_mode.timestamp = hrt_absolute_time(); _offboard_control_mode_pub.publish(offboard_control_mode); /* If we are in offboard control mode, publish the actuator controls */ vehicle_control_mode_s control_mode{}; _control_mode_sub.copy( & control_mode); if (control_mode. flag_control_offboard_enabled ) { actuator_controls_s actuator_controls{}; actuator_controls.timestamp = hrt_absolute_time(); /* Set duty cycles for the servos in the actuator_controls message */ for (size_t i = 0 ; i < 8 ; i + + ) { actuator_controls.control[i] = set_actuator_control_target.controls[i]; } switch (set_actuator_control_target.group_mlx) { case 0 : _actuator_controls_pubs[ 0 ].publish(actuator_controls); break ; case 1 : _actuator_controls_pubs[ 1 ].publish(actuator_controls); break ; case 2 : _actuator_controls_pubs[ 2 ].publish(actuator_controls); break ; case 3 : _actuator_controls_pubs[ 3 ].publish(actuator_controls); break ; default : break ; } } } } Colored by Color Scripter cs

(Used : “https://colorscripter.com/” for this beautiful Syntax Highlighting)

So I’ve highlighted & bold-ed some parts of the code, and let’s see what’s up with them.

3. Overall function flow

First, the mavlink message is decoded(line #5) into actual ‘actuator_control_target‘ structure. Publishes(line #38) ‘offboard_control_mode‘ topic, giving ‘ignore_thrust …‘ variables(line #28~36) true except for the case of controlling gimbal(I don’t get it, lol). If the vehicle control_mode is ‘ Offboard ‘, publishes ‘actuator_controls‘ topic(line #51, etc.), with the ‘ decoded ‘ control_target values in it.

That is pretty much straight forward, right?

The code is just acting as a ‘Receiver’, so it doesn’t do much fancy calculations, etc. It just directly publishes the data it needs to.

4. Actuator Controls topic in uORB

So let’s see how ‘_actuator_controls_pubs’ Array is defined in header file.

uORB::Publication<actuator_controls_s> _actuator_controls_pubs[4] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2), ORB_ID(actuator_controls_3)}; (\Firmware\src\modules\mavlink\mavlink_receiver.h)

It can be found that the instance used for ‘publication’ accepts data in ‘actuator_controls_s’ structure. And the uORB topic is named: ‘actuator_controls_#’.(#=0~3). 0 being Primary control.

As an example of how the published data gets used(subscription):

// Grab new controls data

orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls); // Mix to the outputs

_outputs.timestamp = hrt_absolute_time();

int16_t motor_rpms[UART_ESC_MAX_MOTORS]; (\Firmware\src\drivers\qurt\fc_addon\uart_esc\uart_esc_main.cpp)

And as you can see above, it used across driver / navigation / control part of the codebase. For example, you can see how the uart_esc grabs ‘actuator_controls_0′ topic data. Although it doesn’t seem to use that for any good. Ha!

Actually, here’s a better example(FW Attitude Control):

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 // Add feed-forward from roll control output to yaw control output // This can be used to counteract the adverse yaw effect when rolling the plane _actuators.control[actuator_controls_s::INDEX_YAW] + = _param_fw_rll_to_yaw_ff.get() * constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], – 1. 0f, 1. 0f); _actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied; _actuators.control[ 5 ] = _manual.aux1; _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied; // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future _actuators.control[ 7 ] = _manual.aux3; /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _att.timestamp; _actuators_airframe.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp_sample = _att.timestamp; /* Only publish if any of the proper modes are enabled */ if (_vcontrol_mode.flag_control_rates_enabled | | _vcontrol_mode.flag_control_attitude_enabled | | _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub ! = nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, & _actuators); } _actuators_2_pub.publish(_actuators_airframe); } Colored by Color Scripter cs (\Firmware\src\modules\fw_att_control\FixedwingAttitudeControl.cpp)

That is a ‘FixedwingAttitudeControl’ module code.

And that part of the code is a part of “Run” function. That is, basically, the high-level execution code for fixed wing attitude control.

You can see above, how this module ‘publishes’ the actuators_controls_0 topic. Just like how the MavLink module did before!

5. MAVLink function: “mavlink_msg_set_actuator_control_target_decode”

You might’ve seen above, in the ‘handle_message_set_actuator_control_target‘ function, that this function(mavlink_msg_set_actuator_control_target_decode) with a strangely- well structured name gets called. I wondered, how that function is defined, but ha! it was nowhere to be found.

When you don’t see the direct definition of the function in the codebase, you can expect these two cases: One. That is a function from external library, that gets pulled from upstream during the compilation process. Two. It is an arbitrary compiler generated function, that also gets built during the compilation.

I highly suspected the second case, since I thought that ‘decoding’ mavlink messages would be a trivial thing for a compiler to figure out(especially since the data type is well defined in .msg files).

Turns out, this is done via the fist case! I could know that since the ‘CMakeLists’ had the reference to the external library.

px4_add_git_submodule(TARGET git_mavlink_v2 PATH “${PX4_SOURCE_DIR}/mavlink/include/mavlink/v2.0“) px4_add_module(

MODULE modules__mavlink

MAIN mavlink

COMPILE_FLAGS

-Wno-cast-align # TODO: fix and enable

-Wno-address-of-packed-member # TODO: fix in c_library_v2

INCLUDES

${PX4_SOURCE_DIR}/mavlink/include/mavlink SRCS

mavlink.c

mavlink_command_sender.cpp … (\Firmware\src\modules\mavlink\CMakeLists.txt)

And in that submodule directory:

“${PX4_SOURCE_DIR}/mavlink/include/mavlink/v2.0“,

I could find the definition of the function.

5-1. Set Actuator Control Message decoding related

The ‘v2.0’ folder leads to the another repository. You can check it for yourself here: LINK.

The code maintainers regularly update the linked MAVLink module to latest. For example, as of right now(November 20th, 2019), it links to: LINK.

You can see that in this commit that updated the module: LINK, the module was updated just 9 hours ago, so it’s really FRESH(haha).

And that function, I could finally find(LINK) in the common/ folder of MAVLink_v2 library. Here’s some snippet of the code:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 #define MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET 139 MAVPACKED( typedef struct __mavlink_set_actuator_control_target_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number.*/ float controls[ 8 ]; /*< Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs.*/ uint8_t group_mlx; /*< Actuator group. The “_mlx” indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances.*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ }) mavlink_set_actuator_control_target_t; #define MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN 43 #define MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_MIN_LEN 43 #define MAVLINK_MSG_ID_139_LEN 43 #define MAVLINK_MSG_ID_139_MIN_LEN 43 #define MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_CRC 168 #define MAVLINK_MSG_ID_139_CRC 168 #define MAVLINK_MSG_SET_ACTUATOR_CONTROL_TARGET_FIELD_CONTROLS_LEN 8 … /** * @brief Decode a set_actuator_control_target message into a struct * * @param msg The message to decode * @param set_actuator_control_target C-struct to decode the message contents into */ static inline void mavlink_msg_set_actuator_control_target_decode( const mavlink_message_t * msg, mavlink_set_actuator_control_target_t * set_actuator_control_target) { #if MAVLINK_NEED_BYTE_SWAP | | ! MAVLINK_ALIGNED_FIELDS set_actuator_control_target – > time_usec = mavlink_msg_set_actuator_control_target_get_time_usec(msg); mavlink_msg_set_actuator_control_target_get_controls(msg, set_actuator_control_target – > controls); set_actuator_control_target – > group_mlx = mavlink_msg_set_actuator_control_target_get_group_mlx(msg); set_actuator_control_target – > target_system = mavlink_msg_set_actuator_control_target_get_target_system(msg); set_actuator_control_target – > target_component = mavlink_msg_set_actuator_control_target_get_target_component(msg); #else uint8_t len = msg – > len < MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN? msg – > len : MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN; memset(set_actuator_control_target, 0 , MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); memcpy(set_actuator_control_target, _MAV_PAYLOAD(msg), len); #endif } Colored by Color Scripter cs

Above, you can see the definition of struct “mavlink_set_actuator_control_target_t“, which was used in: MavlinkReceiver::handle_message_set_actuator_control_target( ) function in the core Source-code above.

6. Yeap, decode with submodule & process with core code.

So yeah, the fact that PX4 uses submodules like Mavlink-v2(especially these decoding parts) now makes the whole code implementation make more sense.

This whole framework of dividing the modules makes a lot of sense too, since dividing and conquering is definitely much easier, in terms of updating the code, as well as bug-fixing, etc.

Note, MAVLink itself can be used in any languages, and ‘library’ needed can also be generated in various languages. This example of how the MAVLink module is used for getting ‘set_actuator_control’ message used C / C++ of course, because the PX4 core software is written in that. But I’m just saying that this is just a protocol, hence has a lot of other variations.

You can learn more about the Mavlink protocol at it’s official website: LINK.

Summary

I went through how ‘actuator_control’ topic gets published(incl. Gimbal case, distinguished by ‘group_mlx’ parameter) by Mavlink Reciever in the core. How the PX4 codebase uses ‘submodule’ architecture to use external Mavlink C++ library.

That’s it for this post! Thanks for reading! Below are some of my thoughts.

In the future I want to dive deeper on:

How MAVLink code / library gets ‘generated’, from XML (.msg) files. And how the .msg files in PX4/Firmware Codebase are actually used. How ‘set_actuator_control’ is actually used on the subscription’s side. As well as why the ‘gimbal’ case doesn’t ignore setpoints(Mmmm!).

(2019. 11. 21. 00:35 AM.)