EFTZ_PMU/user_src/soft_radar_handle.c

#include "soft_radar_handle.h"
#include "string.h"
#include "main.h"
#include "soft_obstacle.h"
#include "soft_terrain.h"
#include "main_task.h"
#include "usart_data_handle.h"
#include "soft_can.h"
#include "math.h"
#include "soft_update.h"
#include "soft_device.h"

/**
  * @file    get_radar_info
  * @brief   获取雷达信息
  * @param   Info_Type:避障雷达 X：1  Y：2
  * @details 
  * @author  Zhang Sir 
 **/
short get_radar_info(uint8_t Radar_Type,uint8_t Info_Type)
{
    uavr_terrain *Ptr_T = NULL;
    uavr_obs *Ptr_O = NULL;
    
    if(Radar_Type == T_RADAR)
    {
        if(mimo_ter_info.Link.connect_status != COMP_NOEXIST)
            Ptr_T = &mimo_ter_info;
        else if(uavr56_info.Link.connect_status != COMP_NOEXIST)
            Ptr_T = &uavr56_info;
        else if(DM_ter_info.Link.connect_status != COMP_NOEXIST)
            Ptr_T = &DM_ter_info;

        if(Ptr_T->Link.connect_status == COMP_LOST) {return -2;}
        else if(Ptr_T == NULL) {return -1;}
        else {return Ptr_T->height;}
    }
    else if(Radar_Type == F_RADAR)
    {
        if(uavr11_info.Link.connect_status != COMP_NOEXIST)
            Ptr_O = &uavr11_info;
        else if(mimo_f_info.Link.connect_status != COMP_NOEXIST)
            Ptr_O = &mimo_f_info;

        if(Ptr_O->Link.connect_status == COMP_LOST) {return -2;}
        else if(Ptr_O == NULL) {return -1;}
        else
        {
            if(Info_Type == OBS_X)
                return Ptr_O->distance_x;
            else if((Info_Type == OBS_Y))
                return Ptr_O->distance_y;
        }
    }
    else if(Radar_Type == B_RADAR)
    {
        if(uavr12_info.Link.connect_status != COMP_NOEXIST)
            Ptr_O = &uavr12_info;
        else if(mimo_b_info.Link.connect_status != COMP_NOEXIST)
            Ptr_O = &mimo_b_info;

        if(Ptr_O->Link.connect_status == COMP_LOST) {return -2;}
        else if(Ptr_O == NULL) {return -1;}
        else
        {
            if(Info_Type == OBS_X)
                return Ptr_O->distance_x;
            else if((Info_Type == OBS_Y))
                return Ptr_O->distance_y;
        }
    }
    return 0;
}

