當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

Pixhawk原生固件PX4之串口添加读取传感器实现

發布時間：2024/4/18 编程问答 38 豆豆

生活随笔收集整理的這篇文章主要介紹了 Pixhawk原生固件PX4之串口添加读取传感器实现小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

本博客承接前一篇，對FreeApe的串口添加超聲波傳感器博文后半部分進行學習。

為什么叫前奏呢，因為用了偽傳感器，把單片機用串口發送的有規律的數據當作了傳感器讀取到的數據。但是無礙的。

開發環境：Ubuntu Firmware 1.4.1

在無人機運行時，首先是要將應用隨系統啟動時就啟動起來的，且將獲得的超聲波數據不斷的發布出去，從而讓其他應用得以訂閱使用。這里也使用Pixhawk里面的通用模式，即主線程，檢測app命令輸入，創建一個線程來不斷的發布數據。

定義主題和發布主題

在Firmware/msg目錄下新建read_uart_sensor.msg文件。傳送門

char[4] datastr int16 data#TOPICS read_uart_sensor

并添加到CMakeLists.txt中，編譯后自動生成uORB/topics/read_uart_sensor.h頭文件

串口讀取例程

在Firmware/src/modules目錄下新建文件夾read_uart_sensor

添加文件read_uart_sensor.c。傳送門

/** read_uart_sensor.c* * read sensor through uart*/#include <stdio.h> #include <termios.h> #include <unistd.h> #include <stdbool.h> #include <errno.h> #include <drivers/drv_hrt.h> #include <string.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> #include <fcntl.h> #include <sys/types.h> #include <sys/stat.h> #include <uORB/topics/read_uart_sensor.h>__EXPORT int read_uart_sensor_main(int argc, char *argv[]);static bool thread_should_exit = false; /*Ddemon exit flag*/ static bool thread_running = false; /*Daemon status flag*/ static int daemon_task;/*** Main loop*/ int read_uart_thread_main(int argc, char *argv[]);static int uart_init(const char * uart_name); static int set_uart_baudrate(const int fd, unsigned int baud); static void usage(const char *reason);int set_uart_baudrate(const int fd, unsigned int baud) {int speed;switch (baud) {case 9600: speed = B9600; break;case 19200: speed = B19200; break;case 38400: speed = B38400; break;case 57600: speed = B57600; break;case 115200: speed = B115200; break;default:warnx("ERR: baudrate: %d\n", baud);return -EINVAL;}struct termios uart_config;int termios_state;/* fill the struct for the new configuration */tcgetattr(fd, &uart_config);/* clear ONLCR flag (which appends a CR for every LF) */uart_config.c_oflag &= ~ONLCR;/* no parity, one stop bit */uart_config.c_cflag &= ~(CSTOPB | PARENB);/* set baud rate */if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {warnx("ERR: %d (cfsetispeed)\n", termios_state);return false;}if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {warnx("ERR: %d (cfsetospeed)\n", termios_state);return false;}if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {warnx("ERR: %d (tcsetattr)\n", termios_state);return false;}return true; }int uart_init(const char * uart_name) {int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);if (serial_fd < 0) {err(1, "failed to open port: %s", uart_name);return false;}return serial_fd; }static void usage(const char *reason) {if (reason) {fprintf(stderr, "%s\n", reason);}fprintf(stderr, "usage: read_uart_sensor {start|stop|status} [param]\n\n");exit(1); }int read_uart_sensor_main(int argc, char *argv[]) {if (argc < 2) {usage("[Fantasy]missing command");}if (!strcmp(argv[1], "start")) {if (thread_running) {warnx("[Fantasy]already running\n");return 0;}thread_should_exit = false;daemon_task = px4_task_spawn_cmd("read_uart_sensor",SCHED_DEFAULT,SCHED_PRIORITY_MAX - 5,2000,read_uart_thread_main,(argv) ? (char * const *)&argv[2] : (char * const *)NULL);return 0;}if (!strcmp(argv[1], "stop")) {thread_should_exit = true;return 0;}if (!strcmp(argv[1], "status")) {if (thread_running) {warnx("[Fantasy]running");return 0;} else {warnx("[Fantasy]stopped");return 1;}return 0;}usage("unrecognized command");return 1; }int read_uart_thread_main(int argc, char *argv[]) {if (argc < 2) {errx(1, "[Fantasy]need a serial port name as argument");usage("eg:");}const char *uart_name = argv[1];warnx("[Fantasy]opening port %s", uart_name);char data = '0';char buffer[4] = "";/** TELEM1 : /dev/ttyS1* TELEM2 : /dev/ttyS2* GPS : /dev/ttyS3* NSH : /dev/ttyS5* SERIAL4: /dev/ttyS6* N/A : /dev/ttyS4* IO DEBUG (RX only):/dev/ttyS0*/int uart_read = uart_init(uart_name);if(false == uart_read)return -1;if(false == set_uart_baudrate(uart_read,9600)){printf("[Fantasy]set_uart_baudrate is failed\n");return -1;}printf("[Fantasy]uart init is successful\n");thread_running = true;/*初始化數據結構體 */struct read_uart_sensor_s sensordata;memset(&sensordata, 0, sizeof(sensordata));/* 公告主題 */orb_advert_t read_uart_sensor_pub = orb_advertise(ORB_ID(read_uart_sensor), &sensordata);while(!thread_should_exit){read(uart_read,&data,1);if(data == 'R'){for(int i = 0;i <4;++i){read(uart_read,&data,1);buffer[i] = data;data = '0';}strncpy(sensordata.datastr,buffer,4);// 復制字符串Buffer中前４個數字到Datastr中sensordata.data = atoi(sensordata.datastr);//將字符串轉換成整形數據//printf("[Fantasy]sensor.data=%d\n",sensordata.data);orb_publish(ORB_ID(read_uart_sensor), read_uart_sensor_pub, &sensordata);}}warnx("[Fantasy]exiting");thread_running = false;close(uart_read);fflush(stdout);return 0; }

