#include "soft_imu.h"
#include "hpm_math.h"
#include "auto_pilot.h"
#include "common.h"
#include "control_attitude.h"
#include "control_rate.h"
#include "control_throttle.h"
#include "dcm.h"
#include "flight_mode.h"
#include "hard_imu_uart3.h"
#include "helpler_funtions.h"
#include "lowpass_filter.h"
#include "math.h"
#include "my_crc.h"
#include "my_math.h"
#include "params.h"
#include "quaternion.h"
#include "soft_can_yy.h"
#include "soft_delay.h"
#include "soft_gs.h"
#include "soft_motor_output.h"
#include "soft_port_uart4.h"
#include "soft_time.h"
#include "ver_config.h"
#include "mode_attitude.h"
#include "bsp_V8M_YY_led.h"
#include "vklink.h"
#include <stdint.h>
#include <string.h>


/* 高度数据使用哪个的宏定义 */
#define USE_BARO_ALT 0  // 气压高度
#define USE_INTEG_ALT 1 // gps速度积分高度

// 磁校准
bool ci_jiao_zhun = false;

// 通知 IMU 解锁
bool notify_imu_unlock = false;
// 通知 IMU 上锁
bool notify_imu_lock = false;

// 通知 IMU 禁用 GPS 高度
bool notify_imu_disable_gpsalt = false;
// 通知 IMU 启用 GPS 高度
bool notify_imu_enable_gpsalt = false;

/* 高度清零使用的记录偏移量 */
static int clear_alt_offset = 0; // cm
static int offset_alt = 0;       // cm

/* 导航数据结构体 */
struct INS_DATA ins;

/* imu 数据接收时间和互斥变量 */
static volatile char ins_data_time_sem = 0;
unsigned int ins_data_time = 0;

/* imu 数据接收 */
VKlink_Msg_Type imu_rx_msg;

/* 地面站转发 imu 接收到的消息 */
uint8_t need_gcs_tx_imu_rx_msg = 0;
VKlink_Msg_Type gcs_tx_imu_msg = {0};

// 原始IMU数据结构体
REV_IMU raw_imu;
// 原始GPS数据结构体
REV_GPS raw_gps;
// 原始传感器数据结构体
REV_SENSOR raw_sensor;

// 收到Imu数据标志位
bool recieve_isimudata = false;
// 收到Gps数据标志位
bool recieve_isgpsdata = false;
// 收到Sensor数据标志位
bool recieve_issensordata = false;

// DMA传输出错标志位
bool dma_iserror = false;
// imu 链接状态
comp_status imu_link_status = COMP_NOEXIST;

// 0---无校准  1-- 校准XY    2--校准XZ   3--校准完成	 4--校准失败
unsigned char mag_cal_status = MAG_CAL_NO;

// 获得IMU版本号
bool get_imu_version = false;
// 获得 GPS 安装位置偏差
bool get_gps_pos_param = false;
// 获得 IMU 安装位置偏差
bool get_imu_pos_param = true;

struct imu_vklink_system
{
    uint8_t head;
    uint8_t sysid;
    uint8_t compid;
    uint8_t tx_seq;
    uint8_t rx_seq;
};
static struct imu_vklink_system imu_vklink_sys = {
    .head = 0xfe, .sysid = 0, .tx_seq = 0, .rx_seq = 0, .compid = 0};

/* 水平位置平均滤波 */
static void horizental_pos_aver_filter(void);

/* 时区转换 */
void UTCToLocalTime(int timezone);

/**
 * @brief imu 串口初始化
 *
 * @param imu_bps 串口波特率
 */
void imu_uart3_initial(unsigned int imu_bps) { imu_uart3_init(imu_bps); }


void imu_euler_angle_clear(void)
{
    uint8_t calib_data[3] = {0};
    send_calibration_msg_to_imu(ATTITUE_CALIB, calib_data, 0);
}

/**
 * @brief gps 运动航向规范化
 */
float get_normal_gpsyaw(float raw_yaw)
{
    // 将航向角的范围归一化到-180--180之间
    if (raw_yaw > 180.0f)
        raw_yaw -= 360.0f;
    return raw_yaw;
}

#define BARO_NUM 5 // 要平均的气压高度的个数
static int plane_baro_alt = 0;

/**
 * @brief imu data 数据解析
 *
 */
