本文主要內容來自於：https://dev.px4.io/en/tutorials/tutorial_hello_sky.html，並對文檔中的部分問題進行更正。
本文假設已經建立好開發環境並能正確編譯，關於編譯過程，可參見本人博客中的[pixhawk筆記]1-編譯過程。
程序員學習一門語言時第一個例子一般是學習怎么寫一個“Hello World”，本文中的簡單程序就是類似於該功能，能夠讓讀者搞明白如何實現一個px4程序。
- 最小程序
進入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; }該代碼中有默認頭和默認的主函數。
- 在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 )
然后編譯之： -
make px4fmu-v3_default編譯通過后編譯並上傳
make px4fmu-v3_default upload - 連接控制台
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中的代碼得到了執行。
-
訂閱傳感器數據
上面的例子只是為了演示如何寫一個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, 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); 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]); } }
注意到上面的代碼與源碼里給出的有出入，px4_poll無返回值，因為筆者發現網上下載的源碼有返回值而沒有用，編譯時會在gcc中產生錯誤而編譯不通過。所以采用無返回值的形式。
然后編譯，下載，並使用mavlink shell連接，運行px4_simple_app后控制台輸出如下：
- 發布數據
上面的例子給出了如何通過uORB獲得訂閱數據，下面給出如何發布數據。發布的接口比較簡單：使用如下代碼初始化要發布的內容：
#include <uORB/topics/vehicle_attitude.h> .. /* advertise attitude topic */ struct vehicle_attitude_s att; memset(&att, 0, 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,200);/* limit the update rate to 5 Hz */ struct vehicle_attitude_s att; memset(&att,0,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 = 0; for(int i=0;i<5;i++){ int poll_ret = px4_poll(fds,1,1000); if(poll_ret == 0){ PX4_ERR("Got no data within a second"); }else if(poll_ret<0){ if(error_counter<10 || error_counter % 50 == 0){ PX4_ERR("ERROR return value from poll():%d",poll_ret); } error_counter++; }else{ if(fds[0].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[0], (double)raw.accelerometer_m_s2[1], (double)raw.accelerometer_m_s2[2]); att.rollspeed = raw.accelerometer_m_s2[0]; att.pitchspeed = raw.accelerometer_m_s2[1]; att.yawspeed = raw.accelerometer_m_s2[2]; 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左右的值，因為程序中用加速度的值填充到角速度並進行發布。