PIXHAWK源码分析之二—Hello Sky(PIXHAWK读取姿态数据并且显示)

阿木社区玩也要玩的专业，转载请注明出处！http://www.amovauto.com/?p=905　QQ群:526221258

　　　我们先放开那些复杂的数学算法，向卡尔曼滤波，四元函数什么的，对于PX4构架我们先有个直观的认识。知道整个工程怎么组织构建，留有什么修改接口，我们编译出一个Demo运行下，就会对整个PX4的框架有直观的了解。对框架了解之后，我们再去尝试分析飞控算法，或者在PX4的算法之上，添加自己的功能，毕竟不建议重复造轮子的事情发生。个人感觉PX4的构架，有望成为无人机领域，标准系统之一，我们也可以为这个框架做些事情，去完善和发展他。 　　上一篇文章是讲述PX4的工程构架，怎么用Cmake组织起来的，并且修改了构建，添加了一个应用程序。我们把这个程序运行起来看看效果

我下面讲述的都是基于Ubuntu14.04系统。
首先make upload px4fmu-v2_default 把我们修改好的固件下载到PIX里面。

然后拔掉SD卡(如果不拔SD卡，无法进入NUttx的终端)，给PIX上电用，任何一款串口调试助手都可以。波特率56700。

　　　我们点击回车键可以看到 nsh> 输入help 我们会看到很多命令列表。其中就有我们编译得到的px4_simple_app这个程序。我们输入px4_simple_app这个命令。我们会发现打印出了加速度数据。也就是读出了板子系统的加速度数据。哈哈 我们已经可以操作板子的传感器接口啊。我们写一个什么脚本，就可以自动执行这句话。来达到我们的目的。下面我们就分析下这个程序是怎么读出板子的加速度计的，如果我们的程序要怎么使用PX4提供的接口函数。我们知道我们是修改了nuttx_px4fmu-v2_default.cmake这个文件来实现编译出px4_simple_app，同理我们也可以写一个程序，来实现我们想要的功能。但是具体怎么写，调用什么接口可以实现对板子的控制呢。我们就来分析下这个px4_simple_app读取加速度计的例程。这个例子在源码的\Firmware-master\src\examples文件夹下面。 源码分析

  
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 

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

int px4_simple_app_main(int argc, char *argv[])
{
	PX4_INFO("Hello Sky!");

	/* 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));
	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);//功能：公告发布者的主题；
        说明：在发布主题之前是必须的；否则订阅者虽然能订阅，但是得不到数据；

	/* one could wait for multiple topics with this technique, just using one here */
	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 },//订阅文件描述符定义
		 */
	};

	int error_counter = 0;

	for (int i = 0; i < 5; i++) {
		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
		int poll_ret = px4_poll(fds, 1, 1000);//功能：监控文件描述符（多个）,如果有变化，说明订阅的数据可以有更新读取。
		/* handle the poll result */
		if (poll_ret == 0) {
			/* this means none of our providers is giving us data */
			PX4_ERR("[px4_simple_app] Got no data within a second");

		} 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) */
				PX4_ERR("[px4_simple_app] ERROR return value from poll(): %d"
					, 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);//读取订阅的数据
				PX4_WARN("[px4_simple_app] 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]);

				/* 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, &att);//发布数据，可以用mavlink发布到地面站
			}

			/* there could be more file descriptors here, in the form like:
			 * if (fds[1..n].revents & POLLIN) {}
			 */
		}
	}

	PX4_INFO("exiting");

	return 0;
}

　　　　　为了做有用的事情，应用需要订阅subscribe输入并发布publish输出 (e.g. 电机 或伺服指令). PX4平台真正的硬件抽象（true hardware abstraction ）开始有用——不需要与传感器驱动以任何方式相互作用，并且电路板或者传感器升级后不需要升级。
　　　　 PX4中应用间独立的消息通道被称作topics. 在这里，感兴趣的是 sensor_combined topic, 其调用整个系统的同步传感器数据。 订阅到topic非常快和干净：int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); sensor_sub_fd 是一个文件描述符，非常高效的执行一块等待新的数据的数据块。当前的进程进入休眠，当有新的数据可用时，由调度程序自动地唤醒，在等待的时候，不占用任何CPU周期。为实现这点，使用poll()，调用 POSIX system 。
　　　　　uORB为PX4系统的数据集散中心,应用程序间的数据共享全部要通过uORB来传递, 而数据传递的载体被称为 topic. 其实,一个topic就是一个事先已经定义好的结构体,用来反映该topic的数据构成. 数据传递的方法是通过几个uORB数据管理中心提供的几个接口函数,这些接口中最重要的4个是:
orb_subscribe(...) ,订阅消息
orb_copy(...),读取消息
orb_advertise(...),申明消息
orb_publish(...).发布消息
topic是一个被生产和被消费的过程.
某个APP要消费一个topic,它首先要使用函数orb_subscribe来订阅该topic,然后才能用rb_copy函数来获取被订阅的数据;
某个APP要发布一个topic,它首先要使用函数orb_advertise来广告这个topic给uORB,然后用orb_publish函数来发布这个topic的数据.
　　　 uORB全称为micro object request broker (uORB),即 微对象请求代理器，实际上uORB是一套跨进程的IPC通讯模块。在Pixhawk中， 所有的功能被独立以进程模块为单位进行实现并工作。而进程间的数据交互就由为重要，必须要能够符合实时、有序的特点。 Pixhawk 使用NuttX实时ARM系统， 而uORB对于NuttX而言，它仅仅是一个普通的文件设备对象，这个设备支持Open、Close、Read、Write、Ioctl以及Poll机制。 通过这些接口的实现，uORB提供了一套“点对多”的跨进程广播通讯机制， “点”指的是通讯消息的“源”，“多”指的是一个源可以有多个用户来接收、处理。而“源”与“用户”的关系在于，源不需要去考虑用户是否可以收到某条被广播的消息或什么时候收到这条消息。它只需要单纯的把要广播的数据推送到uORB的消息“总线”上。对于用户而言，源推送了多少次的消息也不重要，重要的是取回最新的这条消息。 下面是uORB示意图:这个oURB有点像生产者消费者模型，内部实现了进程间数据的同步，方便我们在PIX上读取数据。 　　　所以我们的读取传感器数据也很简单，第一我们订阅数据，第二数据有更新我们读取数据显示即可。我们已经知道订阅数据用什么。 前期对于框架的理解，要难些，如果我们理解了，这个Nuttx构建，我们在PIX上很容易做二次开发了。要得到传感器数据就先订阅，监听订阅文件描述符变化，有变化就标示可以读取数据，读取数据！就这个Demo还是很好理解的，确实PIX很好 很强大....节省了我们很多开发时间

阿木社区　玩也要玩的专业