497 lines
12 KiB
C
497 lines
12 KiB
C
#include "MSP.h"
|
|
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
|
|
|
|
void msp_init(msp_port_t *msp)
|
|
{
|
|
|
|
}
|
|
|
|
uint8_t crc8_dvb_s2_apm(uint8_t crc, unsigned char a)
|
|
{
|
|
crc ^= a;
|
|
for (int ii = 0; ii < 8; ++ii)
|
|
{
|
|
if (crc & 0x80)
|
|
{
|
|
crc = (crc << 1) ^ 0xD5;
|
|
}
|
|
else
|
|
{
|
|
crc = crc << 1;
|
|
}
|
|
}
|
|
return crc;
|
|
}
|
|
void msp_send(msp_port_t *msp, uint8_t messageID, void *payload, uint8_t size)
|
|
{
|
|
uint8_t msp_data_packet[6 + size];
|
|
msp_data_packet[0] = '$';
|
|
msp_data_packet[1] = 'M';
|
|
msp_data_packet[2] = '<';
|
|
msp_data_packet[3] = size;
|
|
msp_data_packet[4] = messageID;
|
|
|
|
uint8_t checksum = size ^ messageID;
|
|
uint8_t *payloadPtr = (uint8_t *)payload;
|
|
for (int i = 0; i < size; i++)
|
|
{
|
|
uint8_t b = *(payloadPtr++);
|
|
checksum ^= b;
|
|
msp_data_packet[5 + i] = b;
|
|
}
|
|
msp_data_packet[6 + size - 1] = checksum;
|
|
msp->write(&msp_data_packet[0], 6 + size, 10);
|
|
}
|
|
void msp_send2(msp_port_t *msp, uint16_t messageID, void *payload, uint16_t size)
|
|
{
|
|
uint8_t _crc = 0;
|
|
uint8_t msp_data_packet[size + 9];
|
|
msp_data_packet[0] = '$';
|
|
msp_data_packet[1] = 'X';
|
|
msp_data_packet[2] = '<';
|
|
msp_data_packet[3] = 0; // flag
|
|
msp_data_packet[4] = messageID; // function
|
|
msp_data_packet[5] = messageID >> 8;
|
|
msp_data_packet[6] = size; // payload size
|
|
msp_data_packet[7] = size >> 8;
|
|
for (uint8_t i = 3; i < 8; i++)
|
|
{
|
|
_crc = crc8_dvb_s2_apm(_crc, msp_data_packet[i]);
|
|
}
|
|
// Start of Payload
|
|
uint8_t *payloadPtr = (uint8_t *)payload;
|
|
for (uint16_t i = 0; i < size; ++i)
|
|
{
|
|
msp_data_packet[i + 8] = *(payloadPtr++);
|
|
_crc = crc8_dvb_s2_apm(_crc, msp_data_packet[i + 8]);
|
|
}
|
|
msp_data_packet[size + 8] = _crc;
|
|
|
|
msp->write(&msp_data_packet[0], 9 + size, 10);
|
|
}
|
|
// 解析接收数据是否正确
|
|
bool msp_parse_received_data(msp_port_t *msp, uint8_t c)
|
|
{
|
|
switch (msp->c_state)
|
|
{
|
|
default:
|
|
case MSP_IDLE: // Waiting for '$' character
|
|
if (c == '$')
|
|
{
|
|
msp->msp_version = MSP_V1;
|
|
msp->c_state = MSP_HEADER_START;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
break;
|
|
|
|
case MSP_HEADER_START: // Waiting for 'M' (MSPv1 / MSPv2_over_v1) or 'X' (MSPv2 native)
|
|
switch (c)
|
|
{
|
|
case 'M':
|
|
msp->c_state = MSP_HEADER_M;
|
|
break;
|
|
case 'X':
|
|
msp->c_state = MSP_HEADER_X;
|
|
break;
|
|
default:
|
|
msp->c_state = MSP_IDLE;
|
|
break;
|
|
}
|
|
break;
|
|
|
|
case MSP_HEADER_M: // Waiting for '<'
|
|
if (c == '<' || c == '>')
|
|
{
|
|
msp->offset = 0;
|
|
msp->checksum1 = 0;
|
|
msp->checksum2 = 0;
|
|
msp->c_state = MSP_HEADER_V1;
|
|
}
|
|
else
|
|
{
|
|
msp->c_state = MSP_IDLE;
|
|
}
|
|
break;
|
|
|
|
case MSP_HEADER_X:
|
|
if (c == '<' || c == '>')
|
|
{
|
|
msp->offset = 0;
|
|
msp->checksum2 = 0;
|
|
msp->msp_version = MSP_V2_NATIVE;
|
|
msp->c_state = MSP_HEADER_V2_NATIVE;
|
|
}
|
|
else
|
|
{
|
|
msp->c_state = MSP_IDLE;
|
|
}
|
|
break;
|
|
|
|
case MSP_HEADER_V1: // Now receive v1 header (size/cmd), this is already checksummable
|
|
msp->in_buf[msp->offset++] = c;
|
|
msp->checksum1 ^= c;
|
|
if (msp->offset == sizeof(msp_header_v1_t))
|
|
{
|
|
msp_header_v1_t *hdr = (msp_header_v1_t *)&msp->in_buf[0];
|
|
// Check incoming buffer size limit
|
|
if (hdr->size > MSP_PORT_INBUF_SIZE)
|
|
{
|
|
msp->c_state = MSP_IDLE;
|
|
}
|
|
else if (hdr->cmd == MSP_V2_FRAME_ID)
|
|
{
|
|
// MSPv1 payload must be big enough to hold V2 header + extra checksum
|
|
if (hdr->size >= sizeof(msp_header_v2_t) + 1)
|
|
{
|
|
msp->msp_version = MSP_V2_OVER_V1;
|
|
msp->c_state = MSP_HEADER_V2_OVER_V1;
|
|
}
|
|
else
|
|
{
|
|
msp->c_state = MSP_IDLE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
msp->data_size = hdr->size;
|
|
msp->cmd_msp = hdr->cmd;
|
|
msp->cmd_flags = 0;
|
|
msp->offset = 0; // re-use buffer
|
|
msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1; // If no payload - jump to checksum byte
|
|
msp->msp_packet_type = msp->data_size > 0 ? 1 : 2;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case MSP_PAYLOAD_V1:
|
|
msp->in_buf[msp->offset++] = c;
|
|
msp->checksum1 ^= c;
|
|
if (msp->offset == msp->data_size)
|
|
{
|
|
msp->c_state = MSP_CHECKSUM_V1;
|
|
}
|
|
break;
|
|
|
|
case MSP_CHECKSUM_V1:
|
|
if (msp->checksum1 == c)
|
|
{
|
|
msp->c_state = MSP_COMMAND_RECEIVED;
|
|
}
|
|
else
|
|
{
|
|
msp->c_state = MSP_IDLE;
|
|
}
|
|
break;
|
|
|
|
case MSP_HEADER_V2_OVER_V1: // V2 header is part of V1 payload - we need to calculate both checksums now
|
|
msp->in_buf[msp->offset++] = c;
|
|
msp->checksum1 ^= c;
|
|
msp->checksum2 = crc8_dvb_s2_apm(msp->checksum2, c);
|
|
if (msp->offset == (sizeof(msp_header_v2_t) + sizeof(msp_header_v1_t)))
|
|
{
|
|
msp_header_v2_t *hdrv2 = (msp_header_v2_t *)&msp->in_buf[sizeof(msp_header_v1_t)];
|
|
msp->data_size = hdrv2->size;
|
|
|
|
// Check for potential buffer overflow
|
|
if (hdrv2->size > MSP_PORT_INBUF_SIZE)
|
|
{
|
|
msp->c_state = MSP_IDLE;
|
|
}
|
|
else
|
|
{
|
|
msp->cmd_msp = hdrv2->cmd;
|
|
msp->cmd_flags = hdrv2->flags;
|
|
msp->offset = 0; // re-use buffer
|
|
msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1;
|
|
msp->msp_packet_type = msp->data_size > 0 ? 1 : 2;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case MSP_PAYLOAD_V2_OVER_V1:
|
|
msp->checksum2 = crc8_dvb_s2_apm(msp->checksum2, c);
|
|
msp->checksum1 ^= c;
|
|
msp->in_buf[msp->offset++] = c;
|
|
|
|
if (msp->offset == msp->data_size)
|
|
{
|
|
msp->c_state = MSP_CHECKSUM_V2_OVER_V1;
|
|
}
|
|
break;
|
|
|
|
case MSP_CHECKSUM_V2_OVER_V1:
|
|
msp->checksum1 ^= c;
|
|
if (msp->checksum2 == c)
|
|
{
|
|
msp->c_state = MSP_CHECKSUM_V1; // Checksum 2 correct - verify v1 checksum
|
|
}
|
|
else
|
|
{
|
|
msp->c_state = MSP_IDLE;
|
|
}
|
|
break;
|
|
|
|
case MSP_HEADER_V2_NATIVE:
|
|
msp->in_buf[msp->offset++] = c;
|
|
msp->checksum2 = crc8_dvb_s2_apm(msp->checksum2, c);
|
|
if (msp->offset == sizeof(msp_header_v2_t))
|
|
{
|
|
msp_header_v2_t *hdrv2 = (msp_header_v2_t *)&msp->in_buf[0];
|
|
|
|
// Check for potential buffer overflow
|
|
if (hdrv2->size > MSP_PORT_INBUF_SIZE)
|
|
{
|
|
msp->c_state = MSP_IDLE;
|
|
}
|
|
else
|
|
{
|
|
msp->data_size = hdrv2->size;
|
|
msp->cmd_msp = hdrv2->cmd;
|
|
msp->cmd_flags = hdrv2->flags;
|
|
msp->offset = 0; // re-use buffer
|
|
msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE;
|
|
msp->msp_packet_type = msp->data_size > 0 ? 1 : 2;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case MSP_PAYLOAD_V2_NATIVE:
|
|
msp->checksum2 = crc8_dvb_s2_apm(msp->checksum2, c);
|
|
msp->in_buf[msp->offset++] = c;
|
|
|
|
if (msp->offset == msp->data_size)
|
|
{
|
|
msp->c_state = MSP_CHECKSUM_V2_NATIVE;
|
|
}
|
|
break;
|
|
|
|
case MSP_CHECKSUM_V2_NATIVE:
|
|
if (msp->checksum2 == c)
|
|
{
|
|
msp->c_state = MSP_COMMAND_RECEIVED;
|
|
}
|
|
else
|
|
{
|
|
msp->c_state = MSP_IDLE;
|
|
}
|
|
break;
|
|
}
|
|
return true;
|
|
}
|
|
// MSP解析成功还会进行数据处理
|
|
void msp_process_received_command(msp_port_t *msp)
|
|
{
|
|
uint8_t out_buf[MSP_PORT_OUTBUF_SIZE];
|
|
|
|
msp_packet_t reply = {
|
|
.buf = {
|
|
.ptr = out_buf,
|
|
.end = MSP_ARRAYEND(out_buf),
|
|
},
|
|
.cmd = -1,
|
|
.flags = 0,
|
|
.result = 0,
|
|
.msp_packet_type = msp->msp_packet_type,
|
|
};
|
|
|
|
// uint8_t *out_buf_head = reply.buf.ptr;
|
|
|
|
msp_packet_t command = {
|
|
.buf = {
|
|
.ptr = msp->in_buf,
|
|
.end = msp->in_buf + msp->data_size,
|
|
},
|
|
.cmd = (int16_t)msp->cmd_msp,
|
|
.flags = msp->cmd_flags,
|
|
.result = 0,
|
|
.msp_packet_type = msp->msp_packet_type,
|
|
};
|
|
|
|
const MSPCommandResult status = msp_process_command(&command, &reply);
|
|
|
|
if (status != MSP_RESULT_NO_REPLY)
|
|
{
|
|
// sbuf_switch_to_reader(&reply.buf, out_buf_head); // change streambuf direction
|
|
// msp_serial_encode(msp, &reply, msp->msp_version);
|
|
}
|
|
|
|
msp->c_state = MSP_IDLE;
|
|
}
|
|
MSPCommandResult msp_process_command(msp_packet_t *cmd, msp_packet_t *reply)
|
|
{
|
|
MSPCommandResult ret = MSP_RESULT_ACK;
|
|
sbuf_t *dst = &reply->buf;
|
|
sbuf_t *src = &cmd->buf;
|
|
const uint16_t cmd_msp = cmd->cmd;
|
|
// initialize reply by default
|
|
reply->cmd = cmd->cmd;
|
|
// 收到有数据包
|
|
if (cmd->msp_packet_type == 1)
|
|
{
|
|
ret = msp_process_sensor_command(cmd_msp, src);
|
|
}
|
|
else if (cmd->msp_packet_type == 2)
|
|
{ // 收到无数据包
|
|
ret = msp_process_out_command(cmd_msp, dst);
|
|
}
|
|
|
|
// Process DONT_REPLY flag
|
|
if (cmd->flags & 0x1)
|
|
{
|
|
ret = MSP_RESULT_NO_REPLY;
|
|
}
|
|
|
|
reply->result = ret;
|
|
return ret;
|
|
}
|
|
|
|
// MSP解析数据处理
|
|
MSPCommandResult msp_process_sensor_command(uint16_t cmd_msp, sbuf_t *src)
|
|
{
|
|
MSP_UNUSED(src);
|
|
switch (cmd_msp)
|
|
{
|
|
case 1:
|
|
{
|
|
}
|
|
break;
|
|
case MSP_FC_VARIANT:
|
|
{
|
|
const msp_apm_data_t *pkt = (const msp_apm_data_t *)src->ptr;
|
|
msp_handle_fc(*pkt);
|
|
}
|
|
break;
|
|
case MSP_GCS_DATA:
|
|
{
|
|
const msp_gcs_t *pkt = (const msp_gcs_t *)src->ptr;
|
|
msp_handle_get_gcs(*pkt);
|
|
}
|
|
break;
|
|
case MSP_FC_DATA:
|
|
{
|
|
const msp_fc_t *pkt = (const msp_fc_t *)src->ptr;
|
|
msp_handle_get_fc(*pkt);
|
|
}
|
|
break;
|
|
case MSP_RAW_GPS:
|
|
{
|
|
const msp_raw_gps_t *pkt = (const msp_raw_gps_t *)src->ptr;
|
|
msp_handle_get_gps(*pkt);
|
|
}
|
|
break;
|
|
case MSP_ANALOG:
|
|
{
|
|
const msp_analog_t *pkt = (const msp_analog_t *)src->ptr;
|
|
msp_handle_get_analog(*pkt);
|
|
}
|
|
break;
|
|
case MSP_ATTITUDE:
|
|
{
|
|
// const msp_attitude_t *pkt = (const msp_attitude_t *)src->ptr;
|
|
// msp_handle_get_attitude(*pkt);
|
|
printf("ATTITUDE\n");
|
|
}
|
|
break;
|
|
}
|
|
|
|
|
|
return MSP_RESULT_NO_REPLY;
|
|
}
|
|
// 接收数据包处理
|
|
void msp_handle_fc(msp_apm_data_t pkt)
|
|
{
|
|
|
|
}
|
|
void msp_handle_get_gps(msp_raw_gps_t pkt)
|
|
{
|
|
|
|
}
|
|
void msp_handle_get_fc(msp_fc_t pkt)
|
|
{
|
|
}
|
|
void msp_handle_get_analog(msp_analog_t pkt)
|
|
{
|
|
|
|
}
|
|
void msp_handle_get_gcs(msp_gcs_t pkt)
|
|
{
|
|
|
|
}
|
|
// MSP回复请求包处理
|
|
MSPCommandResult msp_process_out_command(uint16_t cmd_msp, sbuf_t *dst)
|
|
{
|
|
switch (cmd_msp)
|
|
{
|
|
case MSP_FC_VARIANT:
|
|
{
|
|
fc_put_ack();
|
|
return MSP_RESULT_ACK;
|
|
}
|
|
break;
|
|
default:
|
|
// MSP always requires an ACK even for unsupported messages
|
|
return MSP_RESULT_ACK;
|
|
}
|
|
}
|
|
|
|
// 请求回复包
|
|
void fc_put_ack(void)
|
|
{
|
|
// uint8_t data[5] = {'T', 'X', 'S', 'T', 0};
|
|
// msp_send(2, &data[0], 5);
|
|
// printf("ACK\n");
|
|
}
|
|
|
|
// MSP重启命令
|
|
void msp_send_reboot(msp_port_t *msp)
|
|
{
|
|
msp_send2(msp, MSP_REBOOT, NULL, 0);
|
|
}
|
|
void msp_request_type(msp_port_t *msp)
|
|
{
|
|
msp_send2(msp, MSP_FC_VARIANT, NULL, 0);
|
|
}
|
|
// MSP数据读取,并根据读取的数据进行处理
|
|
void msp_recv_loop(msp_port_t *msp)
|
|
{
|
|
uint8_t c;
|
|
|
|
while (msp->read(&c, 1, 0) > 0)
|
|
{
|
|
if (msp_parse_received_data(msp, c))
|
|
{
|
|
if (msp->c_state == MSP_COMMAND_RECEIVED)
|
|
{
|
|
msp_process_received_command(msp);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
int msp_recv_buf(msp_port_t *msp, void *data, uint32_t size)
|
|
{
|
|
uint8_t res = 0;
|
|
uint32_t size_lat = size;
|
|
uint8_t* data_char = (uint8_t*)data;
|
|
|
|
while (size_lat --)
|
|
{
|
|
uint8_t c = data_char[size - size_lat - 1];
|
|
if (msp_parse_received_data(msp, c))
|
|
{
|
|
if (msp->c_state == MSP_COMMAND_RECEIVED)
|
|
{
|
|
msp_process_received_command(msp);
|
|
res ++;
|
|
}
|
|
}
|
|
}
|
|
return res;
|
|
}
|