void dma_analysis_imudata(void)
{
    // 上次接收解析时间
    static uint32_t last_recv_time = 0;

    uint32_t time_interval = micros() - last_recv_time;
    float dt = time_interval / 1000000.0f;
    if (dt < 0.0f || dt > 0.1f)
        dt = 0.005f;

    DCM_ByEuler(deg_to_rad(raw_imu._zitai_yaw * 0.01f),
                deg_to_rad(raw_imu._zitai_pitch * 0.01f),
                deg_to_rad(raw_imu._zitai_roll * 0.01f),
                ins.dcm);
    Quaternion_ByEuler(deg_to_rad(raw_imu._zitai_yaw * 0.01f),
                       deg_to_rad(raw_imu._zitai_pitch * 0.01f),
                       deg_to_rad(raw_imu._zitai_roll * 0.01f),
                       ins.Q);

    pid_m_posx.vel_c = raw_imu._x_vel; // E向速度 cm/s
    pid_m_posy.vel_c = raw_imu._y_vel; // N向速度 cm/s

    pid_m_alt.vel_c = raw_imu._z_vel; // 垂直速度   IMU传递过来的是cm/s

    ins.horz_vel =
        get_norm(pid_m_posx.vel_c, pid_m_posy.vel_c); // 合成速度，单位cm/s

    pid_m_posx.acc_c = raw_imu._x_acc;
    pid_m_posy.acc_c = raw_imu._y_acc;

    horizental_pos_aver_filter();

    pid_m_alt.loc_c = (raw_imu._baro_alt);

    // _cxy.pc[0] = pid_m_posx.loc_c * 0.01f;
    // _cxy.pc[1] = pid_m_posy.loc_c * 0.01f;

    // _cxy.vc[0] = pid_m_posx.vel_c * 0.01f;
    // _cxy.vc[1] = pid_m_posy.vel_c * 0.01f;

    // _cxy.ac[0] = pid_m_posx.acc_c * 0.01f;
    // _cxy.ac[1] = pid_m_posy.acc_c * 0.01f;
}

void imu_attitude_ananlysis(void)
{
    const struct yy_can_rx_msg_data *yy_can_rx_data = yy_can_rx_data_get();

    if ((yy_can_rx_data->imu_status & 0x01) == 0x01)
    {
        pid_m_yaw.angle_c = yy_can_rx_data->imu_yaw;
    }
    else
    {
        pid_m_yaw.angle_c = 360.0f - raw_imu._zitai_yaw / 100.0f;
    }
    pid_m_roll.angle_c = raw_imu._zitai_roll / 100.0f;
    pid_m_pitch.angle_c = raw_imu._zitai_pitch / 100.0f;

    pid_m_roll.gyro_c = raw_imu._y_gyro;
    pid_m_pitch.gyro_c = raw_imu._x_gyro;

    pid_m_yaw.gyro_c = apply(raw_imu._z_gyro, pid_m_yaw.gyro_c, 10.0f, 0.005f);

    pid_m_alt.acc_c = raw_imu._z_acc;
    // 垂直加速度
    //  IMU传递过来的是cm/s/s..限制加速度试图解决加速度异常导致的飞机一直爬升。
    // 限制小了至少油门拉到底飞机可以下来。发现限制小了之后(+-100)会造成严重的超调震荡。猛加速猛减速。所以建议不能限制太小。
    pid_m_alt.acc_c = constrain_float(pid_m_alt.acc_c, -500.0f, +500.0f);
}

/**
 * @brief dma解析gps数据
 */
