V9_PMU/Src/soft_obstacle.c

#include "soft_obstacle.h"
#include "tim.h"
#include "string.h"
#include "math.h"
#include "soft_terrain.h"
#include "soft_uart.h"
#include "common.h"
#include "soft_can.h"
#include "soft_p_2_c.h"
#include "stdlib.h"
#include "soft_flow.h"
#include "common.h"
#include "soft_seed_device.h"
#include "soft_water_device.h"
#include "soft_version.h"
#include "soft_eft.h"
#include "common.h"

uavr_obs uavr11_info = {.get_radar_sensi = 50};
uavr_obs uavr12_info= {.get_radar_sensi = 50};
uavr_obs mimo_f_info = {.signal_qulity = 0};
uavr_obs mimo_b_info = {.signal_qulity = 0};

uavr_obs DM_f_info;
/**
  * @file    can_recv_enzhao_obstacle
  * @brief   恩曌多点避障解析
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
mimo_part_radar F_radar[3];
mimo_part_radar B_radar[3];
uint8_t recv_comF_flag = 0,recv_comB_flag = 0;
void can_recv_enzhao_obstacle(uint32_t cellCanID, uint8_t data[], uint8_t len)
{

    switch (cellCanID)
    {
    case CAN_MIMO_FOBS_ID1:
        memcpy(&F_radar[0], data, 8);
        recv_comF_flag = recv_comF_flag | 1;
        Dev.Radar.facid_F = FAC_MIMO_RF;
        break;
    case CAN_MIMO_FOBS_ID2:
        memcpy(&F_radar[1], data, 8);
        recv_comF_flag = recv_comF_flag | 2;
        break;
    case CAN_MIMO_FOBS_ID3:
        memcpy(&F_radar[2], data, 8);
        recv_comF_flag = recv_comF_flag | 4;
        break;
    case CAN_MIMO_BOBS_ID1:
        memcpy(&B_radar[0], data, 8);
        recv_comB_flag = recv_comB_flag | 1;
        Dev.Radar.facid_F = FAC_MIMO_RB;
        break;
    case CAN_MIMO_BOBS_ID2:
        memcpy(&B_radar[1], data, 8);
        recv_comB_flag = recv_comB_flag | 2;
        break;
    case CAN_MIMO_BOBS_ID3:
        memcpy(&B_radar[2], data, 8);
        recv_comB_flag = recv_comB_flag | 4;
        break;
    default:
        break;
    }

    if(recv_comF_flag == 7)
    {
        recv_comF_flag = 0;      
        mimo_buf_sort(&F_radar[0], 3);

        for (uint8_t i = 0; i < 3; i++)
        {
            //X轴小于4M内数据
            if (/*(abs(F_radar[i].Distance * 0.05f * 100 * sin(F_radar[i].Amuzith * 0.1f / RAD)) < 400) &&*/ F_radar[i].Distance > 0)
            {
                mimo_f_info.distance_x  = F_radar[i].Distance * 0.05f * 100 * sin(F_radar[i].Amuzith * 0.1f / RAD);
                mimo_f_info.distance_y = F_radar[i].Distance * 0.05f * 100 * cos(F_radar[i].Amuzith * 0.1f / RAD);
                break;
            }
            if(i == 2)
            {
                mimo_f_info.distance_x = 0;
                mimo_f_info.distance_y = 0;
            }
        }
        mimo_f_info.Link.connect_status = COMP_NORMAL;
        mimo_f_info.Link.recv_time = HAL_GetTick();
    }
    
    if(recv_comB_flag == 7)
    {
        recv_comB_flag = 0;
        mimo_buf_sort(&B_radar[0], 3);

        for (uint8_t i = 0; i < 3; i++)
        {
            //X轴小于4M内数据
            if (/*(abs(B_radar[i].Distance * 0.05f * 100 * sin(B_radar[i].Amuzith * 0.1f / RAD)) < 400) &&*/ B_radar[i].Distance > 0)
            {
                mimo_b_info.distance_x  = B_radar[i].Distance * 0.05f * 100 * sin(B_radar[i].Amuzith * 0.1f / RAD);
                mimo_b_info.distance_y = B_radar[i].Distance * 0.05f * 100 * cos(B_radar[i].Amuzith * 0.1f / RAD);
                break; 
            }
            if(i == 2)
            {
                mimo_b_info.distance_x = 0;
                mimo_b_info.distance_y = 0;
            }
        }
        mimo_b_info.Link.recv_time = HAL_GetTick();
        mimo_b_info.Link.connect_status = COMP_NORMAL;
    }
}


