本博客承接前一篇,对FreeApe的串口添加超声波传感器博文后半部分进行学习。
为什么叫前奏呢,因为用了伪传感器,把单片机用串口发送的有规律的数据当作了传感器读取到的数据。但是无碍的。
开发环境:Ubuntu Firmware 1.4.1
在无人机运行时,首先是要将应用随系统启动时就启动起来的,且将获得的超声波数据不断的发布出去,从而让其他应用得以订阅使用。这里也使用Pixhawk里面的通用模式,即主线程,检测app命令输入,创建一个线程来不断的发布数据。
定义主题和发布主题
- 在
Firmware/msg
目录下新建read_uart_sensor.msg
文件。传送门
char[4] datastr
int16 data
#TOPICS read_uart_sensor
- 1
- 2
- 3
- 4
- 5
并添加到CMakeLists.txt中,编译后自动生成uORB/topics/read_uart_sensor.h
头文件
串口读取例程
在Firmware/src/modules
目录下新建文件夹read_uart_sensor
- 添加文件
read_uart_sensor.c
。传送门
/*
* read_uart_sensor.c
*
* read sensor through uart
*/
#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 <systemlib/systemlib.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <uORB/topics/read_uart_sensor.h>
__EXPORT int read_uart_sensor_main(int argc, char *argv[]);
static bool thread_should_exit = false; /*Ddemon exit flag*/
static bool thread_running = false; /*Daemon status flag*/
static int daemon_task;
/**
* Main loop
*/
int read_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: read_uart_sensor {start|stop|status} [param]\n\n");
exit(1);
}
int read_uart_sensor_main(int argc, char *argv[])
{
if (argc < 2) {
usage("[Fantasy]missing command");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("[Fantasy]already running\n");
return 0;
}
thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("read_uart_sensor",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
read_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("[Fantasy]running");
return 0;
} else {
warnx("[Fantasy]stopped");
return 1;
}
return 0;
}
usage("unrecognized command");
return 1;
}
int read_uart_thread_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "[Fantasy]need a serial port name as argument");
usage("eg:");
}
const char *uart_name = argv[1];
warnx("[Fantasy]opening port %s", uart_name);
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(uart_name);
if(false == uart_read)return -1;
if(false == set_uart_baudrate(uart_read,9600)){
printf("[Fantasy]set_uart_baudrate is failed\n");
return -1;
}
printf("[Fantasy]uart init is successful\n");
thread_running = true;
/*初始化数据结构体 */
struct read_uart_sensor_s sensordata;
memset(&sensordata, 0, sizeof(sensordata));
/* 公告主题 */
orb_advert_t read_uart_sensor_pub = orb_advertise(ORB_ID(read_uart_sensor), &sensordata);
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(sensordata.datastr,buffer,4);// 复制字符串Buffer中前4个数字到Datastr中
sensordata.data = atoi(sensordata.datastr);//将字符串转换成整形数据
//printf("[Fantasy]sensor.data=%d\n",sensordata.data);
orb_publish(ORB_ID(read_uart_sensor), read_uart_sensor_pub, &sensordata);
}
}
warnx("[Fantasy]exiting");
thread_running = false;
close(uart_read);
fflush(stdout);
return 0;
}
- 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
- 198
- 199
- 200
- 201
- 202
- 203
- 204
- 205
- 206
- 207
- 添加
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 :
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 在
Firmware/cmake/configs/nuttx/nuttx_px4fmu-v2_default.cmake
中注册该模块
测试发布的主题
- 测试可以随便一个启动的app中进行主题订阅,然后将订阅的数据打印出来,看是否是超声波的数据。这里新建一个应用px4_test进行测试。
px4_test.c
/*
* px4_test.c
*
* test the uart sensor app
*/
#include <px4_config.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/read_uart_sensor.h>
__EXPORT int px4_test_main(int argc, char *argv[]);
int px4_test_main(int argc, char *argv[])
{
PX4_INFO("Hello Sky!\n");
/* subscribe to rw_uart_sensor topic */
int sensor_sub_fd = orb_subscribe(ORB_ID(read_uart_sensor));
/*limit the update rate to 20 Hz */
orb_set_interval(sensor_sub_fd, 50);
/* one could wait for multiple topics with this technique, just using one here */
px4_pollfd_struct_t fds[] = {
{ .fd = sensor_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++) { // infinite loop
/* 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_test] 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_test] 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 read_uart_sensor_s raw;
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(read_uart_sensor), sensor_sub_fd, &raw);
PX4_INFO("[px4_test] sensor data:\t%s\t%d\n",
raw.datastr,
raw.data);
}
/* there could be more file descriptors here, in the form like:
* if (fds[1..n].revents & POLLIN) {}
*/
}
}
PX4_INFO("exiting");
return 0;
}
- 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
-
编译并上传固件
- make px4fmu-v2_default upload
-
在NSH中测试
- read_uart_sensor start /dev/ttyS2
- px4_test
这个大有搞头!
添加到脚本文件
在rcS中仿照mavlink的启动方式添加了上面的应用,使得与ttyS2连接的外设默认为启动状态
现在如果ttyS2上连接了设备,就是自动启动的,在nsh中可以直接调用px4_test
应用就可以读取数据了,与使用px4_simple_app读取内部传感器的方式无异。
大致模型就是这样了,接下来就是具体的代码优化以及应用了。
By Fantasy