本次小项目是通过PX4读取STM32F4发过来的数据，之前博客介绍了我做的STM32端项目，再稍微啰嗦一下：解析AIRMAR和测深仪数据，该数据采用NEMA0183协议传输的。PX4接收到数据会通过mavlink消息发布出去，通过433MHz来进行无线传输，然后在岸边读取内河中的气象信息。
本项目会通过几篇博客去介绍，本次主要介绍PX4中通过串口读取STM32F4串口发送过来消息并发布UORB主题，本次帖子主要是参考网络上一些大神方法进行编写，希望对大家有用，欢迎各位朋友及时提出问题。
在Firmware/msg目录下新建read_uart_sensor.msg文件并添加到CMakeLists.txt中，编译后自动生成uORB/topics/read_uart_sensor.h头文件
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
在Firmware/src/modules目录下新建文件夹read_uart_sensor。添加文件read_uart_sensor.c
PX4接收串口接收代码
/*
* 串口读取函数
* 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;
/* 以新的配置填充结构体 */
/* 设置某个选项，那么就使用"|="运算，
* 如果关闭某个选项就使用"&="和"~"运算
* */
tcgetattr(fd, &uart_config); // 获取终端参数
/* clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;// 将NL转换成CR(回车)-NL后输出。
/* 无偶校验，一个停止位 */
uart_config.c_cflag &= ~(CSTOPB | PARENB);// CSTOPB 使用两个停止位，PARENB 表示偶校验
/* 设置波特率 */
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;
}
// 设置与终端相关的参数，TCSANOW 立即改变参数
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 {start|stop|status} [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); //公告，公告必须在发布之前，否则即使订阅也得不到任何数据
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);// 复制字符串
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;
}
添加CMakeLists.txt文件，并编写里面内容。
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 :
对应位置如下：
在Firmware/cmake/configs/nuttx/nuttx_px4fmu-v2_default.cmake中注册该模块
以上就是对PX4中通过串口读取STM32F4串口发送过来消息并发布UORB主题的代码编写和环境配置，接下来最关心的事情便是查看结果正确性，具体操作如下：
显示在nsh终端上面
1、输入read_uart_sensor start 就会运行对应程序并运行打印程序打印在nsh终端上。
2、输入read_uart_sensor stop就会停止对程序运行
如上方法，如果可以看到如上图所示效果，则说明成功了，恭喜你可以开始后面自定义mavlink消息！