void dma_analysis_gpsdata(void)
{
    static int recieve_gps_time = 0;
    float recieve_gps_dt = 0.0f;
    recieve_gps_dt = (micros() - recieve_gps_time) / 1000000.0f;
    if (recieve_gps_dt < 0.0f)
        recieve_gps_dt = 0.05f;
    recieve_gps_time = micros();

    // 解析GPS航向角
    ins.gps_yaw = get_normal_gpsyaw(raw_gps._gps_yaw / 10.0f);

    ins.gps_alt = raw_gps._gps_alt_mm * 0.1f; // 单位：cm
    ins.gps_vel = raw_gps._gps_vel * 10;      // 单位：cm/s
    ins.gps_num = raw_gps._gps_num;
    ins.gps_pdop = raw_gps._gps_pdop;
    ins.lng = raw_gps._longitude;
    ins.lat = raw_gps._latitude;

    UTCToLocalTime(8);

    // 振动系数
    ins.oscillating_coefficient = raw_gps._oscillating_coefficient;

    ins.rtk_state = raw_gps._rtk_state;

    ins.temprature = raw_gps._temprature;

    // 姿态模式标志位，也就是是否可以起飞标志位
    ins.insgps_ok = raw_gps._insgps_ok;

    // 磁校准状态标志位
    ins.mag_calib_flag = raw_gps._state_flag;

    ins.imu_conf.filter_param = (raw_gps._imu_assembly_direction >> 4) & 0x0f;
    ins.imu_conf.assemble_direction_param = raw_gps._imu_assembly_direction & 0x0f;
    ins.imu_conf.dgps_direction_param = raw_gps._dgps_assembly_direction;

    // 只有磁校准模式下才来赋值mag_cal_status
    if (pilot_mode == PILOT_MAG_CLB)
    {
        if (ins.mag_calib_flag != MAG_CAL_NO)
        {
            mag_cal_status = ins.mag_calib_flag;
        }
    }
}

/**
 * @brief dma解析sensor数据
 */
void dma_analysis_sensordata(void) {}

/**
 * @brief 解析 imu 收到的 vklink 类型消息
 *
 * @param rxMsg
 */
