!REDIRECT “https://docs.px4.io/master/ja/apps/hello_sky.html

First Application Tutorial (Hello Sky)

This topic explains how to create and run your first onboard application. It covers all the basic concepts and APIs required for app development on PX4.

Note For simplicity, more advanced features like start/stop functionality and command-line arguments are omitted. These are covered in Application/Module Template.

Prerequisites

You will require the following:

The source code PX4-Autopilot/src/examples/px4_simple_app directory contains a completed version of this tutorial that you can review if you get stuck.

  • Rename (or delete) the px4_simple_app directory.

Minimal Application

In this section we create a minimal application that just prints out Hello Sky!. This consists of a single C file and a cmake definition (which tells the toolchain how to build the application).

  1. Create a new directory PX4-Autopilot/src/examples/px4_simple_app.
  2. Create a new C file in that directory named px4_simple_app.c:
  • Copy in the default header to the top of the page. This should be present in all contributed files!

    ```c /** *

    • Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
    • Redistribution and use in source and binary forms, with or without
    • modification, are permitted provided that the following conditions
    • are met:
      1. Redistributions of source code must retain the above copyright
    • notice, this list of conditions and the following disclaimer.
      1. Redistributions in binary form must reproduce the above copyright
    • notice, this list of conditions and the following disclaimer in
    • the documentation and/or other materials provided with the
    • distribution.
      1. Neither the name PX4 nor the names of its contributors may be
    • used to endorse or promote products derived from this software
    • without specific prior written permission.
    • THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    • “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    • LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
    • FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
    • COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
    • INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
    • BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
    • OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
    • AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
    • LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
    • ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    • POSSIBILITY OF SUCH DAMAGE.
    • **/ ```
  • Copy the following code below the default header. This should be present in all contributed files!

    ```c /**

    • @file px4_simple_app.c
    • Minimal application example for PX4 autopilot
    • @author Example User mail@example.com */

      include

      __EXPORT int px4_simple_app_main(int argc, char *argv[]);

      int px4_simple_app_main(int argc, char *argv[]) { PX4_INFO(“Hello Sky!”); return OK; } ```

      Tip The main function must be named <module_name>_main and exported from the module as shown.

      Tip PX4_INFO is the equivalent of printf for the PX4 shell (included from px4_platform_common/log.h). There are different log levels: PX4_INFO, PX4_WARN, PX4_ERR, PX4_DEBUG. Warnings and errors are additionally added to the ULog and shown on Flight Review.

  1. Create and open a new cmake definition file named CMakeLists.txt. Copy in the text below:

    1. ############################################################################
    2. #
    3. # Copyright (c) 2015 PX4 Development Team. All rights reserved.
    4. #
    5. # Redistribution and use in source and binary forms, with or without
    6. # modification, are permitted provided that the following conditions
    7. # are met:
    8. #
    9. # 1. Redistributions of source code must retain the above copyright
    10. # notice, this list of conditions and the following disclaimer.
    11. # 2. Redistributions in binary form must reproduce the above copyright
    12. # notice, this list of conditions and the following disclaimer in
    13. # the documentation and/or other materials provided with the
    14. # distribution.
    15. # 3. Neither the name PX4 nor the names of its contributors may be
    16. # used to endorse or promote products derived from this software
    17. # without specific prior written permission.
    18. #
    19. # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    20. # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    21. # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
    22. # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
    23. # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
    24. # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
    25. # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
    26. # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
    27. # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
    28. # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
    29. # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    30. # POSSIBILITY OF SUCH DAMAGE.
    31. #
    32. ############################################################################
    33. px4_add_module(
    34. MODULE examples__px4_simple_app
    35. MAIN px4_simple_app
    36. STACK_MAIN 2000
    37. SRCS
    38. px4_simple_app.c
    39. DEPENDS
    40. )

    The px4_add_module() method builds a static library from a module description.

    • The MODULE block is the Firmware-unique name of the module (by convention the module name is prefixed by parent directories back to src).
    • The MAIN block lists the entry point of the module, which registers the command with NuttX so that it can be called from the PX4 shell or SITL console.

      Tip The px4_add_module() format is documented in PX4-Autopilot/cmake/px4_add_module.cmake.

      Note If you specify DYNAMIC as an option to px4_add_module, a shared library is created instead of a static library on POSIX platforms (these can be loaded without having to recompile PX4, and shared to others as binaries rather than source code). Your app will not become a builtin command, but ends up in a separate file called examples__px4_simple_app.px4mod. You can then run your command by loading the file at runtime using the dyn command: dyn ./examples__px4_simple_app.px4mod

Build the Application/Firmware

The application is now complete. In order to run it you first need to make sure that it is built as part of PX4. Applications are added to the build/firmware in the appropriate board-level cmake file for your target:

To enable the compilation of the application into the firmware create a new line for your application somewhere in the cmake file:

  1. examples/px4_simple_app

Note The line will already be present for most files, because the examples are included in firmware by default.