/**
  * @file    can_recv_enzhao_obstacle
  * @brief   恩曌单避障解析
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
void can_recv_mimo_signal_obstacle(uint32_t cellCanID, uint8_t data[], uint8_t len)
{
    uint16_t frame_header = 0;

    memcpy(&frame_header,&data[0],2);

    if(frame_header == 0x5AA5 && data[2] == 0x04)
    {
        switch (cellCanID)
        {
        case CAN_MIMO_FOBS_SIG:
            mimo_f_info.distance_y = data[3] + data[4] * 256;
            mimo_f_info.signal_qulity = data[5];

            mimo_f_info.Link.recv_time = HAL_GetTick();
            mimo_f_info.Link.connect_status = COMP_NORMAL;
            Dev.Radar.facid_F = FAC_MIMO_RF;
            break;
        case CAN_MIMO_BOBS_SIG:
            mimo_b_info.distance_y = data[3] + data[4] * 256;
            mimo_b_info.signal_qulity = data[5];
                        
            mimo_b_info.Link.recv_time = HAL_GetTick();
            mimo_b_info.Link.connect_status = COMP_NORMAL;
            Dev.Radar.facid_B = FAC_MIMO_RB;            
            break;
        default:
            break;
        }
    }
}

/**
  * @file    can_recv_mimo_radar_version
  * @brief   恩曌设备版本获取
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
void can_recv_mimo_radar_version(uint32_t cellCanID, uint8_t data[], uint8_t len)
{
    static uint8_t mimo_version[28] = {0};
    static uint8_t frame_num = 0;
    static bool frame_head = false;
    uint8_t i = 0;
    uint32_t checksum = 0;
    uint32_t soft_ver = 0;
    switch (cellCanID)
    {
    case 0XFB:
        if(data[0] == 0XAA && data[1] == 0X55)
        {
            if(data[2] == 0X03 && data[3] == 0X61 && data[4] == 0X03 && data[5] == 0X01 && data[6] == 0X65)
            {
                pmu_set_ack(_MSGID_SET,MSGID_SET_RADAR_FB,0x11,0);
            }
            else if(data[2] == 0X03 && data[3] == 0X61 && data[4] == 0X03 && data[5] == 0X02 && data[6] == 0X66)
            {
                pmu_set_ack(_MSGID_SET,MSGID_SET_RADAR_FB,0x12,0);
            }
            else if(data[2] == 0X03 && data[3] == 0X61 && data[4] == 0X00 && data[5] == 0X03 && data[6] == 0X64)
            {
                pmu_set_ack(_MSGID_SET,MSGID_SET_RADAR_FB,0x100,0);
            }
            else
            {
                frame_num = 0;
                memcpy(&mimo_version[frame_num],&data[4],4);
                frame_num += 4;
                frame_head = true;
            }
            
        }
        //断料记
        else if(data[0] == 0xFB && data[1] == 0x03 && data[2] == 0)
        {   
            //设置距离最大值反馈
            if(data[3] == 0xB1 && data[4] == 0xc1 && data[5] == 0)
            {
                pmu_set_ack(_MSGID_SET,MSGID_SET_LACKLOSS_CAL,mimo_lackloss.cal_distance,mimo_lackloss.strength);
            }
        }
        //流量计
        else if(data[0] == 0xFB && data[1] == 0x10) //雷达版本和流量计协议有冲突
        {
            switch (data[2])
            {
            case 0:
                if(data[4] == 0XD1) //流速K
                {
                    flow_mimo1.flow_k = data[5] * 256 + data[6]; 
                }
                else if(data[4] == 0XC1)//设置流速K ack
                {
                    if(data[6] == 0)
                    {
                        flow_mimo1.flow_k = flow_mimo1.flow_calk;
                        if(Dev.Flow_Link1.connect_status == COMP_NORMAL && Dev.Flow_Link2.connect_status != COMP_NORMAL)
                        {
                            pmu_set_ack(_MSGID_SET,MSGID_SET_MIMO_FLOW,flow_mimo1.flow_k,0);
                        }
                    }
                }
                else if(data[4] == 0xE2)
                {
                    if(data[6] == 0 && Dev.Flow_Link1.connect_status == COMP_NORMAL && Dev.Flow_Link2.connect_status != COMP_NORMAL)
                    {
                        pmu_set_ack(_MSGID_SET,MSGID_SET_FLOW_BACKGROUND,0,0);
                    }
                    flow_inf.ch1.clear_background = false;
                }
                else if (data[4] == 0XEB)//sn 两包 协议冲突 协议有问题
                {
                    for( i= 0;i<2;i++)  //内容第一自字节0X0F？先舍弃一字节，
                    {
                        flow_mimo1.sn[2*i] = ((data[6 + i] >> 4) & 0xf) + '0';
                        flow_mimo1.sn[2*i+1] = (data[6 + i] & 0xf )+ '0';
                    }
                    frame_num = 21;//流量序列号的标记
                }
                else if(data[4] == 0XEC)//软件号
                {
                    flow_mimo1.soft_version[0] = data[5] + '0';
                    flow_mimo1.soft_version[1] = data[6] + '0';

                }
                break;
            case 1:
                if(data[4] == 0XD1)
                {
                    flow_mimo2.flow_k = data[5] * 256 + data[6];
                }
                else if(data[4] == 0XC1)
                {
                    if(data[6] == 0)
                    {
                        flow_mimo2.flow_k = flow_mimo2.flow_calk;
                        if(Dev.Flow_Link1.connect_status == COMP_NORMAL && Dev.Flow_Link2.connect_status == COMP_NORMAL)
                        {
                            pmu_set_ack(_MSGID_SET,MSGID_SET_MIMO_FLOW,flow_mimo1.flow_k,flow_mimo2.flow_k);
                        }
                        else if(Dev.Flow_Link1.connect_status != COMP_NORMAL && Dev.Flow_Link2.connect_status == COMP_NORMAL)
                        {
                            pmu_set_ack(_MSGID_SET,MSGID_SET_MIMO_FLOW,0,flow_mimo2.flow_k);
                        }
                    }
                }
                else if(data[4] == 0xE2)
                {
                    if(data[6] == 0 && Dev.Flow_Link1.connect_status == COMP_NORMAL && Dev.Flow_Link2.connect_status == COMP_NORMAL)
                    {
                        pmu_set_ack(_MSGID_SET,MSGID_SET_FLOW_BACKGROUND,0,0);
                    }
                    flow_inf.ch1.clear_background = false;
                }
                break;
            default:
                break;
            }
        }
        else
        {   
            if(frame_num == 21)
            {
                for( i=0;i<5;i++)
                {
                    flow_mimo1.sn[4+2*i] = ((data[i] >> 4) & 0xf) + '0';
                    flow_mimo1.sn[4+2*i+1] = (data[i] & 0xf )+ '0';
                }
                //flow_mimo1.sn[1] = 'F';
                frame_num = 0;
            }
            if(frame_num <= 20)
            {
                if(frame_head == true)
                {
                    memcpy(&mimo_version[frame_num],&data[0],8);
                    frame_num += 8;
                }
                if(frame_num == 0x1c) //恩曌SN取中间日期12位，其他有问题
                {
                    checksum = 0x01;
                    for(uint8_t i = 0; i < 27; i++)
                    {
                        checksum += mimo_version[i];
                    }
                    if((checksum & 0xFF) == mimo_version[27])
                    {
                        memcpy(&soft_ver,&mimo_version[23],4);

                        switch (mimo_version[18])
                        {
                        case 0x00:
                            mimo_ter_info.version[0] = 'E';
                            mimo_ter_info.version[1] = 'B';
                            mimo_ter_info.version[2] = '0';
                            mimo_ter_info.version[3] = '0';
                            Int2String(soft_ver,&mimo_ter_info.version[4],6);
                            memcpy(&mimo_ter_info.hard_version,&mimo_version[19],4);
                            if(mimo_ter_info.hard_version == 210221)
                            {
                                mimo_ter_info.version[3] = 'J';
                            }
                            for( i= 0;i<9;i++)
                            {
                                mimo_ter_info.sn[2*i] = ((mimo_version[7 + i] >> 4) & 0xf) + '0';
                                mimo_ter_info.sn[2*i+1] = (mimo_version[7 + i] & 0xf )+ '0';
                            }
                           //memcpy(&mimo_ter_info.sn[0],&mimo_version[4],18);

                            regist_dev_info(&dev_ter,DEVICE_TERRA,false,mimo_ter_info.sn,18,mimo_ter_info.version,10,NULL,0,"mimo",5);

                            mimo_ter_info.get_radar_ver_flag = true;
                            mimo_ter_info.Link.boot_flag = false;
                            break;
                        case 0x03:
                            mimo_f_info.version[0] = 'E';
                            mimo_f_info.version[1] = '1';
                            mimo_f_info.version[2] = '0';
                            mimo_f_info.version[3] = '0';
                            Int2String(soft_ver,&mimo_f_info.version[4],6);
                            memcpy(&mimo_f_info.hard_version,&mimo_version[19],4);
                            if(mimo_f_info.hard_version == 190302) //恩曌协议定义
                            {
                                mimo_f_info.version[2] = '1'; //极翼
                                mimo_f_info.version[3] = 'J';
                            }
                            else
                            {
                                mimo_f_info.version[2] = '0'; //vk
                                mimo_f_info.version[3] = 'V';
                            }

                            for( i= 0;i<9;i++)
                            {
                                mimo_f_info.sn[2*i] = ((mimo_version[7 + i] >> 4) & 0xf) + '0';
                                mimo_f_info.sn[2*i+1] = (mimo_version[7 + i] & 0xf )+ '0';
                            }
                            //memcpy(&mimo_f_info.sn[0],&mimo_version[4],12);

                            regist_dev_info(&dev_obsf,DEVICE_OBSF,false,mimo_f_info.sn,18,mimo_f_info.version,10,mimo_f_info.version,10,"mimo",5);
                            mimo_f_info.get_radar_ver_flag = true;
                            mimo_f_info.Link.boot_flag = false;
                            break;
                        case 0x04:
                            mimo_b_info.version[0] = 'E';
                            mimo_b_info.version[1] = '2';
                            mimo_b_info.version[2] = '0';
                            mimo_b_info.version[3] = '0';
                            Int2String(soft_ver,&mimo_b_info.version[4],6);
                            memcpy(&mimo_b_info.hard_version,&mimo_version[19],4);
                            if(mimo_b_info.hard_version == 190302) //恩曌协议定义
                            {
                                mimo_b_info.version[2] = '1'; //极翼
                                mimo_b_info.version[3] = 'J';
                            }
                            else
                            {
                                mimo_b_info.version[2] = '0'; //vk
                                mimo_b_info.version[3] = 'V';
                            }

                            for( i= 0;i<9;i++)
                            {
                                mimo_b_info.sn[2*i] = ((mimo_version[7 + i] >> 4) & 0xf) + '0';
                                mimo_b_info.sn[2*i+1] = (mimo_version[7 + i] & 0xf )+ '0';
                            }
                            //memcpy(&mimo_b_info.sn[0],&mimo_version[4],12);
                            regist_dev_info(&dev_obsb,DEVICE_OBSB,false,mimo_b_info.sn,18,mimo_b_info.version,10,mimo_b_info.version,10,"mimo",5);
                            mimo_b_info.get_radar_ver_flag = true;
                            mimo_b_info.Link.boot_flag = false;
                            break;
                        default:
                            break;
                        }
                        frame_num = 0;
                        frame_head = false;
                        pmu_send = PMU_SEND_VERSION;
                    } 
                }
            }
            else
            {   
            }

        }

        // char_to_hex_string(&data[5], 3, &mimo_ter_info.version[4], 6, "00");
        // mimo_ter_info.soft_verison = ((data[5] & 0xff) << 16) + ((data[6] & 0xff) << 8) + (data[7] & 0xff);        
        break;
    
    default:
        break;
    }
}


/**
  * @file    can_recv_mocib_F_obstacle
  * @brief   莫之比前避障解析
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
bool obs_f_is_link = false;
void can_recv_mocib_F_obstacle(uint8_t *data)
{

    uavr11_info.distance_x= (data[0] << 8) + data[1] - 32768;
    uavr11_info.distance_y = (data[2] << 8) + data[3];

    // if(abs(uavr11_info.distance_x) > 400)
    // {
    //     uavr11_info.distance_y = 0;
    //     uavr11_info.distance_x = 0;
    // }
    uavr11_info.Link.connect_status = COMP_NORMAL;
    uavr11_info.Link.recv_time = HAL_GetTick();
}

// /**
//   * @brief 恩曌360雷达
//   */
// #pragma pack(push)
// #pragma pack(1)
// typedef struct
// {
//     uint8_t totalSect; //总分区数
//     uint8_t validSect; // 有效分区数
//     uint8_t cycleCounter;
//     uint8_t reserve0;
//     uint32_t reserve : 20;
//     uint32_t height : 12; // 精度 0.01m
// } FrameHeader1S;
// typedef struct
// {
//     uint8_t sectID; //分区 ID
//     uint16_t dis;   //距离
//     int16_t ele;
//     uint8_t rcs; //目标雷达截面积
//     int16_t reserve1;
// } SectionPackS;
// typedef struct
// {
//     uint32_t ID;
//     uint8_t data[8];
// } CanMessage_t;
// #pragma pack(pop)

