第一个机载应用程序教程 (Hello Sky)

本教程详细解释了如何创建一个新的机载应用程序以及如何运行它。

先决条件

第一步: 获取文件

为了方便管理自己的自定义代碼,并在从主存储库拉取更新,建议从使用着Git版本控制系统的固件库中分支出来:

如果你现在不想注册GitHub,你可以通过在PX4控制台键入以下命令克隆库,然后从第5步继续。

git clone https://github.com/PX4/Firmware

  1. 注册Github帐号
  2. 前往固件仓库网站然后单击页面右上部分的FORK
  3. If you are not already there, open the website of your fork and copy the private repository URL in the center.
  4. Clone the repository to your hard drive, e.g. on the command line via git clone https://github.com/<youraccountname>/Firmware.git. Windows users please refer to the Github help and e.g. fork / clone with their Github for Windows app.
  5. Update the git submodules: Run in your shell (on Windows in the PX4 console).
cd Firmware
git submodule init
git submodule update --recursive

Enter the Firmware/src/modules directory on your local hard drive and create a new directory named px4_simple_app. In this directory, create a new file named module.mk and add this content:

MODULE_COMMAND	= px4_simple_app
SRCS			= px4_simple_app.c

第二步: 最小的应用程序

Create a new C file named px4_simple_app.c in the px4_simple_app folder.

Edit it and start with the default header and a main function. Note that the other project users appreciate if you stick to the code_style.

/****************************************************************************
 *
 *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
 *   Author: @author Example User <mail@example.com>
 *
 * 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.
 * 2. 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.
 * 3. 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.
 *
 ****************************************************************************/
 
/**
 * @file px4_simple_app.c
 * Minimal application example for PX4 autopilot.
 */
 
#include <nuttx/config.h>
#include <stdio.h>
#include <errno.h>
 
__EXPORT int px4_simple_app_main(int argc, char *argv[]);
 
int px4_simple_app_main(int argc, char *argv[])
{
	printf("Hello Sky!\n");
	return OK;
}

Step 3: Register the Application in NuttShell and build it

The application is now complete and could be run, but it is not registered as NuttShell command yet. To enable the compilation of the application into the firmware, add it to the list of modules to build, which is here:

Create a new line for your application somewhere in the file:

 modules/px4_simple_app

Build it:

make px4fmu-v2_default

will be sufficient for further incremental builds.

Step 4: Upload and Test the app

Enable the uploader and then reset the board:

make px4fmu-v2_default upload

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

Generating /Users/user/src/Firmware/Images/px4fmu.px4
Loaded firmware for 5,0, waiting for the bootloader...

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

Found board 5,0 on /dev/tty.usbmodem1
erase...
program...
verify...
done, rebooting.

Now change to the shell with NSH connected. If you do not see any output, hinter Enter. You should get the NSH prompt:

Alternatively, you can also use QGroundControl for NuttShell (NSH). Connect Mini-USB to Pixhawk and follow according to the picture below. Once connected, click on the terminal output on QGroundControl and press Enter on your keyboard.

nsh>

Type help and hit Enter:

nsh> help
  help usage:  help [-v] [<cmd>]

  [           df          kill        mkfifo      ps          sleep       
  ?           echo        losetup     mkrd        pwd         test        
  cat         exec        ls          mh          rm          umount      
  cd          exit        mb          mount       rmdir       unset       
  cp          free        mkdir       mv          set         usleep      
  dd          help        mkfatfs     mw          sh          xd          

Builtin Apps:
  reboot
  perf
  top
  ..
  px4_simple_app
  ..
  sercon
  serdis

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

nsh> px4_simple_app
Hello Sky!

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

Step 5: Subscribing Sensor Data

To to something useful, the application needs to subscribe inputs and publish outputs (e.g. motor or servo commands). Note that the true hardware abstraction of the PX4 platform comes into play here – 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 in PX4. For this tutorial, we are interested in the sensor_combined topic, which holds the synchronized sensor data of the complete system.

Make sure to fully read up the shared_object_communication page, as it contains lots of relevant and useful additional information.

Subscribing to a topic is swift and clean:

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

The sensor_sub_fd is a file descriptor 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):

