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

发表于   |   分类于   |     |   阅读次数

详见博客 : http://blog.csdn.net/freeape/article/details/47837415

Pixhawk—添加一个自定义传感器—超声波（串口方式）

说明

  首先超声波模块是通过串口方式发送（Tx）出数据，使用的模块数据发送周期为100ms，数据格式为：

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

则可以通过Pixhawk板上的串口来接收（Rx）数据，即将超声波的Tx接口连接到Pixhawk板上的Rx接口。
  Pixhawk板上串口说明：
  这里写图片描述
  测试使用Pixhawk板上TELEM2接口的USART2，对应的Nuttx UART设备文件尾/dev/ttyS2：
  这里写图片描述

读取数据测试

  步骤：

  • 在Firmware/src/modules中添加一个新的文件夹，命名为rw_uart
  • 在rw_uart文件夹中创建module.mk文件，并输入以下内容：
    • MODULE_COMMAND = rw_uart
    • SRCS = rw_uart.c
  • 在rw_uart文件夹中创建rw_uart.c文件
  • 注册新添加的应用到NuttShell中。Firmware/makefiles/nuttx/config_px4fmu-v2_default.mk文件中添加如下内容：
    • MODULES += modules/rw_uart

rw_uart.c

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
#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中出现rw_uart应用。
  • 运行rw_uart应用（前提是模块与Pixhawk连接好）
    • 在NSH终端中输入rw_uart，回车，查看超声波的打印数据。

发布超声波的数据

  在无人机运行时，首先是要将应用随系统启动时就启动起来的，且将获得的超声波数据不断的发布出去，从而让其他应用得以订阅使用。这里也使用Pixhawk里面的通用模式，即主线程，检测app命令输入，创建一个线程来不断的发布数据。

定义主题和发布主题

  • 在modules/rw_uart文件夹下创建一个文件：rw_uart_sonar_topic.h

rw_uart_sonar_topic.h

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
#ifndef __RW_UART_SONAR_H_
#define __RW_UART_SONAR_H_

#include <stdint.h>
#include <uORB/uORB.h>

/*声明主题，主题名自定义*/
ORB_DECLARE(rw_uart_sonar);

/* 定义要发布的数据结构体 */
struct rw_uart_sonar_data_s{
	char datastr[5];		//原始数据
	int data;				//解析数据，单位：mm
};

#endif

rw_uart.c

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
#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;

	/*初始化数据结构体 */
	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;
}

测试发布的主题—订阅主题

  测试可以随便一个启动的app中进行主题订阅，然后将订阅的数据打印出来，看是否是超声波的数据。这里测试是在固件的src/examples文件夹中的px4_simple_app应用进行测试的。

  • 将px4_simple_app应用添加到NuttShell中。Firmware/makefiles/nuttx/config_px4fmu-v2_default.mk文件中添加如下内容：
    • MODULES += examples/px4_simple_app
  • 在px4_simple_app.c中代码内容：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
#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));
	/*设置以一秒钟接收一次，并打印出数据*/
	orb_set_interval(sonar_sub_fd, 1000);
	bool updated;
	struct rw_uart_sonar_data_s sonar;

	/*接收数据方式一：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");
	}
	*/
	/*接收数据方式一：end*/

	/*接收数据方式二：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) {}
			 */
		}
	}
	/*接收数据方式二：end*/

	return 0;
}

  • 编译并刷固件

    • make upload px4fmu-v2_default

  • 在NSH中测试(已加入自启动脚本中)

    • rw_uart start /dev/ttyS2
    • px4_simple_app

  这里写图片描述

加入系统启动脚本

  可以加入到光流的自定义启动脚本中：/fs/microsd/etc/extras.txt。这样随着系统的自启动，rw_uart就会默认启动了。

1
2
# start sonar
rw_uart start /dev/ttyS2