// #define MAX_SECTNUM 180

// typedef struct
// {
//     uint32_t totalSect; //总分区个数，数组中对应位置对应目标所在分区
//     uint32_t validSect; //总分区个数，数组中对应位置对应目标所在分区
//     uint32_t completeQ; // 0 代表无数据 1 代表数据接收到但不全 2 代表全部接收成功
//     float height;
//     float dis[MAX_SECTNUM]; // 雷达到目标距离,当目标不存在时距离为
//     float ele[MAX_SECTNUM]; // 目标的 RCS
//     float rcs[MAX_SECTNUM]; // 目标的 RCS
// } TargetInfoS;              // 原始目标结构信息

// TargetInfoS Targetbuffer = {0}; // 用于缓存
// TargetInfoS TargetOut = {0};

// static int LastSection = -1;
// int ValidSect = 0;

// comp_status mimo360_link_status = COMP_NOEXIST;
// void can_recv_mocib_360_obstacle(uint32_t cellCanID, uint8_t data[], uint8_t len)
// {
//     int index;
//     CanMessage_t *can_tmp_ptr = NULL;
//     CanMessage_t can_tmp;
//     FrameHeader1S *frameHead_ptr;
//     SectionPackS *pack_ptr;

//     can_tmp_ptr = &can_tmp;
//     can_tmp_ptr->ID = cellCanID;
//     memcpy(&can_tmp_ptr->data[0], data, len);
//     mimo360_link_status = COMP_NORMAL;

//     if (can_tmp_ptr->ID == 0x301) // 检测到帧头
//     {
//         if (Targetbuffer.completeQ == 1) // 如果数据未接完
//         {
//             memcpy(&TargetOut, &Targetbuffer, sizeof(Targetbuffer));
//         }

//         memset(&Targetbuffer, 0, sizeof(Targetbuffer));
//         frameHead_ptr = (FrameHeader1S *)can_tmp_ptr->data;
//         Targetbuffer.totalSect = frameHead_ptr->totalSect;
//         Targetbuffer.height = frameHead_ptr->height / 100.0F;

