#include "gcs_vklink_v30.h"
#include "auto_pilot.h"
#include "control_attitude.h"
#include "control_rate.h"
#include "control_throttle.h"
#include "data_save.h"
#include "flight_mode.h"
#include "geomatry.h"
#include "helpler_funtions.h"
#include "math.h"
#include "my_math.h"
#include "params.h"
#include "payload.h"
#include "pilot_navigation.h"
#include "soft_can_yy.h"
#include "soft_flash.h"
#include "soft_gs.h"
#include "soft_imu.h"
#include "soft_motor_output.h"
#include "soft_port_uart4.h"
#include "soft_rc_input.h"
#include "soft_sdcard.h"
#include "soft_time.h"
#include "soft_voltage.h"
#include "string.h"
#include "um482.h"
#include "ver_config.h"
#include "vklink.h"
#include "mode_gcs_tax_launch_run.h"
#include "mode_attitude.h"
#include <stdio.h>

/* COMP ID 分配 */
enum
{
    COMPID_IMUBL = 201,
    COMPID_IMU = 1,
    COMPID_FMU = 0,

};

/* CTL 指令子 ID 分配 */
enum
{
    CTL_MSG_UNLOCK = 1,                      // 解锁
    CTL_MSG_TAKTOFF = 2,                     // 起飞
    CTL_MSG_FIRSTPOINT = 3,                  // 去一点
    CTL_MSG_BREAKPOINT = 4,                  // 断点续飞
    CTL_MSG_CRUISE = 5,                      // 开始巡航
    CTL_MSG_PAUSE = 6,                       // 暂停航线
    CTL_MSG_CONTINUE = 7,                    // 继续航线
    CTL_MSG_RTH = 8,                         // 返航
    CTL_MSG_LAND = 9,                        // 一键降落
    CTL_MSG_ALT_CHANGE = 10,                 // 一键变高
    CTL_MSG_WPT_CHANGE = 11,                 // 一键变点
    CTL_MSG_MANUL_PHOTO = 30,                // 手动拍照
    CTL_MSG_AUTO_YAW_LOCK = 46,              // 自动模式机头锁定
    CTL_MSG_AUTO_YAW_UNLOCK = 47,            // 自动模式机头取消锁定
    CTL_MSG_WP_CYCLE_TIME = 50,              // 循环圈数
    CTL_MSG_PUMPCLOSE = 51,                  // 关闭喷洒
    CTL_MSG_ENABLE_CONTINUOUSLY_PHOTO = 53,  // 开启连续拍照
    CTL_MSG_DISABLE_CONTINUOULSY_PHOTO = 54, // 关闭连续拍照
    CTL_MSG_ENGINE_CTRL = 56,                // 发动机控制
    CTL_MSG_VEHICLE_LAUNCH = 60,             // 车载起飞
    CTL_MSG_VEHICLE_FOLLOW = 61,             // 车载跟随
    CTL_MSG_VEHICLE_LAND = 62,               // 车载降落
    CTL_MSG_VEHICLE_FOLLOW_EXIT = 63,        // 结束车载跟随
    CTL_MSG_CANESC_CONF = 70,                // 电调配置指令
    CTL_MSG_AFC_CONTROL = 80,                // AFC 控制
    CTL_MSG_SELF_ROTATION = 82,              // 原地自旋
    CTL_MSG_ENABLE_TUNING = 90,              // 使能调式模式
    CTL_MSG_ALLSTOP = 100,                   // 紧急停转
    CTL_MSG_PAYLOAD_CTL = 101,               // 载荷遥控
    CTL_MSG_VINS_CTL = 110,                  // 视觉模块指令
    CTL_MSG_SET_SIMULATOR = 140,             // 仿真固件使能
};

/* CAL 指令子 ID 分配 */
enum
{
    CAL_MSG_ATT = 1,                // 姿态校准
    CAL_MSG_MAG = 2,                // 磁校准
    CAL_MSG_REMOTE_S = 3,           // 遥控器校准-开始
    CAL_MSG_REMOTE_E = 4,           // 遥控器校准-结束
    CAL_MSG_MOTOR = 5,              // 电机检测
    CAL_MSG_ASSEMBLY_DIRECTION = 6, // 装配方向
    CAL_MSG_BATTERY_VOLTAGE = 50    // 电压校准
};

#pragma pack(1)

struct global_pos_msg_data
{
    uint32_t time_stamp_us; /* 本地 us 时间戳 */
    int32_t slng;           /* 解算经度 */
    int32_t slat;           /* 解算纬度 */
    int32_t gps_alt_cm;     /* 海拔高度 */
    int32_t salt_cm;        /* 解算高度 */
    int16_t roll;           /* 横滚 0.1deg */
    int16_t pitch;          /* 俯仰 */
    int16_t yaw;            /* 航向 */
    int16_t evel;           /* 水平速度 cm/s */
    int16_t nvel;           /* 解算垂直速度 cm/s */
    int16_t uvel;           /* 天向垂直速度 cm/s */
    uint16_t airspeed;      /* 空速 cm/s */
};

struct bat_bms_msg_data
{
    char manufactory[8];                    /* 厂商代号 */
    char product[8];                        /* 产品型号 */
    uint16_t voltage;                       /* 电压 0.1 V */
    int16_t current;                        /* 电流 0.1 A, 正冲负放 */
    int16_t temperature;                    /* 温度 */
    uint8_t electricity_percentage;         /* 电量百分比 */
    uint8_t fault_status;                   /* 故障状态 */
    uint16_t number_of_circles;             /* 循环次数 */
    uint16_t capacity_total;                /* 电池设计容量 mAh */
    uint16_t capacity_remaining_percentage; /* 电池剩余容量 mAh */
    uint16_t reserve0;                      /* 预留 */
    uint16_t cell_volt[16];                 /* 电芯电压 */
    uint8_t user_data[32];                  /* 其它信息, 根据不同电池数据特定 */
};

struct can_servo_msg_data
{
    uint16_t manufactory;
    uint8_t link_status[8];
    uint8_t servo_pwmin[8];
    uint16_t error_status[8];
    uint16_t servo_rpm[8];
    uint16_t servo_voltin[8];
    uint16_t servo_currentout[8];
    int16_t servo_temperature[8];
};

struct gps_imu_pos_fix_msg_data
{
    int16_t gps_pos_fix[6];
    int16_t imu_pos_fix[3];
};

struct afc_ctl_msg_data
{
    uint8_t link_status;   /* 连接状态 0-无 1-正常 2-断开 */
    uint8_t ap_type;       /* 动力构型 1-旋翼 2-固定翼 */
    int16_t forward_speed; /* 前向速度指令 */
    int16_t right_speed;   /* 右向速度指令 */
    int16_t up_speed;      /* 上向速度指令 */
    int16_t tar_yaw;       /* 目标航向 0.1deg */
    int16_t tar_roll;      /* 目标横滚角 0.1deg */
    uint32_t reserve;
    int16_t tar_pitch; /* 目标俯仰角 0.1deg */
    uint8_t ctl_id1;   /* 收到的指令字1 */
    uint8_t ctl_id2;   /* 收到的指令字2 */
    int32_t tar_lon;   /* 目标位置经度 */
    int32_t tar_lat;   /* 目标位置纬度 */
    int32_t tar_alt;   /* 目标位置高度 */
};


