PX4 reads the message sent by STM32F4 serial port through serial port and publishes the UORB topic
This small project is to read the data sent by STM32F4 through PX4. The previous blog introduced the STM32 end project I made, and then a little more verbose: analyze the data of AIRMAR and bathymeter, which is transmitted by NEMA0183 protocol. When PX4 receives the data, it will release it through mavlink message, transmit it wirelessly through 433MHz, and then read the weather information in the river on the bank.
This project will be introduced through several blogs. This time, it mainly introduces PX4 reading messages sent by STM32F4 serial port through serial port and publishing UORB theme. This post is mainly written by referring to some god methods on the network, hoping to be useful to you. Welcome to ask questions in time.
Create a new read UART sensor.msg file in the Firmware/msg directory and add it to CMakeLists.txt. After compilation, the header file of uorb / topics / read UART sensor.h will be generated automatically
char[10] jingdustr char[10] weidustr char[10] hangsustr char[10] hangxiangstr char[10] fengsustr char[10] fengxiangstr char[10] wendustr char[10] shendustr float32 longitude float32 latitude float32 hang_su float32 hang_xiang float32 feng_su float32 feng_xiang float32 wen_du float32 shen_du # TOPICS read_uart_sensor
In the Firmware/src/modules directory, create a new folder read? UART? Sensor. Add file read? UART? Sensor. C
PX4 receiving serial port receiving code
/* * Serial read function * read_uart_sensor.c */ #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 <nuttx/config.h> #include <fcntl.h> #include <sys/types.h> #include <sys/stat.h> #include <poll.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/read_uart_sensor.h> #include <px4_tasks.h> __EXPORT int read_uart_sensor_main(int argc, char *argv[]); static int daemon_task; static bool thread_should_exit = true; static bool thread_running = false; int read_uart_thread_main(int argc, char *argv[]); static void usage(const char *reason); 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 = 9600; break; case 19200: speed = 19200; break; case 38400: speed = 38400; break; case 57600: speed = 57600; break; case 115200: speed = 115200; break; default: warnx("ERR: baudrate: %d\n", baud); return -EINVAL; } struct termios uart_config; int termios_state; /* Fill structure with new configuration */ /* If an option is set, the "| =" operation is used, * Use & = "and" ~ "if you turn off an option * */ tcgetattr(fd, &uart_config); // Get terminal parameters /* clear ONLCR flag (which appends a CR for every LF) */ uart_config.c_oflag &= ~ONLCR;// Convert NL to Cr (enter) - NL and output. /* Parity, one stop bit */ uart_config.c_cflag &= ~(CSTOPB | PARENB);// Cstop B uses two stop bits, and PARENB indicates even parity /* 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; } // Set the parameters related to the terminal, and TCSANOW immediately changes the parameters 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; } // printf("Open the %s\n",serial_fd); return serial_fd; } static void usage(const char *reason) { if (reason) { fprintf(stderr, "%s\n", reason); } fprintf(stderr, "usage: read_uart_sensor [param]\n\n"); exit(1); } int read_uart_sensor_main(int argc, char *argv[]) { if (argc < 2) { usage("missing command"); } if (!strcmp(argv[1], "start")) { if (thread_running) { warnx("already running\n"); return 0; } thread_should_exit = false; daemon_task = px4_task_spawn_cmd("read_uart_sensor", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, //SCHED_PRIORITY_MAX - 5, 2000, read_uart_thread_main, (argv) ? (char * const *)&argv[2] : (char * const *)NULL); warnx("already running2222\n"); return 0; } if (!strcmp(argv[1], "stop")) { thread_should_exit = true; return 0; } if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("running"); return 0; } else { warnx("stopped"); return 1; } return 0; } usage("unrecognized command"); return 1; } int read_uart_thread_main(int argc,char *argv[]) { //char *uart_name = argv[1]; char data = '0'; char jingdu[10] = ""; char weidu[10] = ""; char hangsu[10] = ""; char hangxiang[10] = ""; char fengsu[10] = ""; char fengxiang[10] = ""; char wendu[10] = ""; char shendu[10] = ""; int index=0; int jingdu_counter=0; int weidu_counter=0; int hangsu_counter=0; int hangxiang_counter=0; int fengsu_counter=0; int fengxiang_counter=0; int wendu_counter=0; int shendu_counter=0; int end=0; int uart_read = uart_init("/dev/ttyS6"); if(false == uart_read) return -1; if(false == set_uart_baudrate(uart_read,9600)) { printf("set_uart_baudrate is failed\n"); return -1; } printf("uart init is successful\n"); thread_running = true; struct read_uart_sensor_s orbtest; memset(&orbtest,0,sizeof(orbtest)); orb_advert_t pub_fd=orb_advertise(ORB_ID(read_uart_sensor),&orbtest); //Announcement. The announcement must be prior to publication, or no data will be obtained even if the subscription is made while (!thread_should_exit) { read(uart_read,&data,1); warnx("already running3333\n"); if(data=='$') { while(end==0) { read(uart_read,&data,1); if(data ==',') { index++; } else { switch(index) { case 1:jingdu[jingdu_counter++]=data;break; case 2:weidu[weidu_counter++]=data;break; case 3:hangsu[hangsu_counter++]=data;break; case 4:hangxiang[hangxiang_counter++]=data;break; case 5:fengsu[fengsu_counter++]=data;break; case 6:fengxiang[fengxiang_counter++]=data;break; case 7:wendu[wendu_counter++]=data;break; case 8:shendu[shendu_counter++]=data;break; case 9:end=1;break; } } } } if(end==1) { jingdu_counter=0; weidu_counter=0; hangsu_counter=0; hangxiang_counter=0; fengsu_counter=0; fengxiang_counter=0; wendu_counter=0; shendu_counter=0; index=0; end=0; //printf("%s\n",jingdu); //printf("%s\n",weidu); //printf("%s\n",hangsu); //printf("%s\n",hangxiang); //printf("%s\n",fengsu); //printf("%s\n",fengxiang); //printf("%s\n",wendu); //printf("%s\n",shendu); warnx("already running4444\n"); strncpy(orbtest.jingdustr,jingdu,10);// Copy string strncpy(orbtest.weidustr,weidu,10); strncpy(orbtest.hangsustr,hangsu,10); strncpy(orbtest.hangxiangstr,hangxiang,10); strncpy(orbtest.fengsustr,fengsu,10); strncpy(orbtest.fengxiangstr,fengxiang,10); strncpy(orbtest.wendustr,wendu,10); strncpy(orbtest.shendustr,shendu,10); orbtest.longitude= atof(orbtest.jingdustr); orbtest.latitude = atof(orbtest.weidustr); orbtest.hang_su = atof(orbtest.hangsustr); orbtest.hang_xiang= atof(orbtest.hangxiangstr); orbtest.feng_su= atof(orbtest.fengsustr); orbtest.feng_xiang= atof(orbtest.fengxiangstr); orbtest.wen_du= atof(orbtest.wendustr); orbtest.shen_du= atof(orbtest.shendustr); orb_publish(ORB_ID(read_uart_sensor),pub_fd,&orbtest); /*warnx("DATA:longitude=%f,latitude=%f,hang_su=%f,hang_xiang=%f", (double) orbtest.longitude, (double) orbtest.latitude, (double) orbtest.hang_su, (double) orbtest.hang_xiang);*/ printf("success!\n"); int sensor_sub_fd = orb_subscribe(ORB_ID(read_uart_sensor));// orb_set_interval(sensor_sub_fd, 1000); struct read_uart_sensor_s data_copy; orb_copy(ORB_ID(read_uart_sensor),sensor_sub_fd,&data_copy); //PX4_INFO("DATA:%f\t%f", // (double)data_copy.lat, // (double)data_copy.lng); warnx("DATA:longitude=%f,latitude=%f,hang_su=%f,hang_xiang=%f", (double) data_copy.longitude, (double) data_copy.latitude, (double) data_copy.hang_su, (double) data_copy.hang_xiang); } } warnx("exiting"); thread_running = false; close(uart_read); fflush(stdout); return 0; }
Add CMakeLists.txt file and write the contents.
set(MODULE_CFLAGS) px4_add_module( MODULE modules_read_uart_sensor MAIN read_uart_sensor COMPILE_FLAGS -Os SRCS read_uart_sensor.c DEPENDS platforms__common ) # vim: set noet ft=cmake fenc=utf-8 ff=unix :
The corresponding positions are as follows:
Register the module in firmware / cmake / configs / nuttx / nuttx ﹣ px4fmu-v2 ﹣ default.cmake
The above is to read the messages sent from STM32F4 serial port through PX4 serial port and publish the code writing and environment configuration of UORB topic. Next, the most concerned thing is to check the correctness of the results. The specific operations are as follows:
Display on the nsh terminal
1. Enter read? UART? Sensor start to run the corresponding program and run the print program to print on the nsh terminal.
2. Enter read? UART? Sensor stop to stop running the program
The above method, if you can see the effect as shown in the above figure, is successful. Congratulations, you can start to customize the mavlink message later!
Want to learn new technology Published 5 original articles, praised 0, visited 63 Private letter follow