//         if (frameHead_ptr->validSect == 0) // 如果不存在分区数据
//         {
//             Targetbuffer.completeQ = 2;
//             Targetbuffer.validSect = 0;
//             memcpy(&TargetOut, &Targetbuffer, sizeof(Targetbuffer));
//         }
//         else // 如果存在分区数据
//         {
//             ValidSect = frameHead_ptr->validSect;
//             Targetbuffer.completeQ = 1;
//         }
//         LastSection = -1;
//     }
//     else if (can_tmp_ptr->ID == 0x302) // 检测到目标数据
//     {
//         pack_ptr = (SectionPackS *)can_tmp_ptr->data;
//         if (Targetbuffer.completeQ == 1) //如果数据未接收完
//         {
//             index = pack_ptr->sectID; //获取分区 ID
//             if (LastSection == -1)
//             {
//                 Targetbuffer.dis[index] = pack_ptr->dis * 0.01F;
//                 Targetbuffer.ele[index] = pack_ptr->ele * 0.01F;
//                 Targetbuffer.rcs[index] = pack_ptr->rcs;
//                 Targetbuffer.validSect++;
//                 LastSection = index;
//             }
//             else if (LastSection < index) //分区数据是按照从小到大输出才是正确的
//             {
//                 Targetbuffer.dis[index] = pack_ptr->dis * 0.01F;
//                 Targetbuffer.ele[index] = pack_ptr->ele * 0.1F;
//                 Targetbuffer.rcs[index] = pack_ptr->rcs;
//                 Targetbuffer.validSect++;
//             }
//             else // 如果不是则存在丢失包含帧 ID 的多个数据包
//             {
//                 Targetbuffer.completeQ = 0; //数据不保存
//             }
//             if (Targetbuffer.completeQ == 1)
//             {
//                 if (ValidSect == Targetbuffer.validSect) //有效个数等于实际接收个数
//                 {
//                     Targetbuffer.completeQ = 2;
//                     memcpy(&TargetOut, &Targetbuffer, sizeof(Targetbuffer));
//                     memset(&Targetbuffer, 0, sizeof(Targetbuffer));
//                 }
//             }
//         }
//     }
// }