/**
  * @file    radar_version_check
  * @brief   更改雷达版本格式
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
char radar_version[3][10] = {0}; //0 前避障  1后避障 2仿地
void radar_version_check(void)
{   
    //前避障
    if(uavr11_info.Link.connect_status == COMP_NORMAL  || uavr11_info.Link.boot_flag == true)
    {
        if(uavr11_info.soft_verison == 0 && uavr11_info.get_radar_ver_flag == false)
        {
            uavr11_info.version[0] = 'M';
            uavr11_info.version[1] = '1';
            for(uint8_t i = 2;i < 10; i++)
            {
                uavr11_info.version[i] = '0';
            }
        }
        memcpy(&radar_version[0][0],&uavr11_info.version[0],10);
    }
    else if (mimo_f_info.Link.connect_status == COMP_NORMAL)
    {
        if(mimo_f_info.Link.boot_flag == true)
        {   
            memcpy(&mimo_f_info.version,"E100000000",10);
        }
        memcpy(&radar_version[0][0],&mimo_f_info.version[0],10);
    }
    
    //后避障
    if(uavr12_info.Link.connect_status == COMP_NORMAL )
    {
        if(uavr12_info.soft_verison == 0 && uavr12_info.get_radar_ver_flag == false)
        {
            uavr12_info.version[0] = 'M';
            uavr12_info.version[1] = '2';
            for(uint8_t i = 2;i < 10; i++)
            {
                uavr12_info.version[i] = '0';
            }
        }
        memcpy(&radar_version[1][0],&uavr12_info.version[0],10);
    }
    else if(mimo_b_info.Link.connect_status == COMP_NORMAL)
    {
        if(mimo_b_info.Link.boot_flag == true)
        {   
            memcpy(&mimo_b_info.version,"E200000000",10);
        }
        memcpy(&radar_version[1][0],&mimo_b_info.version[0],10);
    }

    if(uavr56_info.Link.connect_status == COMP_NORMAL )
    {
        if(uavr56_info.soft_verison == 0 && uavr56_info.get_radar_ver_flag == false)
        {
            uavr56_info.version[0] = 'M';
            uavr56_info.version[1] = 'B';
            for(uint8_t i = 2;i < 10; i++)
            {
                uavr56_info.version[i] = '0';
            }
        }
        
        memcpy(&radar_version[2][0],&uavr56_info.version[0],10);
    }
    else if(mimo_ter_info.Link.connect_status == COMP_NORMAL)
    {
        if(mimo_ter_info.Link.boot_flag == true)
        {   
            memcpy(&mimo_ter_info.version,"EB00000000",10);
        }
        memcpy(&radar_version[2][0],&mimo_ter_info.version[0],10);
    }
    else if(DM_ter_info.Link.connect_status == COMP_NORMAL)
    {
        memcpy(&radar_version[2][0],&DM_ter_info.version[0],10);
    }
}

/**
  * @file    check_radar_link_status
  * @brief   检查雷达连接函数 
  * @param   
  * @details 
  * @author  Zhang Sir 
 **/
void check_radar_link_status(void)
{
  Check_dev_link(&mimo_ter_info.Link,3000,(char *)&mimo_ter_info,sizeof(uavr_terrain));
  Check_dev_link(&uavr56_info.Link,3000,(char *)&uavr56_info,sizeof(uavr_terrain));
  Check_dev_link(&uavr11_info.Link,3000,(char *)&uavr11_info,sizeof(uavr_obs));
  Check_dev_link(&uavr12_info.Link,3000,(char *)&uavr12_info,sizeof(uavr_obs));
  Check_dev_link(&mimo_f_info.Link,3000,(char *)&mimo_f_info,sizeof(uavr_obs));
  Check_dev_link(&mimo_b_info.Link,3000,(char *)&mimo_b_info,sizeof(uavr_obs));
  Check_dev_link(&mimo_360_info,3000,NULL,0);
  Check_dev_link(&DM_ter_info.Link,3000,(char *)&DM_ter_info,sizeof(uavr_terrain));
}