#include <poll.h>
#include <uORB/topics/sensor_combined.h>
..
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
 
/* one could wait for multiple topics with this technique, just using one here */
struct pollfd fds[] = {
	{ .fd = sensor_sub_fd,   .events = POLLIN },
};
 
while (true) {
	/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
	int poll_ret = poll(fds, 1, 1000);
..
	if (fds[0].revents & POLLIN) {
		/* obtained data for the first file descriptor */
		struct sensor_combined_s raw;
		/* copy sensors raw data into local buffer */
		orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
		printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
					(double)raw.accelerometer_m_s2[0],
					(double)raw.accelerometer_m_s2[1],
					(double)raw.accelerometer_m_s2[2]);
	}
}

The full C code for the px4_simple_app looks now like this:

/****************************************************************************
 *
 *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
 *   Author: @author Example User <mail@example.com>
 *
 * 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.
 * 2. 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.
 * 3. 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.
 *
 ****************************************************************************/
 
/**
 * @file px4_simple_app.c
 * Minimal application example for PX4 autopilot
 */
 
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
 
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
 
__EXPORT int px4_simple_app_main(int argc, char *argv[]);
 
int px4_simple_app_main(int argc, char *argv[])
{
	printf("Hello Sky!\n");
 
	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
 
	/* one could wait for multiple topics with this technique, just using one here */
	struct pollfd fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		/* there could be more file descriptors here, in the form like:
		 * { .fd = other_sub_fd,   .events = POLLIN },
		 */
	};
 
	int error_counter = 0;
 
	while (true) {
		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
		int poll_ret = poll(fds, 1, 1000);
 
		/* handle the poll result */
		if (poll_ret == 0) {
			/* this means none of our providers is giving us data */
			printf("[px4_simple_app] Got no data within a second\n");
		} else if (poll_ret < 0) {
			/* this is seriously bad - should be an emergency */
			if (error_counter < 10 || error_counter % 50 == 0) {
				/* use a counter to prevent flooding (and slowing us down) */
				printf("[px4_simple_app] ERROR return value from poll(): %d\n"
					, poll_ret);
			}
			error_counter++;
		} else {
 
			if (fds[0].revents & POLLIN) {
				/* obtained data for the first file descriptor */
				struct sensor_combined_s raw;
				/* copy sensors raw data into local buffer */
				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
				printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
					(double)raw.accelerometer_m_s2[0],
					(double)raw.accelerometer_m_s2[1],
					(double)raw.accelerometer_m_s2[2]);
			}
			/* there could be more file descriptors here, in the form like:
			 * if (fds[1..n].revents & POLLIN) {}
			 */
		}
	}
 
	return 0;
}

Compile the app now by issuing

make

Testing the uORB Subscription

Run your updated application. The UORB app might have already been started by the system, attempting to do so again does no harm, so we just run it again:

uorb start

The object request broker is now active, time to start the sensors:

sh /etc/init.d/rc.sensors

The final step is to start your application – but why is the syntax different?

px4_simple_app &

The difference is that the uorb and sensor apps run as Daemon (computing). This allows to start / stop them without ever loosing control of the NuttShell if you forget to enter the ampersand (&) at the end of the line.

Your app will flood the console with the current sensor values:

[px4_simple_app] Accelerometer:   0.0483          0.0821          0.0332
[px4_simple_app] Accelerometer:   0.0486          0.0820          0.0336
[px4_simple_app] Accelerometer:   0.0487          0.0819          0.0327
[px4_simple_app] Accelerometer:   0.0482          0.0818          0.0323
[px4_simple_app] Accelerometer:   0.0482          0.0827          0.0331
[px4_simple_app] Accelerometer:   0.0489          0.0804          0.0328

Since it is not a daemon, there is no way to stop it – either just let it run, or reboot the autopilot by issuing:

