6005.boost多线程与mavlink协议结合实现消息收发

boost多线程与mavlink协议结合实现消息收发

 本文将实现boost库创建多线程,利用mavlink协议进行数据链消息打包、解包,解放了很多数据解析的工作,不得不佩服
 mavlink协议功能的强大。

boost创建多线程

/*****************************************************************************************************************
 * 文件名称:main.cpp
 * 描述: 本文旨在创建一个数据链标准程序.以mavlink协议v2版本为基准,实现数据链层面的通信。
 * 作者:xhome
 * 时间:2020/1/15
 ****************************************************************************************************************/


#include "datalink_base.h"

using namespace std;
using namespace boost;

config_t config;

int main() {
    std::cout << "mavlink_datalink start ok." << std::endl;

    //定义线程组,管理线程
    boost::thread_group  threadGroup;

    //串口设备名称.
    config.uart_name = "/dev/ttyUSB0";
    config.baud = 1;

    threadGroup.create_thread(thread_send_msg);
    threadGroup.create_thread(thread_recv_msg);

    //阻塞等待.
    threadGroup.join_all();

    return 0;
}

头文件定义

/**********************************************************************************************************************
 * datalink_base.h
 * 描述:基本全局头文件,存储全局变量申明等
 * 作者:xhome
 * 时间:2020/1/15
 *********************************************************************************************************************/

#ifndef MAVLINK_DATALINK_DATALINK_BASE_H
#define MAVLINK_DATALINK_DATALINK_BASE_H

#include <iostream>
#include <string>
#include <boost/thread.hpp>
#include <boost/thread/lock_factories.hpp>
#include <boost/thread/condition.hpp>


using namespace std;

//定义配置文件
typedef struct config_ {
    string uart_name; //串口名称.
    int baud; //波特率选择值.
}config_t;

//配置文件.
extern config_t config;

//消息收发线程.
extern  void thread_recv_msg(void);
extern  void thread_send_msg(void);

#endif //MAVLINK_DATALINK_DATALINK_BASE_H

串口类定义

/********************************************************************************************************
 * SerialPort.hpp 自定义出串口类,运行平台linux操作系统.
 * 描述:实现串口的初始化.
 * 作者:xhome
 * 时间:2020/1/10
 *******************************************************************************************************/

#ifndef MAVLINK_DATALINK_SERIALPORT_HPP
#define MAVLINK_DATALINK_SERIALPORT_HPP

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <stdio.h>
#include <string>
#include <iostream>
#include "mavlink.h"

using namespace std;

//串口操作类.
class SerialPort {

public:
    SerialPort(string uart_name, speed_t baudrate){
            uartName = uart_name;
            speedBaud = baudrate;
    }

public:
    /**
     * 功能: 打开设备文件
     * 参数: 无
     * 返回值: 文件描述符.
     * 作者:xhome
     * 时间:2020/1/10
     */
    int open_serial_dev() {
        int fd_ = 0;
        if(uartName.length() == 0){
            cout << "devName is NULL." << endl;
            return -1;
        }
        fd_ = open(uartName.c_str(), O_RDWR);
        if(fd_ < 0){
            fd = fd_;
            cout << uartName << " is opened failed." << endl;
            return -1;
        }else{
            fd = fd_;
            cout << uartName << " is opened ok." << endl;
        }

        return fd_;
    }
    int  close_serial()//关闭串口设备文件.
    {
        close(fd);
    }

    /**
     * 功能: 串口初始化
     * 参数: fd  文件描述符
     * 作者:  xhome
     * 时间:  2020/1/10
     */
    void serial_init() //串口初始化.
    {
        if(fd < 0){
            cout << "serial_init failed ." << endl;
            return;
        }

        struct termios options;
        tcgetattr(fd, &options);                //读取终端参数
        options.c_cflag |= ( CLOCAL | CREAD );  //忽略调试解调器线路状态,使用接受器
        options.c_cflag &= ~CSIZE;              //清目前字符长度
        options.c_cflag &= ~CRTSCTS;            //不实用RTS/CTS控制流
        options.c_cflag |= CS8;                 //字符长度设置为8
        options.c_cflag &= ~CSTOPB;             //设置一个标志位
        options.c_iflag |= IGNPAR;              //允许输入奇偶校验
        options.c_iflag &= ~(ICRNL | IXON);     //回车不转换为换行,不允许输入时对XON/XOFF>
        options.c_oflag = 0;
        options.c_lflag = 0;

        options.c_cflag |= CBAUDEX; //设置特定波特率的标志位.

        cfsetispeed(&options, speedBaud);         //设置波特率为115200
        cfsetospeed(&options, speedBaud);
        tcsetattr(fd,TCSANOW,&options);         //设置终端参数

        cout << "serial_init ok." << endl;

    }
    /**
    * 读取串口消息.
    * 参数:message  mavlink 标准消息结构体.
    * 作者:xhome
    * 时间:2020/1/15
    */
    int  read_message(mavlink_message_t &message){
        int result = 0;
        uint8_t ch;
        mavlink_status_t status;
        uint8_t msgReceived = false;

        result = read(fd, &ch, 1);
        if(result > 0){
            msgReceived = mavlink_parse_char(MAVLINK_COMM_0, ch, &message, &status);
            // check for dropped packets
            if ( (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) )
            {
                printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count);
                unsigned char v= ch;
                fprintf(stderr,"%02x ", v);
            }
            lastStatus = status;
        }

