树莓派与pixhawk串口通信
一、Pixhawk部分
1.读取数据测试
步骤：
- 在Firmware/src/modules中添加一个新的文件夹，命名为rw_uart
- 在rw_uart文件夹中创建CMakeLists.txt文件，并输入以下内容：
px4_add_module(
MODULE modules__rw_uart
MAIN rw_uart
COMPILE_FLAGS
-Os
SRCS
rw_uart.c
DEPENDS
platforms__common
)
- 在rw_uart文件夹中创建rw_uart.c文件
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
__EXPORT int rw_uart_main(int argc, char *argv[]);
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 rw_uart_main(int argc, char *argv[])
{
char data = '0';
char buffer[4] = "";
int uart_read = open("/dev/ttyS2", O_RDWR | O_NOCTTY);//打开串口设备
if (uart_read < 0) {
err(1, "failed to open port: %s", "/dev/ttyS2");
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;
}
- 注册新添加的应用到NuttShell中。/src/Firmware/cmake/configs/nuttx_px4fmu-v2_default.cmake文件中添加如下内容：
modules/rw_uart
- 编译并刷固件
make clean
make px4fmu-v2_default
2.定义和发布主题
2.1 新建主题
- msg文件夹下新建rw_uart_raspberry_topic.msg文件
char[4] datastr0
uint8 data
- 在msg文件夹中的CMakeList文件中加入
rw_uart_raspberry_topic.msg
2.2 补充修改rw_uart.c文件
- rw_uart.c
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
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 void usage(const char *reason);
static int uart_init(char * uart_name);
static int set_uart_baudrate(const int fd, unsigned int baud);
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 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[])
{
if (argc < 2) {
usage("[YCM]missing command");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("[YCM]already running\n");
return 0;
}
thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("rw_uart",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2000,
rw_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("[YCM]running");
} else {
warnx("[YCM]stopped");
}
return 0;
}
usage("unrecognized command");
return 1;
}
int rw_uart_thread_main(int argc, char *argv[])
{
warnx("[daemon] starting\n");
thread_running = true;
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");
struct rw_uart_raspberry_topic_s test_data; //定义类型为rw_uart_raspberry_topic_s的结构体变量rd
orb_advert_t rw_uart_raspberry_topic_pub = orb_advertise(ORB_ID(rw_uart_raspberry_topic), &test_data);
while(!thread_should_exit){
read(uart_read,&data,1);
if(data == 'R'){
for(int i = 0;i <4;++i){
read(uart_read,&data,1); //读取串口设备数据1个字节，放到data中
buffer[i] = data;
data = '0';}
write(uart_read,&buffer,4);
char * s;
strncpy(test_data.datastr0,buffer,4);
test_data.data = strtol(test_data.datastr0,&s,10);
orb_publish(ORB_ID(rw_uart_raspberry_topic), rw_uart_raspberry_topic_pub, &test_data);
int b=strtol(test_data.datastr0,&s,10);
printf("\t%s\t%d\t%d\n",test_data.datastr0,test_data.data,b);
}
}
warnx("[YCM]exiting");
thread_running = false;
close(uart_read);
fflush(stdout);
return 0;
}
2.3设置自启动
- 在ROMFS/px4fmu_common/init.d/rcS中添加自启动
rw_uart start
- 编译并刷固件
make clean
make px4fmu-v2_default
make px4fmu-v2_default_upload
二、Raspberry部分
- 循环发送’R1100’小程序（python）
# -*- coding: utf-8 -*-
import serial
import time
def port_send(send_date):
if (serial.isOpen()):
serial.write(send_date.encode('utf-8'))
time.sleep(0.5)
else:
print("send failed")
if __name__ == "__main__":
serial = serial.Serial('/dev/ttyS2', 9600, timeout=0.5) # /dev/ttyUSB0
if serial.isOpen():
print("open success")
else:
print("open failed")
while True:
port_send("R1100")
三、Pixhawk与树莓派连接
- TX—RX
- RX—TX
- GND—GND
- TELEM2从左到右分别是VC TXD RXD 未知 未知 GND
测试结果：
四、编译过程中产生的问题
在编译的过程中碰到了很多问题，但是大部分都是可以百度谷歌到的。我主要将不能搜索到的问题摆出来，供大家在开发过程时参考。
- implicit declaration of function ‘px4_task_spawn_cmd’
问题原因及解决方案：
缺失相关的头文件，补充头文件：
#include
- compilation terminated due to -Wfatal -errors
问题原因及解决方案：
对应函数‘set_uart_baudrate’格式不对
删除对应函数，重新书写函数。
参考资料
Pixhawk与树莓派3的串口通信
Pixhawk—通过串口方式添加一个自定义传感器（超声波为例）
pixhawk串口读取传感器数据
Python 串口读写实现