From 6f67d8d773b8a2d77cfd8ac93c7349dd1a3968ee Mon Sep 17 00:00:00 2001 From: OPTOC <9159397+optoc@user.noreply.gitee.com> Date: Tue, 2 Sep 2025 14:20:13 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0MSP=E6=B6=88=E6=81=AF?= =?UTF-8?q?=E8=A7=A3=E6=9E=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/drivers/sertrf/protocol/MSP.c | 486 ++++++++++++++++++++++++++++++ app/drivers/sertrf/protocol/MSP.h | 372 +++++++++++++++++++++++ 2 files changed, 858 insertions(+) create mode 100644 app/drivers/sertrf/protocol/MSP.c create mode 100644 app/drivers/sertrf/protocol/MSP.h diff --git a/app/drivers/sertrf/protocol/MSP.c b/app/drivers/sertrf/protocol/MSP.c new file mode 100644 index 0000000..817bf82 --- /dev/null +++ b/app/drivers/sertrf/protocol/MSP.c @@ -0,0 +1,486 @@ +#include "MSP.h" + +#include +#include + + +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_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; + } + } + } +} +void msp_recv_buf(msp_port_t *msp, 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]; + if (msp_parse_received_data(msp, c)) + { + if (msp->c_state == MSP_COMMAND_RECEIVED) + { + msp_process_received_command(msp); + // break; + } + } + } +} diff --git a/app/drivers/sertrf/protocol/MSP.h b/app/drivers/sertrf/protocol/MSP.h new file mode 100644 index 0000000..e45a207 --- /dev/null +++ b/app/drivers/sertrf/protocol/MSP.h @@ -0,0 +1,372 @@ +#pragma once + +#include "stdint.h" +#include "stdbool.h" + +#define MSP_API_VERSION 1 +#define MSP_FC_VARIANT 2 +#define MSP_FC_VERSION 3 +#define MSP_BOARD_INFO 4 +#define MSP_BUILD_INFO 5 +#define MSP_NAME 10 // out message Returns user set board name - betaflight +#define MSP_CALIBRATION_DATA 14 +#define MSP_FEATURE 36 +#define MSP_BOARD_ALIGNMENT 38 +#define MSP_CURRENT_METER_CONFIG 40 +#define MSP_RX_CONFIG 44 +#define MSP_SONAR_ALTITUDE 58 +#define MSP_ARMING_CONFIG 61 +#define MSP_RX_MAP 64 // get channel map (also returns number of channels total) +#define MSP_LOOP_TIME 73 // FC cycle time i.e looptime parameter +#define MSP_STATUS 101 +#define MSP_RAW_IMU 102 +#define MSP_SERVO 103 +#define MSP_MOTOR 104 +#define MSP_RC 105 +#define MSP_RAW_GPS 106 +#define MSP_COMP_GPS 107 // distance home, direction home +#define MSP_ATTITUDE 108 +#define MSP_ALTITUDE 109 +#define MSP_ANALOG 110 +#define MSP_RC_TUNING 111 // rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID +#define MSP_PID 112 // P I D coeff +#define MSP_MISC 114 +#define MSP_SERVO_CONFIGURATIONS 120 +#define MSP_NAV_STATUS 121 // navigation status +#define MSP_SENSOR_ALIGNMENT 126 // orientation of acc,gyro,mag + +#define MSP_FC_DATA 148 // 请求飞控信息 + +#define MSP_STATUS_EX 150 +#define MSP_SENSOR_STATUS 151 +#define MSP_BOXIDS 119 +#define MSP_UID 160 // Unique device ID +#define MSP_GPSSVINFO 164 // get Signal Strength (only U-Blox) +#define MSP_GPSSTATISTICS 166 // get GPS debugging data +#define MSP_SET_PID 202 // set P I D coeff + +// commands +#define MSP_SET_HEAD 211 // define a new heading hold direction +#define MSP_SET_RAW_RC 200 // 8 rc chan +#define MSP_SET_RAW_GPS 201 // fix, numsat, lat, lon, alt, speed +#define MSP_SET_WP 209 // sets a given WP (WP#, lat, lon, alt, flags) + +// radar commands +#define MSP_SET_RADAR_POS 248 // SET radar position information +#define MSP_SET_RADAR_ITD 249 // SET radar information to display + +// v2 commands +#define MSP2_ESP32 0x2040 + +#define MSP2_COMMON_SET_RADAR_POS 0x100B // SET radar position information +#define MSP2_COMMON_SET_RADAR_GCS 0x7537 // SET radar position information +#define MSP2_COMMON_SET_RADAR_ITD 0x100C // SET radar information to display + +#define MSP2_SENSOR_GPS 0x1F03 // INAV expects this, instead of MSP_SET_RAW_GPS + +#define MSP_GCS_DATA 0x7578 // 接收地面站信息 + +#define MSP_PORT_OUTBUF_SIZE 512 +#define MSP_V2_FRAME_ID 255 +#define MSP_PORT_INBUF_SIZE 192 +#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0])) +#define MSP_ARRAYEND(x) (&(x)[ARRAY_SIZE(x)]) +#define MSP_UNUSED(x) (void)(x) + +// 接收数据类型 +typedef enum +{ + MSP_V1 = 0, + MSP_V2_OVER_V1 = 1, + MSP_V2_NATIVE = 2, + MSP_VERSION_COUNT +} msp_version_e; +// 接收状态 +typedef enum +{ + MSP_IDLE, + MSP_HEADER_START, + MSP_HEADER_M, + MSP_HEADER_X, + + MSP_HEADER_V1, + MSP_PAYLOAD_V1, + MSP_CHECKSUM_V1, + + MSP_HEADER_V2_OVER_V1, + MSP_PAYLOAD_V2_OVER_V1, + MSP_CHECKSUM_V2_OVER_V1, + + MSP_HEADER_V2_NATIVE, + MSP_PAYLOAD_V2_NATIVE, + MSP_CHECKSUM_V2_NATIVE, + + MSP_COMMAND_RECEIVED +} msp_state_e; +// return positive for ACK, negative on error, zero for no reply +typedef enum +{ + MSP_RESULT_ACK = 1, + MSP_RESULT_ERROR = -1, + MSP_RESULT_NO_REPLY = 0 +} MSPCommandResult; + +// V1接收消息头部与数据大小 +typedef struct __attribute__((packed)) +{ + uint8_t size; + uint8_t cmd; +} msp_header_v1_t; +// V2接收消息头部与数据大小与命令标志 +typedef struct __attribute__((packed)) +{ + uint8_t flags; + uint16_t cmd; + uint16_t size; +} msp_header_v2_t; + +typedef union +{ + int uart_fd; +}msp_device_t; +typedef struct msp_port_s +{ + msp_device_t* device; + msp_state_e c_state; + uint8_t msp_packet_type; + uint8_t in_buf[MSP_PORT_INBUF_SIZE]; + uint_fast16_t offset; + uint_fast16_t data_size; + msp_version_e msp_version; + uint8_t cmd_flags; + uint16_t cmd_msp; + uint8_t checksum1; + uint8_t checksum2; + + int (*read)(void *data, uint32_t size, uint32_t wait_ms); + int (*write)(const void *data, uint32_t size, uint32_t wait_ms); +} msp_port_t; +// 解析成功后接收到的数据内容 +typedef struct sbuf_s +{ + uint8_t *ptr; // data pointer must be first (sbuff_t* is equivalent to uint8_t **) + uint8_t *end; +} sbuf_t; +// 接收到的数据包 +typedef struct msp_packet_s +{ + sbuf_t buf; + int16_t cmd; + uint8_t flags; + int16_t result; + uint8_t msp_packet_type; +} msp_packet_t; + +// MSP_FC_VARIANT 接收到的消息包内容 +typedef struct msp_apm_data_s +{ + uint8_t variant[4]; +} msp_apm_data_t; + +/**************数据类型结构体*****************/ +// MSP_RAW_GPS reply +typedef struct __attribute__((packed)) +{ + uint8_t fixType; // MSP_GPS_NO_FIX, MSP_GPS_FIX_2D, MSP_GPS_FIX_3D + uint8_t numSat; + int32_t lat; // 1 / 10000000 deg + int32_t lon; // 1 / 10000000 deg + int16_t alt; // meters + int16_t groundSpeed; // cm/s + int16_t groundCourse; // unit: degree x 10 + uint16_t hdop; +} msp_raw_gps_t; + +typedef struct __attribute__((packed)) +{ + uint8_t sysid; + int8_t _off_alt; + int8_t _interval; + uint8_t from_mode; // 队列信息 + uint8_t filt_factor; // 防止超调的阻尼系数 + uint8_t _min_alt; // 最低高度 + int8_t Open_follow; // 是否切入guided模式 + int8_t armed; // 是否解锁 + int16_t curr_nav_roll_cd; // 期望翻滚角 + float throttle_input; // 油门杆量 + float yaw_input; // 偏航杆量 +} msp_fc_t; + +typedef struct __attribute__((packed)) +{ + uint8_t vbat; // 0...255 + uint16_t mAhDrawn; // milliamp hours drawn from battery + uint16_t rssi; // 0..1023 + int16_t amperage; // send amperage in 0.01 A steps, range is -320A to 320A +} msp_analog_t; + +typedef struct __attribute__((packed)) +{ + uint8_t target_id; // 要修改那个ID雷达 + uint8_t change_id; // 要修改为什么ID + int8_t off_alt; // 要修改什么高度差 + int8_t _interval; // 要修改什么水平差 + uint8_t from_mode; // 要修改什么队形 + uint32_t Formation_char; // 队形表 + uint8_t test[3]; +} msp_gcs_t; + +typedef struct __attribute__((packed)) +{ + uint8_t id; + uint8_t state; // disarmed(0) armed (1) + int32_t lat; // decimal degrees latitude * 10000000 + int32_t lon; // decimal degrees longitude * 10000000 + int32_t alt; // cm + uint16_t heading; // deg + uint16_t speed; // cm/s + int16_t target_nav_roll_cd; + uint8_t lq; // lq + + uint8_t sys_id; // 目标ID + uint8_t curr_id; // 雷达ID + uint16_t time_cycle; // 雷达数据接收间隔 + uint32_t lost_pack; // 丢包数 + + uint8_t gcs_target_id; // 地面站发送需要修改的雷达号 + uint8_t form_mode; // 队形模式 + int8_t off_alt; // 修改高度差 + int8_t interval; // 队形水平间距 + + // 控制所需数据 + int32_t lat_form; + int32_t lon_form; + int32_t alt_form; + int32_t target_yaw_gps; + int32_t curr_yaw_gps; + float target_distance_control_error; // 目标距离误差 + int32_t target_sight_yaw; // 目标基于视线的航向 + int32_t target_bearing_yaw; // 目标前置角 + int32_t target_clip_yaw; // 目标夹角 + uint8_t error_num_type; // 错误码 +} msp_radar_pos_t; + +typedef struct __attribute__((packed)) +{ + uint8_t id; + uint8_t state; // disarmed(0) armed (1) + int32_t lat; // decimal degrees latitude * 10000000 + int32_t lon; // decimal degrees longitude * 10000000 + int32_t alt; // cm + uint16_t heading; // deg + uint16_t speed; // cm/s + uint8_t lq; // lq + + uint8_t sys_id; // 目标ID + uint16_t time_cycle; // 雷达数据接收间隔 + uint32_t lost_pack; // 丢包数 + int8_t off_alt; // 高度差 + int8_t interval; // 队形水平间距 + uint8_t form_mode; // 队形模式 + uint8_t vbat; // 目标飞控电压值 + int8_t Open_follow; // 是否跟随 + int8_t armed; // 是否解锁 +} msp_radar_gcs_t; + +extern msp_port_t msp; + +/** + * @brief MSP初始化 + * + * @param msp 接口 + */ +void msp_init(msp_port_t *msp); +// MSP消息解析包 +/** + * @brief MSP消息解析包 + * + * @param crc 原先CRC值 + * @param a 需要添加校验对象 + * @return uint8_t + */ +uint8_t crc8_dvb_s2_apm(uint8_t crc, unsigned char a); +/** + * @brief MSPV1消息发送包 + * + * @param messageID 消息ID + * @param payload 消息内容 + * @param size 消息长度 + */ +void msp_send(msp_port_t *msp, uint8_t messageID, void *payload, uint8_t size); +/** + * @brief MSPV2消息发送包 + * + * @param messageID 消息ID + * @param payload 消息内容 + * @param size 消息长度 + */ +void msp_send2(msp_port_t *msp, uint16_t messageID, void *payload, uint16_t size); +/** + * @brief MSP数据接收处理 + * + * @param msp 接口 + */ +void msp_recv_loop(msp_port_t *msp); +/** + * @brief MSP数据接收处理(data赋值方式) + * + * @param msp 接口 + * @param data 接收数据 + * @param size 接收数据长度 + */ +void msp_recv_buf(msp_port_t *msp,void *data, uint32_t size); +/** + * @brief MSP数据包解析 + * + * @param msp 接口 + * @param c 数据 + * @return true + * @return false + */ +bool msp_parse_received_data(msp_port_t *msp, uint8_t c); +/** + * @brief MSP封装数据为命令数据和传输数据 + * + * @param msp 接口 + */ +void msp_process_received_command(msp_port_t *msp); +/** + * @brief MSP数据包处理，根据命令速度或者传输数据调用对应的处理 + * + * @param cmd 命令数据 + * @param reply 传输数据 + */ +MSPCommandResult msp_process_command(msp_packet_t *cmd, msp_packet_t *reply); +/** + * @brief MSP传输数据处理 + * + * @param cmd_msp 命令数据 + * @param src 传输数据 + */ +MSPCommandResult msp_process_sensor_command(uint16_t cmd_msp, sbuf_t *src); +/** + * @brief MSP命令数据处理 + * + * @param cmd_msp 命令数据 + * @param dst 传输数据 + */ +MSPCommandResult msp_process_out_command(uint16_t cmd_msp, sbuf_t *dst); +// 接收数据包处理 +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); +// 回复请求包处理 +void fc_put_ack(void); + + +// 接口层 +int MSP_wirite(const void *data, uint32_t size, uint32_t wait_ms); +int MSP_read(void *data, uint32_t size, uint32_t wait_ms); +uint32_t MSP_get_rx_length(void);