[pixhawk笔记]4-如何写一个简单的应用程序

本文主要内容来自于：https://dev.px4.io/en/tutorials/tutorial_hello_sky.html，并对文档中的部分问题进行更正。

本文假设已经建立好开发环境并能正确编译，关于编译过程，可参见本人博客中的[pixhawk笔记]1-编译过程。

程序员学习一门语言时第一个例子一般是学习怎么写一个“Hello World”，本文中的简单程序就是类似于该功能，能够让读者搞明白如何实现一个px4程序。

1. 最小程序
   进入Firmware/src/examples/目录，在px4_simple_app文件夹下面创建px4_simple_app.c文件（该文件已经存在于源码中，为了提高学习效果，建议将其删除，重新写）。
   在其中键入如下代码：

   #include <px4_config.h>
   #include <px4_tasks.h>
   #include <px4_posix.h>
   #include <unistd.h>
   #include <stdio.h>
   #include <poll.h>
   #include <string.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[])
   {
       PX4_INFO("Hello Sky!");
       return OK;
   }
   

   该代码中有默认头和默认的主函数。　　

2. 在Nuttx系统中注册应用并编译
   为了使该程序能够编译进固件，需要在系统的cmake文件中注册该程序，笔者使用的是pixhawk2，对应的cmake文件是：Firmware/cmake/config/nuttx_px4_fmu-v3_default.cmake，其他平台可以找到对应的cmake文件。
   然后在该文件中config_moule_list代码段加入模块路径：
   examples/px4_simple_app
笔者发现官方文档有问题，加入.c文件后，还得在px4_simple_app文件夹下面建立CMakeLists.txt文件，否则编译不过去。
建立CMakeLists.txt并加入如下内容：


   px4_add_module(
   	MODULE examples__px4_simple_app
   	MAIN px4_simple_app
   	STACK_MAIN 2000
   	SRCS
   		px4_simple_app.c
   	DEPENDS
   		platforms__common
   	)

   然后编译之：

3. make px4fmu-v3_default

   编译通过后编译并上传

   make px4fmu-v3_default upload

4. 连接控制台
   Ubuntu下面可以通过screen来连接px4控制台，使用如下命令安装screen：

   sudo apt-get install screen

   然后使用如下命令连接px4控制台：

   screen /dev/ttyXXX BAUDRATE 8N1

   其中，ttyXXX为pixhawk对应的串口设备，如下命令可以列出串口设备列表：

   ls /dev/tty*

   通过插拔pixhawk并对比插拔前后的列表变化，可以确定pixhawk对应的串口设备ID。
   通过对比，本机的pixhawk使用的ttyACM0

   但是笔者这么连接全是乱码……不知何故o(╯□╰)o……

   尝试使用MAVLink shell来连接控制台，先使用pip安装pymavlink和pyserial

   sudo pip install pymavlink pyserial

   GFW的原因，使用pip安装时经常出现连接超时的问题。笔者使用下载安装包离线安装的方法。

   然后使用mavlink shell 来连接：

   ./Tools/mavlink_shell.py /dev/ttyACM0

   连接成功后出现如下界面：

   
   控制太nsh即为px4代码运行的nuttx操作系统的shell，可在此运行系统内置命令，由于我们已经将写的代码注册到nuttx，所以可以直接运行该应用。键入：px4_simple_app，则出现如下运行界面：
   

   打印出Hello Sky！的字符串，证明px4_simple_app.c中的代码得到了执行。

5. 订阅传感器数据
   上面的例子只是为了演示如何写一个px4上运行的应用，为了做一些有用的事情，下面演示如何获得传感器数据并显示出来。
   在px4中，各个应用之间传递的消息称为“主题”（topic），这些消息通过uORB进行传递，当需要传递传感器信息时，我们感兴趣的是 sensor_combined 主题，该主题将整个系统的传感器信息保持同步。
   可以通过如下代码订阅该主题：

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

   得到的sensor_sub_fd是一个主题句柄，使用该句柄可以通过阻塞等待来获得新的传感器数据。 阻塞使该应用开始休眠，知道新数据产生后将该应用唤醒，在等待期间不消耗任何CPU运算。可使用POSIX中的poll函数来实现该功能，整个代码如下：

   #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 */
   px4_pollfd_struct_t fds[] = {
       { .fd = sensor_sub_fd,   .events = POLLIN },
   };
   
   while (true) {
       /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
   　　
   
      px4_poll(fds, , );
   ..
       if (fds[].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_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
                       (double)raw.accelerometer_m_s2[],
                       (double)raw.accelerometer_m_s2[],
                       (double)raw.accelerometer_m_s2[]);
       }
   }

   注意到上面的代码与源码里给出的有出入，px4_poll无返回值，因为笔者发现网上下载的源码有返回值而没有用，编译时会在gcc中产生错误而编译不通过。所以采用无返回值的形式。

   然后编译，下载，并使用mavlink shell连接，运行px4_simple_app后控制台输出如下：
   

6. 发布数据
   上面的例子给出了如何通过uORB获得订阅数据，下面给出如何发布数据。发布的接口比较简单：使用如下代码初始化要发布的内容：

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

   当传感器信息获得后，在主循环中使用如下代码来发布信息

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

   完整代码如下：

       #include <px4_config.h>
       #include <px4_tasks.h>
       #include <px4_posix.h>
       #include <unistd.h>
       #include <stdio.h>
       #include <poll.h>
       #include <string.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[])
   {
       PX4_INFO("Hello Sky!");
       int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
           orb_set_interval(sensor_sub_fd,);/* limit the update rate to 5 Hz */
   
       struct vehicle_attitude_s att;
       memset(&att,,sizeof(att));
       orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude),&att);
   
       px4_pollfd_struct_t fds[] = {
           {.fd = sensor_sub_fd, .events = POLLIN},
       };        
   
       int error_counter = ;
   
       for(int i=;i<;i++){
           int poll_ret = px4_poll(fds,,);
   
           if(poll_ret == ){
               PX4_ERR("Got no data within a second");
           }else if(poll_ret<){
               if(error_counter< || error_counter %  == ){
                   PX4_ERR("ERROR return value from poll():%d",poll_ret);
               }
   
               error_counter++;
           }else{
               if(fds[].revents & POLLIN){
                   struct sensor_combined_s 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[],
                       (double)raw.accelerometer_m_s2[],
                       (double)raw.accelerometer_m_s2[]);
   
                   att.rollspeed = raw.accelerometer_m_s2[];
                   att.pitchspeed = raw.accelerometer_m_s2[];
                   att.yawspeed = raw.accelerometer_m_s2[];
   
                   orb_publish(ORB_ID(vehicle_attitude),att_pub,&att);
               }
           }
       }
       PX4_INFO("exiting");
       return OK;
   }

   笔者注意到使用下载到的默认代码会出现att中没有roll，pitch和yaw的错误，通过查看编译文件夹下面的topics目录中的vehicle_attitude.h头文件，可以看出vehicle_attitude_s结构体中没有roll，pitch和yaw成员。所以笔者将代码中姿态改为角速度，然后编译上传并运行程序。连接地面站，通过控制台运行px4_simple_app，可以看到，其角速率信号会间隔性被发布信息劫持，显示如下：
   
   可以看出，在运行程序之后，角速率的值间歇性被px4_simple_app劫持，出现-9.8左右的值，因为程序中用加速度的值填充到角速度并进行发布。