V9_PMU/Src/soft_terrain.c

#include "soft_terrain.h"
#include "stdint.h"
#include "stdbool.h"
#include "tim.h"
#include "can.h"
#include "soft_can.h"
#include "common.h"
#include "soft_seed_device.h"
#include "soft_crc.h"
#include "string.h"
#include "soft_obstacle.h"
#include "soft_seed_device.h"
#include "soft_version.h"
#include "soft_p_2_c.h"


UAVH30 uavh30_dist;

uavr_terrain uavr56_info = {.get_radar_sensi = 50};
uavr_terrain mimo_ter_info; 

//muniu muniu_ter_info;

bool terrain_is_link = false;
uint16_t terrain_height = 0;

/**
  * @file    can_recv_mocib_terrain
  * @brief   莫之比防地解析
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
void can_recv_mocib_terrain(uint8_t *data)
{
    uavh30_dist.powerful = (data[0] << 8) + data[1];
    uavh30_dist.near = (data[2] << 8) + data[3];
    uavh30_dist.far = (data[4] << 8) + data[5];
    
    uavr56_info.height = uavh30_dist.powerful;
    uavr56_info.Link.recv_time = HAL_GetTick();
    uavr56_info.Link.connect_status = COMP_NORMAL;
}


/**
  * @file    can_recv_enzhao_terrain
  * @brief   恩曌防地解析
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
mimo_part_radar T_radar[3];
void can_recv_enzhao_terrain(uint32_t CanID, uint8_t data[], uint8_t len)
{
    switch (CanID)
    {
    case CAN_MIMO_T_ID: //单点雷达
        mimo_ter_info.crc = data[3] + data[4] + data[5] + data[6];
        if (data[7] == mimo_ter_info.crc)
        {
            mimo_ter_info.height = (256 * data[4]) + (data[3]); //cm
            //  printf("%d          %d\n", mimo_ter_info.height, m.muniu_hight);   //输出到串口助手上 需要pringtf重定向
        }

        mimo_ter_info.Link.recv_time = HAL_GetTick();
        mimo_ter_info.Link.connect_status = COMP_NORMAL;
        Dev.Radar.facid_T = FAC_MIMO_RT;
        break;
    case CAN_MIMO_T_ID1:
        memcpy(&T_radar[0], data, 8);
        Dev.Part_Tradar_Link.recv_time = HAL_GetTick();
        Dev.Part_Tradar_Link.connect_status = COMP_NORMAL;
        Dev.Part_radarT.facid = FAC_MIMO_RT;
        Dev.Radar.facid_T = FAC_MIMO_RT;
        break;
    case CAN_MIMO_T_ID2:
        memcpy(&T_radar[1], data, 8);
        break;
    case CAN_MIMO_T_ID3:
        memcpy(&T_radar[2], data, 8);
        break;
    default:
        break;
    }

}

//木牛仿地雷达
// void can_recv_muniu_terrain(uint8_t *data)
// {
//     muniu_ter_info.muniu_hight = (data[0] << 8) + data[1];
//     muniu_ter_info.muniu_SNR = (data[2] << 8) + data[3];

//     muniu_ter_info.Link.connect_status = COMP_NORMAL;
//     muniu_ter_info.Link.recv_time = HAL_GetTick();
// }


//电目雷达
DM_RADAR DM_T_info,FMU_DM_info; //原始数据
uint8_t dm_i = 0;
uint8_t DM_recv_flag = 0;
uint8_t DM4d_recv_flag = 0;
Connect_check DM_status;
Connect_check DM_4dstatus;
uavr_terrain DM_ter_info; 
void DM_terrain_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len)
{
    if(cellCanID == 0x901300) //多点协议
    {
        DM_T_info.byte7.frame_flag = data[7];

        DM_status.connect_status = COMP_NORMAL;
        DM_status.recv_time = HAL_GetTick();
        
        if(DM_T_info.byte7.flag.head != 0 ) //头
        {
            memcpy(&DM_T_info.target_num,&data[0],7);
            if( DM_T_info.target_num > 30 )
                    DM_T_info.target_num = 30;
            dm_i = 0;
            if(DM_T_info.target_num == 0 && DM_recv_flag == 0)
            {
                FMU_DM_info.target_num = 0;
            }
        }
        else if(DM_T_info.byte7.flag.tail != 0) //尾
        {
            if(DM_T_info.target_num != 1)
            {
                memcpy(&DM_T_info.buf[dm_i],&data[0],DM_T_info.target_num*4%7);
            }
            else
            {
                memcpy(&DM_T_info.buf[dm_i],&data[0],4);
            }

            if(DM_T_info.crc == Get_Crc16(&DM_T_info.buf[0],DM_T_info.target_num * 4) &&  DM_recv_flag == 0)
            {
                memcpy(&FMU_DM_info.target_num,&DM_T_info.target_num,DM_T_info.target_num * 4 + 8);
                //memcpy(&FMU_DM_info.buf[0], &DM_T_info.buf[0], DM_T_info.target_num * 4);
            }
        }
        else
        {
            memcpy(&DM_T_info.buf[dm_i],&data[0],7);
            dm_i += 7;
        }
        if(dm_i >= 255 -7)
        {
            dm_i = 0;
        }
    }
    else if(cellCanID == 0x901301)//单点协议
    {
        DM_ter_info.Link.connect_status = COMP_NORMAL;
        DM_ter_info.Link.recv_time = HAL_GetTick();
        Dev.Radar.facid_T = FAC_DM_RT;

        DM_ter_info.height = data[3] + data[4] * 256;
    }

    //版本信息
    if(cellCanID == 0x981301 && data[0] == 0x1)
    {
        uint32_t version_temp = 0;
        DM_T_info.byte7.frame_flag = data[7];

        if(DM_T_info.byte7.flag.head != 0 ) //头
        {
            memcpy(&version_temp,&data[1],4);
            Int2String(version_temp,DM_ter_info.sn,9);
            //通过SN序号判断新旧boot
            if((version_temp % 10000000) < 2502999)    
                DM_ter_info.version[3] = 'O';
            else
                DM_ter_info.version[3] = 'N';

            regist_dev_info(&dev_ter,DEVICE_TERRA,false,DM_ter_info.sn,9,NULL,0,NULL,0,"dmter",6);
        }
        else if(DM_T_info.byte7.flag.tail != 0) //尾
        {
           memcpy(&version_temp,&data[1],4);
           Int2String(version_temp,&DM_ter_info.version[4],6);
           DM_ter_info.version[0] = 'D';
           DM_ter_info.version[1] = 'S';
           DM_ter_info.version[2] = '1';

           regist_dev_info(&dev_ter,DEVICE_TERRA,false,NULL,0,DM_ter_info.version,10,NULL,0,"dmter",6);

           DM_ter_info.get_radar_ver_flag = true;
           pmu_send = PMU_SEND_VERSION; //旧版APP
        }
    }
    else if(cellCanID == 0x981301 && (data[0] == 0x8 || data[0] == 0x5))
    {
        if(data[0] == 0x8)
            DM_ter_info.get_radar_blind_flag = true;
        pmu_set_ack(_MSGID_SET,MSGID_SET_TR_BLIND,0x56,data[1] + data[2] * 256);
    }
    else if(cellCanID == 0x981301 && (data[0] == 0x9 || data[0] == 0x7))
    {
        if(data[0] == 0x9)
            DM_ter_info.get_radar_power_flag = true;
        pmu_set_ack(_MSGID_SET,MSGID_SET_BR_POWER,0x56,data[1] + data[2] * 256);
    }
    else if(cellCanID == 0x981301 && (data[0] == 0xA || data[0] == 0xB))
    {
        if(data[0] == 0xB)
            DM_ter_info.get_radar_rawSwitch_flag = true;
        pmu_set_ack(_MSGID_SET,MSGID_SET_RAW_SWITCH,0x56,data[1] + data[2] * 256);
    }
    else if(cellCanID == 0x981301 && data[0] == 0x4 )
    {
        pmu_set_ack(_MSGID_SET,MSGID_SET_R_FUNC,0,data[1] + data[2] * 256);
    }
}

int16_t F_4DRadar[3][3]={0};  // X Y Z
DM_4dFRADAR DM_F4d;
int dm_4df_i = 0;
DM_4DRADAR FMU_4D_info;
bool F4d_send_flag = false;

void DM_Fobs_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len)
{
    if (cellCanID == 0XA01300) // 多点协议
    {
        DM_T_info.byte7.frame_flag = data[7];

        DM_status.connect_status = COMP_NORMAL;
        DM_status.recv_time = HAL_GetTick();

        if (DM_T_info.byte7.flag.head != 0) // 头
        {
            memcpy(&DM_T_info.target_num, &data[0], 7);
            if( DM_T_info.target_num > 30 )
                    DM_T_info.target_num = 30;
            dm_i = 0;
            if (DM_T_info.target_num == 0 && DM_recv_flag == 0)
            {
                FMU_DM_info.target_num = 0;
            }
        }
        else if (DM_T_info.byte7.flag.tail != 0) // 尾
        {
            if (DM_T_info.target_num != 1)
            {
                memcpy(&DM_T_info.buf[dm_i], &data[0], DM_T_info.target_num * 4 % 7);
            }
            else
            {
                memcpy(&DM_T_info.buf[dm_i], &data[0], 4);
            }

            if (DM_T_info.crc == Get_Crc16(&DM_T_info.buf[0], DM_T_info.target_num * 4) && DM_recv_flag == 0)
            {
                memcpy(&FMU_DM_info.target_num, &DM_T_info.target_num, DM_T_info.target_num * 4 + 8);
                // memcpy(&FMU_DM_info.buf[0], &DM_T_info.buf[0], DM_T_info.target_num * 4);
            }
        }
        else
        {
            memcpy(&DM_T_info.buf[dm_i], &data[0], 7);
            dm_i += 7;
        }
        if (dm_i >= 255 - 7)
        {
            dm_i = 0;
        }
    }
    else if(cellCanID == 0XA01301)//单点协议
    {
        DM_f_info.Link.connect_status = COMP_NORMAL;
        DM_f_info.Link.recv_time = HAL_GetTick();
        Dev.Radar.facid_F = FAC_DM_RF;

        DM_f_info.distance_y = data[3] + data[4] * 256;
    }

    // 4D前避障雷达协议 点云
    if (cellCanID == 0XA01310)
    {
        DM_4dstatus.connect_status = COMP_NORMAL;
        DM_4dstatus.recv_time = HAL_GetTick();
        Dev.Part_radarF.facid = FAC_DM_RF_4D;

        DM_F4d.byte7.frame_flag = data[7];

        if (DM_F4d.byte7.flag.head != 0) // 头
        {
            if(DM_F4d.target_num > 130)
                DM_F4d.target_num = 130;
            memcpy(&DM_F4d.target_num, &data[0], 7);
            dm_4df_i = 0;
        }
        else if (DM_F4d.byte7.flag.tail != 0) // 尾
        {
            if (DM_F4d.target_num != 1)
            {
                if ((DM_F4d.target_num * 5) % 7 != 0)
                {
                    memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], (DM_F4d.target_num * 5) % 7);
                    dm_4df_i += (DM_F4d.target_num * 5) % 7;
                }
                else
                {
                    memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], 7);
                    dm_4df_i += 7;
                }
            }
            else
            {
                memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], 5);
                dm_4df_i += 5;
            }
            if (DM_F4d.crc == Get_Crc16(&DM_F4d.RawData[0], DM_F4d.target_num * 5) && DM4d_recv_flag == 0)
            {
                FMU_4D_info.target_num = DM_F4d.target_num;
                memcpy(&FMU_4D_info.buf, &DM_F4d.RawData, DM_F4d.target_num * 5);
                F4d_send_flag = true;
            }
            
        }
        else
        {
            memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], 7);
            dm_4df_i += 7;
        }
        if (dm_4df_i >= 254 * 5)
        {
            dm_4df_i = 0;
        }
    }
    //4D前避障雷达协议 单点
    if(cellCanID == 0XA01302)
    {
        F_4DRadar[0][0] = data[1] + data[2] * 256;
        F_4DRadar[0][1] = data[3] + data[4] * 256;
        F_4DRadar[0][2] = data[5] + data[6] * 256;

        Dev.Part_Fradar_Link.recv_time = HAL_GetTick();
        Dev.Part_Fradar_Link.connect_status = COMP_NORMAL;
        Dev.Part_radarF.facid = FAC_DM_RF_4D;
    }
    else if(cellCanID == 0XA01303)
    {
        F_4DRadar[1][0] = data[1] + data[2] * 256;
        F_4DRadar[1][1] = data[3] + data[4] * 256;
        F_4DRadar[1][2] = data[5] + data[6] * 256;
    }
    else if(cellCanID == 0XA01304)
    {
        F_4DRadar[2][0] = data[1] + data[2] * 256;
        F_4DRadar[2][1] = data[3] + data[4] * 256;
        F_4DRadar[2][2] = data[5] + data[6] * 256;
    }




    //版本信息
    if(cellCanID == 0XA81301 && data[0] == 0x1)
    {
        uint32_t version_temp = 0;
        DM_T_info.byte7.frame_flag = data[7];

        if(DM_T_info.byte7.flag.head != 0 ) //头
        {
            memcpy(&version_temp,&data[1],4);
            Int2String(version_temp,DM_f_info.sn,9);
            //通过SN序号判断新旧boot
            if((version_temp % 10000000) < 2502999)   
                DM_f_info.version[3] = 'O';
            else
                DM_f_info.version[3] = 'N';

            regist_dev_info(&dev_obsf,DEVICE_OBSF,false,DM_f_info.sn,9,NULL,0,NULL,0,"dmter",6);
        }
        else if(DM_T_info.byte7.flag.tail != 0) //尾
        {
            memcpy(&version_temp,&data[1],4);
            Int2String(version_temp,&DM_f_info.version[4],6);
            DM_f_info.version[0] = 'D';
            DM_f_info.version[1] = 'W';
            DM_f_info.version[2] = '1';
            
           regist_dev_info(&dev_obsf,DEVICE_OBSF,false,NULL,0,DM_f_info.version,10,NULL,0,"dmter",6);

           DM_f_info.get_radar_ver_flag = true;
           pmu_send = PMU_SEND_VERSION; //旧版APP
        }
    }
    else if(cellCanID == 0xA81301 && (data[0] == 0x8 || data[0] == 0x5))
    {
        if(data[0] == 0x8)
            DM_f_info.get_radar_blind_flag = true;
        pmu_set_ack(_MSGID_SET,MSGID_SET_TR_BLIND,0x11,data[1] + data[2] * 256);
    }
    else if(cellCanID == 0xA81301 && (data[0] == 0x9 || data[0] == 0x7))
    {
        if(data[0] == 0x9)
            DM_f_info.get_radar_power_flag = true;
        pmu_set_ack(_MSGID_SET,MSGID_SET_BR_POWER,0x11,data[1] + data[2] * 256);
    }
    else if(cellCanID == 0xA81301 && (data[0] == 0xA || data[0] == 0xB))
    {
        if(data[0] == 0xB)
            DM_f_info.get_radar_rawSwitch_flag = true;
        pmu_set_ack(_MSGID_SET,MSGID_SET_RAW_SWITCH,0x11,data[1] + data[2] * 256);
    }
    else if(cellCanID == 0xA81301 && data[0] == 0x4 )
    {
        pmu_set_ack(_MSGID_SET,MSGID_SET_R_FUNC,0,data[1] + data[2] * 256);
    }
    
}