Pixhawk與樹莓派3的串口通信

新建主題

• msg文件夾下新建mytopic.msg文件

      char[4] datastr0  //字符串的寫法 存放發送過來的字符串
      uint8 data//將字符串轉換成整型   
  

• 在msg文件夾中的cmkaelist文件中加入
  

新建pi_uart模塊

• 在src目錄的modules文件夾下新建pi_uart文件夾
• 在pi_uart文件夾下新建pi_uart.c
• 實現的功能：
  
  • 開機自啓動
  • 持續接收串口數據（’R’開頭的字符串）
  • 發送串口數據
  • 將接收到的數據更新發布到mytopic主題中

#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdbool.h>
#include <errno.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/mytopic.h>
#include <uORB/uORB.h>

#include <px4_config.h>
#include <px4_getopt.h>

#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <assert.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <math.h>

#include <nuttx/sched.h>

#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>


#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/mcu_version.h>
#include <systemlib/git_version.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <dataman/dataman.h>


static bool thread_should_exit = false;
static bool thread_running = false;
static int daemon_task;


__EXPORT int pi_uart_main(int argc, char *argv[]);
int pi_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 pi_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("pi_uart",
                         SCHED_DEFAULT,
                         SCHED_PRIORITY_DEFAULT,
                         2000,
                         pi_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 pi_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 mytopic_s test_data; //定義類型爲mytopic_s的結構體變量rd

   orb_advert_t mytopic_pub = orb_advertise(ORB_ID(mytopic), &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(mytopic), mytopic_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;
}

• 新建CMakeList.txt文件

px4_add_module(
    MODULE modules__pi_uart
    MAIN pi_uart
    STACK_MAIN 2000
    SRCS
        pi_uart.c
    DEPENDS
        platforms__common
    )

• 在cmake/config/nuttx_px4fmu-v2_default.cmake中添加

modules/pi_uart

• 在ROMFS/px4fmu_common/rcS中添加

pi_uart start

• 編譯
• 燒寫

樹莓派3串口發送

• 樹莓派3環境配置另寫博文
• 循環發送‘R1100’小程序

#include <stdio.h> 
#include <wiringPi.h>
#include <wiringSerial.h> 
int main() 
{ 
int fd; 
char data[5]=“R1100”; 
int flag=1;
if(wiringPiSetup()<0)return 1; 
if((fd=serialOpen("/dev/ttyS0",9600))<0) return 1;
printf("serial test start ...\n");
serialPrintf(fd,"Hello world!\n"); 
while(flag) 
    { 
      serialPrintf(fd,data);//向串口設備發送data數據 
      delay(300);
        while(serialDataAvail(fd)) 
          { 
          printf("->%3d\n",serialGetchar(fd));
          flag=0; fflush(stdout); 
          } 
     } 
serialFlush(fd); 
serialClose(fd); 
return 0;
 } 

Pixhawk與樹莓派3連接

TX–RX
RX–TX
GND–GND

• TELEM1和TELEM2都是串口，從左到右分別是VC TXD RXD 未知 未知 GND

測試結果

參考

Pixhawk—通過串口方式添加一個自定義傳感器（超聲波爲例） —— FreeApe