EFTZ_PMU/user_src/soft_obstacle.c

#include "soft_obstacle.h"
#include "soft_device.h"
#include "string.h"
#include "usart_data_handle.h"
#include "main_task.h"
#include "soft_can.h"
#include "math.h"
#include "stdlib.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};
_dev_par DM_4DRADARMAG;
/**
  * @file    can_recv_mocib_F_obstacle
  * @brief   莫之比前避障解析
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
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];

    uavr11_info.Link.connect_status = COMP_NORMAL;
    uavr11_info.Link.recv_time = HAL_GetTick();
}


/**
  * @file    can_recv_mocib_B_obstacle
  * @brief   莫之比后避障障解析
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
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];

    uavr12_info.Link.connect_status = COMP_NORMAL;
    uavr12_info.Link.recv_time = HAL_GetTick();
}


/**
  * @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;
        }
    }
}




mimo_360info mimo360_info[MIMO_360_TotalSect];

mimo_360status mimo360_status = {.get_TotalSect_flag = true};
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;
    }
}


//避障处理函数， 减少扫地的情况
int Fobs_delay_time = 0;
uint8_t Fobs_angle[3] = {0};
uint8_t Fobs_time = 0;
bool Fobs_handle_function(void)
{    
    if(planep.lock_status == STA_LOCK)
    {
        return true;
    }        
    if(terrain_is_link == true)
    {
        //仿地雷达连接的情况下检测9度以下
        if(planep.pitch_angle > -1300)
        {
            //从小到大赋值
            if(Fobs_angle[2] != 0)
            {
                Fobs_time = Fobs_angle[2];
            }
            else if (Fobs_angle[1] != 0)
            {
                Fobs_time = Fobs_angle[1];
            }
            else if (Fobs_angle[0] != 0)
            {
                Fobs_time = Fobs_angle[0];
            }
            else{
                Fobs_time = 0;
            }
            if(HAL_GetTick() - Fobs_delay_time >  1000 + Fobs_time * 100)
            {
                memset(&Fobs_angle[0],0,3);
                return true;
            }
        }
        else
        {
            if(planep.pitch_angle < -1500)
            {
                Fobs_angle[2] = 1;//15; 
            }
            else if(planep.pitch_angle < -1350)
            {
                Fobs_angle[1] = 1;//10;
            }
            else
            {
                Fobs_angle[0] = 1;//5;
            }
            Fobs_delay_time = HAL_GetTick();
            return false;
        }
    }
    else
    {
        //无仿地或高于2m  12度
        if(planep.pitch_angle > -1300)
        {
            if(Fobs_angle[2] != 0)
            {
                Fobs_time = Fobs_angle[2];
            }
            else if (Fobs_angle[1] != 0)
            {
                Fobs_time = Fobs_angle[1];
            }
            else if (Fobs_angle[0] != 0)
            {
                Fobs_time = Fobs_angle[0];
            }
            else{
                Fobs_time = 0;
            }
            if(HAL_GetTick() - Fobs_delay_time >  500 + Fobs_time * 100)
            {
                memset(&Fobs_angle[0],0,3);
                return true;
            }
        }
        else
        {
            if(planep.pitch_angle > -1300)
            {
                Fobs_angle[0] = 1;//5; 
            }
            else if(planep.pitch_angle > -1500)
            {
                Fobs_angle[1] = 1;//10;
            }
            else 
            {
                Fobs_angle[2] = 1;//15;
            }
            Fobs_delay_time = HAL_GetTick();
            return false;
        }
    }
    return false;
}

int Bobs_delay_time = 0;
uint8_t Bobs_angle[3] = {0};
uint8_t Bobs_time = 0;
bool Bobs_handle_function(void)
{    
    if(planep.lock_status == STA_LOCK)
    {
        return true;
    }             
    if(terrain_is_link == true)
    {
        //仿地雷达连接的情况下检测9度以下，增加延时到2.5s
        if(planep.pitch_angle < 1300)
        {
            if(Bobs_angle[2] != 0)
            {
                Bobs_time = Bobs_angle[2];
            }
            else if (Bobs_angle[1] != 0)
            {
                Bobs_time = Bobs_angle[1];
            }
            else if (Bobs_angle[0] != 0)
            {
                Bobs_time = Bobs_angle[0];
            }
            else{
                Bobs_time = 0;
            }
            if(HAL_GetTick() - Bobs_delay_time >  500 + Bobs_time * 100)
            {
                memset(&Bobs_angle[0],0,3);
                return true;
            }
        }
        else
        {
            if(planep.pitch_angle > 1500)
            {
                Bobs_angle[2] = 1;//15; 
            }
            else if(planep.pitch_angle > 1350)
            {
                Bobs_angle[1] = 1;//10;
            }
            else 
            {
                Bobs_angle[0] = 1;//5;
            }
            Bobs_delay_time = HAL_GetTick();
            return false;
        }
    }
    else
    {
        //无仿地或高于2m  12度
        if(planep.pitch_angle < 1300)
        {
            if(Bobs_angle[2] != 0)
            {
                Bobs_time = Bobs_angle[2];
            }
            else if (Bobs_angle[1] != 0)
            {
                Bobs_time = Bobs_angle[1];
            }
            else if (Bobs_angle[0] != 0)
            {
                Bobs_time = Bobs_angle[0];
            }
            else{
                Bobs_time = 0;
            }
            if(HAL_GetTick() - Bobs_delay_time >  1000 + Bobs_time * 100)
            {
                memset(&Bobs_angle[0],0,3);
                return true;
            }
        }
        else
        {
            if(planep.pitch_angle > 1500)
            {
                Bobs_angle[2] = 1;//15; 
            }
            else if(planep.pitch_angle > 1350)
            {
                Bobs_angle[1] = 1;//10;
            }
            else
            {
                Bobs_angle[0] = 1;//5;
            }
            Bobs_delay_time = HAL_GetTick();
            return false;
        }
    }
    return false;
}


/**
  * @file    can_recv_enzhao_obstacle
  * @brief   恩曌多点避障解析
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
/*****************************恩曌雷达排序************************/
void mimomocib_buf_sort( mimo_part_radar *arr, uint16_t length )
{
    if ( length < 2 )
    {
        return;
    }
    uint16_t num = 0, num1 = 0;
    mimo_part_radar tmp;
    for ( num = length - 1; num >= 1; num-- )
    {
        for ( num1 = 0; num1 <= num - 1; num1++ )
        {
            if ( ( ( arr + num1 )->Distance ) > ( ( arr + num1 + 1 )->Distance ) )
            {
                tmp = *( arr + num1 );
                *( arr + num1 ) = *( arr + num1 + 1 );
                *( arr + num1 + 1 ) = tmp;
            }
        }
    }
}
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.Part_Fradar_Link.connect_status = COMP_NORMAL;
        Dev.Part_Fradar_Link.recv_time = HAL_GetTick();
        Dev.Part_radarF.facid = 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.Part_Bradar_Link.connect_status = COMP_NORMAL;
        Dev.Part_Bradar_Link.recv_time = HAL_GetTick();
        Dev.Part_radarB.facid = 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;      
        mimomocib_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;
        mimomocib_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;
    }
}
void can_recv_mocib_obstacle(uint32_t cellCanID, uint8_t data[], uint8_t len)
{
    switch (cellCanID)
    {
    case CAN_UAVRH_FOBS_ID1:
        memcpy(&F_radar[0], data, 8);
        recv_comF_flag = recv_comF_flag | 1;
        Dev.Part_Fradar_Link.connect_status = COMP_NORMAL;
        Dev.Part_Fradar_Link.recv_time = HAL_GetTick();
        Dev.Part_radarF.facid = FAC_MOCIB_RF;
        break;
    case CAN_UAVRH_FOBS_ID2:
        memcpy(&F_radar[1], data, 8);
        recv_comF_flag = recv_comF_flag | 2;
        break;
    case CAN_UAVRH_FOBS_ID3:
        memcpy(&F_radar[2], data, 8);
        recv_comF_flag = recv_comF_flag | 4;
        break;
    case CAN_UAVRH_BOBS_ID1:
        memcpy(&B_radar[0], data, 8);
        recv_comB_flag = recv_comB_flag | 1;
        Dev.Part_Bradar_Link.connect_status = COMP_NORMAL;
        Dev.Part_Bradar_Link.recv_time = HAL_GetTick();
        Dev.Part_radarB.facid = FAC_MOCIB_RB;
        break;
    case CAN_UAVRH_BOBS_ID2:
        memcpy(&B_radar[1], data, 8);
        recv_comB_flag = recv_comB_flag | 2;
        break;
    case CAN_UAVRH_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;      
        mimomocib_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)
            {
                uavr11_info.distance_x  = F_radar[i].Distance * 0.05f * 100 * sin(F_radar[i].Amuzith * 0.1f / RAD);
                uavr11_info.distance_y = F_radar[i].Distance * 0.05f * 100 * cos(F_radar[i].Amuzith * 0.1f / RAD);
                break;
            }
            if(i == 2)
            {
                uavr11_info.distance_x = 0;
                uavr11_info.distance_y = 0;
            }
        }
        uavr11_info.Link.connect_status = COMP_NORMAL;
        uavr11_info.Link.recv_time = HAL_GetTick();
    }
    
    if(recv_comB_flag == 7)
    {
        recv_comB_flag = 0;
        mimomocib_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)
            {
                uavr12_info.distance_x  = B_radar[i].Distance * 0.05f * 100 * sin(B_radar[i].Amuzith * 0.1f / RAD);
                uavr12_info.distance_y = B_radar[i].Distance * 0.05f * 100 * cos(B_radar[i].Amuzith * 0.1f / RAD);
                break; 
            }
            if(i == 2)
            {
                uavr12_info.distance_x = 0;
                uavr12_info.distance_y = 0;
            }
        }
        uavr12_info.Link.recv_time = HAL_GetTick();
        uavr12_info.Link.connect_status = COMP_NORMAL;
    }
}