struct vins_msg_data
{
    int32_t pos_lng;
    int32_t pos_lat;
    int32_t pos_alt;
    int16_t espeed_cms;
    int16_t nspeed_cms;
    int16_t uspeed_cms;
    int32_t pos_e;
    int32_t pos_n;
    int32_t pos_u;
    uint8_t flag_p;
    int32_t target_pos_e;
    int32_t target_pos_n;
    int32_t target_pos_u;
    uint16_t target_vel_e;
    uint16_t target_vel_n;
    uint16_t target_vel_u;
    uint16_t target_heading;
    uint8_t flag_t;
};

#pragma pack(0)

/**
 * @brief 初始化一个发送消息
 *
 * @param Msg
 * @param msgid
 */
static void
gcs_vklink_txmsg_init(VKlink_Msg_Type *txMsg, uint8_t msgid)
{
    txMsg->head = VKLINK_MSG_HEAD;
    txMsg->seq++;
    txMsg->sysid = verinf._sysid;
    txMsg->compid = COMPID_FMU;
    txMsg->msgid = msgid;
    txMsg->payload_len = 0;
    txMsg->crc16_check = 0;
}

void gcs_vklink_v300_send_global_pos_data(struct GCS_Link *pgcs)
{
    if (pgcs->_current_sysid != verinf._sysid)
    {
        return;
    }

    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;

    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_GLOBAL_POS_ID);
    struct global_pos_msg_data *data = (struct global_pos_msg_data *)txMsg->payload;
    txMsg->payload_len = sizeof(*data);

    data->time_stamp_us = micros();
    data->slng = pid_m_posx.loc_c;
    data->slat = pid_m_posy.loc_c;
    data->gps_alt_cm = ins.gps_alt;
    data->salt_cm = pid_m_alt.loc_c;
    data->roll = pid_m_roll.angle_c * 10;
    data->pitch = pid_m_pitch.angle_c * 10;
    data->yaw = pid_m_yaw.angle_c * 10;
    data->evel = pid_m_posx.vel_c * 0.1f;
    data->nvel = pid_m_posy.vel_c * 0.1f;
    data->uvel = pid_m_alt.vel_c * 0.1f;
    data->airspeed = 0;

    gs_tx_vklink_msg(pgcs, txMsg);
}

/**
 * @brief 发送遥测消息
 *
 */
void gcs_vklink_v300_send_telemetry_data(struct GCS_Link *pgcs)
{
    uint8_t tmp_uint8;
    int32_t tmp_int32;
    int16_t tmp_int16;
    uint32_t tmp_uint32;

    const struct yy_can_rx_msg_data *yy_can = yy_can_rx_data_get();
    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;

    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_TELEMETRY_ID);

    // 飞控 SN 号
    vklink_msg_payload_put_data(txMsg, &ver_par.serial_id, 4);
    // 航向角,0.1deg
    tmp_int16 = pid_m_yaw.angle_c * 10;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);
    // 横滚角,0.1deg
    tmp_int16 = pid_m_roll.angle_c * 10;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);
    // 俯仰角,0.1deg
    tmp_int16 = pid_m_pitch.angle_c * 10;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);
    // 解算高度,0.1m
    tmp_int32 = pid_m_alt.loc_c / 10;
    vklink_msg_payload_put_data(txMsg, &tmp_int32, 4);
    // 爬升速度,0.1m/s
    tmp_int16 = pid_m_alt.vel_c / 10;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);
    // 水平速度,0.1m/s
    tmp_int16 = ins.horz_vel / 10;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);
    // 空速,0.1m/s
    tmp_int16 = 0;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);
    // 解算经度
    tmp_int32 = _cxy.pos_integ[0] * 10.0f;// pid_m_posx.loc_c;
    vklink_msg_payload_put_data(txMsg, &tmp_int32, 4);
    // 解算纬度
    tmp_int32 = _cxy.pos_integ[1] * 10.0f;//pid_m_posy.loc_c;
    vklink_msg_payload_put_data(txMsg, &tmp_int32, 4);
    // GPS原始经度
    tmp_int32 = raw_gps._longitude;
    vklink_msg_payload_put_data(txMsg, &tmp_int32, 4);
    // GPS原始纬度
    tmp_int32 = raw_gps._latitude;
    vklink_msg_payload_put_data(txMsg, &tmp_int32, 4);
    // GPS高度,0.1m
    // tmp_int32 = ins.gps_alt / 10;
    if (!isnan(yy_can->txgl_alt_sp))
    {
        tmp_int32 = yy_can->txgl_alt_sp * 10;
    }
    else
    {
        tmp_int32 = 0;
    }
    vklink_msg_payload_put_data(txMsg, &tmp_int32, 4);
    // GPS航向,0.1deg
    // tmp_int16 = ins.gps_yaw * 10;
    tmp_int16 = yy_can->txgl_drone_cmd * 10;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);
    // GPS速度,0.1m/s
    // tmp_int16 = ins.gps_vel / 10;
    if (!isnan(yy_can->txgl_yaw_sp))
    {
        tmp_int16 = yy_can->txgl_yaw_sp * 10;
    }
    else
    {
        tmp_int16 = 0;
    }
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    tmp_uint8 = ins.gps_num;
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
    tmp_uint8 = ins.gps_pdop;
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
    vklink_msg_payload_put_data(txMsg, &ins.rtk_state, 1);
    vklink_msg_payload_put_data(txMsg, &ins.oscillating_coefficient, 1);

    // gps 时间
    tmp_int16 = ins.gps_year;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    vklink_msg_payload_put_data(txMsg, &ins.gps_month, 1);
    vklink_msg_payload_put_data(txMsg, &ins.gps_day, 1);
    vklink_msg_payload_put_data(txMsg, &ins.gps_hour, 1);
    vklink_msg_payload_put_data(txMsg, &ins.gps_minute, 1);
    vklink_msg_payload_put_data(txMsg, &ins.gps_second, 1);
    vklink_msg_payload_put_data(txMsg, &ins.gps_millisecond, 1);
    // 备用 GPS 星数
    raw_gps._redundant_gps_num = dist_comp[0] * 10;    
    vklink_msg_payload_put_data(txMsg, &raw_gps._redundant_gps_num, 1);
    // 双天线的从天线星数
    raw_gps._dgps_ant2_gps_num = dist_comp[1] * 10;
    vklink_msg_payload_put_data(txMsg, &raw_gps._dgps_ant2_gps_num, 1);

    // 离家距离，m
    tmp_int16 = status_broadcast;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);
    // 报警信息
    vklink_msg_payload_put_data(txMsg, &warn_reason, 1);
    // 返航信息
    tmp_uint8 = 0;
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    // 已飞时间，s
    tmp_int16 = (short)Get_HaveFlyTime();
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // 偏航距，0.1m
    tmp_int16 = 0;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // 目标距离，0.1m
    tmp_int16 = wp_curtotar_verdistance / 10;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // 目标高度 0.1m
    tmp_int16 = pid_m_alt.loc_t / 10;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // 总飞行里程
    tmp_uint32 = 0;
    vklink_msg_payload_put_data(txMsg, &tmp_uint32, 4);

    // 本架次飞行里程
    tmp_uint32 = 0;
    vklink_msg_payload_put_data(txMsg, &tmp_uint32, 4);

    // 剩余循环圈数
    if (wp_cycle_times >= wp_have_cycle_times)
    {
        tmp_int16 = wp_cycle_times - wp_have_cycle_times;
    }
    else
    {
        tmp_int16 = 0;
    }
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // 高度雷达障碍物距离
    tmp_int16 = 0;
    // tmp_int16 = payload_naroradar_get_raw();
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // 前雷达障碍距离
    tmp_int16 = 0;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // 后雷达障碍距离
    tmp_int16 = 0;
    tmp_int16 = (int16_t)(raw_gps._auxiliary_gps_lon - 80000);
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // 飞行模式
    vklink_msg_payload_put_data(txMsg, &flight_mode, 1);

    // 航点总数
    tmp_int16 = waypoint_totalnum;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // 目标航点序号
    tmp_int16 = tar_wp_no;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // 拍照数

    tmp_int16 = 0;
    if (yy_can->temp_count & 0x01)
    {
        tmp_int16 = 0;
    }
    else
    {
        tmp_int16 = 1;
    }
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // 飞控供电电压
    tmp_int16 = Voltage_GetVolt(Volt_MC_CH) * 10;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    vklink_msg_payload_put_data(txMsg, &thr_lock_status, 1);

    vklink_msg_payload_put_data(txMsg, &ins.temprature, 1);

    // 总飞行时间s
    tmp_uint32 = fly_log.total_flight_time;
    vklink_msg_payload_put_data(txMsg, &tmp_uint32, 4);

    // 解算标志, 低 4 位是组合解算标志, 高 4 位是相对位置锁定标志
    tmp_uint8 = (raw_gps._insgps_ok & 0x0F) + (raw_gps._vehicle_vector_flag << 4);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    // 电机平衡
    tmp_uint8 = 0;
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    // 安装方向
    vklink_msg_payload_put_data(txMsg, &raw_gps._imu_assembly_direction, 1);

    // 是否在禁飞区中
    tmp_uint8 = 0;
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    // 磁校准标志位
    vklink_msg_payload_put_data(txMsg, &raw_gps._state_flag, 1);

    // 飞机状态标志
    vklink_msg_payload_put_data(txMsg, &pilot_mode, 1);

    // 电压 ad0
    tmp_int16 = Voltage_GetVolt(Volt_AD0) * 10;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // 电压 ad1
    tmp_int16 = Voltage_GetVolt(Volt_AD1) * 10;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // 遥控输入 8 通道
    if (comp_rc_status == COMP_NOEXIST && rock_isenable != COMP_NOEXIST)
    {
        for (uint8_t i = 0; i < 4; ++i)
        {
            tmp_uint8 = rock_in[i] / 10;
            vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
        }
        tmp_uint8 = rock_key;
        vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
        tmp_uint8 = 0;
        vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
        vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
        vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
    }
    else
    {
        for (uint8_t i = 0; i < 8; i++)
        {
            tmp_uint8 = tmp_rc_in[i] / 10;
            vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
        }
    }

    // 电机输出 8 通道
    for (uint8_t motor = MOTOR1; motor <= MOTOR8; motor++)
    {
        tmp_uint8 = get_motor_pwm(motor) / 10;
        vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
    }

    // 基础油门
    tmp_int16 = pid_thr;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    // gps 气压计 加速度计 磁传感器 标志位
    vklink_msg_payload_put_data(txMsg, &raw_gps._gps_status, 1);
    vklink_msg_payload_put_data(txMsg, &raw_gps._gps_status2, 1);
    vklink_msg_payload_put_data(txMsg, &raw_gps._acc_gyro_status, 1);
    vklink_msg_payload_put_data(txMsg, &raw_gps._mag_status, 1);

    // 板卡输出相对右前上距离
    tmp_int32 = 0;

    vklink_msg_payload_put_data(txMsg, &tmp_int32, 4);

    vklink_msg_payload_put_data(txMsg, &tmp_int32, 4);

    vklink_msg_payload_put_data(txMsg, &tmp_int32, 4);

    vklink_msg_payload_put_data(txMsg, &tmp_int32, 4);

    vklink_msg_payload_put_data(txMsg, &tmp_int32, 4);

    vklink_msg_payload_put_data(txMsg, &tmp_int32, 4);

    tmp_int16 = 0;
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

    vklink_msg_payload_put_data(txMsg, &tmp_int16, 1);
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 1);
    vklink_msg_payload_put_data(txMsg, &tmp_int16, 1);

    /* 避障开关状态 */
    tmp_uint8 = 0;
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    // 是否允许解锁
    tmp_uint8 = judge_pilot_rcunlock_allow();
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
    tmp_uint8 = judge_pilot_gsunlock_allow();
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    // 将消息发送
    gs_tx_vklink_msg(pgcs, txMsg);
}

