Vehicle-cpp/remote_drive/EgoZJ/EgoSystem/radar_sensor.cpp

#include <stdint.h>
#include <chrono>
#include "../common/comm.h"

#include "../common/iobuffer.h"
#include "../common/sensor_socket.h"
#include "radar_sensor.h"
#include "message_queue.h"


uint16_t make_uint16(uint8_t h,uint8_t l)
{
    return (h * 256+ l);
}

uint16_t GetMinDate(uint8_t * data,int Num)
{
    uint8_t Dst[4] = {0};

    for(int i = 0 ,j = 0; i < Num ; i++,j+=2)
        Dst[i] = make_uint16(*(data + j),*(data + j + 1));


   uint16_t loop, smallest;
   smallest = Dst[0];
   
   for(loop = 1; loop < Num; loop++) 
   {
        if(smallest > Dst[loop]) 
            smallest = Dst[loop];
   }
   return smallest;
}

CRadarSensor::CRadarSensor(CMessageQueue* q):_message(q)
{
       memset(&_message->_Radardata,0,sizeof(_message->_Radardata));
       _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::CRC_DYP(uint8_t *buf)
{
    buf[4] = ((buf[0] + buf[1] + buf[2] + buf[3]) & 0x00ff);
}

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);

            switch(source[2])
            {
                case 0x01:
                _message->_Radardata.r1 = GetMinDate((uint8_t *)source + 4,2);
                _message->_Radardata.r2 = GetMinDate((uint8_t *)source + 8,2);
                break;

                case 0x02:
                _message->_Radardata.r3 = GetMinDate((uint8_t *)source,4);
                break;

                case 0x03:
                _message->_Radardata.r4 = GetMinDate((uint8_t *)source,4);
                break;

                case 0x04:
                _message->_Radardata.r5 = GetMinDate((uint8_t *)source + 4,2);
                _message->_Radardata.r6 = GetMinDate((uint8_t *)source + 8,2);
                break;
            }
        }
        

        _count++;

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

}

unsigned int CRadarSensor::CRC16(uint8_t *buf, int len)
{
    unsigned int crc = 0xFFFF;

    for (int pos = 0; pos < len; pos++)
    {
        crc ^= (unsigned int)buf[pos]; // XOR byte into least sig. byte of crc

        for (int i = 8; i != 0; i--)
        { // Loop over each bit
            if ((crc & 0x0001) != 0)
            {              // If the LSB is set
                crc >>= 1; // Shift right and XOR 0xA001
                crc ^= 0xA001;
            }
            else           // Else LSB is not set
                crc >>= 1; // Just shift right
        }
    }
     
    return crc;
}
 
void CRadarSensor::Start()
{
    if(!_run)
    {
        _thread = std::thread(&CRadarSensor::Run, this);
        _run = true;
    } 
}


void CRadarSensor::Run()
{
    CIOBuffer buffer[5];
    uint8_t Source[5] = {0x55,0xAA,0x01,0x01,0x00};

    for(int i = 0 ; i < 4 ; i++)
    {
        Source[2] = (i + 0x01);
        CRC_DYP(Source);
        buffer[i].Length = 5;
        memcpy(buffer[i].Buffer, Source, buffer[i].Length);
    }
    
    
    while (_run)
    {
        for(int i = 0 ; i < 4 ; i++)
            _socket->Write(&buffer[i]);
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }
}
  
void CRadarSensor::SetSensorSocket(SensorSocket<CRadarSensor>* can)
{
    _socket = can;
}
void CRadarSensor::Stop()
{
    if (_run)
    {
        _run = false;
        _thread.join();
    }
}