Build the example using the board-specific command:

  • jMAVSim Simulator: make px4_sitl_default jmavsim
  • Pixhawk v1/2: make px4_fmu-v2_default (or just make px4_fmu-v2)
  • Pixhawk v3: make px4_fmu-v4_default
  • Other boards: Building the Code

Test App (Hardware)

Upload the firmware to your board

Enable the uploader and then reset the board:

  • Pixhawk v1/2: make px4_fmu-v2_default upload
  • Pixhawk v3: make px4_fmu-v4_default upload

It should print before you reset the board a number of compile messages and at the end:

  1. Loaded firmware for X,X, waiting for the bootloader...

Once the board is reset, and uploads, it prints:

  1. Erase : [====================] 100.0%
  2. Program: [====================] 100.0%
  3. Verify : [====================] 100.0%
  4. Rebooting.
  5. [100%] Built target upload

Connect the Console

Now connect to the system console either via serial or USB. Hitting ENTER will bring up the shell prompt:

  1. nsh>

Type ‘’help’’ and hit ENTER

  1. nsh> help
  2. help usage: help [-v] [<cmd>]
  3. [ df kill mkfifo ps sleep
  4. ? echo losetup mkrd pwd test
  5. cat exec ls mh rm umount
  6. cd exit mb mount rmdir unset
  7. cp free mkdir mv set usleep
  8. dd help mkfatfs mw sh xd
  9. Builtin Apps:
  10. reboot
  11. perf
  12. top
  13. ..
  14. px4_simple_app
  15. ..
  16. sercon
  17. serdis

Note that px4_simple_app is now part of the available commands. Start it by typing px4_simple_app and ENTER:

  1. nsh> px4_simple_app
  2. Hello Sky!

The application is now correctly registered with the system and can be extended to actually perform useful tasks.

Test App (SITL)

If you’re using SITL the PX4 console is automatically started (see Building the Code > First Build (Using the jMAVSim Simulator)). As with the nsh console (see previous section) you can type help to see the list of built-in apps.

Enter px4_simple_app to run the minimal app.

  1. pxh> px4_simple_app
  2. INFO [px4_simple_app] Hello Sky!

The application can now be extended to actually perform useful tasks.

Subscribing to Sensor Data

To do something useful, the application needs to subscribe inputs and publish outputs (e.g. motor or servo commands).

Tip The benefits of the PX4 hardware abstraction comes into play here! There is no need to interact in any way with sensor drivers and no need to update your app if the board or sensors are updated.

Individual message channels between applications are called topics. For this tutorial, we are interested in the sensor_combined topic, which holds the synchronized sensor data of the complete system.

Subscribing to a topic is straightforward:

  1. #include <uORB/topics/sensor_combined.h>
  2. ..
  3. int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));

The sensor_sub_fd is a topic handle and can be used to very efficiently perform a blocking wait for new data. The current thread goes to sleep and is woken up automatically by the scheduler once new data is available, not consuming any CPU cycles while waiting. To do this, we use the poll() POSIX system call.

Adding poll() to the subscription looks like (pseudocode, look for the full implementation below):

  1. #include <poll.h>
  2. #include <uORB/topics/sensor_combined.h>
  3. ..
  4. int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
  5. /* one could wait for multiple topics with this technique, just using one here */
  6. px4_pollfd_struct_t fds[] = {
  7. { .fd = sensor_sub_fd, .events = POLLIN },
  8. };
  9. while (true) {
  10. /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
  11. int poll_ret = px4_poll(fds, 1, 1000);
  12. ..
  13. if (fds[0].revents & POLLIN) {
  14. /* obtained data for the first file descriptor */
  15. struct sensor_combined_s raw;
  16. /* copy sensors raw data into local buffer */
  17. orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
  18. PX4_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
  19. (double)raw.accelerometer_m_s2[0],
  20. (double)raw.accelerometer_m_s2[1],
  21. (double)raw.accelerometer_m_s2[2]);
  22. }
  23. }

Compile the app again by entering:

  1. make

Testing the uORB Subscription

The final step is to start your application as a background process/task by typing the following in the nsh shell:

  1. px4_simple_app &

Your app will display 5 sensor values in the console and then exit:

  1. [px4_simple_app] Accelerometer: 0.0483 0.0821 0.0332
  2. [px4_simple_app] Accelerometer: 0.0486 0.0820 0.0336
  3. [px4_simple_app] Accelerometer: 0.0487 0.0819 0.0327
  4. [px4_simple_app] Accelerometer: 0.0482 0.0818 0.0323
  5. [px4_simple_app] Accelerometer: 0.0482 0.0827 0.0331
  6. [px4_simple_app] Accelerometer: 0.0489 0.0804 0.0328

Tip The Module Template for Full Applications can be used to write background process that can be controlled from the command line.

Publishing Data

To use the calculated outputs, the next step is to publish the results. Below we show how to publish the attitude topic.

Note We’ve chosen attitude because we know that the mavlink app forwards it to the ground control station - providing an easy way to look at the results.