/**
 * @brief 发送载荷状态消息
 *
 */
void gcs_vklink_v300_send_payload_data(struct GCS_Link *pgcs)
{
    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    struct payload_diaocang *payload = port4_get_payload();

    if (payload->_link_status == COMP_NORMAL)
    {
        gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_PAYLOAD_DATA_ID);

        uint16_t laser_dist = payload->_status.laser_dist_m * 10;
        vklink_msg_payload_put_data(txMsg, &laser_dist, 2);

        int16_t tmp_int16 = payload->_status.roll * 10;
        vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);
        tmp_int16 = payload->_status.pitch * 10;
        vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);
        tmp_int16 = payload->_status.yaw * 10;
        vklink_msg_payload_put_data(txMsg, &tmp_int16, 2);

        uint8_t tmp_uint8 = payload->_status.zoom;
        vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
        tmp_uint8 = payload->_status.trace_flag;
        vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

        int32_t tmp_int = payload->_status.tar_lon;
        vklink_msg_payload_put_data(txMsg, &tmp_int, 4);
        tmp_int = payload->_status.tar_lat;
        vklink_msg_payload_put_data(txMsg, &tmp_int, 4);
        tmp_int = payload->_status.tar_alt;
        vklink_msg_payload_put_data(txMsg, &tmp_int, 4);

        gs_tx_vklink_msg(pgcs, txMsg);
    }
}



/**
 * @brief 发送电池 bms 数据信息
 */
void gcs_vklink_v300_send_bat_bms_msg(struct GCS_Link *pgcs)
{
    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    const VoltageBms *bms = Voltage_GetBmsInfo();

    if (bms->link_status == COMP_NORMAL)
    {
        gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_BAT_BMS_ID);

        struct bat_bms_msg_data *data = (struct bat_bms_msg_data *)txMsg->payload;
        memset(data, 0, sizeof(*data));
        txMsg->payload_len = (sizeof(*data));

        memcpy(data->manufactory, bms->manufactory, sizeof(data->manufactory));
        memcpy(data->product, bms->product, sizeof(data->product));
        data->voltage = bms->voltage;
        data->current = bms->current;
        data->temperature = bms->temperature;
        data->electricity_percentage = bms->electricity_percentage;
        data->number_of_circles = bms->number_of_circles;
        data->capacity_remaining_percentage = bms->capacity_remaining;
        data->reserve0 = 0;
        data->fault_status = bms->fault_status;
        memcpy(data->cell_volt, bms->cell_volt, sizeof(data->cell_volt));
        memcpy(data->user_data, bms->user_data, sizeof(data->user_data));

        gs_tx_vklink_msg(pgcs, txMsg);
    }
}


void gcs_vklink_v300_send_confinf_data(struct GCS_Link *pgcs, uint16_t arg)
{
    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_CONFINF_ID);

    /* 参数项序号 */
    uint16_t transport_confinf_num = arg;
    vklink_msg_payload_put_data(txMsg, &transport_confinf_num, 2);

    /* 参数项值  */
    int16_t value = params_get_value(transport_confinf_num);
    vklink_msg_payload_put_data(txMsg, &value, 2);

    gs_tx_vklink_msg(pgcs, txMsg);
}