static void _imu_vklink_rx_msg_decode(const VKlink_Msg_Type *rxMsg)
{
    switch (imu_rx_msg.compid)
    {
    /* 如果是 IMU 的数据 */
    case 1:
    {
        imu_link_status = COMP_NORMAL;
        if (ins_data_time_sem == 0)
            ins_data_time = micros();

        switch (imu_rx_msg.msgid)
        {
        case IMU_MSG_ID:
        {
            memcpy(&raw_imu, imu_rx_msg.payload, sizeof(raw_imu));

            // 中断中需要更新姿态信息
            imu_attitude_ananlysis();

            // 400hz的姿态环
            attitude_pid_ctl_rp(&pid_m_roll, &pid_v_roll);
            attitude_pid_ctl_rp(&pid_m_pitch, &pid_v_pitch);
            attitude_pid_ctl_yaw(&pid_m_yaw, &pid_v_yaw);

            // 400AHZ的内环
            rate_control();

            // 如果不在电调校准程序
            if (pilot_mode == PILOT_NORMAL)
            {
                if(debug_mode == false)
                    unlocked_motor_output(); // 写输出到PWM信号。
            }
            recieve_isimudata = true;
        }
        break;

        case GPS_MSG_ID:
        {
            memcpy(&raw_gps, imu_rx_msg.payload, sizeof(raw_gps));
            recieve_isgpsdata = true;
        }
        break;

        case SENSOR_MSG_ID:
        {
            memcpy(&raw_sensor, imu_rx_msg.payload, sizeof(raw_sensor));
            recieve_issensordata = true;
        }
        break;

        case VER_MSG_ID:
        {
            get_imu_version = true;
            memcpy(&ins.imu_conf.version, &imu_rx_msg.payload[0], 4);
        }
        break;

        case GPS_POS_OFFSET_ID:
        {
            get_gps_pos_param = true;
            for (uint8_t i = 0; i < 6; ++i)
            {
                ins.imu_conf.gps_pos_param[i] = imu_rx_msg.payload[i];
            }
        }
        break;

        case IMU_POS_OFFSET_ID:
        {
            get_imu_pos_param = true;
            for (uint8_t i = 0; i < 3; ++i)
            {
                ins.imu_conf.imu_pos_param[i] = imu_rx_msg.payload[i];
            }
        }
        break;

        case ACK_MSG_ID:
        {
            uint8_t ack_id = imu_rx_msg.payload[0];
            switch (ack_id)
            {
            case CALIBRARION_MSG_ID:
            {
                uint8_t calib_id = imu_rx_msg.payload[1];
                switch (calib_id)
                {
                case LOCK_UNLOCK_CALIB:
                {
                    uint8_t id_content = imu_rx_msg.payload[2];
                    switch (id_content)
                    {
                    case LOCKED:
                        notify_imu_lock = false;
                        break;
                    case UNLOCKED:
                        notify_imu_unlock = false;
                        break;
                    }
                }
                break;
                case DISABLE_GPS_ALT:
                {
                    uint8_t id_content = imu_rx_msg.payload[2];
                    switch (id_content)
                    {
                    case 1:
                        notify_imu_disable_gpsalt = false;
                        break;

                    case 0:
                        notify_imu_enable_gpsalt = false;
                        break;

                    default:
                        break;
                    }
                }
                break;
                // 姿态清零
                case ATTITUE_CALIB:
                    // euler_angel_need_clear = false;
                    break;
                // 安装方向
                case ASSEMBLE_CALIB:
                    // change_imu_assembly_direction = false;
                    break;
                // 滤波参数
                case FILTER_PARAM_CALIB:
                    // change_imu_filter_param = false;
                    break;
                // 修改双差分天线安装方向
                case DGPS_DIRECTION_CALIB:
                    // change_imu_dgps_direction = false;
                    break;
                // 修改 IMU 安装位置
                case GPS_POS_CALIB:
                    for (uint8_t i = 0; i < 6; ++i)
                    {
                        ins.imu_conf.gps_pos_param[i] =
                            (int8_t)imu_rx_msg.payload[i + 2];
                    }
                    break;
                case IMU_POS_CALIB:
                    for (uint8_t i = 0; i < 3; ++i)
                    {
                        ins.imu_conf.imu_pos_param[i] =
                            (int8_t)imu_rx_msg.payload[i + 2];
                    }
                    break;

                default:
                    break;
                }
            }
            break;

            default:
                break;
            }
        }
        break;

        default:
            break;
        }
    }
    break; /* imu_rx_msg.compid case 1 end */

    /* 如果是 IMU BOOTLOADER 的数据 */
    case 201:
    {
        /* 转发给地面站以进行升级 */
        if (need_gcs_tx_imu_rx_msg == 0)
        {
            need_gcs_tx_imu_rx_msg = 1;
            memcpy(&gcs_tx_imu_msg, &imu_rx_msg, sizeof(imu_rx_msg));
        }

        switch (imu_rx_msg.msgid)
        {
        /* 如果是应答消息，从串口转发出去 */
        case 21:
        {
            uint8_t id_ack = imu_rx_msg.payload[0];
            switch (id_ack)
            {
            /* 如果是应答升级 end 消息，则将飞控退出 IMU 升级模式 */
            case 202:
                if (pilot_mode == PILOT_IMU_UPDATE)
                {
                    pilot_mode = PILOT_NORMAL;
                }
                break;
            }
        }
        break;

        /* 如果是 bootloader 状态消息 */
        case 199:
        {
            uint8_t id_status = imu_rx_msg.payload[0];
            switch (id_status)
            {
            /* bootloader 状态正常 */
            case 0:
                if (pilot_mode == PILOT_IMU_UPDATE)
                    pilot_mode = PILOT_NORMAL;
                break;
            /* bootloader 没有正常程序，需要升级 */
            case 1:
                if (pilot_mode != PILOT_IMU_UPDATE)
                    pilot_mode = PILOT_IMU_UPDATE;
                break;
            default:
                break;
            }
        }
        break;

        default:
            break;
        }
    }
    break; /* imu_rx_msg.compid case 201 end */

    default:
        break;
    }
}

/**
 * @brief imu 接收回调函数
 *
 * @param pbuffer
 * @param rx_length
 */
void recv_imu_data_hookfunction(const uint8_t *pbuffer, uint32_t rx_length)
{
    int parse_result = 0;

    if (pbuffer[0] == 0xFE && pbuffer[1] <= 255)
    {
        if (crc16_cyc_cal(0xFFFF, (uint8_t *)pbuffer, pbuffer[1] + 8) == 0)
        {
            parse_result = 1;
            imu_rx_msg.head = pbuffer[0];
            imu_rx_msg.payload_len = pbuffer[1];
            imu_rx_msg.seq = pbuffer[2];
            imu_rx_msg.sysid = pbuffer[3];
            imu_rx_msg.compid = pbuffer[4];
            imu_rx_msg.msgid = pbuffer[5];
            memcpy(imu_rx_msg.payload, (uint8_t *)&pbuffer[6],
                   imu_rx_msg.payload_len);
        }
        else
        {
            parse_result = -1;
        }
    }

    if (parse_result == 1)
    {
        _imu_vklink_rx_msg_decode(&imu_rx_msg);
    }
    else if (micros() > 10000000 && parse_result == -1)
    {
        dma_iserror = true;
    } /* parse char end */
}