        return msgReceived;
    }

    /**
     * 发送消息到串口.
     * 参数:buffer  发送的消息
     *      len     消息长度
     * 返回值:成功0, 出错-1
     */
    int send_msg_to_uart(uint8_t * buffer, uint16_t len){
        if(len < 0 || len > 512){
            return -1;
        }

        for(int i = 0; i < len; i++){
            write(fd, &buffer[i], 1);
        }

        return 0;
    }

public:
    string  uartName; //设备名称
    speed_t speedBaud; //设备波特率
    int fd; //文件描述符.
    mavlink_status_t lastStatus;

};

#endif //MAVLINK_EXAMPLE_SERIALPORT_HPP

数据链发送线程

/************************************************************************************************
 * thread_recv_msg.cpp
 * 描述:消息接收线程,从串口获取消息
 * 作者:xhome
 * 时间:2020/1/15
 ***********************************************************************************************/
#include "datalink_base.h"
#include "SerialPort.hpp"
#include "mavlink.h"

using namespace std;
using namespace boost;


void thread_send_msg(void){

    bool success;               // receive success flag
    bool received_all = false;  // receive only one message
 //   Time_Stamps this_timestamps;
    mavlink_message_t msg ;
    mavlink_mission_count_t  mission_count; //mavlink定义任务数量消息
    uint8_t buffer[512] = {};
    uint16_t nbytes = 0;

    uint8_t system_id = 10;
    uint8_t component_id = 100;

    std::cout << "thread_send_msg start ok." << endl;
    //串口初始化.
    SerialPort serialPort(config.uart_name, B115200);

    //打开串口.
    serialPort.open_serial_dev();
    //串口初始化.
    serialPort.serial_init();

    mission_count.target_component = 0;

    while(1){

        mission_count.count = 10;
        mission_count.target_system = 2;
        mission_count.target_component++;
        mission_count.mission_type = 3;
        
        mavlink_msg_mission_count_encode(system_id, component_id, &msg, &mission_count);
        nbytes = mavlink_msg_to_send_buffer(buffer, &msg);

        //串口发送消息.
        serialPort.send_msg_to_uart(buffer, nbytes);

        cout << "send msg frame ok." << endl;

        sleep(1);
    }

}


数据链接受线程

/************************************************************************************************
 * thread_recv_msg.cpp
 * 描述:消息接收线程,从串口获取消息
 * 作者:xhome
 * 时间:2020/1/15
 ***********************************************************************************************/
#include "datalink_base.h"
#include "SerialPort.hpp"
#include "mavlink.h"
#include <boost/lexical_cast.hpp>


using namespace std;
using namespace boost;

void read_messages(SerialPort * serial_port);

void thread_recv_msg(void){

    std::cout << "thread_recv_msg start ok." << endl;

    bool success;               // receive success flag
    bool received_all = false;  // receive only one message
    // Time_Stamps this_timestamps;

    std::cout << "thread_send_msg start ok." << endl;
    //串口初始化.
    SerialPort serialPort(config.uart_name, B115200);

    //打开串口.
    serialPort.open_serial_dev();
    //串口初始化.
    serialPort.serial_init();

    while(1){

        read_messages(&serialPort);
    }
}


void read_messages(SerialPort * serial_port){

    bool success = false;
    bool received_all = false;
    int sysid;
    int compid;
    mavlink_heartbeat_t heartbeat;
    mavlink_mission_count_t missionCount;

    // Blocking wait for new data
    while ( !received_all) {
        mavlink_message_t message;
        success = serial_port->read_message(message);

        if (success) {

            // Store message sysid and compid.
            // Note this doesn't handle multiple message sources.
            sysid = message.sysid;
            compid = message.compid;

            cout << "sysid:" << sysid << " compid:" << compid << endl;

            // Handle Message ID
            switch (message.msgid) {
                    case MAVLINK_MSG_ID_HEARTBEAT:
                        //printf("MAVLINK_MSG_ID_HEARTBEAT\n");
                        mavlink_msg_heartbeat_decode(&message, &heartbeat);
                        //   heartbeat = get_time_usec();
                        break;
                    case MAVLINK_MSG_ID_MISSION_COUNT:
                        mavlink_msg_mission_count_decode(&message, &missionCount);

                        cout << "mission_type:" << boost::lexical_cast<std::string>((int)missionCount.mission_type)
                                << " count:" << boost::lexical_cast<std::string>(missionCount.count)
                                        << " target_system:" <<  boost::lexical_cast<std::string>((int)missionCount.target_system)
                                                << " target_component:" << boost::lexical_cast<std::string>((int)missionCount.target_component) << endl;
                        break;
            }
        }
    }

    return;
}

实验结果

在这里插入图片描述

发布了46 篇原创文章 · 获赞 12 · 访问量 2万+
展开阅读全文

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: 编程工作室 设计师: CSDN官方博客

分享到微信朋友圈

×

扫一扫,手机浏览