Pixhawk---通过串口方式添加一个自定义传感器(超声波为例)



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

1 说明

  首先超声波模块是通过串口方式发送(Tx)出数据,使用的模块数据发送周期为100ms,数据格式为:

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

则可以通过Pixhawk板上的串口来接收(Rx)数据,即将超声波的Tx接口连接到Pixhawk板上的Rx接口。
  Pixhawk板上串口说明:
  Pixhawk---通过串口方式添加一个自定义传感器(超声波为例)_第1张图片
  测试使用Pixhawk板上TELEM2接口的USART2,对应的Nuttx UART设备文件尾/dev/ttyS2
  Pixhawk---通过串口方式添加一个自定义传感器(超声波为例)_第2张图片

2 读取数据测试

  步骤:

  • Firmware/src/modules中添加一个新的文件夹,命名为rw_uart
  • rw_uart文件夹中创建module.mk文件,并输入以下内容:
    • MODULE_COMMAND = rw_uart
    • SRCS = rw_uart.c
  • rw_uart文件夹中创建rw_uart.c文件
  • 注册新添加的应用到NuttShell中。
  • ardupilot/mk/PX4/px4_common.mk
  • 文件中添加如下内容:
    • MODULES += modules/rw_uart
    • rw_uart.c
#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdbool.h>
#include <errno.h>
#include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <fcntl.h>
#include <poll.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 px4fmu-v2_upload
  • 查看app

    • 在NSH终端中输入help,在Builtin Apps中出现rw_uart应用。
  • 运行rw_uart应用(前提是模块与Pixhawk连接好)
    • 在NSH终端中输入rw_uart,回车,查看超声波的打印数据。

3 发布超声波的数据

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

3.1 定义主题和发布主题

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

rw_uart_sonar_topic.h


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

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

  • px4_simple_app应用添加到NuttShell中。文件中添加如下内容:
    • MODULES += examples/px4_simple_app
  • px4_simple_app.c中代码内容:

rw_uart_sonar_topic.h
#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
#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;
}
3.2 测试发布的主题—订阅主题
  测试可以随便一个启动的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中代码内容:
#include <px4_config.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>

#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include "rw_uart/rw_uart_app_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 sensor_combined topic */
 //int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
 int sonar_sub_fd = orb_subscribe(ORB_ID(rw_uart_sonar));
 //orb_set_interval(sensor_sub_fd, 1000);
 orb_set_interval(sonar_sub_fd, 1000);
      bool updated;
 /* advertise attitude topic */
 //struct vehicle_attitude_s att;
      //struct rw_uart_sonar_dara_s sonar;
 //memset(&att, 0, sizeof(att));
 //orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

 /* one could wait for multiple topics with this technique, just using one here */
 //struct pollfd 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 < 5; i++) {
  /* 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 sensor_combined_s raw;
    /* copy sensors raw data into local buffer */
   /* orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
    printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
           (double)raw.accelerometer_m_s2[0],
           (double)raw.accelerometer_m_s2[1],
           (double)raw.accelerometer_m_s2[2]);
             */
    /* set att and publish this information for other apps */
    /*att.roll = raw.accelerometer_m_s2[0];
    att.pitch = raw.accelerometer_m_s2[1];
    att.yaw = raw.accelerometer_m_s2[2];
    orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
   }*/

   /* there could be more file descriptors here, in the form like:
    * if (fds[1..n].revents & POLLIN) {}
    */
 // }
// }

 //return 0;
//}
      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++) {
              /* 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  px4fmu-v2_upload
  • 在NSH中测试(已加入自启动脚本中)

    • rw_uart start /dev/ttyS2
    • px4_simple_app

  Pixhawk---通过串口方式添加一个自定义传感器(超声波为例)_第3张图片

3.3 加入系统启动脚本

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

rw_uart start /de/ttys2
    





你可能感兴趣的:(传感器)