The interface is pretty simple: initialize the struct of the topic to be published and advertise the topic:

  1. #include <uORB/topics/vehicle_attitude.h>
  2. ..
  3. /* advertise attitude topic */
  4. struct vehicle_attitude_s att;
  5. memset(&att, 0, sizeof(att));
  6. orb_advert_t att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att);

In the main loop, publish the information whenever its ready:

  1. orb_publish(ORB_ID(vehicle_attitude), att_pub_fd, &att);

Full Example Code

The complete example code is now:

  1. /****************************************************************************
  2. *
  3. * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
  4. *
  5. * Redistribution and use in source and binary forms, with or without
  6. * modification, are permitted provided that the following conditions
  7. * are met:
  8. *
  9. * 1. Redistributions of source code must retain the above copyright
  10. * notice, this list of conditions and the following disclaimer.
  11. * 2. Redistributions in binary form must reproduce the above copyright
  12. * notice, this list of conditions and the following disclaimer in
  13. * the documentation and/or other materials provided with the
  14. * distribution.
  15. * 3. Neither the name PX4 nor the names of its contributors may be
  16. * used to endorse or promote products derived from this software
  17. * without specific prior written permission.
  18. *
  19. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  20. * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  21. * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  22. * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  23. * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  24. * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  25. * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
  26. * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
  27. * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  28. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  29. * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  30. * POSSIBILITY OF SUCH DAMAGE.
  31. *
  32. ****************************************************************************/
  33. /**
  34. * @file px4_simple_app.c
  35. * Minimal application example for PX4 autopilot
  36. *
  37. * @author Example User <mail@example.com>
  38. */
  39. #include <px4_platform_common/px4_config.h>
  40. #include <px4_platform_common/tasks.h>
  41. #include <px4_platform_common/posix.h>
  42. #include <unistd.h>
  43. #include <stdio.h>
  44. #include <poll.h>
  45. #include <string.h>
  46. #include <math.h>
  47. #include <uORB/uORB.h>
  48. #include <uORB/topics/sensor_combined.h>
  49. #include <uORB/topics/vehicle_attitude.h>
  50. __EXPORT int px4_simple_app_main(int argc, char *argv[]);
  51. int px4_simple_app_main(int argc, char *argv[])
  52. {
  53. PX4_INFO("Hello Sky!");
  54. /* subscribe to sensor_combined topic */
  55. int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
  56. /* limit the update rate to 5 Hz */
  57. orb_set_interval(sensor_sub_fd, 200);
  58. /* advertise attitude topic */
  59. struct vehicle_attitude_s att;
  60. memset(&att, 0, sizeof(att));
  61. orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
  62. /* one could wait for multiple topics with this technique, just using one here */
  63. px4_pollfd_struct_t fds[] = {
  64. { .fd = sensor_sub_fd, .events = POLLIN },
  65. /* there could be more file descriptors here, in the form like:
  66. * { .fd = other_sub_fd, .events = POLLIN },
  67. */
  68. };
  69. int error_counter = 0;
  70. for (int i = 0; i < 5; i++) {
  71. /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
  72. int poll_ret = px4_poll(fds, 1, 1000);
  73. /* handle the poll result */
  74. if (poll_ret == 0) {
  75. /* this means none of our providers is giving us data */
  76. PX4_ERR("Got no data within a second");
  77. } else if (poll_ret < 0) {
  78. /* this is seriously bad - should be an emergency */
  79. if (error_counter < 10 || error_counter % 50 == 0) {
  80. /* use a counter to prevent flooding (and slowing us down) */
  81. PX4_ERR("ERROR return value from poll(): %d", poll_ret);
  82. }
  83. error_counter++;
  84. } else {
  85. if (fds[0].revents & POLLIN) {
  86. /* obtained data for the first file descriptor */
  87. struct sensor_combined_s raw;
  88. /* copy sensors raw data into local buffer */
  89. orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
  90. PX4_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
  91. (double)raw.accelerometer_m_s2[0],
  92. (double)raw.accelerometer_m_s2[1],
  93. (double)raw.accelerometer_m_s2[2]);
  94. /* set att and publish this information for other apps
  95. the following does not have any meaning, it's just an example
  96. */
  97. att.q[0] = raw.accelerometer_m_s2[0];
  98. att.q[1] = raw.accelerometer_m_s2[1];
  99. att.q[2] = raw.accelerometer_m_s2[2];
  100. orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
  101. }
  102. /* there could be more file descriptors here, in the form like:
  103. * if (fds[1..n].revents & POLLIN) {}
  104. */
  105. }
  106. }
  107. PX4_INFO("exiting");
  108. return 0;
  109. }

Running the Complete Example

And finally run your app:

  1. px4_simple_app

If you start QGroundControl, you can check the sensor values in the real time plot (Analyze > MAVLink Inspector).

Wrap-Up

This tutorial covered everything needed to develop a basic PX4 autopilot application. Keep in mind that the full list of uORB messages/topics is available here and that the headers are well documented and serve as reference.

Further information and troubleshooting/common pitfalls can be found here: uORB.

The next page presents a template for writing a full application with start and stop functionality.