void get_right_ins_data(void)
{
    // 处理DMA中的Imu数据
    if (recieve_isimudata == true)
    {
        dma_analysis_imudata();
        recieve_isimudata = false;
    }
    // 处理DMA中的Gps数据
    if (recieve_isgpsdata == true)
    {
        dma_analysis_gpsdata();
        recieve_isgpsdata = false;
    }
    // 处理DMA中的Sensor数据
    if (recieve_issensordata == true)
    {
        dma_analysis_sensordata();
        recieve_issensordata = false;
    }
}

/**
 * @brief 检测 IMU 链接状态
 */
void get_ins_status(void)
{
    static bool noimu_check_enable = false;
    static unsigned int noimu_time = 0;

    // 检测到IMU丢失,300ms无数据。
    // 中断中更新ins_data_time不加变量锁会出现冲突，出现负值使大于300ms成立。
    ins_data_time_sem = 1;
    uint32_t ins_data_time_err = micros() - ins_data_time;
    ins_data_time_sem = 0;

    if (!noimu_check_enable)
    {
        noimu_check_enable = true;
    }

    if (ins_data_time_err > 2000000 && imu_link_status == COMP_NORMAL &&
        noimu_check_enable)
    {
        imu_link_status = COMP_LOST;
        noimu_time = micros();
    }

    // IMU丢失后再缓冲200ms时间，如果不能恢复则直接落锁
    if (micros() - noimu_time > 100000 && imu_link_status == COMP_LOST)
    {
        if (thr_lock_status != LOCKED)
        {
            set_thr_lock_status(LOCKED, NOIMULOCK);
        }
    }

    // 收到IMU数据后闪板载灯
    if (imu_link_status == COMP_NORMAL)
    {
        v8m_yy_led_toggle(V8M_YY_LED_G);
    }
    else
    {
        v8m_yy_led_on(V8M_YY_LED_G);
    }

    // 收到GPS搜星后闪板载灯
    if (raw_gps._gps_num > 0 && raw_gps._gps_num <= 5)
    {
       v8m_yy_led_toggle(V8M_YY_LED_R);
    }
    else if (raw_gps._gps_num > 5)
    {
       v8m_yy_led_on(V8M_YY_LED_R);
    }
    else
    {
        v8m_yy_led_off(V8M_YY_LED_R);
    }
}

void send_vklink_msg_to_imu(const VKlink_Msg_Type *txMsg)
{
    uint8_t *tx_buff = USART3_Tx_Buf;

    uint32_t tx_len = VKlink_MsgTxFormat(txMsg, tx_buff);

    // open_usart3_dma_tx(tx_len);
    for (size_t i = 0; i < tx_len; ++i)
    {
        UART3_Put_Char(tx_buff[i]);
    }
}

void send_req_msg_to_imu(uint8_t req_id)
{
    VKlink_Msg_Type txMsg;
    uint32_t payload_index = 0;
    txMsg.head = VKLINK_MSG_HEAD;
    txMsg.seq = imu_vklink_sys.tx_seq++;
    txMsg.sysid = imu_vklink_sys.sysid;
    txMsg.compid = imu_vklink_sys.compid;
    txMsg.msgid = REQ_MSG_ID;

    txMsg.payload[payload_index++] = req_id;
    txMsg.payload[payload_index++] = 0;
    txMsg.payload[payload_index++] = 0;
    txMsg.payload[payload_index++] = 0;

    txMsg.payload_len = payload_index;

    send_vklink_msg_to_imu(&txMsg);
}

void send_calibration_msg_to_imu(uint8_t calib_id, uint8_t *calib_value,
                                 uint16_t len)
{
    VKlink_Msg_Type txMsg;
    uint32_t payload_index = 0;
    txMsg.head = VKLINK_MSG_HEAD;
    txMsg.seq = imu_vklink_sys.tx_seq++;
    txMsg.sysid = imu_vklink_sys.sysid;
    txMsg.compid = imu_vklink_sys.compid;
    txMsg.msgid = CALIBRARION_MSG_ID;

    txMsg.payload[payload_index++] = calib_id;
    for (uint16_t i = 0; i < len; ++i)
        txMsg.payload[payload_index++] = calib_value[i];

    txMsg.payload_len = payload_index;

    send_vklink_msg_to_imu(&txMsg);

    if (calib_id == GPS_POS_CALIB)
    {
        get_gps_pos_param = false;
    }

    if (calib_id == IMU_POS_CALIB)
    {
        get_imu_pos_param = false;
    }
}

