From 9f79709f6210aa50cc73f1a0bfb0904e613e20b4 Mon Sep 17 00:00:00 2001 From: OPTOC <9159397+optoc@user.noreply.gitee.com> Date: Tue, 2 Sep 2025 14:20:28 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=8D=8F=E8=AE=AE=E8=A7=A3?= =?UTF-8?q?=E6=9E=90=E5=A4=84=E7=90=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/drivers/sertrf/protocol/p_protocol.c | 137 +++++++++++++++++++++++ app/drivers/sertrf/protocol/p_protocol.h | 69 ++++++++++++ 2 files changed, 206 insertions(+) create mode 100644 app/drivers/sertrf/protocol/p_protocol.c create mode 100644 app/drivers/sertrf/protocol/p_protocol.h diff --git a/app/drivers/sertrf/protocol/p_protocol.c b/app/drivers/sertrf/protocol/p_protocol.c new file mode 100644 index 0000000..ff017f4 --- /dev/null +++ b/app/drivers/sertrf/protocol/p_protocol.c @@ -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; + } +} \ No newline at end of file diff --git a/app/drivers/sertrf/protocol/p_protocol.h b/app/drivers/sertrf/protocol/p_protocol.h new file mode 100644 index 0000000..6625908 --- /dev/null +++ b/app/drivers/sertrf/protocol/p_protocol.h @@ -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 \ No newline at end of file