uORB subscribe to the compass and publish to the LED
You've made it to the last example lab on this gitbook! In this lab, we will learn how to create our own app that harnesses both the publish and subscribe protocols in uORB.
In this lab, we won't be guiding you through creating this program, but we will give instructions on how to complete it. Don't worry though, if you get stuck there is a solution file that you can reference to learn how to complete this lab.
The objective of this lab is to harness the data published by the magnetometer (compass) on the FMU and control the RGB LED when you're facing north. You should also print a message to the Mavlink console that you're facing north along with the magnetometer data.
Here are some tips to help you out:
The two uORB topics that you'll want to use are
vehicle_local_posititon
led_control
The yaw
value in vehicle_local_position
is in radians. You can use this data in radians, or you may convert to degrees if you'd like.
The yaw
value in degrees for North is 350 < degrees < 10
. When the degrees hit 359, they wrap back to 0. (We will use about +/- 10 degree accuracy for "North")
Here's a small demo of what the output should look like. You should have some sort of logging in the Mavlink console as well as the LED flashing red when you're facing north.
If you need the solution to figure it out, or want to view some of the other tutorial files, go to this link: https://github.com/NXPHoverGames/Tutorials/blob/master/TutorialApplications/hg_magnet/hg_magnet.cpp
See if you can make the LED blink a different color for North, South, East, and West!
Thanks for following these tutorials. If you would like to see more, or have feedback or corrections please email us at HoverGames@nxp.com or on https://community.nxp.com/community/mobilerobotics
Examples to get you started with creating your own modules within PX4 Autopilot.
Some simple examples have been created by the HoverGames team. They are available for download on our NXP HoverGames GitHub in the Tutorials repository.
You can place these examples in the src/examples
folder of the PX4 firmware and edit the file boards/nxp/fmuk66-v3/default.cmake
to include these examples in the build process.
These HoverGames PX4 starter examples are complemented with step by step tutorials:
The PX4 firmware already includes a few example applications, some of which are similar to the examples created for HoverGames. They can be found under the src/examples
folder and should be included by default on most builds.
The PX4 Developer Guide also includes a "getting started" page for writing your first PX4 module. This is also based on some of these examples. It explains some of the basics of creating a new module, building your new code and interacting with it from the console. It also gives an introduction to using uORB, the middleware layer that allows you to communicate with other modules.
It is strongly recommended that you start with these examples and then slowly start exploring other parts of the software. The PX4 Developer Guide is a good place to get information and you can always ask your questions on the PX4 Discuss or Slack.
Example software for add-on components used in the HoverGames is available on their own pages. You can find them by scrolling down in the menu on the left. They are listed under "Add-On Hardware".
Compiling our new example code into PX4
Now that we have prepared two applications for use on our drone, we want to run them in order to confirm that they work.
We can test and debug code like this in a few different ways:
using SITL (software in the loop) in a simulation environment like Gazebo or AirSim. The code is running on a simulated flight controller. Fast to iterate and test unusual conditions in a safe way. Only the simulated hardware crashes, not a real drone! Sometimes not representative of real-world environment however.
using HITL (hardware in the loop) where the code is actually running on target hardware, but all the sensor and IO is attached to the simulation environment. It is more complicated to configure than SITL, but can expose things like processor or system hardware limitations or bottlenecks.
on the actual hardware, which is closest to reality, but slower and more dangerous to fully test.
When testing code for complex systems like drones, SITL is good practice because you can make sure it works properly and look for failure conditions in advance of loading it on a real physcial drone. This helps prevent accidents! However since we're just reading sensor measurements and publishing LED commands, we can't really harm our drone, so we're going to go the "real hardware" route. This will also give you experience in loading code to the FMU.
In order to build our code in PX4, we place the code in the correct location with the necessary supporting files. Lets take the hg_led
example from Lab 2. Save the source code you wrote to a file named hg_led.c
as seen here:
Now place this file into a folder also named hg_led
. Your folder name should usually match your main file's name. Next create a text file called CMakeLists.txt
that will tell cmake
how to build our application. Inside our CMakeLists.txt
file, add the following code:
Lets explain what's going on here:
px4_add_module()
: This tells cmake
that we are adding a new module (or application) to the firmware.
MODULE
: examples__hg_led
tells cmake
that this application is located in the Firmware/src/examples
folder. If you add an application to another source folder, you essentially write <folder_name>__<app_name>
.
MAIN
: This tells cmake
what the main function name is. This is your application name. In this case, it's hg_led
.
SRCS
: Add the names of your source files under this parameter.
DEPENDS
: If your application depends on another application, add them here. Since our application is an independent program there are no other file dependencies listed here.
Now that we have our CMakeLists.txt
file configured, we also have to tell PX4 to build it for our board. First, we need to move or copy our hg_led
folder containing the source files and the CMakeLists.txt
that we created into the Firmware/src/examples/
directory.
It should look like this:
Navigate to Firmware/boards/nxp/fmuk66-v3/default.cmake
and add hg_led
to the list under EXAMPLES
. All of these examples listed here will be built into your PX4 image and will be able to be called from the command line on the FMU when running!
Your EXAMPLES
parameter should now look something like this:
Now that we've added our new application to PX4, let's build our new firmware by running this command in our terminal: make nxp_fmuk66-v3
If you were targeting another board, for example the UCANS32K146
, you would just substitute that board name instead of nxp_fmuk66-v3
.
Hint: Since the command line supports "tab completion". if you type part of the name and press tab, the valid and correct board options will then show.
After entering the make command, the software will start building. Assuming no errors or failures, you should see an output similar to this once it's done. (If there is a failure, read the output carefully, often it is something simple that needs correction like a missing ";" at the end of a line of C code.):
Now, you can navigate to /Firmware/build/nxp_fmuk66-v3_default/
to find your *.px4 file which you can flash to your RDDRONE-FMUK66 using QGroundControl! (or a .bin file via the Segger JLINK SWD debugger tools.)
You're probably wondering "now how do I upload this new firmware to my board"? There are a couple of ways. We will use the easiest method, using the on board bootloader and QGroundControl. Note: This assumes your FMU already has a bootloader loaded from the section Programming FMUK66 for first use. Assuming that the bootloader is already in place, loading code is a simple process, so lets go over that next.
Start with the FMUK66 powered off, and disconnected from USB on your computer. Run QGroundControl and navigate to the Firmware tab as shown below:
Once you've navigated to this screen, follow the instructions and plug the FMU into your computer using the provided microUSB cable. You should see a prompt asking you which firmware you want to upload.
You'll want to check the "Advanced settings" box and select "Custom firmware file..." from the dropdown box as shown in the image below. Click OK at the top right to go to the next step.
Once you click OK, a dialog to select your file will appear. You'll need to navigate to your PX4 firmware directory and go to Firmware/build/nxp_fmuk66-v3_default/
. Inside that folder you'll find an nxp_fmuk66-v3_default.px4
file. Select this file to upload the firmware to your board.
Note that this directory and filename depends on the target board you are building for.
If the firmware upgrade successfully finished, you should see a message in the log that says "Upgrade complete." in yellow. If you didn't get that message, try again, check to see that the code did in fact complete compiling without errors.
Assuming you did get the "Upgrade complete." message, then you can continue in this lab guide!
Now that we have built our PX4 application into the firmware, and loaded it onto the FMU hardware, it's time to run it. After flashing your FMU it is recommended to hard reset or power-cycle the board. Wait for QGroundControl to reconnect with your board over USB, and then open up the Mavlink console as shown below.
The terminal shell or console gives you command line access to the software running on the FMU.
It should be noted that the console is actually available through several interfaces. A hardware "root console" is available when using the FMU debugger interface connected to a FTDI-USB-UART cable. This exposes a serial UART directly on the FMU itself. The root console is particularly helpful since it is initialized on power-up and can specifically show what is happening immediately on power-up, before something like the the USB interface is initialized and working.
Note: Since the FTDI-USB-UART cable is a hardware solution that converts the FMU debugger UART interface to USB it doesn't require any FMU software. This is different than the FMU USB port itself - which makes use of a PX4 software driver in order to function.
A console is also available through a direct USB connection or over a telemetry radio to a program such as QGroundControl. (and there are even other methods, such as SSH over an Ethernet connection). With any of these connections you can run PX4/NuttX commands in the Mavlink console. To see a list of commands you can run, type "?" into the console input field.
With any of these connections discussed above you can run PX4/NuttX commands in the console program. In QGroundControl this is known as the Mavlink Console. As with other operating systems such as Linux, commands are actually just programs that can be run on the command line or called from other programs or scripts. To see a list of PX4 commands you can run, type "?" into the console input field.
At the top left, you can see our new program, hg_led! Go ahead and run it by typing its name in the console and pressing <enter> to see your hg_led
program in action!
The main multicolor LED is the one we are controlling in the program, the other red and orange LEDs are other indicators. Fun! Next, we will go over a more advanced example and you can try coding it yourself!
uORB messaging protocol
In this section, we will go through some of the PX4 example code and explain how it works, and then write our own PX4 app to run on the RDDRONE-FMUK66. As a prerequisite, you should download the PX4 examples outlined in the previous section, and then come back to this section.
Lets review uORB, a messaging protocol that will be used in many of the examples.
uORB is a publish/subscribe messaging protocol that is deeply embedded in the PX4 system. Now, you may ask, what is a publish/subscribe messaging protocol? Lets find out!
In the image above, you can see that PX4 is a publisher of a multitude of different "topics". These topics are similar to a "struct" in C. They have a name (such as led_control) and can contain different variables that correspond to that topic.
For example the topic "sensor_gyro" contains the following information:
The sensor_gyro topic contains quite a few variables. Each topic is able to be sent as a message from a publisher to a subscriber. A timestamp is included each message, and that is defined in the .msg file.
The primary data of interest is for this topic is x, y, z, and temperature variables. The gyro on the RDDRONE-FMUK66 outputs angular velocities in the x, y, and z directions, and also reports the temperature.
Again in the image above, it shows that hg_gyro subscribes to the sensor_gyro topic to harness the data published to it.
Above we've shown the sensor_gyro uORB message, lets go through the HoverGames HG_gyro example code and find out how we can print gyro measurements to the MavLink console.
First, we have to include the header license comment since the software is permissively licensed:
Alright, with that out of the way, let's start!
We will review each section of code and then combine everything into one file at the end. The first section of code is where we include all of the necessary header (*.h) files we will need for this example.
Now that we have all of our headers included in our application, we can begin the real stuff. We define our main function and export it so that other applications can call upon it in PX4.
When creating our own PX4 user apps, it's necessary to name your main function <filename>_main
so that PX4 can tell what the new function is named and how to call it.
The int argc, char *argv[]
arguments in the main function allow the addition of command line parameters to your app when you call it from the PX4 MavLink console or in a script. While they aren't necessary for this example, it doesn't hurt to include them for future iterations of our app!
Next, we will go through the code to print out our gyro measurements. We need to set up the uORB messaging protocol to subscribe to the sensor_gyro topic:
After subscribing to the "sensor_gyro" topic and limit the update rate (interval) to 200ms, we set up a polling structure which allows the application to enter sleep mode until another set of data gets published to the topic. This makes our application efficient and prevents it from eating CPU cycles when waiting for new data!
Using a simple integer counter variable in a loop allows the program to end after a specified number of times. We will print out a text chart so that we can visualize the data a little bit easier.
That was easy enough, now lets create our for loop and actually start grabbing data from the "sensor_gyro" topic.
This section of code is polling the sensor_gyro uORB topic and then copying the data to a structure that allows us to print it out on the MavLink console. If the poll doesn't receive data within 1 second, it will timeout and we will send an error message.
There we have it! We have just written our first example program for the FMU RDDRONE-FMUK66 running PX4 flight stack on NuttX RTOS.
In the next lab we will look at publishing to a uORB topic to control the RGB LED on the FMU.
In the last lab, we learned what uORB and publish/subscribe protocols were. Then, we wrote a bit of code that allowed us to print out some gyro measurements to the MavLink console! The program we wrote was only half of the equation, though, as we only used the "subscribe" portion of uORB. In this lab, we will publish some data to a topic that will control the RGB LED on the RDDRONE-FMUK66. The topic we are publishing to is subscribed to by an internal PX4 program that handles LED control.
To start, let's get the license for this source code out of the way:
As in the last lab, we include our headers to the source code. The headers will be nearly the same, except we will change the uORB topic from sensor_gyro
to led_control
.
As done in the previous lab we create our main function and export it.
Here we export our hg_led_main
function. Remember, your main function should be called <filename>_main
. ThePX4_INFO()
function to show text indicating that the program has started.
A structure is created that we can save LED control data to. This structure will be used to publish the data to the led_control
uORB topic.
First step is to create the structure itself. Here, we created an led_control_s
structure and named it led_control
. Then, we call memset()
to fill the structure we created with zeros. We do this because the memory we allocate could have uninitialized random data in it, and in a safety critical platform such as a drone, we don't want to inadvertently send bad data if for some reason our data allocation code fails. The last line of code creates a uORB topic advertisement variable that allows us to publish to the led_control
topic.
Once the initial setup is done, we can now start filling our led_control
structure with data.
Colors, modes, and priorities can be set using the parameters as described in the link above. In this example, we are going to tell the LED to blink 10 times at max priority with the color green. Here's how to do that:
These parameters are easily set simply by referring to the parameter and giving it a value.
num_blinks
: We set the number of blinks to 10. You can make this whatever number you want, but don't make it too large or your LED will blink for a very long time!
priority
: Make our LED app max priority so that something else can't override it by setting LED_CONTROL_MAX_PRIORITY
. Note that there is are priority levels for a reason. Think carefully about your application as not everything should be max priority.
in practice, max priority is reserved for things like error LED sequences.
mode
: We set the mode to LED_CONTROL_MODE_BLINK_NORMAL
for a normal blink sequence.
led_mask
: We set this to 0xff
which tells the led controller to blink all LEDs. (FYI - This parameter is used in *some* custom LED drivers to allow a variety of LEDs to be controlled with uORB message. For example - LEDs located on each of the motor pods that change color in flight to indicate direction of forward travel. Technically it is not fully implemented in the standard PX4 LED driver.
color
: Set to LED_CONTROL_COLOR_GREEN
! If you'd like to set it to a different color, feel free!
In order to tell the LED controller to use the parameters we set, we have to send a uORB message with the structure we created. We will use the orb_publish()
function to do so:
And there we have it! Once this command is run, the uORB message with our data will be sent, and our LEDs will start blinking green. To close out the program, let's print something to the MavLink console that shows our program is done running.
We have now learned how to both subscribe and publish uORB messages in PX4. Next, we will create our own program that uses our new found knowledge to manipulate the LED on the RDDRONE-FMUK66.
The led_control uORB message definition can be found in the PX4 source code . This file outlines all of the different parameters than can be set for led_control.