PX4应用examples[1]:px4_simple_app解析与自己的功能模块构建

px4_simple_app源代码学习

代码位置位于PX4-Autopilot/src/examples/px4_simple_app文件夹下:
代码主要完成了对于sensor_combined传感器信息的订阅以及关于加速度计accelerometer_m_s2信息的发布

代码分析

我们可以将程序剥离为三个部分来看，关于PX4_INFO试探信息的发布、传感器消息的订阅、以及传感器消息的发布

第一部分 PX4简单打印消息的发布

#include<px4_config.h>
__EXPORT int px4_simple_app_main(int argc,char *argv[]);
int px4_simple_app_main(int argc,char *argv[])
{
	PX4_INFO("Hello Sky!");
	return 0;
}

main函数必须命名为<module_name>_main并从模块中导出（__EXPORT）
PX4_INFO()在PX4 shell中的作用相当于c语言中的printf(),与PX4_INFO()相关的分析日志函数有：PX4_WARN
()、PX4_ERR()、PX4_DEBUG()

第二部分 传感器信息的获取

该代码获取的是sensor_combined传感器消息,该消息的定义是一个结构体,我们需要包含它的定义头文件==#include <uORB/topics/sensor_combined.h>==

关于sensor_combined的介绍：

sensor_combined是一个冗余的传感器集合的信息，通过订阅多个传感器的数据，将冗余的数据经过VoteSensorsUpdate投票机制，筛选出较为可靠的一组数据，用于状态估计模块的姿态解算和位置解算。
陀螺仪的英文名称是Gyro
加速度计的英文名称是Accelerometer

#ifdef __cplusplus
struct __EXPORT sensor_combined_s {
#else
struct sensor_combined_s {
#endif
	uint64_t timestamp; // required for logger
	float gyro_rad[3];//上一采样周期内，陀螺仪在body系下xyz的测量平均值（rad/s）
	uint32_t gyro_integral_dt;//陀螺仪测量采样周期（us）
	int32_t accelerometer_timestamp_relative;
	float accelerometer_m_s2[3];//上一采样周期内，加速度计在body系下xyz的测量平均值（m/s2）
	uint32_t accelerometer_integral_dt;//加速度计测量采用周期（us）
	uint8_t _padding0[4]; // required for logger
#ifdef __cplusplus
	static constexpr int32_t RELATIVE_TIMESTAMP_INVALID = 2147483647;

#endif
};

话题订阅代码

#include<uORB/topics/sensor_combined.h>
#include<px4_tasks.h>
#include<poll.h>
__EXPORT int px4_simple_app_main(int argc,char *argv[])

int px4_simple_app_main(int argc,char *argv[])
{
	//获取主题的fd
	int sensor_sub_fd=orb_subscribe(ORB_ID(sensor_combined));
		/* limit the update rate to 5 Hz */
	orb_set_interval(sensor_sub_fd, 200);
		/* one could wait for multiple topics with this technique, just using one here */
 
 	//把订阅主题的fd添加到检测列表中
	px4_pollfd_struct_t fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		/* there could be more file descriptors here, in the form like:
		 * { .fd = other_sub_fd,   .events = POLLIN },
		 */
	};
	
//阻塞等待传感器信息的更新，最多等待1000，如果1s后还没有数据poll_ret返回0,1s内有数据返回1
	int poll_ret = px4_poll(fds, 1, 1000);
	if(poll_ret>0)
	{
		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把数据放在raw结构中
				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
				PX4_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
					 (double)raw.accelerometer_m_s2[0],
					 (double)raw.accelerometer_m_s2[1],
					 (double)raw.accelerometer_m_s2[2]);
			}
			}
}

我们创造了一个话题订阅句柄，可用于非常有效地执行阻塞等待新数据。当前线程进入睡眠状态，一旦有新数据可用，调度程序就会自动唤醒该线程，在等待时不消耗任何CPU周期。为此，我们使用poll() POSIX系统调用。，在代码中，我们只进行一次传感器信息的获取。

第三部分 消息的发布

通过某种方式获得消息后将消息发布出去。我们选择发布attitude话题

#include<uORB/topics/vehicle_attitude.h>
//先定义一个用于存放数据的结构并申请内存，根据事件的id，得到发布数据需要的fd
struct vehicle_attitude_s att;
memset(&att,0,sizeof(att));
orb_advert_t att_pub=orb_advertise(ORB_ID(vehicle_attitude),&att);

//把需要更新的数据放入att中，最后发布出去
att.q[0] = raw.accelerometer_m_s2[0];
att.q[1] = raw.accelerometer_m_s2[1];
att.q[2] = raw.accelerometer_m_s2[2];
orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);

创建自己的应用

1. 我们可以在examples(或者modules)下面建立自己的应用,可以命名为my_example_app,在相应的examples目录下建立my_example_app文件夹
2. 撰写my_example_app.c文件，设计自己想要实现的功能,在定义的文件中一定要有一个my_example_app_main的函数，否则编译的时候会找不到这个函数，导致nuttx无法创建app应用程序。为了能够使得其他模块使用这个主函数，需要__EXPORT 这个函数。

   __EXPORT int px4_simple_app_main(int argc, char *argv[]); //.c文件
   

extern “C” __EXPORT int px4_simple_app_main(int argc, char *argv[]);//.cpp文件
```
4. 撰写CmakeLists.txt内容(用于编译，不可缺少)添加如下代码

PX4_add_module(
MODULE examples__my_example_app
MAIN my_example_app
STACK_MAIN 2000
SRCS
	my_example_app.c
DEPENDS
	platform__common
)

5. 源文件和CMake文件编写好之后需要在编译配置中加入我们的样例， 然后在Firmware\cmake\configs\nuttx_px4fmu-v2_default.cmake(如果是在仿真环境中的话Firmware\cmake\configs\posix_sitl_default.cmake:的配置文件中加上需要编译的文件夹，比如examples/my_example_app

测试自己的应用是否创建成功

1.软件在环测试：记住在make的时候(如果是软件在环仿真,一定要加gazebo/jmavsim这样的仿真器),不然没有窗口提示符让你运行自己的应用程序

make  posix_sitl_default gazebo

2.硬件在环测试：如果我们需要在飞控板中运行，比如是上面指定的px4fmu-v2_default固件版本
*

make px4_fmu-v2_default upload

• 编译并下载代码到飞控板子上，连接飞控计算机
• 打开QGC，进入Analyse->Mavlink Console,这个 Console就是前面说的控制台,在控制台下先输入help, 可以看到可以启动的模块,然后再输入指令my_example_app。