/**
  * @file    can_recv_mocib_B_obstacle
  * @brief   莫之比后避障障解析
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
bool obs_b_is_link = false;
void can_recv_mocib_B_obstacle(uint8_t *data)
{

    uavr12_info.distance_x = (data[0] << 8) + data[1] - 32768;
    uavr12_info.distance_y = (data[2] << 8) + data[3];

    // if(abs(uavr12_info.distance_x) > 400)
    // {
    //     uavr12_info.distance_x = 0;
    //     uavr12_info.distance_y = 0;
    // }
    uavr12_info.Link.connect_status = COMP_NORMAL;
    uavr12_info.Link.recv_time = HAL_GetTick();
}

uint32_t uavr20_ver_time = 0;

uint32_t uavr20_sensi_time = 0;
char can_get_uavr21_ver_comp = 0;


/**
  * @file    can_sendmsg_uavr20
  * @brief   给墨汁比雷达发送无人机姿态信息
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
uint32_t uavr20_send_time = 0;
void can_sendmsg_uavr20(void)
{
    if (uavr12_info.Link.connect_status == COMP_NORMAL || uavr11_info.Link.connect_status == COMP_NORMAL || 
        uavr56_info.Link.connect_status == COMP_NORMAL)
    {
        //10hz发送
        if ((HAL_GetTick() - uavr20_send_time > 100) && planep.lock_status == 1)
        {
            uavr20_send_time = HAL_GetTick();

            int16_t index = 0;
            short tmpShort = 0;
            uint8_t send_uavr20_data[16] = {0};

            // 开头
            send_uavr20_data[index++] = 0XA5;

            // 俯仰
            tmpShort = planep.pitch_angle;
            short2buf(&send_uavr20_data[index], &tmpShort);
            index += 2;

            // 前后速度
            tmpShort = planep.E_vel * sinf(planep.yaw / 100.0f * DEG_TO_RAD) +
                       planep.N_vel * cosf(planep.yaw / 100.0f * DEG_TO_RAD);
            short2buf(&send_uavr20_data[index], &tmpShort);
            index += 2;

            // 横滚
            tmpShort = planep.roll_angle;
            short2buf(&send_uavr20_data[index], &tmpShort);
            index += 2;

            // 左右速度
            tmpShort = planep.E_vel * cosf(planep.yaw / 100.0f * DEG_TO_RAD) +
                       planep.N_vel * sinf(planep.yaw / 100.0f * DEG_TO_RAD);
            short2buf(&send_uavr20_data[index], &tmpShort);
            index += 2;

            // 后边的都没用上
            // 上下加速度
            tmpShort = planep.alt_vel;
            short2buf(&send_uavr20_data[index], &tmpShort);
            index += 2;

            // 仿地最近距离
            tmpShort = uavh30_dist.near;
            short2buf(&send_uavr20_data[index], &tmpShort);
            index += 2;

            // 仿地最远距离
            tmpShort = uavh30_dist.far;
            short2buf(&send_uavr20_data[index], &tmpShort);
            index += 2;

            // 结束
            send_uavr20_data[index++] = 0X5A;

            can_send_msg_normal(send_uavr20_data, sizeof(send_uavr20_data), SEND_UAV20_MSG);
        }
        //读取前雷达版本
        else
        {
            
            //读取前雷达灵敏度
            if (uavr11_info.Link.connect_status == COMP_NORMAL && uavr11_info.get_radar_sensi_flag == false &&
                 uavr11_info.get_radar_sensi_count < 5)
            {
                if (HAL_GetTick() - uavr20_sensi_time > 1000)
                {
                    // 开头
                    uint8_t send_uavr20_sensi[1] = {0};
                    send_uavr20_sensi[0] = 0x11;

                    can_send_msg_normal(send_uavr20_sensi, 1, CAN_UAVRH_SENSI_RA);

                    uavr20_sensi_time = HAL_GetTick();
                    uavr11_info.get_radar_sensi_count++;
                }
            }
            else
            {
                //读取后雷达灵敏度
                if (uavr12_info.Link.connect_status == COMP_NORMAL && uavr12_info.get_radar_sensi_flag == false && 
                    uavr12_info.get_radar_sensi_count < 5)
                {
                    if (HAL_GetTick() - uavr20_sensi_time > 1000)
                    {
                        // 开头
                        uint8_t send_uavr20_sensi[1] = {0};
                        send_uavr20_sensi[0] = 0x12;

                        can_send_msg_normal(send_uavr20_sensi, 1, CAN_UAVRH_SENSI_RA);

                        uavr20_sensi_time = HAL_GetTick();
                        uavr12_info.get_radar_sensi_count++;
                    }
                }
                else if(uavr56_info.Link.connect_status == COMP_NORMAL && uavr56_info.get_radar_sensi_flag == false &&
                        uavr56_info.get_radar_sensi_count < 5)
                {
                    if (HAL_GetTick() - uavr20_sensi_time > 1000)
                    {
                        // 开头
                        uint8_t send_uavr20_sensi[1] = {0};
                        send_uavr20_sensi[0] = 0x0B;

                        can_send_msg_normal(send_uavr20_sensi, 1, CAN_UAVRH_SENSI_RA);

                        uavr20_sensi_time = HAL_GetTick();
                        uavr56_info.get_radar_sensi_count++;
                    }

                }
            }
        }
    }
}



bool uavrhup_getr1_ack = false;

short obsfradar_sensitivity = 50;
short obsbradar_sensitivity = 50;


/**
  * @file    can_set_radar_sensi
  * @brief   设置雷达灵敏度
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
void can_set_radar_sensi()
{   
    static int radar_sensi_ack_time = 0;
    // 设置前雷达灵敏度
    if (uavr11_info.get_radar_sensi_flag == true && 
        uavr11_info.fcu_set_sensi_flag == true && uavr11_info.set_radar_sensi_count < 5 &&
        HAL_GetTick() - uavr20_sensi_time > 1000 && uavr11_info.Link.connect_status == COMP_NORMAL)
    {
        uint8_t send_uavr20_sensi[3] = {0};

        uavr11_info.set_radar_sensi_count++;
        uavr20_sensi_time = HAL_GetTick();
        if (uavrhup_getr1_ack == false)
        {
            //设置灵敏度先进入boot模式 新版本不进入boot
            if(uavr11_info.soft_verison >= RADAR_NER_VERSION )
            {
                uavrhup_getr1_ack = true;
            }
            else
            {
                send_uavr20_sensi[0] = 0x11;
                can_send_msg_normal(send_uavr20_sensi, 1, CAN_UAVRH_UPDATE_S1);                
            }

            uavr11_info.set_radar_sensi_ack = false;
            
        }
        else
        {
            if (uavr11_info.set_radar_sensi_ack == false)
            {
                send_uavr20_sensi[0] = 0x11;
                //大端方式发送
                send_uavr20_sensi[1] = (obsfradar_sensitivity >> 8) & 0xff;
                send_uavr20_sensi[2] = (obsfradar_sensitivity)&0xff;
                can_send_msg_normal(send_uavr20_sensi, sizeof(send_uavr20_sensi), CAN_UAVRH_SENSI_SA);
            }
            else
            {
                uavr11_info.fcu_set_sensi_flag = false;
                uavrhup_getr1_ack = false;
                uavr11_info.set_radar_sensi_ack = false;

                uavr11_info.set_radar_sensi_count = 0;
            }
        }

        //超过5次失败后恢复
        if (uavr11_info.set_radar_sensi_count >= 5)
        {
            uavr11_info.fcu_set_sensi_flag = false;
            uavrhup_getr1_ack = false;
            uavr11_info.set_radar_sensi_ack = false;
            uavr11_info.set_radar_sensi_count = 0;
        }
    }
    //设置后雷达灵敏度
    else if (uavr12_info.get_radar_sensi_flag == true &&
             uavr12_info.fcu_set_sensi_flag == true && uavr12_info.set_radar_sensi_count < 5 &&
             HAL_GetTick() - uavr20_sensi_time > 1000 && uavr12_info.Link.connect_status == COMP_NORMAL)
    {
        uint8_t send_uavr20_sensi[3] = {0};

        uavr12_info.set_radar_sensi_count++;
        uavr20_sensi_time = HAL_GetTick();


        if (uavrhup_getr1_ack == false)
        {
            //设置灵敏度先进入boot模式 新版本不进入boot
            if(uavr12_info.soft_verison >= RADAR_NER_VERSION )
            {
                uavrhup_getr1_ack = true;
            }
            else
            {
                //设置灵敏度先进入boot模式
                send_uavr20_sensi[0] = 0x12;
                can_send_msg_normal(send_uavr20_sensi, 1, CAN_UAVRH_UPDATE_S1);
            }

            uavr12_info.set_radar_sensi_ack = false;
        }
        else
        {
            if (uavr12_info.set_radar_sensi_ack == false)
            {
                send_uavr20_sensi[0] = 0x12;
                send_uavr20_sensi[1] = (obsbradar_sensitivity >> 8) & 0xff;
                send_uavr20_sensi[2] = (obsbradar_sensitivity)&0xff;
                can_send_msg_normal(send_uavr20_sensi, sizeof(send_uavr20_sensi), CAN_UAVRH_SENSI_SA);
            }
            else
            {
                uavr12_info.fcu_set_sensi_flag = false;
                uavrhup_getr1_ack = false;
                uavr12_info.set_radar_sensi_ack = false;

                uavr12_info.set_radar_sensi_count = 0;
            }
        }

        //超过5次失败后恢复
        if (uavr12_info.set_radar_sensi_count >= 5)
        {
            uavr12_info.fcu_set_sensi_flag = false;
            uavrhup_getr1_ack = false;
            uavr12_info.set_radar_sensi_ack = false;
            uavr12_info.set_radar_sensi_count = 0;
        }
    }
    //设置仿地雷达灵敏度
    else if (uavr56_info.get_radar_sensi_flag == true &&
             uavr56_info.fcu_set_sensi_flag == true && uavr56_info.set_radar_sensi_count < 5 &&
             HAL_GetTick() - uavr20_sensi_time > 1000 && uavr56_info.Link.connect_status == COMP_NORMAL)
    {
        uint8_t send_uavr20_sensi[3] = {0};

        uavr56_info.set_radar_sensi_count++;
        uavr20_sensi_time = HAL_GetTick();

        if (uavrhup_getr1_ack == false)
        {
            if(uavr56_info.soft_verison >= RADAR_NER_VERSION )
            {
                uavrhup_getr1_ack = true;
            }
            else
            {
                //设置灵敏度先进入boot模式
                send_uavr20_sensi[0] = 0x0B;
                can_send_msg_normal(send_uavr20_sensi, 1, CAN_UAVRH_UPDATE_S1);
            }

            uavr56_info.set_radar_sensi_ack = false;
        }
        else
        {
            if (uavr56_info.set_radar_sensi_ack == false)
            {
                send_uavr20_sensi[0] = 0x0B;
                send_uavr20_sensi[1] = (uavr56_info.fcu_set_sensi >> 8) & 0xff;
                send_uavr20_sensi[2] = (uavr56_info.fcu_set_sensi)&0xff;
                can_send_msg_normal(send_uavr20_sensi, sizeof(send_uavr20_sensi), CAN_UAVRH_SENSI_SA);
            }
            else
            {
                uavr56_info.fcu_set_sensi_flag = false;
                uavrhup_getr1_ack = false;
                uavr56_info.set_radar_sensi_ack = false;

                uavr56_info.set_radar_sensi_count = 0;
            }
        }

        //超过5次失败后恢复
        if (uavr56_info.set_radar_sensi_count >= 5)
        {
            uavr56_info.fcu_set_sensi_flag = false;
            uavrhup_getr1_ack = false;
            uavr56_info.set_radar_sensi_ack = false;
            uavr56_info.set_radar_sensi_count = 0;
        }
    }

    //设置莫之比避障灵敏度成功后ACK主控
    if (uavr11_info.set_radar_sensi_ack == true || uavr12_info.set_radar_sensi_ack == true || uavr56_info.set_radar_sensi_ack == true)
    {   
        //同时设置有个1.5s间隔
        if(HAL_GetTick() - radar_sensi_ack_time > 1500)
        {
            radar_sensi_ack_time = HAL_GetTick();
            if (uavr11_info.set_radar_sensi_ack == true)
            {
                pmu_set_ack(22, 1, uavr11_info.get_radar_sensi,0);
                uavr11_info.set_radar_sensi_ack = false;
                uavr11_info.fcu_set_sensi_flag = false;
            }
            else if (uavr12_info.set_radar_sensi_ack == true)
            {
                pmu_set_ack(22, 2, uavr12_info.get_radar_sensi,0);
                uavr12_info.set_radar_sensi_ack = false;
                uavr12_info.fcu_set_sensi_flag = false;
            }
            else if(uavr56_info.set_radar_sensi_ack == true)
            {
                pmu_set_ack(22, 6, uavr56_info.get_radar_sensi,0);
                uavr56_info.set_radar_sensi_ack = false;
                uavr56_info.fcu_set_sensi_flag = false;  
            }
        }
    }
}

void can_recv_mocib_updata_read_set_hookfunction(uint32_t cellCanID, uint8_t data[])
{
    //AG代码 和雷达升级不兼容,优先升级
    if (Rupdate.update_flag == true)
    {
        //避障雷达升级
        if (Rupdate.buf_flag == false)
        {
            memcpy(Rupdate.update_buf, data, 8);
            Rupdate.buf_flag = true;

            switch (cellCanID)
            {
            case 0x7E1:
                Rupdate.U7E1 = true;
                break;
            case 0x7E3:
                Rupdate.U7E3 = true;
                break;
            case 0x7E6:
                Rupdate.U7E6 = true;
                break;
            default:
                break;
            }
        }
    }
    else
    {
        switch (cellCanID)
        {
        //莫之比雷达反馈版本信息
        case CAN_UAVRH_UPDATE_R1:
            uavrhup_getr1_ack = true;
            break;
        //case CAN_UAVRH_VER_R:
            // if (uavr11_info.get_radar_ver_flag == false && can_get_uavr21_ver_comp == 1)
            // {
            //     //char_to_hex_string(data, 4, uavr11_info.version, 10, "11");
            //     uavr11_info.get_radar_ver_flag = true;
            //     //升完级发送版本信息
            //     if(HAL_GetTick() > 10000)
            //     {
            //         pmu_send = VERSION;
            //     }
            // }
            // else if (uavr12_info.get_radar_ver_flag == false && can_get_uavr21_ver_comp == 2)
            // {
            //     //char_to_hex_string(data, 4, uavr12_info.version, 10, "12");
            //     uavr12_info.get_radar_ver_flag = true;
            //     if(HAL_GetTick() > 10000)
            //     {
            //         pmu_send = VERSION;
            //     }
            // }
            // else if (uavr56_info.get_radar_ver_flag == false && can_get_uavr21_ver_comp == 3)
            // {
            //     //char_to_hex_string(data, 4, uavr56_info.version, 10, "56");
            //     uavr56_info.get_radar_ver_flag = true;
            //     if(HAL_GetTick() > 10000)
            //     {
            //         pmu_send = VERSION;
            //     }
            // }
            // break;
        //莫之比雷达设置灵敏度及反馈
        case CAN_UAVRH_SENSI_SA:
            if (data[0] == 0x11)
            {
                uavr11_info.set_radar_sensi_ack = true;
                //莫之比大端模式
                uavr11_info.get_radar_sensi = ((data[1] << 8) & 0xff00) + data[2];
            }
            else if (data[0] == 0x12)
            {
                uavr12_info.set_radar_sensi_ack = true;
                //莫之比大端模式
                uavr12_info.get_radar_sensi = ((data[1] << 8) & 0xff00) + data[2];
            }
            else if(data[0] == 0x0B)
            {
                uavr56_info.set_radar_sensi_ack = true;
                uavr56_info.get_radar_sensi = ((data[1] << 8) & 0xff00) + data[2];
            }
            break;
        //莫之比雷达读取灵敏度及反馈
        case CAN_UAVRH_SENSI_RA:
            if (data[0] == 0x11)
            {
                uavr11_info.get_radar_sensi_flag = true;
                //莫之比大端模式
                uavr11_info.get_radar_sensi = ((data[1] << 8) & 0xff00) + data[2];
            }
            else if (data[0] == 0x12)
            {
                uavr12_info.get_radar_sensi_flag = true; //莫之比大端模式
                uavr12_info.get_radar_sensi = ((data[1] << 8) & 0xff00) + data[2];
            }
            else if(data[0] == 0x0B)
            {
                uavr56_info.get_radar_sensi_flag = true;
                uavr56_info.get_radar_sensi = ((data[1] << 8) & 0xff00) + data[2];
            }
        default:
            break;
        }
    }
}


/**
  * @file    can_send_info_to_mimo
  * @brief   给恩曌避障发送姿态信息
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
void can_send_info_to_mimo()
{
    static int mimo_50HZ = 0;
    if (mimo_f_info.Link.connect_status == COMP_NORMAL || mimo_b_info.Link.connect_status == COMP_NORMAL /*|| 
        mimo_360_info.connect_status == COMP_NORMAL*/)
    {
        if (HAL_GetTick() - mimo_50HZ > 20) 
        {
            mimo_50HZ = HAL_GetTick();

            int16_t index = 0;
            short tmpShort = 0;
            int8_t tmpChar = 0;
            uint8_t send_mimo_data[8] = {0};

            tmpShort = 1;
            short2buf(&send_mimo_data[index], &tmpShort);
            index += 2;
            // 俯仰
            tmpShort = planep.pitch_angle;
            short2buf(&send_mimo_data[index], &tmpShort);
            index += 2;

            // 横滚
            tmpShort = planep.roll_angle;
            short2buf(&send_mimo_data[index], &tmpShort);
            index += 2;

            //航向
            if(planep.yaw < 0)
                tmpShort = planep.yaw + 360;
            else
                tmpShort = planep.yaw;
            
            short2buf(&send_mimo_data[index], &tmpShort);
            index += 2;

            can_send_msg_normal(send_mimo_data, sizeof(send_mimo_data), 0x3740403);

            index = 0;
            // 俯仰
            tmpShort = -planep.roll_angle;
            short2buf(&send_mimo_data[index], &tmpShort);
            index += 2;

            // 横滚
            tmpShort = planep.pitch_angle;
            short2buf(&send_mimo_data[index], &tmpShort);
            index += 2;

            //航向
            if(planep.yaw < 0)
                tmpShort = planep.yaw + 360;
            else
                tmpShort = planep.yaw;
            short2buf(&send_mimo_data[index], &tmpShort);
            index += 2;

            // 前后速度
            tmpChar = (planep.E_vel * sinf(planep.yaw / 100.0f * DEG_TO_RAD) +
                       planep.N_vel * cosf(planep.yaw / 100.0f * DEG_TO_RAD)) /
                      10; //0.1m/s
            send_mimo_data[index++] = tmpChar;

            //雷达安装俯仰角
            tmpChar = 0;
            send_mimo_data[index] = tmpChar;

            can_send_msg_normal(send_mimo_data, sizeof(send_mimo_data), CAN_MIMO_ATTI_INFO1);

            index = 0;
            //高度
            tmpShort = planep.alt;
            short2buf(&send_mimo_data[index], &tmpShort);
            index += 2;

            //俯仰角速度
            tmpShort = 0;
            short2buf(&send_mimo_data[index], &tmpShort);
            index += 2;

            //横滚角速度
            tmpShort = 0;
            short2buf(&send_mimo_data[index], &tmpShort);
            index += 2;

            // 左右速度
            tmpChar = (planep.E_vel * cosf(planep.yaw / 100.0f * DEG_TO_RAD) +
                       planep.N_vel * sinf(planep.yaw / 100.0f * DEG_TO_RAD)) /
                      10;
            send_mimo_data[index++] = tmpChar;

            //Z速度
            tmpChar = planep.alt_vel / 10; //0.1m/s
            send_mimo_data[index] = tmpChar;

            can_send_msg_normal(send_mimo_data, sizeof(send_mimo_data), CAN_MIMO_ATTI_INFO2);
        }
    }
}