// msgid0x20 互定位数据->IMU
void send_imu_hdw_data(uint8_t update_type, void *data)
{
    static VKlink_Msg_Type txMsg;
    txMsg.head = VKLINK_MSG_HEAD;
    txMsg.seq = imu_vklink_sys.tx_seq++;
    txMsg.sysid = imu_vklink_sys.sysid;
    txMsg.compid = imu_vklink_sys.compid;
    txMsg.msgid = HDW_MSG_ID;

#pragma pack(1)
    struct imu_hdw_data
    {
        int16_t x;  // dm
        int16_t y;  // dm
        uint16_t z; // dm
        uint16_t flag;
        int16_t delay_time; // ms
    };
    struct imu_pos_data
    {
        int lon; // 1e-7
        int lat; // 1e-7
    };
    struct imu_vel_data
    {
        int gps_alt_mm;     // mm
        int16_t gps_vel_mm; // mm/s
        int16_t gps_yaw;    // 0.1deg
        uint8_t fix_status;
        uint8_t gps_num;
    };
#pragma pack()
    switch (update_type)
    {
    case 0:
    {
        struct imu_hdw_data *p = (struct imu_hdw_data *)txMsg.payload;
        struct imu_hdw_data *pdata = (struct imu_hdw_data *)data;
        p->x = pdata->x;
        p->y = pdata->y;
        p->z = pdata->z;
        p->flag = pdata->flag;
        p->delay_time = hdw_can_delay;
        break;
    }
    case 1:
    {
        struct imu_pos_data *p = (struct imu_pos_data *)(txMsg.payload + sizeof(struct imu_hdw_data));
        struct imu_pos_data *pdata = (struct imu_pos_data *)data;
        p->lon = pdata->lon;
        p->lat = pdata->lat;
        break;
    }
    case 2:
    {
        struct imu_vel_data *p = (struct imu_vel_data *)(txMsg.payload + sizeof(struct imu_hdw_data) + sizeof(struct imu_pos_data));
        struct imu_vel_data *pdata = (struct imu_vel_data *)data;
        p->gps_alt_mm = pdata->gps_alt_mm;
        p->gps_vel_mm = pdata->gps_vel_mm;
        p->gps_yaw = pdata->gps_yaw;
        p->fix_status = pdata->fix_status;
        p->gps_num = pdata->gps_num;
        break;
    }
    case 3:
    {
        struct imu_hdw_data *p = (struct imu_hdw_data *)txMsg.payload;
        int16_t *pdata = (int16_t *)data;
        p->delay_time = *pdata;
    }
    default:
        break;
    }

    txMsg.payload_len = sizeof(struct imu_hdw_data)
        /*+ sizeof(struct imu_pos_data) + sizeof(struct imu_vel_data)*/;
    send_vklink_msg_to_imu(&txMsg);
}

/**
 * @brief 向IMU发送数据
 */