添加CMakeLists.txt文件

set(MODULE_CFLAGS) px4_add_module(MODULE modules__read_uart_sensorMAIN read_uart_sensorCOMPILE_FLAGS-OsSRCSread_uart_sensor.cDEPENDSplatforms__common) # vim: set noet ft=cmake fenc=utf-8 ff=unix :

在Firmware/cmake/configs/nuttx/nuttx_px4fmu-v2_default.cmake中注冊該模塊

測試發布的主題

測試可以隨便一個啟動的app中進行主題訂閱，然后將訂閱的數據打印出來，看是否是超聲波的數據。這里新建一個應用px4_test進行測試。

px4_test.c

/** px4_test.c* * test the uart sensor app*/ #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/read_uart_sensor.h>__EXPORT int px4_test_main(int argc, char *argv[]);int px4_test_main(int argc, char *argv[]) {PX4_INFO("Hello Sky!\n");/* subscribe to rw_uart_sensor topic */int sensor_sub_fd = orb_subscribe(ORB_ID(read_uart_sensor));/*limit the update rate to 20 Hz */orb_set_interval(sensor_sub_fd, 50);/* 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++) { // infinite loop/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */int poll_ret = poll(fds, 1, 1000);/* handle the poll result */if (poll_ret == 0) {/* this means none of our providers is giving us data */printf("[px4_test] Got no data within a second\n");} 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) */printf("[px4_test] ERROR return value from poll(): %d\n", poll_ret);}error_counter++;} else {if (fds[0].revents & POLLIN) {/* obtained data for the first file descriptor */struct read_uart_sensor_s raw;/* copy sensors raw data into local buffer */orb_copy(ORB_ID(read_uart_sensor), sensor_sub_fd, &raw);PX4_INFO("[px4_test] sensor data:\t%s\t%d\n",raw.datastr,raw.data);}/* there could be more file descriptors here, in the form like:* if (fds[1..n].revents & POLLIN) {}*/}}PX4_INFO("exiting");return 0; }

編譯并上傳固件
- make px4fmu-v2_default upload
在NSH中測試
- read_uart_sensor start /dev/ttyS2
- px4_test

這個大有搞頭！

添加到腳本文件

在rcS中仿照mavlink的啟動方式添加了上面的應用，使得與ttyS2連接的外設默認為啟動狀態

現在如果ttyS2上連接了設備，就是自動啟動的，在nsh中可以直接調用px4_test應用就可以讀取數據了，與使用px4_simple_app讀取內部傳感器的方式無異。

大致模型就是這樣了，接下來就是具體的代碼優化以及應用了。

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? By Fantasy

總結

以上是生活随笔為你收集整理的Pixhawk原生固件PX4之串口添加读取传感器实现的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。

上一篇： float与double类型区别比较
下一篇：卡尔曼滤波器学习笔记（一）