void gcs_vklink_v300_send_confinf_total_data(struct GCS_Link *pgcs)
{
    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_CONFINF_TOTAL_ID);

    for (uint16_t i = ParamNum_APType; i < ParamNum_MaxNum; ++i)
    {
        uint16_t value = params_get_value(i);
        vklink_msg_payload_put_data(txMsg, &value, 2);
    }

    gs_tx_vklink_msg(pgcs, txMsg);
}

void gcs_vklink_v300_send_ack_data(struct GCS_Link *pgcs, const struct gcs_ack_arg *arg)
{
    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    ;
    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_ACK_ID);

    vklink_msg_payload_put_data(txMsg, &arg->ack_id, 1);

    for (uint8_t i = 0; i < 5; ++i)
    {
        vklink_msg_payload_put_data(txMsg, &arg->ack_content[i], sizeof(arg->ack_content[i]));
    }

    gs_tx_vklink_msg(pgcs, txMsg);
}

/**
 * @brief 发送机型参数消息
 *
 */
void gcs_vklink_v300_send_aptype_data(struct GCS_Link *pgcs)
{
    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_APTYPE_ID);

    uint16_t tmp_uint16;
    uint8_t tmp_uint8;

    /* 旋翼机型 */
    tmp_uint8 = params_get_value(ParamNum_APType);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
    /* 盘旋半径 */
    tmp_uint16 = params_get_value(ParamNum_APCircleRadiusM);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    /* 电台失联判定时间 */
    tmp_uint16 = params_get_value(ParamNum_RadioLinkLostTimeS);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    /* 一级低电压阈值和动作 */
    tmp_uint16 = params_get_value(ParamNum_LowVoltValue1);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    tmp_uint8 = params_get_value(ParamNum_LowVoltAction1);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
    /* 二级低电压阈值和动作 */
    tmp_uint16 = params_get_value(ParamNum_LowVoltValue2);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    tmp_uint8 = params_get_value(ParamNum_LowVoltAction2);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
    /* 自动起飞高度 */
    tmp_uint16 = params_get_value(ParamNum_APTakeoffAltM);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    /* 返航最低高度 */
    tmp_uint16 = params_get_value(ParamNum_APRthAltM);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    /* 磁偏角 */
    tmp_uint8 = params_get_value(ParamNum_MagDeclinationDeg);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
    /* imu 滤波参数 */
    tmp_uint8 = params_get_value(ParamNum_ImuFilterLever);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
    /* 旋翼电机怠速等级 */
    tmp_uint16 = params_get_value(ParamNum_APIdleSpeed);
    tmp_uint8 = (tmp_uint16 - 1050) / 50 + 1;
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
    /* 遥控器失控悬停时间 */
    tmp_uint16 = params_get_value(ParamNum_RcFailLoiterTimeS);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);

    gs_tx_vklink_msg(pgcs, txMsg);
}

/**
 * @brief 发送安装位置补偿
 *
 */
void gcs_vklink_v300_send_centerfix_data(struct GCS_Link *pgcs)
{
    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_CENTER_FIX_ID);

    struct gps_imu_pos_fix_msg_data *data = (struct gps_imu_pos_fix_msg_data *)txMsg->payload;
    txMsg->payload_len = sizeof(*data);

    for (uint8_t i = 0; i < 6; ++i)
    {
        data->gps_pos_fix[i] = ins.imu_conf.gps_pos_param[i];
    }

    for (uint8_t i = 0; i < 3; ++i)
    {
        data->imu_pos_fix[i] = ins.imu_conf.imu_pos_param[i];
    }

    gs_tx_vklink_msg(pgcs, txMsg);
}

/**
 * @brief 发送 pid 参数消息
 *
 */
void gcs_vklink_v300_send_pid_data(struct GCS_Link *pgcs)
{
    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_PID_ID);

    uint16_t tmp_uint16;
    uint8_t tmp_uint8;

    // 横滚角度，角速度 比例系数
    tmp_uint16 = params_get_value(ParamNum_RollAngleKp);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    tmp_uint16 = params_get_value(ParamNum_RollGyroKp);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    // 俯仰角度，角速度 比例系数
    tmp_uint16 = params_get_value(ParamNum_PitchAngleKp);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    tmp_uint16 = params_get_value(ParamNum_PitchGyroKp);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    // 航向角度, 角速度 比例系数
    tmp_uint16 = params_get_value(ParamNum_YawAngleKp);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    tmp_uint16 = params_get_value(ParamNum_YawGyroKp);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    // 垂直速度比例系数
    tmp_uint16 = params_get_value(ParamNum_AltholdSpeedKp);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    // 横滚 俯仰 航向 角速率微分系数
    tmp_uint8 = params_get_value(ParamNum_RollGyroKd);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
    tmp_uint8 = params_get_value(ParamNum_PitchGyroKd);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);
    tmp_uint8 = params_get_value(ParamNum_YawGyroKd);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    // 高度误差比例系数
    tmp_uint16 = params_get_value(ParamNum_AltholdDistKp);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);
    // 垂直加速度误差比例系数
    tmp_uint16 = params_get_value(ParamNum_AltholdAccKp);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);

    // 垂直加速度误差积分系数
    tmp_uint8 = params_get_value(ParamNum_AltholdAccKi);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    // 水平误差比例系数
    tmp_uint16 = params_get_value(ParamNum_LoiterDistKp);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);

    // 水平速度误差比例系数
    tmp_uint16 = params_get_value(ParamNum_LoiterSpeedKp);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);

    // 水平加速度比例系数
    tmp_uint16 = params_get_value(ParamNum_LoiterAccKp);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);

    // 水平速度误差积分系数
    tmp_uint16 = params_get_value(ParamNum_LoiterAccKi);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 1);

    // 水平刹车陀螺参数 10~50
    tmp_uint8 = params_get_value(ParamNum_GyroBrakeRatio);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    // 水平加速度变化率 4~50
    tmp_uint8 = params_get_value(ParamNum_MaxHorJet);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    gs_tx_vklink_msg(pgcs, txMsg);
}

/**
 * @brief 发送 par 参数消息
 *
 */
void gcs_vklink_v300_send_par_data(struct GCS_Link *pgcs)
{
    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_PAR_ID);

    uint8_t tmp_uint8;
    uint16_t tmp_uint16;

    tmp_uint8 = params_get_value(ParamNum_APMaxTilteAngleDeg);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint8 = params_get_value(ParamNum_APMaxHorizonalSpeedGpsModeDms);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint8 = params_get_value(ParamNum_APMaxClimbSpeedGpsModeDms);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint8 = params_get_value(ParamNum_APMaxDeclineSpeedGpsModeDms);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint16 = params_get_value(ParamNum_APHoverThrottle);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);

    tmp_uint16 = params_get_value(ParamNum_AltRestrictionA);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);

    tmp_uint16 = params_get_value(ParamNum_AltRestrictionB);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);

    tmp_uint8 = params_get_value(ParamNum_APMaxYawRate);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint8 = params_get_value(ParamNum_VoltCalibOffset0);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint8 = params_get_value(ParamNum_VoltCalibOffset1);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint8 = params_get_value(ParamNum_VoltCalibOffset2);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint8 = params_get_value(ParamNum_FollowDistM);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint8 = params_get_value(ParamNum_APMaxClimbSpeedAutoModeDms);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint8 = params_get_value(ParamNum_APMaxDeclineSpeedAutoModeDms);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint8 = params_get_value(ParamNum_APMinLandingRateAutoModeDms);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint8 = params_get_value(ParamNum_APRthSpeedDms);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint16 = params_get_value(ParamNum_ObstacleHoldDistCm);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);

    tmp_uint8 = params_get_value(ParamNum_LinklostAction);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint8 = params_get_value(ParamNum_ServoFailsafe);
    vklink_msg_payload_put_data(txMsg, &tmp_uint8, 1);

    tmp_uint16 = params_get_value(ParamNum_VoltCalibKp1);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);

    tmp_uint16 = params_get_value(ParamNum_MaxHorizonalDistanceM);
    vklink_msg_payload_put_data(txMsg, &tmp_uint16, 2);

    gs_tx_vklink_msg(pgcs, txMsg);
}