void send_msg_to_imu(void)
{
    // 记录上一次发送指令的时间
    static uint32_t send_imu_time;

    // ========== IMU 升级 ==============
    if (pilot_mode == PILOT_IMU_UPDATE)
    {
    }
    else
    {
        // --------- 请求IMU版本 -----------------
        if (get_imu_version == false && thr_lock_status == LOCKED &&
            micros() > 10000000)
        {
            if (!usart3_tx_isbusy() && micros() - send_imu_time > 50000)
            {
                send_req_msg_to_imu(VER_MSG_ID);
                send_imu_time = micros();
            }
        }
        // ---------- 请求 GPS 安装位置偏差 ------------
        else if (get_gps_pos_param == false && thr_lock_status == LOCKED)
        {
            if (!usart3_tx_isbusy() && micros() - send_imu_time > 50000)
            {
                send_req_msg_to_imu(GPS_POS_OFFSET_ID);
                send_imu_time = micros();
            }
        }
        // ---------- 请求 IMU 安装位置偏差 ------------
        else if (get_imu_pos_param == false && thr_lock_status == LOCKED)
        {
            if (!usart3_tx_isbusy() && micros() - send_imu_time > 50000)
            {
                send_req_msg_to_imu(IMU_POS_OFFSET_ID);
                send_imu_time = micros();
            }
        }
        // ----------- 磁 校 准 ---------------
        else if (ci_jiao_zhun == true && thr_lock_status == LOCKED &&
                 mag_cal_status == MAG_CAL_NO)
        {
            if (!usart3_tx_isbusy() && micros() - send_imu_time > 50000)
            {
                uint8_t calib_data[3] = {0};
                send_calibration_msg_to_imu(MAG_CALIB, calib_data, 0);
                send_imu_time = micros();

                ci_jiao_zhun = false;

                // 主动进入磁校准模式
                pilot_mode = PILOT_MAG_CLB;
            }
        }
        // ========== 通知 IMU 飞控解锁 =====
        else if (notify_imu_unlock == true)
        {
            if (!usart3_tx_isbusy() && micros() - send_imu_time > 200000)
            {
                uint8_t calib_value[3] = {1, 0, 0};
                send_calibration_msg_to_imu(LOCK_UNLOCK_CALIB, calib_value, 1);
                send_imu_time = micros();
            }
        }
        else if (notify_imu_disable_gpsalt)
        {
            if (!usart3_tx_isbusy() && micros() - send_imu_time > 50000)
            {
                uint8_t calib_value[3] = {1, 0, 0};
                send_calibration_msg_to_imu(DISABLE_GPS_ALT, calib_value, 1);
                send_imu_time = micros();
            }
        }
        else if (notify_imu_enable_gpsalt)
        {
            if (!usart3_tx_isbusy() && micros() - send_imu_time > 50000)
            {
                uint8_t calib_value[3] = {0, 0, 0};
                send_calibration_msg_to_imu(DISABLE_GPS_ALT, calib_value, 1);
                send_imu_time = micros();
            }
        }
    }
}

/**
 * @brief 对水平位置做窗口均值滤波，窗口宽度80
 * @retval none
 */
#define POS_FILTER_COUNT 80
char average_filter_counts;
static void horizental_pos_aver_filter(void)
{
    static uint8_t position_counts;
    static int pos_long_filter_buf[POS_FILTER_COUNT + 1] = {0};
    static int pos_lati_filter_buf[POS_FILTER_COUNT + 1] = {0};
    static int miss_long_filter_buf[POS_FILTER_COUNT + 1] = {0};
    static int miss_lati_filter_buf[POS_FILTER_COUNT + 1] = {0};

    // 定点的时候对经纬度做均值滤波，以及丢失的精度的找回
    pos_long_filter_buf[POS_FILTER_COUNT] -=
        pos_long_filter_buf[position_counts];
    pos_long_filter_buf[position_counts] = raw_imu._c_lng / POS_FILTER_COUNT;
    pos_long_filter_buf[POS_FILTER_COUNT] +=
        pos_long_filter_buf[position_counts];
    miss_long_filter_buf[POS_FILTER_COUNT] -=
        miss_long_filter_buf[position_counts];
    miss_long_filter_buf[position_counts] =
        raw_imu._c_lng -
        pos_long_filter_buf[position_counts] * POS_FILTER_COUNT;
    //	miss_long_filter_buf[position_counts] = Longitude_GPS %
    // POS_FILTER_COUNT;
    miss_long_filter_buf[POS_FILTER_COUNT] +=
        miss_long_filter_buf[position_counts];

    pos_lati_filter_buf[POS_FILTER_COUNT] -=
        pos_lati_filter_buf[position_counts];
    pos_lati_filter_buf[position_counts] = raw_imu._c_lat / POS_FILTER_COUNT;
    pos_lati_filter_buf[POS_FILTER_COUNT] +=
        pos_lati_filter_buf[position_counts];
    miss_lati_filter_buf[POS_FILTER_COUNT] -=
        miss_lati_filter_buf[position_counts];
    miss_lati_filter_buf[position_counts] =
        raw_imu._c_lat -
        pos_lati_filter_buf[position_counts] * POS_FILTER_COUNT;
    //	miss_lati_filter_buf[position_counts] = Latitude_GPS %
    // POS_FILTER_COUNT;
    miss_lati_filter_buf[POS_FILTER_COUNT] +=
        miss_lati_filter_buf[position_counts];

    position_counts++;
    if (position_counts >= POS_FILTER_COUNT)
        position_counts = 0;

    average_filter_counts++;

    if (average_filter_counts >= POS_FILTER_COUNT)
    {
        average_filter_counts = POS_FILTER_COUNT;
        pid_m_posx.loc_c =
            pos_long_filter_buf[POS_FILTER_COUNT] +
            miss_long_filter_buf[POS_FILTER_COUNT] / POS_FILTER_COUNT;

        pid_m_posy.loc_c =
            pos_lati_filter_buf[POS_FILTER_COUNT] +
            miss_lati_filter_buf[POS_FILTER_COUNT] / POS_FILTER_COUNT;
    }
    else
    {
        pid_m_posx.loc_c = raw_imu._c_lng;

        pid_m_posy.loc_c = raw_imu._c_lat;
    }
}