/**
  * @file    lidar_function
  * @brief   雷达相关函数
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
void send_mocib_radar_sensi(void)
{
    //给FMU发送雷达灵敏度
    //上电后 检测到有雷达连接，向飞控发送雷达灵敏度信息
    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++;
    }
}

/**
  * @file    can_send_info_to_mimo
  * @brief   给恩曌避障发送姿态信息
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
void can_send_info_to_mimo()
{
    //static int mimofb_10HZ = 0;
    static int mimoatti_50HZ = 0;
    static int mimoatti2_50HZ = 0;
    
    int16_t index = 0;
    short tmpShort = 0;
    int8_t tmpChar = 0;
    uint8_t send_mimo_data[8] = {0};

    if(update_info.vk_dev_update_flag == true)
        return;
            
    if(  mimo_f_info.Link.connect_status == COMP_NORMAL || mimo_b_info.Link.connect_status == COMP_NORMAL
        || mimo_360_info.connect_status == COMP_NORMAL || (Dev.Part_Tradar_Link.connect_status == COMP_NORMAL && Dev.Part_radarT.facid == FAC_MIMO_RT) || 
          (uavr12_info.Link.connect_status == COMP_NORMAL || uavr11_info.Link.connect_status == COMP_NORMAL || uavr56_info.Link.connect_status == COMP_NORMAL))
    {
        if(radar_update_flag == true)
        {
            return;
        }
        if (HAL_GetTick() - mimoatti_50HZ > 19) 
        {
            mimoatti_50HZ = HAL_GetTick();
            
            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_Func(CANID1, send_mimo_data, sizeof(send_mimo_data), CAN_MIMO_ATTI_INFO1, CAN_ID_EXT);
        }

        if(HAL_GetTick() - mimoatti2_50HZ > 21)
        {
            mimoatti2_50HZ = HAL_GetTick();

            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_Func(CANID1, send_mimo_data, sizeof(send_mimo_data), CAN_MIMO_ATTI_INFO2, CAN_ID_EXT);
        }
    }
}

/**
  * @file    can_set_radar_sensi
  * @brief   设置雷达灵敏度
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
uint32_t uavr20_sensi_time = 0;
short obsfradar_sensitivity = 50;
short obsbradar_sensitivity = 50;
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_Func(CANID1, send_uavr20_sensi, 1, CAN_UAVRH_UPDATE_S1, CAN_ID_EXT);                
            }

            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_Func(CANID1, send_uavr20_sensi, sizeof(send_uavr20_sensi), CAN_UAVRH_SENSI_SA, CAN_ID_EXT);
            }
            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_Func(CANID1, send_uavr20_sensi, 1, CAN_UAVRH_UPDATE_S1, CAN_ID_EXT);
            }

            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_Func(CANID1, send_uavr20_sensi, sizeof(send_uavr20_sensi), CAN_UAVRH_SENSI_SA, CAN_ID_EXT);
            }
            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_Func(CANID1, send_uavr20_sensi, 1, CAN_UAVRH_UPDATE_S1, CAN_ID_EXT);
            }

            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_Func(CANID1, send_uavr20_sensi, sizeof(send_uavr20_sensi), CAN_UAVRH_SENSI_SA, CAN_ID_EXT);
            }
            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;  
            }
        }
    }
}



/**
  * @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 ||(Dev.Part_Tradar_Link.connect_status == COMP_NORMAL && Dev.Part_radarT.facid == FAC_MOCIB_RT ))
    {
        //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 = 0;
            short2buf(&send_uavr20_data[index], &tmpShort);
            index += 2;

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

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

            Can_Send_Msg_Func(CANID1, send_uavr20_data, sizeof(send_uavr20_data), SEND_UAV20_MSG, CAN_ID_EXT);
        }
        //读取前雷达版本
        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_Func(CANID1, send_uavr20_sensi, 1, CAN_UAVRH_SENSI_RA, CAN_ID_EXT);

                    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_Func(CANID1, send_uavr20_sensi, 1, CAN_UAVRH_SENSI_RA, CAN_ID_EXT);

                        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_Func(CANID1, send_uavr20_sensi, 1, CAN_UAVRH_SENSI_RA, CAN_ID_EXT);

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

                }
            }
        }
    }
}

/**
  * @file    check_radar_update
  * @brief   雷达升级检测
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
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;
}

/**
  * @file    lidar_function
  * @brief   设置360雷达分区数
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
void lidar360_set_TotalSect(void)
{
    if(mimo_360_info.connect_status == COMP_NORMAL && (mimo360_status.get_TotalSect_flag == true ||
        mimo360_status.set_TotalSect_flag == true))
    {
        uint8_t can_buf[8] = {0};
        static uint32_t cur_time = 0;

        if(Check_Timer_Ready(&cur_time,_1_HZ_))
        {
            if(mimo360_status.get_TotalSect_flag == true)
            {
                put_date_to_can(can_buf,0xAA,0x24,0x03,0x02,0x07,0x00,0x00,(0x24+0x03+0x02+0x07)&0xff);
                Can_Send_Msg_Func(CANID1, can_buf, 8, 0xFA, CAN_ID_STD);
            }
            if(mimo360_status.set_TotalSect_flag == true)
            {
                //设置12分区
                put_date_to_can(can_buf,0xAA,0x24,0x03,0x82,0x07,0xC,0x00,(0x24+0x03+0x82+0x07+0xC)&0xff);
                Can_Send_Msg_Func(CANID1, can_buf, 8, 0xFA, CAN_ID_STD);
            }
        }

        
    }
}

/**
  * @file    lidar_function
  * @brief   雷达
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
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();
    }

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

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

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

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

    //给恩曌雷达设置灵敏度信息
    //can_set_sensi_to_mimo();
}