/**
 * @brief 发送相机参数消息
 *
 */
void gcs_vklink_v300_send_caminf_data(struct GCS_Link *pgcs)
{
    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_CAMINF_ID);

    vklink_msg_payload_put_data(txMsg, &caminf._signal_type, 1);
    vklink_msg_payload_put_data(txMsg, &caminf._signal_interval, 2);
    vklink_msg_payload_put_data(txMsg, &caminf._signal_pwm_takephoto_on, 2);
    vklink_msg_payload_put_data(txMsg, &caminf._signal_pwm_takephoto_off, 2);

    gs_tx_vklink_msg(pgcs, txMsg);
}

/**
 * @brief 发送版本信息
 *
 */
void gcs_vklink_v300_send_ver_data(struct GCS_Link *pgcs)
{
    int32_t tmpInt = 0;

    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    ;
    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_VER_ID);

    vklink_msg_payload_put_data(txMsg, verinf._ap_name, 10);
    vklink_msg_payload_put_data(txMsg, &verinf._serial_id, 4);

    // imu 固件号
    tmpInt = ins.imu_conf.version;
    vklink_msg_payload_put_data(txMsg, &tmpInt, 4);

    // 主控固件号
    tmpInt = FW_VER;
    vklink_msg_payload_put_data(txMsg, &tmpInt, 4);

    vklink_msg_payload_put_data(txMsg, &verinf._hw_ver, 2);
    vklink_msg_payload_put_data(txMsg, &verinf._sysid, 1);

    gs_tx_vklink_msg(pgcs, txMsg);
}

/**
 * @brief 发送航点消息
 *
 */
void gcs_vklink_v300_send_wp_data(struct GCS_Link *pgcs, uint16_t arg)
{

    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_WP_ID);

    uint16_t transport_wp_num = arg;

    /* 从 flash 中读取一个航点 */
    struct waypoint_data wp_temp;
    flash_read_bytes(WAYPOINT_ADDR + transport_wp_num * sizeof(wp_temp),
                     (uint8_t *)&wp_temp, sizeof(wp_temp));
    vklink_msg_payload_put_data(txMsg, &wp_temp.wp_num, 2);
    vklink_msg_payload_put_data(txMsg, &wp_temp.wp_lng, 4);
    vklink_msg_payload_put_data(txMsg, &wp_temp.wp_lat, 4);
    vklink_msg_payload_put_data(txMsg, &wp_temp.wp_alt, 4);
    vklink_msg_payload_put_data(txMsg, &wp_temp.wp_vel, 2);
    vklink_msg_payload_put_data(txMsg, &wp_temp.wp_mode, 1);
    vklink_msg_payload_put_data(txMsg, &wp_temp.wp_mode_param, 2);
    vklink_msg_payload_put_data(txMsg, &wp_temp.wp_task, 1);
    vklink_msg_payload_put_data(txMsg, &wp_temp.wp_task_param, 2);
    vklink_msg_payload_put_data(txMsg, &wp_temp.wp_param3, 2);
    vklink_msg_payload_put_data(txMsg, &wp_temp.wp_alt_type, 1);
    vklink_msg_payload_put_data(txMsg, &wp_temp.wp_totalnum, 2);
    vklink_msg_payload_put_data(txMsg, &wp_temp.wp_param4, 2);

    gs_tx_vklink_msg(pgcs, txMsg);
}

/**
 * @brief 飞行数据
 *
 */
void gcs_vklink_v300_send_flydata_data(struct GCS_Link *pgcs,
                                       uint16_t index_offset,
                                       uint32_t pack_num)
{
    static uint8_t data_buff[128] = {0};

    // 计算一下总包数有多少，一包128个字节
    // 总字节数+127 / 128 正好向上取整
    char file_name[24] = "", logfile_path[24] = "";
    sprintf(file_name, "%d", fly_log.sorties - index_offset);
    strcat(logfile_path, "LOG/");
    strcat(logfile_path, file_name);

    if (f_open(&fnew_data, logfile_path, FA_READ) == FR_OK)
    {
        VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
        gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_DATA_ID);

        // 当前发送数据包序号
        vklink_msg_payload_put_data(txMsg, &pack_num, 4);

        // 字节长度
        uint8_t byte_length = 128;
        vklink_msg_payload_put_data(txMsg, &byte_length, 1);

        // 包总数
        uint32_t data_total_num = (f_size(&fnew_data) + 127) / 128;
        vklink_msg_payload_put_data(txMsg, &data_total_num, 4);

        // 读取数据
        f_lseek(&fnew_data, (pack_num - 1) * 128);
        f_read(&fnew_data, data_buff, 128, &frnum);
        for (uint8_t i = frnum; i < 128; i++)
        {
            data_buff[i] = 0xFF;
        }
        vklink_msg_payload_put_data(txMsg, data_buff, 128);

        gs_tx_vklink_msg(pgcs, txMsg);
    }
}

/*
 * 发送飞行log数据，前两字节为总
 */
void gcs_vklink_v300_send_flylog_data(struct GCS_Link *pgcs, uint16_t log_num)
{

    f_open(&fnew_log, "LOG/LOG_FLY.DAT", FA_READ);
    if (f_size(&fnew_log) >= sizeof(fly_log))
    {
        VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
        gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_LOG_ID);

        // 移动到尾部最后一包，并获取总的log条数
        f_lseek(&fnew_log, f_size(&fnew_log) - sizeof(fly_log));
        uint16_t total_log_num;
        f_read(&fnew_log, &total_log_num, sizeof(total_log_num), &frnum);

        vklink_msg_payload_put_data(txMsg, &total_log_num, 2);

        // 再移动到需要读取的log条数位置，读取一条log信息填充发送buf
        FLY_LOG tmp_log;
        f_lseek(&fnew_log, (log_num - 1) * sizeof(tmp_log));
        f_read(&fnew_log, &tmp_log, sizeof(tmp_log), &frnum);
        vklink_msg_payload_put_data(txMsg, &tmp_log, sizeof(tmp_log));

        gs_tx_vklink_msg(pgcs, txMsg);
    }
    f_close(&fnew_log);
}

/**
 * @brief 发送SD卡文件索引
 */
