373 lines
10 KiB
C
373 lines
10 KiB
C
#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);
|