添加协议解析处理
This commit is contained in:
OPTOC
137
app/drivers/sertrf/protocol/p_protocol.c
Normal file
137
app/drivers/sertrf/protocol/p_protocol.c
Normal file
@@ -0,0 +1,137 @@
|
||||
#include "p_protocol.h"
|
||||
|
||||
protocol_t protocol;
|
||||
|
||||
int Protocol_read(void *data, uint32_t size, uint32_t wait_ms)
|
||||
{
|
||||
switch (protocol.port_type)
|
||||
{
|
||||
case PORT_LINUX_UART:
|
||||
break;
|
||||
case PORT_LINUX_SBDATA:
|
||||
return sb_data_port_read(protocol.port.data_port, data, size, 0);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
int Protocol_write(const void *data, uint32_t size, uint32_t wait_ms)
|
||||
{
|
||||
switch (protocol.port_type)
|
||||
{
|
||||
case PORT_LINUX_UART:
|
||||
break;
|
||||
case PORT_LINUX_SBDATA:
|
||||
return sb_data_port_write(protocol.port.data_port, data, size, 0);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void Protocol_init(port_type_m port_type, void* port)
|
||||
{
|
||||
protocol.port.data_port = port;
|
||||
switch (port_type)
|
||||
{
|
||||
case PORT_LINUX_UART:
|
||||
protocol.port.uart_fd = (int*)port;
|
||||
break;
|
||||
case PORT_LINUX_SBDATA:
|
||||
protocol.port.data_port = (sb_data_port_t*)port;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
protocol.msp.read = protocol.mavlink_device.read = Protocol_read;
|
||||
protocol.msp.write = protocol.mavlink_device.write = Protocol_write;
|
||||
|
||||
msp_init(&protocol.msp);
|
||||
mavlink_init(&protocol.mavlink_device);
|
||||
|
||||
}
|
||||
int Protocol_search()
|
||||
{
|
||||
uint8_t c;
|
||||
while(Protocol_read(&c,1,0))
|
||||
{
|
||||
// MAVLINK 解析
|
||||
mavlink_message_t message;
|
||||
if(mavlink_message(&protocol.mavlink_device, &message,c)){
|
||||
// printf("Received message from serial with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid);
|
||||
mavlik_packet_processing(&protocol.mavlink_device, &message);
|
||||
return PROTOCOL_MAVLINK;
|
||||
}
|
||||
// MSP 解析
|
||||
if (msp_parse_received_data(&protocol.msp, c))
|
||||
{
|
||||
if (protocol.msp.c_state == MSP_COMMAND_RECEIVED)
|
||||
{
|
||||
msp_process_received_command(&protocol.msp);
|
||||
return PROTOCOL_MSP;
|
||||
}
|
||||
}
|
||||
}
|
||||
return PROTOCOL_IDLE;
|
||||
}
|
||||
|
||||
void Protocol_loop(void* arg)
|
||||
{
|
||||
switch(protocol.pro_type)
|
||||
{
|
||||
case PROTOCOL_IDLE:
|
||||
protocol.pro_type = Protocol_search();
|
||||
break;
|
||||
case PROTOCOL_MSP:
|
||||
msp_recv_loop(&protocol.msp);
|
||||
break;
|
||||
case PROTOCOL_MAVLINK:
|
||||
mavlink_recv_loop(&protocol.mavlink_device);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int Protocol_buf_search(void* data, uint32_t size)
|
||||
{
|
||||
uint32_t size_lat = size;
|
||||
uint8_t* data_char = (uint8_t*)data;
|
||||
while (size_lat --)
|
||||
{
|
||||
uint8_t c = data_char[size - size_lat - 1];
|
||||
// MAVLINK 解析
|
||||
mavlink_message_t message;
|
||||
if(mavlink_message(&protocol.mavlink_device, &message,c)){
|
||||
mavlik_packet_processing(&protocol.mavlink_device, &message);
|
||||
return PROTOCOL_MAVLINK;
|
||||
}
|
||||
// MSP 解析
|
||||
if (msp_parse_received_data(&protocol.msp, c))
|
||||
{
|
||||
if (protocol.msp.c_state == MSP_COMMAND_RECEIVED)
|
||||
{
|
||||
msp_process_received_command(&protocol.msp);
|
||||
return PROTOCOL_MSP;
|
||||
}
|
||||
}
|
||||
}
|
||||
// printf("search %c %c\r\n", data_char[0], data_char[1]);
|
||||
return PROTOCOL_IDLE;
|
||||
}
|
||||
void Protocol_buf_decode(void* data, uint32_t size)
|
||||
{
|
||||
switch(protocol.pro_type)
|
||||
{
|
||||
case PROTOCOL_IDLE:
|
||||
protocol.pro_type = Protocol_buf_search(data, size);
|
||||
break;
|
||||
case PROTOCOL_MSP:
|
||||
msp_recv_buf(&protocol.msp, data, size);
|
||||
break;
|
||||
case PROTOCOL_MAVLINK:
|
||||
mavlink_recv_buf(&protocol.mavlink_device, data, size);
|
||||
break;
|
||||
}
|
||||
}
|
||||
69
app/drivers/sertrf/protocol/p_protocol.h
Normal file
69
app/drivers/sertrf/protocol/p_protocol.h
Normal file
@@ -0,0 +1,69 @@
|
||||
#pragma once
|
||||
|
||||
#ifndef PROJECT_H
|
||||
#define PROJECT_H
|
||||
|
||||
#include "../device.h"
|
||||
#include "mavlink_control.h"
|
||||
#include "MSP.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int* uart_fd;
|
||||
sb_data_port_t* data_port;
|
||||
}protocol_port_t;
|
||||
typedef enum
|
||||
{
|
||||
PROTOCOL_IDLE,
|
||||
PROTOCOL_MSP,
|
||||
PROTOCOL_MAVLINK,
|
||||
}protocol_type_m;
|
||||
typedef enum
|
||||
{
|
||||
PORT_LINUX_UART = 0,
|
||||
PORT_LINUX_SBDATA,
|
||||
}port_type_m;
|
||||
typedef struct
|
||||
{
|
||||
protocol_port_t port;
|
||||
protocol_type_m pro_type;
|
||||
port_type_m port_type;
|
||||
|
||||
mavlink_device_t mavlink_device;
|
||||
msp_port_t msp;
|
||||
|
||||
}protocol_t;
|
||||
|
||||
/**
|
||||
* @brief 初始化协议前端
|
||||
*
|
||||
* @param port_type 使用的设备类型（如串口或其他）
|
||||
* @param port 由对应的驱动提供的绑定接口获得的句柄
|
||||
*/
|
||||
void Protocol_init(port_type_m port_type, void* port);
|
||||
/**
|
||||
* @brief 搜索协议
|
||||
*
|
||||
* @retval 协议类型
|
||||
*/
|
||||
int Protocol_search();
|
||||
/**
|
||||
* @brief 使用串口进行数据获取的主运行函数（针对需要使用串口读取）
|
||||
*/
|
||||
void Protocol_loop(void* arg);
|
||||
/**
|
||||
* @brief 搜索协议
|
||||
*
|
||||
* @param data 数据缓存
|
||||
* @param size 数据长度
|
||||
* @retval 协议类型
|
||||
*/
|
||||
int Protocol_buf_search(void* data, uint32_t size);
|
||||
/**
|
||||
* @brief 解析数据 （针对已获取到数据缓存）
|
||||
*
|
||||
* @param data 数据缓存
|
||||
* @param size 数据长度
|
||||
*/
|
||||
void Protocol_buf_decode(void* data, uint32_t size);
|
||||
#endif
|
||||
Reference in New Issue
Block a user