Pixhawk原生固件PX4之串口添加读取传感器实现

本博客承接前一篇，对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中前４个数字到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