void unclock_clear_altitude(void)
{
    // 清0气压高度
    clear_alt_offset = plane_baro_alt + offset_alt;
    // 清0积分高度
    pid_m_alt.loc_c = 0;
}

/**
 * @brief 时间按照时区转换
 *
 * @param timezone 时区
 */
void UTCToLocalTime(int timezone)
{
    int year, month, day, hour;
    int lastday = 0;     // 月的最后一天的日期
    int lastlastday = 0; // 上月的最后一天的日期

    year = raw_gps._gps_year;            // 已知的UTC时间
    month = raw_gps._gps_month;          // 已知的UTC时间
    day = raw_gps._gps_day;              // 已知的UTC时间
    hour = raw_gps._gps_hour + timezone; // 已知的UTC时间

    switch (month)
    {
    case 1:
    case 3:
    case 5:
    case 7:
    case 8:
    case 10:
    case 12:
        lastday = 31;
        lastlastday = 30;
        if (month == 3)
        {
            // 判断是否为闰年，年号能被400整除或年号能被4整除，而不能被100整除为闰年
            if ((year % 400 == 0) || (year % 4 == 0 && year % 100 != 0))
                lastlastday = 29; // 闰年的2月为29天，平年为28天
            else
                lastlastday = 28;
        }
        if (month == 8)
            lastlastday = 31;
        break;

    case 4:
    case 6:
    case 9:
    case 11:
        lastday = 30;
        lastlastday = 31;
        break;

    case 2:
        lastlastday = 31;
        if ((year % 400 == 0) ||
            (year % 4 == 0 && year % 100 != 0)) // 闰年的2月为29天，平年为28天
            lastday = 29;
        else
            lastday = 28;
        break;

    default:
        break;
    }

    if (hour >= 24)
    { // 当算出的区时大于或等于24:00时，应减去24:00，日期加一天
        hour -= 24;
        day += 1;

        if (day > lastday)
        { // 当算出的日期大于该月最后一天时，应减去该月最后一天的日期，月份加上一个月
            day -= lastday;
            month += 1;

            if (month > 12)
            { // 当算出的月份大于12时，应减去12，年份加上一年
                month -= 12;
                year += 1;
            }
        }
    }

    if (hour < 0)
    { // 当算出的区时为负数时，应加上24:00，日期减一天
        hour += 24;
        day -= 1;
        if (day < 1)
        { // 当算出的日期为0时，日期变为上一月的最后一天，月份减去一个月
            day = lastlastday;
            month -= 1;
            if (month < 1)
            { // 当算出的月份为0时，月份变为12月，年份减去一年
                month = 12;
                year -= 1;
            }
        }
    }
    // 得到转换后的本地时间
    ins.gps_year = year;
    ins.gps_month = month;
    ins.gps_day = day;
    ins.gps_hour = hour;

    ins.gps_minute = raw_gps._gps_minute;
    if (ins.gps_second != raw_gps._gps_second)
    {
        ins.gps_second = raw_gps._gps_second;
        ins.last_gps_second_local_time = micros();
    }
    ins.gps_millisecond = raw_gps._gps_millisecond;
}


/**
 * @brief 向地面站发送 imu 来的消息
 *
 */
void imu_tx_msg_to_gcs(struct GCS_Link *pgcs)
{

    if (need_gcs_tx_imu_rx_msg == 1)
    {
        need_gcs_tx_imu_rx_msg = 0;
        gs_tx_vklink_msg(pgcs, &gcs_tx_imu_msg);
    }
}