/**
  * @file    lidar_function
  * @brief   雷达相关函数
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
void send_mocib_radar_sensi(void)
{
    static int radar_sensi_send_time = 0;
    //uint8_t radar_can_buf[8] = {0};
    if(HAL_GetTick() > 7000 && HAL_GetTick() - radar_sensi_send_time > 300)
    {   
        //给FMU发送雷达灵敏度
        radar_sensi_send_time = HAL_GetTick();
        //上电后 检测到有雷达连接，向飞控发送雷达灵敏度信息
        if (uavr11_info.Link.connect_status == COMP_NORMAL && uavr11_info.get_radar_sensi_flag == true &&
             uavr11_info.send_fcu_sensi_count <= 3)
        {
            pmu_set_ack(22, 1, uavr11_info.get_radar_sensi,0);
            uavr11_info.send_fcu_sensi_count++;
        }
        else if (uavr12_info.Link.connect_status == COMP_NORMAL && uavr12_info.get_radar_sensi_flag == true &&
                uavr12_info.send_fcu_sensi_count <= 3)
        {
            pmu_set_ack(22, 2, uavr12_info.get_radar_sensi,0);
            uavr12_info.send_fcu_sensi_count++;
        }
        else if (uavr56_info.Link.connect_status == COMP_NORMAL && uavr56_info.get_radar_sensi_flag == true &&
                uavr56_info.send_fcu_sensi_count <= 3)
        {
            pmu_set_ack(22, 6, uavr56_info.get_radar_sensi,0);
            uavr56_info.send_fcu_sensi_count++;
        }
    }
}

void lidar_function(void)
{
    //莫之比避障雷达升级
    if (radar_update_flag == true && uavr11_info.fcu_set_sensi_flag != true && uavr12_info.fcu_set_sensi_flag != true
        && uavr56_info.fcu_set_sensi_flag != true)
    {
        Rupdate.update_flag = true;

        Can_obstacle_update();
    }

    //上电给fcu发送雷达灵敏度 
    send_mocib_radar_sensi();

    //设置莫之比避障雷达灵敏度
    can_set_radar_sensi();

    //获取电目雷达盲区、能量、原始数据开关
    get_radar_blindAndPower_function();

    //雷达升级不再给雷达发送信息,莫之比雷达发送姿态信息
    if (radar_update_flag == false)
    {
        can_sendmsg_uavr20();
    }

    //给恩曌雷达发送姿态信息数据
    can_send_info_to_mimo();

}


void can_recv_mocib_version_info(uint32_t cellCanID, uint8_t data[], uint8_t len)
{
    static uint8_t frame_fi = 0,frame_bi = 0,frame_ti = 0;
    switch (cellCanID)
    {
    //SN号
    case CAN_OBSTARCLE11_SN:

        if(frame_fi == 0)
        {
            memcpy(&uavr11_info.sn[0],&data[2],6);//要后六个字节
            frame_fi += 6;
        }
        else if(frame_fi == 6)
        {
            memcpy(&uavr11_info.sn[frame_fi],&data[1],7);//要后7个字节
            frame_fi += 7;
        }
        else if(frame_fi >= 13)
        {
            memcpy(&uavr11_info.sn[frame_fi],&data[1],3);
            frame_fi = 0;
            
            regist_dev_info(&dev_obsf,DEVICE_OBSF,false,uavr11_info.sn,20,NULL,0,NULL,0,"mocib",6);
            uavr11_info.get_radar_sn_flag = true;
        }
        
        break;
    case CAN_OBSTARCLE12_SN:
        if(frame_bi == 0)
        {
            memcpy(&uavr12_info.sn[0],&data[2],6);//要后六个字节
            frame_bi += 6;
        }
        else if(frame_bi == 6)
        {
            memcpy(&uavr12_info.sn[frame_bi],&data[1],7);//要后7个字节
            frame_bi += 7;
        }
        else if(frame_bi >= 13)
        {
            memcpy(&uavr12_info.sn[frame_bi],&data[1],3);
            frame_bi = 0;

            regist_dev_info(&dev_obsb,DEVICE_OBSB,false,uavr12_info.sn,20,NULL,0,NULL,0,"mocib",6);
            uavr12_info.get_radar_sn_flag = true;
        }
        break;
    case CAN_OBSTARCLE56_SN:
        if(frame_ti == 0)
        {
            memcpy(&uavr56_info.sn[0],&data[2],6);//要后六个字节
            frame_ti += 6;
        }
        else if(frame_ti == 6)
        {
            memcpy(&uavr56_info.sn[frame_ti],&data[1],7);//要后7个字节
            frame_ti += 7;
        }
        else if(frame_ti >= 13)
        {
            memcpy(&uavr56_info.sn[frame_ti],&data[1],3);
            frame_ti = 0;

            regist_dev_info(&dev_ter,DEVICE_TERRA,false,uavr56_info.sn,20,NULL,0,NULL,0,"mocib",6);
            uavr56_info.get_radar_sn_flag = true;
        }
        break;
    //版本信息
    case 0x00eeff11:
        uavr11_info.version[0] = 'M';
        uavr11_info.version[1] = '1';
        uavr11_info.version[2] = (data[1] + data[2]) + '0';
        uavr11_info.version[3] = (data[3] + data[4]) + '0';
        char_to_hex_string(&data[5], 3, &uavr11_info.version[4], 6, "00");
        uavr11_info.soft_verison = ((data[5] & 0xff) << 16) + ((data[6] & 0xff) << 8) + (data[7] & 0xff);
        uavr11_info.get_radar_ver_flag = true;

        regist_dev_info(&dev_obsf,DEVICE_OBSF,false,NULL,0,uavr11_info.version,10,NULL,0,"mocib",6);
        break;
    case 0x00eeff12:
        uavr12_info.version[0] = 'M';
        uavr12_info.version[1] = '2';
        uavr12_info.version[2] = (data[1] + data[2]) + '0';
        uavr12_info.version[3] = (data[3] + data[4]) + '0';
        char_to_hex_string(&data[5], 3, &uavr12_info.version[4], 6, "00");
        uavr12_info.soft_verison = ((data[5] & 0xff) << 16) + ((data[6] & 0xff) << 8) + (data[7] & 0xff);
        uavr12_info.get_radar_ver_flag = true;

        regist_dev_info(&dev_obsb,DEVICE_OBSB,false,NULL,0,uavr12_info.version,10,NULL,0,"mocib",6);
        break;
    case 0x00eeff0b:
        uavr56_info.version[0] = 'M';
        uavr56_info.version[1] = 'B';
        uavr56_info.version[2] = (data[1] + data[2]) + '0';
        uavr56_info.version[3] = (data[3] + data[4]) + '0';
        char_to_hex_string(&data[5], 3, &uavr56_info.version[4], 6, "00");
        uavr56_info.soft_verison = ((data[5] & 0xff) << 16) + ((data[6] & 0xff) << 8) + (data[7] & 0xff);
        uavr56_info.get_radar_ver_flag = true;

        regist_dev_info(&dev_ter,DEVICE_TERRA,false,NULL,0,uavr56_info.version,10,NULL,0,"mocib",6);
        break;    
    default:
        break;
    }
    // if(HAL_GetTick() > 10000)
    // {
        pmu_send = PMU_SEND_VERSION;
    // }
}

bool check_radar_update(void)
{	

	if (uavr12_info.Link.connect_status == COMP_NORMAL && uavr12_info.get_radar_sensi_flag == false &&
         uavr12_info.get_radar_sensi_count < 5)
	{

		return false;
	}
	if (uavr11_info.Link.connect_status == COMP_NORMAL && uavr11_info.get_radar_sensi_flag == false && 
        uavr11_info.get_radar_sensi_count < 5)
	{

		return false;
	}
	if (uavr56_info.Link.connect_status == COMP_NORMAL && uavr56_info.get_radar_sensi_flag == false && 
        uavr56_info.get_radar_sensi_count < 5)
	{
		return false;
	}
	if(uavr11_info.fcu_set_sensi_flag == true ||uavr12_info.fcu_set_sensi_flag == true
        || uavr56_info.fcu_set_sensi_flag == true)
	{
		return false;
	}
	return true;
}

void get_radar_blindAndPower_function( void )
{
    uint8_t can_buf[8] = {0};
    uint32_t can_id = 0;
    static uint32_t time_1hz = 0;
    if(!Check_Timer_Ready(&time_1hz,_1_HZ_))
        return;

    if(DM_f_info.Link.connect_status == COMP_NORMAL && DM_f_info.get_radar_blind_flag == false)
    {
        can_id = 0xA81300;
        put_date_to_can(can_buf,0x8,0,0,0,0,0,0,0X7);
        can_send_msg_normal(&can_buf[0], 8, can_id);
    }
    else if(DM_f_info.Link.connect_status == COMP_NORMAL && DM_f_info.get_radar_power_flag == false)
    {
        can_id = 0xA81300;
        put_date_to_can(can_buf,0x9,0,0,0,0,0,0,0X7);
        can_send_msg_normal(&can_buf[0], 8, can_id);
    }
    else if(DM_f_info.Link.connect_status == COMP_NORMAL && DM_f_info.get_radar_rawSwitch_flag == false)
    {
        can_id = 0xA81300;
        put_date_to_can(can_buf,0xB,0,0,0,0,0,0,0X7);
        can_send_msg_normal(&can_buf[0], 8, can_id);
    }

    if(DM_ter_info.Link.connect_status == COMP_NORMAL && DM_ter_info.get_radar_blind_flag == false)
    {
        can_id = 0x981300;
        put_date_to_can(can_buf,0x8,0,0,0,0,0,0,0X7);
        can_send_msg_normal(&can_buf[0], 8, can_id);
    }
    else if(DM_ter_info.Link.connect_status == COMP_NORMAL && DM_ter_info.get_radar_power_flag == false)
    {
        can_id = 0x981300;
        put_date_to_can(can_buf,0x9,0,0,0,0,0,0,0X7);
        can_send_msg_normal(&can_buf[0], 8, can_id);
    }
    else if(DM_ter_info.Link.connect_status == COMP_NORMAL && DM_ter_info.get_radar_rawSwitch_flag == false)
    {
        can_id = 0x981300;
        put_date_to_can(can_buf,0xB,0,0,0,0,0,0,0X7);
        can_send_msg_normal(&can_buf[0], 8, can_id);
    }
}
// mimo_360info mimo360_info[MIMO_360_TotalSect];

// mimo_360status mimo360_status;
// Connect_check mimo_360_info;
// void mimo360_sort(mimo_360info *arr,uint16_t length)
// {
//     if ( length < 2 )
//     {
//         return;
//     }
//     uint16_t num = 0, num1 = 0;
//     mimo_360info tmp;
//     for ( num = length - 1; num >= 1; num-- )
//     {
//         for ( num1 = 0; num1 <= num - 1; num1++ )
//         {
//             if ( ( ( arr + num1 )->dis0 ) > ( ( arr + num1 + 1 )->dis0 ) )
//             {
//                 tmp = *( arr + num1 );
//                 *( arr + num1 ) = *( arr + num1 + 1 );
//                 *( arr + num1 + 1 ) = tmp;
//             }
//         }
//     }
// }
// uint16_t test_index = 0;
// uint16_t compelte = 0;
// uint8_t radar360_proflag = 0;
// mimo_360_cont fmu_360info;
// void can_recv_mocib_new360_obstacle(uint32_t cellCanID, uint8_t data[], uint8_t len)
// {
//     mimo_360_info.connect_status = COMP_NORMAL;
//     mimo_360_info.recv_time = HAL_GetTick();

//     switch (cellCanID)
//     {
//     case CAN_360MIMO_1ID:
//         memcpy(&mimo360_status,&data[0],8);
//         mimo360_status.index = 0;
        
//         if(mimo360_status.nTarget == 0)
//         {
//             fmu_360info.total_tar = 0;
//             memset(&fmu_360info.data[0],0,sizeof(mimo_360_data));
//         }
        
//         break;
//     case CAN_360MIMO_2ID:
//         if(mimo360_status.nTarget <= MIMO_360_TotalSect  && mimo360_status.nTarget > 0)
//         {
//             for(uint8_t i = 0;i < 2;i++)
//             {
//                 if(i == 0)
//                 {
//                     memcpy(&mimo360_info[mimo360_status.index].sectId0,&data[0],sizeof(mimo_360info));
//                 }
//                 else
//                 {
//                     memcpy(&mimo360_info[mimo360_status.index].sectId0,&data[4],sizeof(mimo_360info));
//                 }

//                 if(mimo360_status.nTarget - 1 == mimo360_status.index && radar360_proflag == 0)
//                 {
//                     //mimo360_sort(&mimo360_info[0],mimo360_status.nTarget);
//                     fmu_360info.TotalSect = mimo360_status.TotalSect;
//                     fmu_360info.total_tar = mimo360_status.nTarget;
//                     for(uint8_t j = 0;j < mimo360_status.nTarget;j++)
//                     {
//                         fmu_360info.data[j].sec_angle = mimo360_info[j].sectId0 * 360 / mimo360_status.TotalSect;
//                         fmu_360info.data[j].distance = mimo360_info[j].dis0 * mimo360_status.RangeRes/100; //cm
//                         fmu_360info.data[j].rcs0 = mimo360_info[j].rcs0;
//                         fmu_360info.data[j].el0 = mimo360_info[j].el0 * 0.5;                       
//                     }
//                 }
//                 mimo360_status.index++;
//             }
//         }
//         break;
//     default:
//         break;
//     }
// }