PX4SITL仿真——uORB實例
飛控串口讀取外部傳感器數據：飛控開啓一個進程讀取外部傳感器數據，發佈一個uORB主題；另一個進程訂閱前一個進程發佈的主題，訂閱到的主題通過mavlink消息發送到地面站（QGroundControl）。
1 新增一個自定義uORB主題
在源碼 Firmware/msg 文件夾下是飛控所有的uORB主題
可以看到vehicle_global_position.msg是全球位置，vehicle_attitude.msg是姿態消息。姿態的成員變量包括橫滾 roll, 俯仰 pitch,偏航 yaw, 橫滾速度，俯仰速度等相關姿態的數據。相關的加速度計，磁力計，陀螺儀經過算法濾波整合之後會發布姿態數
據，而姿態控制進程會訂閱這個 vehicle_attitude.msg 主題。
1.1 自定義主題
在msg文件夾下添加一個具體的消息：
topic_name.msg,文件裏的結構體成員自己定義：
然後修改msg/CMakeList.txt ：將自定義的 topic_name.msg 名字寫入到該文件中。在編譯源碼的時候會自動寫好頭文件。之後make 就可以在Firmware/uORB/topics下看到我們自定義的消息頭文件了。
1.2 訂閱和發佈示例
1、Create a new directory Firmware/src/examples/px4_simple_app.
2、Create a new C file in that directory named px4_simple_app.c:
示例px4_simple_app.c內容如下：
/**
* @file px4_simple_app.c
* Minimal application example for PX4 autopilot
*
* @author Example User <[email protected]>
*/
#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 <math.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!");
/* subscribe to sensor_combined topic */
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
/* limit the update rate to 5 Hz */
orb_set_interval(sensor_sub_fd, 200);
/* 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("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("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_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]);
/* set att and publish this information for other apps
the following does not have any meaning, it's just an example
*/
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);
}
/* there could be more file descriptors here, in the form like:
* if (fds[1..n].revents & POLLIN) {}
*/
}
}
PX4_INFO("exiting");
return 0;
}
3、Create and open a new cmake definition file named CMakeLists.txt. Copy in the text below:
px4_add_module(
MODULE examples__px4_simple_app
MAIN px4_simple_app
STACK_MAIN 2000
SRCS
px4_simple_app.c
DEPENDS
)
1.4 編譯訂閱發佈進程
PX4 SITL (Simulator): Firmware/boards/px4/sitl/default.cmake
在default.cmake中添加模塊示例，此處的EXAMPLE對應創建的examples/px4_simple_app模塊的位置。
編譯命令：
make px4 gazebo
1.5 使用進程
uORB進行消息的訂閱與發佈示例參考：https://dev.px4.io/en/apps/hello_sky.html#subscribing-to-sensor-data