void gcs_vklink_v300_send_fileindex_data(struct GCS_Link *pgcs, uint16_t index_num)
{
    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_FILE_INDEX_ID);

    f_opendir(&logdir, "LOG");
    for (char i = 0; i < index_num; ++i)
        f_readdir(&logdir, &fno);
    f_closedir(&logdir);
    if (fno.fname[0] != '\0')
    {
        vklink_msg_payload_put_data(txMsg, &fno, sizeof(fno));
    }
    else
    {
        for (uint32_t i = 0; i < sizeof(fno); ++i)
        {
            uint8_t data = 0;
            vklink_msg_payload_put_data(txMsg, &data, 1);
        }
    }

    gs_tx_vklink_msg(pgcs, txMsg);
}

/**
 * @brief 转发载荷串口数据
 *
 */
void gcs_vklink_v300_send_uart4_rx_data(struct GCS_Link *pgcs, const void *data, uint32_t len)
{
    VKlink_Msg_Type *txMsg = &pgcs->vklink_tx_msg;
    gcs_vklink_txmsg_init(txMsg, GCS_VKLINK_V300_PORT_UART4_DATA);

    vklink_msg_payload_put_data(txMsg, data, len);
    port4_data.len = 0;

    gs_tx_vklink_msg(pgcs, txMsg);
}

/**
 * @brief 接收消息解包
 *
 * @param rxMsg
 */
