154 lines
4.0 KiB
C
154 lines
4.0 KiB
C
#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:
|
|
protocol.analysis_sussess_count += msp_recv_buf(&protocol.msp, data, size);
|
|
break;
|
|
case PROTOCOL_MAVLINK:
|
|
protocol.analysis_sussess_count += mavlink_recv_buf(&protocol.mavlink_device, data, size);
|
|
break;
|
|
}
|
|
}
|
|
|
|
protocol_status_t get_protocol_status(void)
|
|
{
|
|
protocol.protocol_status = PROTOCOL_STATUS_OK;
|
|
|
|
if(protocol.pro_type == PROTOCOL_IDLE)
|
|
{
|
|
protocol.protocol_status = PROTOCOL_STATUS_TYPE_IDLE;
|
|
} else if(protocol.analysis_sussess_count == 0)
|
|
{
|
|
protocol.protocol_status = PROTOCOL_STATUS_NO_DATA;
|
|
}
|
|
|
|
protocol.analysis_sussess_count = 0;
|
|
|
|
return protocol.protocol_status;
|
|
} |