reboot

or hitting the reset button on PX4FMU. Check out this tutorial to learn how to transform an application into a daemon – but finish this page first.

Step 6: Rate-limiting subscriptions

The test setup of the last step flooded the console with sensor data. Often one is no interested in the data at the full rate, trying to keep up with the full rate often unnecessarily slows down the whole system.

The API is very simple:

orb_set_interval(int handle, unsigned interval);

To limit our topic to 1 Hz, we just have to add

orb_set_interval(sensor_sub_fd, 1000);

To our example code right below this line (around line 55):

/* subscribe to sensor_combined topic */
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));

The console will print the sensor data now just at convenient 1 Hz.

Step 7: Publishing Data

To use the calculated outputs, the next step is to publish the results. If we use a topic from which we know that the mavlink app forwards it to the ground control station, we can even look at the results. Let's hijack the attitude topic for this purpose.

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

#include <uORB/topics/vehicle_attitude.h>
..
/* advertise attitude topic */
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
int att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att);

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

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

The modified complete example code is now:

/****************************************************************************
 *
 *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
 *   Author: @author Example User <mail@example.com>
 *
 * 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.
 * 2. 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.
 * 3. 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.
 *
 ****************************************************************************/
 
/**
 * @file px4_simple_app.c
 * Minimal application example for PX4 autopilot
 */
 
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
 
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
 
__EXPORT int px4_simple_app_main(int argc, char *argv[]);
 
int px4_simple_app_main(int argc, char *argv[])
{
	printf("Hello Sky!\n");
 
	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
	orb_set_interval(sensor_sub_fd, 1000);
 
	/* advertise attitude topic */
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	int att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
	/* one could wait for multiple topics with this technique, just using one here */
	struct pollfd fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		/* there could be more file descriptors here, in the form like:
		 * { .fd = other_sub_fd,   .events = POLLIN },
		 */
	};
 
	int error_counter = 0;
 
	while (true) {
		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
		int poll_ret = poll(fds, 1, 1000);
 
		/* handle the poll result */
		if (poll_ret == 0) {
			/* this means none of our providers is giving us data */
			printf("[px4_simple_app] Got no data within a second\n");
		} else if (poll_ret < 0) {
			/* this is seriously bad - should be an emergency */
			if (error_counter < 10 || error_counter % 50 == 0) {
				/* use a counter to prevent flooding (and slowing us down) */
				printf("[px4_simple_app] ERROR return value from poll(): %d\n"
					, poll_ret);
			}
			error_counter++;
		} else {
 
			if (fds[0].revents & POLLIN) {
				/* obtained data for the first file descriptor */
				struct sensor_combined_s raw;
				/* copy sensors raw data into local buffer */
				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
				printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
					(double)raw.accelerometer_m_s2[0],
					(double)raw.accelerometer_m_s2[1],
					(double)raw.accelerometer_m_s2[2]);
 
				/* set att and publish this information for other apps */
				att.roll = raw.accelerometer_m_s2[0];
				att.pitch = raw.accelerometer_m_s2[1];
				att.yaw = raw.accelerometer_m_s2[2];
				orb_publish(ORB_ID(vehicle_attitude), att_pub_fd, &att);
			}
			/* there could be more file descriptors here, in the form like:
			 * if (fds[1..n].revents & POLLIN) {}
			 */
		}
	}
 
	return 0;
}

Running the final example

Again some infrastructure apps are needed:

uorb start
sh /etc/init.d/rc.sensors
mavlink start -d /dev/ttyS1 -b 115200

And finally run your app:

px4_simple_app &

If you start QGroundControl or Mission Planner, you can check that the sensor values displayed in the realtime plot (Main Menu: Main Widget → Realtime Plot) reflects the values your process is sending.

Wrap-Up

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

This example application is available at src/examples/px4_simple_app. To try it, just uncomment the corresponding line in the app config at Firmware/makefiles/config_px4fmu-v2_default.mk.

本页面的其他翻译:


Quick Links

QR Code: URL of current page