void gcs_vklink_v300_rx_decode(struct GCS_Link *pgcs)
{
    const VKlink_Msg_Type *rxMsg = &pgcs->vklink_rx_msg;

    /* 过滤 sysid  */
    if (rxMsg->sysid != verinf._sysid && rxMsg->sysid != 0xFF)
        return;

    /* 如果是来自地面站的 IMU 升级消息，则转发给 IMU */
    if (rxMsg->compid == COMPID_IMUBL)
    {
        if (thr_lock_status == LOCKED)
        {
            pilot_mode = PILOT_IMU_UPDATE;
            send_vklink_msg_to_imu(rxMsg);
        }
    }
    /* 如果是正常的消息, 则进行解析  */
    else
    {
        switch (rxMsg->msgid)
        {
        case GCS_VKLINK_V300_HEART_ID:
        {
            /* 地面站连接状态置为正常，清零失联记时 */
            pgcs->link_status = COMP_NORMAL;
            pgcs->link_lost_time_us = 0;

            /* 心跳包带版本号 */
            uint16_t vklink_protocal_version;
            memcpy(&vklink_protocal_version, rxMsg->payload, 2);
            gs_set_vklink_protocal_version(pgcs, vklink_protocal_version);

            /* 心跳包带多机当前选中的飞机 */
            pgcs->_current_sysid = rxMsg->payload[2];
        }
        break;
        case GCS_VKLINK_V300_REQ_ID:
        {
            uint8_t send_what = rxMsg->payload[0];

            switch (send_what)
            {
            case GCS_VKLINK_V300_APTYPE_ID:
                gcs_vklink_v300_send_aptype_data(pgcs);
                break;

            case GCS_VKLINK_V300_CAMINF_ID:
                gcs_vklink_v300_send_caminf_data(pgcs);
                break;

            case GCS_VKLINK_V300_CENTER_FIX_ID:
                gcs_vklink_v300_send_centerfix_data(pgcs);
                break;

            case GCS_VKLINK_V300_CONFINF_ID:
            {
                uint16_t arg;
                memcpy(&arg, rxMsg->payload + 1, 2);
                gcs_vklink_v300_send_confinf_data(pgcs, arg);
            }
            break;

            case GCS_VKLINK_V300_CONFINF_TOTAL_ID:
                gcs_vklink_v300_send_confinf_total_data(pgcs);
                break;

            case GCS_VKLINK_V300_PID_ID:
                gcs_vklink_v300_send_pid_data(pgcs);
                break;

            case GCS_VKLINK_V300_PAR_ID:
                gcs_vklink_v300_send_par_data(pgcs);
                break;

            case GCS_VKLINK_V300_VER_ID:
                gcs_vklink_v300_send_ver_data(pgcs);
                break;
            case GCS_VKLINK_V300_WP_ID:
            {
                uint16_t transport_wp_num;
                memcpy(&transport_wp_num, rxMsg->payload + 1, 2);
                gcs_vklink_v300_send_wp_data(pgcs, transport_wp_num);
            }
            break;

            case GCS_VKLINK_V300_DATA_ID:
                if (thr_lock_status == LOCKED)
                {
                    uint16_t index_offset = 0;
                    uint32_t data_pack_num = 0;
                    memcpy(&index_offset, rxMsg->payload + 1, 2);
                    memcpy(&data_pack_num, rxMsg->payload + 3, 4);
                    gcs_vklink_v300_send_flydata_data(pgcs, index_offset, data_pack_num);
                }
                break;

            case GCS_VKLINK_V300_LOG_ID:
                if (thr_lock_status == LOCKED)
                {
                    uint16_t log_num = 0;
                    memcpy(&log_num, rxMsg->payload + 1, 2);

                    if (log_num == 0)
                    {
                        sd_card_clear_fly_log();
                    }
                    else
                    {
                        gcs_vklink_v300_send_flylog_data(pgcs, log_num);
                    }
                }
                break;
            case GCS_VKLINK_V300_FILE_INDEX_ID:
                if (thr_lock_status == LOCKED)
                {
                    uint16_t index_num;
                    memcpy(&index_num, rxMsg->payload + 1, 2);
                    gcs_vklink_v300_send_fileindex_data(pgcs, index_num);
                }
                break;
            default:
                break;
            }
        }
        break;
        case GCS_VKLINK_V300_HOME_ID:
        {
            int x, y, z;
            int16_t yaw;
            memcpy(&x, rxMsg->payload, 4);
            memcpy(&y, rxMsg->payload + 4, 4);
            memcpy(&z, rxMsg->payload + 8, 4);
            memcpy(&yaw, rxMsg->payload + 12, 2);

            if (rxMsg->payload[14] == 0)
            {
                home_pos_isrecord = HOME_POS_MANUL_SET;
                home_position.lng = x;
                home_position.lat = y;
                home_position.gps_alt = z;
                home_position.yaw = wrap_180(yaw);
            }

            struct gcs_ack_arg arg = {0};
            arg.ack_id = GCS_VKLINK_V300_HOME_ID;
            arg.ack_content[0] = rxMsg->payload[14];
            gcs_vklink_v300_send_ack_data(pgcs, &arg);
        }
        break;
        case GCS_VKLINK_V300_CTL_ID:
        {
            uint8_t ctl_id = rxMsg->payload[0];
            switch (ctl_id)
            {
            case CTL_MSG_ENABLE_TUNING:
                if (thr_lock_status == LOCKED)
                {
                    throttle_enable_unlock_tune();
                }
                break;

            /* 车载起飞指令 */
            case CTL_MSG_VEHICLE_LAUNCH:
                if (thr_lock_status == LOCKED)
                {
                    control_mode = CONTROL_GCS;
                    flight_mode_flag = GCS_VEHICLE_LAUNCH;
                }
                break;
            default:
                break;
            }

            struct gcs_ack_arg arg = {0};
            arg.ack_id = GCS_VKLINK_V300_CTL_ID;
            arg.ack_content[0] = ctl_id;
            gcs_vklink_v300_send_ack_data(pgcs, &arg);
        }
        break;
        case GCS_VKLINK_V300_CAL_ID:
        {
            uint8_t cal_id = rxMsg->payload[0];
            switch (cal_id)
            {
            case CAL_MSG_ATT:
                // 上锁状态下才能执行
                if (thr_lock_status == LOCKED)
                {
                    imu_euler_angle_clear();
                }
                break;
            case CAL_MSG_MAG:
                // 上锁状态下才能执行
                if (thr_lock_status == LOCKED)
                {
                    // 角度清零以及磁校准有时候执行不了，已测试与地面站无关
                    ci_jiao_zhun = true;
                }
                break;
            case CAL_MSG_REMOTE_S:
                // 上锁状态下才能执行
                if (thr_lock_status == LOCKED)
                {
                    rc_input_calib_start();
                }
                break;
            case CAL_MSG_REMOTE_E:
                // 开始校准并且上锁状态下
                if (thr_lock_status == LOCKED)
                {
                    rc_input_calib_end();
                }
                break;
            // 电机检测
            case CAL_MSG_MOTOR:
                if (thr_lock_status == LOCKED)
                {
                    uint16_t motor_num;
                    memcpy(&motor_num, rxMsg->payload + 1, 2);
                    if (motor_num == 1)
                    {
                        motor_num = 3;
                    }
                    else if (motor_num == 3)
                    {
                        motor_num = 1;
                    }
                    MotorCheck_SetCheckNum(motor_num);
                }
                break;
            // 安装方向
            case CAL_MSG_ASSEMBLY_DIRECTION:
                if (thr_lock_status == LOCKED)
                {
                    uint8_t calib_value[3] = {
                        rxMsg->payload[1], 0, 0};
                    send_calibration_msg_to_imu(ASSEMBLE_CALIB, calib_value, 1);
                }
                break;
            // 电压校准参数
            case CAL_MSG_BATTERY_VOLTAGE:
                if (thr_lock_status == LOCKED)
                {
                    uint8_t calib_flag = rxMsg->payload[1];
                    uint8_t ad_num = rxMsg->payload[2];

                    switch (calib_flag)
                    {
                    /* 恢复默认电压校准系数 */
                    case 0:
                        switch (ad_num)
                        {
                        case 0:
                            params_set_value(ParamNum_VoltCalibKp0, 1000);
                            params_set_value(ParamNum_VoltCalibOffset0, 0);
                            break;

                        case 1:
                            params_set_value(ParamNum_VoltCalibKp1, 1000);
                            params_set_value(ParamNum_VoltCalibOffset1, 0);
                            break;

                        case 2:
                            params_set_value(ParamNum_VoltCalibKp2, 1000);
                            params_set_value(ParamNum_VoltCalibOffset2, 0);
                            break;
                        }
                        break;

                    /* 设置电压校准系数  */
                    case 1:
                    {
                        int16_t volt_kp;
                        memcpy(&volt_kp, rxMsg->payload + 3, 2);
                        int8_t volt_offset = rxMsg->payload[5];
                        ParamsEnumType paramnumKp = ParamNum_VoltCalibKp0;
                        ParamsEnumType paramnumOffset =
                            ParamNum_VoltCalibOffset0;
                        switch (ad_num)
                        {
                        case 0:
                            paramnumKp = ParamNum_VoltCalibKp0;
                            paramnumOffset = ParamNum_VoltCalibOffset0;
                            break;

                        case 1:
                            paramnumKp = ParamNum_VoltCalibKp1;
                            paramnumOffset = ParamNum_VoltCalibOffset1;
                            break;

                        case 2:
                            paramnumKp = ParamNum_VoltCalibKp2;
                            paramnumOffset = ParamNum_VoltCalibOffset2;
                            break;
                        }
                        params_set_value(paramnumKp, volt_kp);
                        params_set_value(paramnumOffset, volt_offset);
                    }
                    break;
                    }

                    write_par_information = true;
                }
                break;
            }
            struct gcs_ack_arg arg = {0};
            arg.ack_id = GCS_VKLINK_V300_CAL_ID;
            arg.ack_content[0] = cal_id;
            gcs_vklink_v300_send_ack_data(pgcs, &arg);
        }
        break;

        case GCS_VKLINK_V300_CONFINF_ID:
        {
            uint16_t num;
            memcpy(&num, rxMsg->payload, 2);
            int16_t value;
            memcpy(&value, rxMsg->payload + 2, 2);
            params_set_value(num, value);

            struct gcs_ack_arg arg = {0};
            arg.ack_id = GCS_VKLINK_V300_CONFINF_ID;
            arg.ack_content[0] = num;
            arg.ack_content[1] = value;
            gcs_vklink_v300_send_ack_data(pgcs, &arg);
        }
        break;

        case GCS_VKLINK_V300_ROCK_ID:
        {
            for (uint8_t i = 0; i < 4; ++i)
            {
                memcpy(&rock_in[i], rxMsg->payload + 2 * i, 2);
            }

            rock_key = rxMsg->payload[8];

            rock_isenable = COMP_NORMAL;

        }
        break;

        case GCS_VKLINK_V300_APTYPE_ID:
        {
            uint8_t index = 0;
            int16_t value = 0;
            /* 机型 */
            params_set_value(ParamNum_APType, rxMsg->payload[index++]);

            /* 盘旋半径 m */
            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_APCircleRadiusM, value);

            /* 失联时间 */
            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_RadioLinkLostTimeS, value);

            /* 1 级电压保护阈值 */
            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_LowVoltValue1, value);
            /* 1 级电压保护动作 */
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_LowVoltAction1, value);

            /* 2 级电压保护阈值 */
            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_LowVoltValue2, value);
            /* 2 级电压保护动作 */
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_LowVoltAction2, value);

            /* 起飞高度 */
            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_APTakeoffAltM, value);
            /* 返航高度 */
            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_APRthAltM, value);
            /* 磁偏角 */
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_MagDeclinationDeg, value);

            /* IMU 滤波参数 */
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_ImuFilterLever, value);

            /* 怠速等级 */
            value = rxMsg->payload[index++];
            if (value >= 1 && value <= 5)
            {
                uint16_t mc_idle_speed = (value - 1) * 50 + 1050;
                params_set_value(ParamNum_APIdleSpeed, mc_idle_speed);
            }

            /* 遥控器失控后悬停等待时间 */
            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_RcFailLoiterTimeS, value);

            struct gcs_ack_arg arg = {0};
            arg.ack_id = GCS_VKLINK_V300_APTYPE_ID;
            gcs_vklink_v300_send_ack_data(pgcs, &arg);
        }
        break;
        case GCS_VKLINK_V300_CENTER_FIX_ID:
        {
            struct gps_imu_pos_fix_msg_data *data =
                (struct gps_imu_pos_fix_msg_data *)rxMsg->payload;

            int8_t gps_pos_fix[6];
            int8_t imu_pos_fix[3];

            if (rxMsg->payload_len >= 12)
            {
                for (size_t i = 0; i < 6; ++i)
                {
                    gps_pos_fix[i] = data->gps_pos_fix[i];
                }

                if (thr_lock_status == LOCKED)
                {
                    send_calibration_msg_to_imu(
                        GPS_POS_CALIB, (uint8_t *)gps_pos_fix, 6);
                }
            }

            if (rxMsg->payload_len >= 18)
            {
                for (size_t i = 0; i < 3; ++i)
                {
                    imu_pos_fix[i] = data->imu_pos_fix[i];
                }

                if (thr_lock_status == LOCKED)
                {
                    send_calibration_msg_to_imu(
                        IMU_POS_CALIB, (uint8_t *)imu_pos_fix, 3);
                }
            }

            struct gcs_ack_arg arg = {0};
            arg.ack_id = GCS_VKLINK_V300_CENTER_FIX_ID;
            gcs_vklink_v300_send_ack_data(pgcs, &arg);
        }
        break;
        case GCS_VKLINK_V300_PID_ID:
        {
            uint8_t index = 0;
            int16_t value = 0;

            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_RollAngleKp, value);

            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_RollGyroKp, value);

            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_PitchAngleKp, value);

            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_PitchGyroKp, value);

            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_YawAngleKp, value);

            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_YawGyroKp, value);

            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_AltholdSpeedKp, value);

            value = rxMsg->payload[index++];
            params_set_value(ParamNum_RollGyroKd, value);
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_PitchGyroKd, value);
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_YawGyroKd, value);

            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_AltholdDistKp, value);
            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_AltholdAccKp, value);
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_AltholdAccKi, value);

            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_LoiterDistKp, value);
            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_LoiterSpeedKp, value);
            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_LoiterAccKp, value);

            value = rxMsg->payload[index++];
            params_set_value(ParamNum_LoiterAccKi, value);
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_GyroBrakeRatio, value);

            value = rxMsg->payload[index++];
            params_set_value(ParamNum_MaxHorJet, value);

            struct gcs_ack_arg arg = {0};
            arg.ack_id = GCS_VKLINK_V300_PID_ID;
            gcs_vklink_v300_send_ack_data(pgcs, &arg);
        }
        break;

        case GCS_VKLINK_V300_PAR_ID:
        {
            uint8_t index = 0;
            int16_t value = 0;

            value = rxMsg->payload[index++];
            params_set_value(ParamNum_APMaxTilteAngleDeg, value);
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_APMaxHorizonalSpeedGpsModeDms, value);
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_APMaxClimbSpeedGpsModeDms, value);
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_APMaxDeclineSpeedGpsModeDms, value);

            // buf2short((short *)&parinf._par_hover_throttle,
            // &rx_buffer[index]);
            index += 2;

            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_AltRestrictionA, value);

            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_AltRestrictionB, value);

            value = rxMsg->payload[index++];
            params_set_value(ParamNum_APMaxYawRate, value);

            value = rxMsg->payload[index++];
            params_set_value(ParamNum_VoltCalibOffset0, value);
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_VoltCalibOffset1, value);
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_VoltCalibOffset2, value);
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_FollowDistM, value);
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_APMaxClimbSpeedAutoModeDms, value);
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_APMaxDeclineSpeedAutoModeDms, value);
            value = rxMsg->payload[index++];
            params_set_value(ParamNum_APMinLandingRateAutoModeDms, value);

            value = rxMsg->payload[index++];
            params_set_value(ParamNum_APRthSpeedDms, value);

            memcpy(&value, &rxMsg->payload[index], 2);
            index += 2;
            params_set_value(ParamNum_ObstacleHoldDistCm, value);

            value = rxMsg->payload[index++];
            params_set_value(ParamNum_LinklostAction, value);

            value = rxMsg->payload[index++];
            params_set_value(ParamNum_ServoFailsafe, value);

            /* 预留 */
            index += 2;

            memcpy(&value, rxMsg->payload + index, 2);
            index += 2;
            params_set_value(ParamNum_MaxHorizonalDistanceM, value);

            struct gcs_ack_arg arg = {0};
            arg.ack_id = GCS_VKLINK_V300_PAR_ID;
            gcs_vklink_v300_send_ack_data(pgcs, &arg);
        }
        break;
        case GCS_VKLINK_V300_VER_ID:
        {
            if (thr_lock_status == LOCKED)
            {
                /* 飞控名字 */
                memcpy(verinf._ap_name, &rxMsg->payload[0],
                       sizeof(verinf._ap_name));
                /* 飞控序列号 */
                memcpy(&verinf._serial_id, rxMsg->payload + 10, 4);

                /*  */
                verinf._hw_ver = rxMsg->payload[22] + (rxMsg->payload[23] << 8);
                /* 飞机编号 */
                verinf._sysid = rxMsg->payload[24];

                write_ver_information = true;
            }

            struct gcs_ack_arg arg = {0};
            arg.ack_id = GCS_VKLINK_V300_VER_ID;
            gcs_vklink_v300_send_ack_data(pgcs, &arg);
        }
        break;
        /* 固件升级开始 */
        case GCS_VKLINK_V300_FIRMWARE_UPDATE_ID:
            if (thr_lock_status == LOCKED)
                write_iap_flag = true;
            break;

        default:
            break;
        }
    }
}

