歡迎交流~ 個人 Gitter 交流平臺，點選直達：

本部落格承接前一篇，對FreeApe的串列埠新增超聲波感測器博文後半部分進行學習。

為什麼叫前奏呢，因為用了偽感測器，把微控制器用串列埠傳送的有規律的資料當作了感測器讀取到的資料。但是無礙的。

開發環境：Ubuntu Firmware 1.4.1

在無人機執行時，首先是要將應用隨系統啟動時就啟動起來的，且將獲得的超聲波資料不斷的釋出出去，從而讓其他應用得以訂閱使用。這裡也使用Pixhawk裡面的通用模式，即主執行緒，檢測app命令輸入，建立一個執行緒來不斷的釋出資料。

定義主題和釋出主題

• 在Firmware/msg
  目錄下新建read_uart_sensor.msg檔案。傳送門

char[4] datastr
int16 data

#TOPICS read_uart_sensor

並新增到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;
}

• 新增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中註冊該模組

測試釋出的主題

• 測試可以隨便一個啟動的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;
}

• 編譯並上傳韌體

  • 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