當(dāng)前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

Pixhawk---通过串口方式添加一个自定义传感器（超声波为例）

發(fā)布時間：2024/4/18 编程问答 44 豆豆

生活随笔收集整理的這篇文章主要介紹了 Pixhawk---通过串口方式添加一个自定义传感器（超声波为例）小編覺得挺不錯的,現(xiàn)在分享給大家,幫大家做個參考.

Pixhawk—添加一個自定義傳感器—超聲波（串口方式）

1 說明

??首先超聲波模塊是通過串口方式發(fā)送（Tx）出數(shù)據(jù)，使用的模塊數(shù)據(jù)發(fā)送周期為100ms，數(shù)據(jù)格式為：

R0034 R0122 R0122 R0046 R0127 R0044 R0044 R0125 R0034 R0037 R0041 R0122 R0122 .....

則可以通過Pixhawk板上的串口來接收（Rx）數(shù)據(jù)，即將超聲波的Tx接口連接到Pixhawk板上的Rx接口。
??Pixhawk板上串口說明：
??
??測試使用Pixhawk板上TELEM2接口的USART2，對應(yīng)的Nuttx UART設(shè)備文件尾/dev/ttyS2：
??

2 讀取數(shù)據(jù)測試

??步驟：

在Firmware/src/modules中添加一個新的文件夾，命名為rw_uart
在rw_uart文件夾中創(chuàng)建module.mk文件，并輸入以下內(nèi)容：
- MODULE_COMMAND = rw_uart
- SRCS = rw_uart.c
在rw_uart文件夾中創(chuàng)建rw_uart.c文件
注冊新添加的應(yīng)用到NuttShell中。Firmware/makefiles/nuttx/config_px4fmu-v2_default.mk文件中添加如下內(nèi)容：
- MODULES += modules/rw_uart

rw_uart.c

#include <stdio.h> #include <termios.h> #include <unistd.h> #include <stdbool.h> #include <errno.h> #include <drivers/drv_hrt.h>__EXPORT int rw_uart_main(int argc, char *argv[]);static int uart_init(char * uart_name); static int set_uart_baudrate(const int fd, unsigned int baud);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(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; }int rw_uart_main(int argc, char *argv[]) {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("/dev/ttyS2");if(false == uart_read)return -1;if(false == set_uart_baudrate(uart_read,9600)){printf("[YCM]set_uart_baudrate is failed\n");return -1;}printf("[YCM]uart init is successful\n");while(true){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';}printf("%s\n",buffer);}}return 0; }

編譯并刷固件
- make clean
- make upload px4fmu-v2_default
查看app
- 在NSH終端中輸入help，在Builtin Apps中出現(xiàn)rw_uart應(yīng)用。
運(yùn)行rw_uart應(yīng)用（前提是模塊與Pixhawk連接好）
- 在NSH終端中輸入rw_uart，回車，查看超聲波的打印數(shù)據(jù)。

3 發(fā)布超聲波的數(shù)據(jù)

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

3.1 定義主題和發(fā)布主題

在modules/rw_uart文件夾下創(chuàng)建一個文件：rw_uart_sonar_topic.h

rw_uart_sonar_topic.h

#ifndef __RW_UART_SONAR_H_ #define __RW_UART_SONAR_H_#include <stdint.h> #include <uORB/uORB.h>/*聲明主題，主題名自定義*/ ORB_DECLARE(rw_uart_sonar);/* 定義要發(fā)布的數(shù)據(jù)結(jié)構(gòu)體 */ struct rw_uart_sonar_data_s{char datastr[5]; //原始數(shù)據(jù)int data; //解析數(shù)據(jù)，單位：mm };#endif

rw_uart.c

