Vehicle-cpp/EgoJC/EgoSystem/radar_sensor.cpp

#include <stdint.h>
#include <chrono>
#include "../common/comm.h"
#include "PCANBasic.h"
#include "../common/iobuffer.h"
#include "../common/sensor_socket.h"
#include "radar_sensor.h"
#include "message_queue.h"
//#define make_int16(h,l) (((int16_t)h)<<8)|l
CRadarSensor::CRadarSensor(CMessageQueue* q):_message(q)
{
   _channelReady=false;
}
inline int16_t make_int16(int8_t h, int8_t l)
{
	int16_t hi = (int16_t)(h & 0x00FF);
	int16_t low = (int16_t)(l & 0x00FF);

	return (hi << 8) | low;
}

void CRadarSensor::Notify(int8_t * buffer,int32_t size)
{
    // std::vector<radar_node> _node;
     int32_t receivedCnt = size/CAN_MSG_LEN;
	 cannet_frame* p = (cannet_frame*)buffer;
     auto tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
     for(int32_t i=0;i<receivedCnt;i++)
     {
         int32_t id_normal=ntohl(p[i].canid);
       
       
        
        // assert(rid<5);
         int32_t message_id=(id_normal&0xF0F);
        
          
        
         switch (message_id)
         {
         
         case 0x70C:
            {
                //rid>>=8;
               // radar_info info;
                 //radar_node n;
                 int32_t rid=(id_normal&0x0F0)/0x10;
                 rid-=1;

                 bool bset=(mask&(0x1<<rid))!=0;
                 uint32_t range=make_int16(p[i].data[2], p[i].data[3]);
               //   _node[rid].range=(p[i].data[2]*0x100+p[i].data[3]);
                 if(_node[rid].range>range||!bset)
                 {
                  _node[rid].range = range;
                  mask|=(0x1<<rid);
                 }
               // _info[rid].rcs=p[i].data[1]*0.5-50;
                 _node[rid].radar_tick=tick;
              //std::cout<<_node[0].range<<" "<<_node[1].range<<" "<<_node[2].range<<" "<<_node[3].range<<" "<<_node[4].range<<" "<<_node[5].range<<" "<<_node[6].range<<" "<<std::endl;   
              //   float range =_info[rid]*0.01;
             //   std::cout<<std::hex<<id_normal<<" "<<std::dec<<range<<"  ";
              //  _info[rid].azimuth=p[i].data[4]-90;
              //  _info[rid].verl=(p[i].data[5]*0x100+p[i].data[6])*0.05-35;
              //  _info[rid].snr=p[i].data[7]-127;
                

            };
            break;
           
         default:
            break;
         }
        
     }
     /*
     bool bComplete=true;
     for(int32_t i=0;i<sizeof(_info)/sizeof(int32_t);i++)
     {
          if(_info[i]==-1)
          {
            bComplete=false;
            break;
          }
     }
     if(bComplete)
     */
     {
  //    auto tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();;
      if(tick-_tick>200)
      if(tick-_tick>200/*&&((mask&0x1F)==0x1F)*/)
      {

             _message->WriteSensor(_node,5);
             mask=0;
             _tick=tick;
  
      }
     //  memset(_info,-1,sizeof(_info));
     }
     //std::cout<<std::endl;
   
    /*
     for(int32_t i=0;i<5;i++)
     {
        _node[i].clear();
     }
     
 */
       std::this_thread::sleep_for(std::chrono::milliseconds(100));   

}
 
 
void CRadarSensor::Start()
{
     _channelReady=false;
     memset(_node,0,sizeof(_node));
     _tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();;
     /*
     CIOBuffer pBuffer;
     cannet_frame* frame=reinterpret_cast<cannet_frame *>(pBuffer.Buffer);
     memset(frame->data,0,sizeof(frame->data));
     frame->dlc=2;
     frame->canid=0x0000;
     frame->data[0]=0x01;
     frame->data[1]=0x00;
     _socket->Write(pBuffer.Buffer,sizeof(cannet_frame));
     */

}
 
void CRadarSensor::SetSensorSocket(SensorSocket<CRadarSensor>* can)
{
    _socket=can;
}
void CRadarSensor::Stop()
{
    _channelReady=false;
}