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;
}

實驗結果