#include <stdio.h> #include <termios.h> #include <unistd.h> #include <stdlib.h> #include <string.h> #include <stdbool.h> #include <errno.h> #include <drivers/drv_hrt.h>#include "rw_uart_sonar_topic.h"/* 定義主題 */ ORB_DEFINE(rw_uart_sonar, struct rw_uart_sonar_data_s);static bool thread_should_exit = false; static bool thread_running = false; static int daemon_task;__EXPORT int rw_uart_main(int argc, char *argv[]); int rw_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: position_estimator_inav {start|stop|status} [param]\n\n");exit(1); }int rw_uart_main(int argc, char *argv[]) {if (argc < 2) {usage("[YCM]missing command");}if (!strcmp(argv[1], "start")) {if (thread_running) {warnx("[YCM]already running\n");exit(0);}thread_should_exit = false;daemon_task = px4_task_spawn_cmd("rw_uart",SCHED_DEFAULT,SCHED_PRIORITY_MAX - 5,2000,rw_uart_thread_main,(argv) ? (char * const *)&argv[2] : (char * const *)NULL);exit(0);}if (!strcmp(argv[1], "stop")) {thread_should_exit = true;exit(0);}if (!strcmp(argv[1], "status")) {if (thread_running) {warnx("[YCM]running");} else {warnx("[YCM]stopped");}exit(0);}usage("unrecognized command");exit(1); }int rw_uart_thread_main(int argc, char *argv[]) {if (argc < 2) {errx(1, "[YCM]need a serial port name as argument");usage("eg:");}const char *uart_name = argv[1];warnx("[YCM]opening port %s", uart_name);char data = '0';char buffer[5] = "";/** 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("[YCM]set_uart_baudrate is failed\n");return -1;}printf("[YCM]uart init is successful\n");thread_running = true;/*初始化數(shù)據(jù)結(jié)構(gòu)體 */struct rw_uart_sonar_data_s sonardata;memset(&sonardata, 0, sizeof(sonardata));/* 公告主題 */orb_advert_t rw_uart_sonar_pub = orb_advertise(ORB_ID(rw_uart_sonar), &sonardata);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(sonardata.datastr,buffer,4);sonardata.data = atoi(sonardata.datastr);//printf("[YCM]sonar.data=%d\n",sonardata.data);orb_publish(ORB_ID(rw_uart_sonar), rw_uart_sonar_pub, &sonardata);}}warnx("[YCM]exiting");thread_running = false;close(uart_read);fflush(stdout);return 0; }

3.2 測試發(fā)布的主題—訂閱主題

??測試可以隨便一個啟動的app中進(jìn)行主題訂閱，然后將訂閱的數(shù)據(jù)打印出來，看是否是超聲波的數(shù)據(jù)。這里測試是在固件的src/examples文件夾中的px4_simple_app應(yīng)用進(jìn)行測試的。

將px4_simple_app應(yīng)用添加到NuttShell中。Firmware/makefiles/nuttx/config_px4fmu-v2_default.mk文件中添加如下內(nèi)容：
- MODULES += examples/px4_simple_app
在px4_simple_app.c中代碼內(nèi)容：

#include <px4_config.h> #include <unistd.h> #include <stdio.h> #include <poll.h> #include <string.h>#include <uORB/uORB.h> #include "rw_uart/rw_uart_sonar_topic.h"__EXPORT int px4_simple_app_main(int argc, char *argv[]);int px4_simple_app_main(int argc, char *argv[]) {printf("Hello Sky!\n");/* subscribe to rw_uart_sonar topic */int sonar_sub_fd = orb_subscribe(ORB_ID(rw_uart_sonar));/*設(shè)置以一秒鐘接收一次，并打印出數(shù)據(jù)*/orb_set_interval(sonar_sub_fd, 1000);bool updated;struct rw_uart_sonar_data_s sonar;/*接收數(shù)據(jù)方式一：start*//*while(true){orb_check(sonar_sub_fd, &updated);if (updated) {orb_copy(ORB_ID(rw_uart_sonar), sonar_sub_fd, &sonar);printf("[YCM]sonar.data=%d\n",sonar.data);}//else printf("[YCM]No soanar data update\n");}*//*接收數(shù)據(jù)方式一：end*//*接收數(shù)據(jù)方式二：start*//* one could wait for multiple topics with this technique, just using one here */struct pollfd fds[] = {{ .fd = sonar_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++) {s/* 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_simple_app] 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_simple_app] 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 rw_uart_sonar_data_s sonar;/* copy sensors raw data into local buffer */orb_copy(ORB_ID(rw_uart_sonar), sonar_sub_fd, &sonar);printf("[px4_simple_app] Sonar data:\t%s\t%d\n",sonar.datastr,sonar.data);}/* there could be more file descriptors here, in the form like:* if (fds[1..n].revents & POLLIN) {}*/}}/*接收數(shù)據(jù)方式二：end*/return 0; }

編譯并刷固件
- make upload px4fmu-v2_default
在NSH中測試(已加入自啟動腳本中)
- rw_uart start /dev/ttyS2
- px4_simple_app

3.3 加入系統(tǒng)啟動腳本

??可以加入到光流的自定義啟動腳本中：/fs/microsd/etc/extras.txt。這樣隨著系統(tǒng)的自啟動，rw_uart就會默認(rèn)啟動了。

# start sonar rw_uart start /dev/ttyS2

總結(jié)

以上是生活随笔為你收集整理的Pixhawk---通过串口方式添加一个自定义传感器（超声波为例）的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。

上一篇： C语言指针详解----指针声明定义赋值
下一篇： Pixhawk---超声波模块添加说明（