Vehicle-cpp/ZJ_PRO/EgoSystem/radar_ip.cpp

#include <stdint.h>
#include <chrono>

#include "radar_ip.h"
#include "message_queue.h"

#pragma pack(1)
struct Radar_Frame_Nl_Revice
{

    int8_t DLC_Nl_radar;
    int32_t Addr_Nl_radar;
    uint8_t RegData_Nl_radar1[8];
};

CRadarSensor::CRadarSensor(CMessageQueue* q):_message(q)
{
    _run = false;
}

uint16_t CRadarSensor::bcd2dec(const int8_t *data)
{
     /*int32_t h=data[0]&0x00FF;
     int32_t l=data[1]&0x00FF;
     return h * 10 + l;*/
     unsigned char Dst[4];
     memset(Dst, 0, 4);
     uint16_t i = (((data[0] & 0x00FF) << 8) | (data[1] & 0x00FF));
     sprintf((char *)Dst,"%x", i);
     return strtol((const char *)Dst, NULL, 10);
}

// void CRadarSensor::Notify(int8_t * buffer,int32_t size)
// {
//     int i = 0;

//     int8_t source[13];

//     if(_run)
//     {
//         for(i = 0 ; i < size  ; i += 13)
//         {
//             memset(source,0,13);
//             memcpy(source,buffer + i,13);

//             Radar_Frame_Nl_Revice * p = (Radar_Frame_Nl_Revice *)source;

//             if ((htonl(p->Addr_Nl_radar) & 0x60B) == 0x60B)
//             {
//                 int Nl_Index = (int)(((htonl(p->Addr_Nl_radar)) & 0x00f0) >> 4);

//                 switch (Nl_Index)
//                 {
//                     case 1:
//                         _data.r0 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
//                         break;
                    
//                     case 2:
//                         _data.r1 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
//                         break;

//                     case 3:
//                         _data.r2 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
//                         break;

//                     case 4:
//                         _data.r3 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
//                         break;

//                     case 5:
//                         _data.r4 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
//                         break;

//                     case 6:
//                         _data.r5 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
//                         break;

//                     case 7:
//                         _data.r6 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
//                         break;

//                     case 8:
//                         _data.r7 = ((((p->RegData_Nl_radar1[1] * 32) + (p->RegData_Nl_radar1[2] >> 3)) * 0.2) - 500);
//                         break;

//                     default:
//                         break;
//                 }
//             }
//         }
//          std::cout << "radar :" << _data.r0 << " " << _data.r1 << " " << _data.r2 << " " << _data.r3 << " "  
//                 << _data.r4 << " " << _data.r5 << " " << _data.r6 << " " << _data.r7 << " "<< std::endl;

//         _count++;

//         if (_count > 8)
//         {
//             _message->WriteRadarData(_data);
//             _count = 0;
//         }
//     }
// }

void CRadarSensor::Notify(int8_t * buffer,int32_t size)
{

    if(_run)
    {
        Radar_Frame_CM_Revice * p = (Radar_Frame_CM_Revice *)buffer;

        if (ntohl(p->Addr_radar1) == 0x611)
        {
            _data.r0 = bcd2dec((const int8_t*)p->RegData_radar1);
            _data.r1 = bcd2dec((const int8_t*)p->RegData_radar1 + 2);
            _data.r2 = bcd2dec((const int8_t*)p->RegData_radar1 + 4);
            _data.r3 = bcd2dec((const int8_t*)p->RegData_radar1 + 6);
            std::cout << "radar1:" << _data.r0 << " " << _data.r1 << " " << _data.r2 << " " << _data.r3 << " " << std::endl;
        }
        if (ntohl(p->Addr_radar2) == 0x612)
        {
            _data.r4 = bcd2dec((const int8_t*)p->RegData_radar2);
            _data.r5 = bcd2dec((const int8_t*)p->RegData_radar2 + 2);
            //װ�ػ�6���״ץ�ֻ�8���״�
            _data.r6 = bcd2dec((const int8_t*)p->RegData_radar2 + 4);
            _data.r7 = bcd2dec((const int8_t*)p->RegData_radar2 + 6);
            std::cout << "radar2:" << _data.r4 << " " << _data.r5 << " " << _data.r6 << " " << _data.r7 << " " << std::endl;
        }

        _count++;

        if (_count > 4)
        {
            _message->WriteRadarData(_data);
            //std::cout << "Send Date" << std::endl;
            _count = 0;
        }
        
    
    }
    

}

void CRadarSensor::Start()
{
    if(!_run)
    {
        _run = true;
        _count = 0;
    } 
}

void CRadarSensor::Run()
{
    if (_run)
    {
    }
}
  
void CRadarSensor::SetSensorSocket(SensorSocket<CRadarSensor>* can)
{
    _socket = can;
}
void CRadarSensor::Stop()
{
    if (_run)
    {
        _run = false;
        _count = 0;
    }
}