/**
 * @brief 地面站发送消息轮询
 *
 */
void gcs_vklink_v300_tx_poll(struct GCS_Link *pgcs)
{
    if (pilot_mode == PILOT_IMU_UPDATE)
    {
        /* 升级 imu 时地面站作为透传转发, 不发送其它常发消息 */
        imu_tx_msg_to_gcs(pgcs);
        return;
    }
    else
    {
        /* 1hz 遥测数据定频发送 */
        if (micros() - pgcs->_last_tx1hz_time_us > (1e6 / 1 / 8))
        {
            pgcs->_last_tx1hz_time_us = micros();
            switch (pgcs->_last_tx1hz_id)
            {
            case 0:
                pgcs->_last_tx1hz_id++;
                break;
            case 1:
                gcs_vklink_v300_send_payload_data(pgcs);
                pgcs->_last_tx1hz_id++;
                break;
            case 5:
                // gcs_vklink_v300_send_telemetry_data(pgcs);
                pgcs->_last_tx1hz_id++;
                break;
            default:
                pgcs->_last_tx1hz_id = 0;
                break;
            }
        }

        /* 2hz 遥测数据定频发送 */
        if (micros() - pgcs->_last_tx2hz_time_us > (1e6 / 2 / 1))
        {
            pgcs->_last_tx2hz_time_us = micros();

            /* 2hz 遥测数据定频发送 */
            switch (pgcs->_last_tx2hz_id)
            {
            case 0:
                pgcs->_last_tx2hz_id = 0;
                break;

            default:
                pgcs->_last_tx2hz_id = 0;
                break;
            }
        }

        /* 5hz 遥测数据定频发送 */
        if (micros() - pgcs->_last_tx5hz_time_us > (1e6 / 5 / 3))
        {
            pgcs->_last_tx5hz_time_us = micros();
            switch (pgcs->_last_tx5hz_id)
            {
            case 0:
                pgcs->_last_tx5hz_id++;
                break;

            case 1:
                gcs_vklink_v300_send_global_pos_data(pgcs);
                pgcs->_last_tx5hz_id++;
                break;

            case 2:
                gcs_vklink_v300_send_telemetry_data(pgcs);
                pgcs->_last_tx5hz_id = 0;
                break;

            default:
                pgcs->_last_tx5hz_id = 0;
                break;
            }
        }
    }
}

void gcs_vklink_v300_set_tx_msg(struct GCS_Link *pgcs, uint8_t msg_id, void *arg)
{
    switch (msg_id)
    {
    case GCS_VKLINK_V300_PORT_UART4_DATA:
    {
        struct gcs_transparent_transmission_arg *tt_arg =
            (struct gcs_transparent_transmission_arg *)arg;
        gcs_vklink_v300_send_uart4_rx_data(pgcs, tt_arg->data, tt_arg->data_len);
    }
    break;

    case GCS_VKLINK_V300_ACK_ID:
    {
        struct gcs_ack_arg *ack_arg = (struct gcs_ack_arg *)arg;
        gcs_vklink_v300_send_ack_data(pgcs, ack_arg);
    }
    break;

    default:
        break;
    }
}