Vehicle-cpp/EgoJC/EgoSystem/car_sensor.cpp

#include <stdint.h>
#include <thread>
#include <sstream>
#include <iomanip>
#include <ctime>
#include <fstream>
#include "PCANBasic.h"
#include "../common/comm.h"
#include "../common/iobuffer.h"
 
#include "../common/can_sensor.h"
#include "../common/sensor_socket.h"
#include "protocol.pb.h"
#include "car_sensor.h"
#include "radar_sensor.h"
#include "message_queue.h"
#include <sys/stat.h> 
#include <sys/types.h>

//#define make_int16(h,l) (((int16_t)h)<<8)|l
#pragma pack(1)

struct can_array
{
    int32_t count;
    cannet_frame frames[3];
};
#pragma pack()

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

CCarSensor::CCarSensor(CMessageQueue * q):_message(q)
{
   //memset(frames,0,sizeof(frames));
}
CCarSensor::~CCarSensor()
{
     
}

bool CCarSensor::Start()
{
     _range_count=0;
     _channelReady=false;
     memset(_msg,0,sizeof(_msg));
    _can=std::make_unique<CCanSensor>();
    _log=std::make_unique<CEventLog>();
    TPCANStatus b=_can->Initialize();
    if(b==PCAN_ERROR_OK)
    {
      _can_ready=true;
      _thread=std::thread(&CCarSensor::Run,this);
      _can_thread=std::thread(&CCarSensor::CanSendProc,this);
    }
    else
    {
    	std::cout << "error:" <<b<<std::endl;
    }
    _log->Start();
    memset(&_data,0,sizeof(_data));
    return b==PCAN_ERROR_OK;
   
}

void CCarSensor::Stop()
{
     // _channelReady=false;
     _run=false;
     _can_run=false;
     _can_thread.join();
     _can_ready=false;
     _can->Uninitialize();
     _thread.join();
     
}
void CCarSensor::CanSendProc()
{
     _can_run=true;
     static int32_t brake_value=0;
     while(_can_run)
     {
         volatile int64_t tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
        // if(_brake<100)
         // {
                EventType type=EventType::NORMAL;
                if(_message->GetParkingState()!=ParkingState::None)
               {
                    _message->GetParkCanMessage(_msg);
                    
               }
               else{
                    if(_automous==true)
                    {
                         if(_brake>100)
                         {
                              _msg[3].DATA[1]=_brake;
                         }
                       
                    }
					 
					if(tick-_last_can_uptime>500)
                             {
                               
                                   _msg[3].DATA[0]=0;
                                   brake_value=std::min(++brake_value,200);
                                  _msg[3].DATA[1]=brake_value>>1;
                                   _log->SetEvent(EventType::OFFLINE,_steer,0,brake_value>>1);
                             }
                             else if(_data.engine_rpm<500)
                          {
                                 _msg[3].DATA[0]=0;
                                  brake_value=std::min(++brake_value,200);
                                 _msg[3].DATA[1]=brake_value>>1;
                                 _log->SetEvent(EventType::LOW_RPM,_steer,0,brake_value>>1);
                          }
                             else
                             {
                                 _log->SetEvent(EventType::NORMAL,0,0,0);
                                  brake_value=0;
                             }
					  auto * node=_message->GetRadar();
                         // std::cout<<"tick :"<<tick-node[0].radar_tick<<"  range :"<<node[0].range<< std::endl;
                           
                        { 
               
                         // if((tick-node[0].radar_tick>5000)|| node[0].range<100) 
                         if(node[0].range<100) 
                              {
                                   //std::cout << "tick :"<<node[0].radar_tick<<"  range :"<<node[0].range<< std::endl;
                                   _range_count++;
                              }
                              else
                              {
                                   _range_count=0;
                              }
                              if(_range_count>=10)
                              {
                                    _log->SetEvent(EventType::EMERGENCY,_steer,0,120);
                                   _msg[3].DATA[0]=0;
                                   _msg[3].DATA[1]=120;
                              
                              }
                        }
                        
                         
               }
         // }
         // else{
         //      _msg[3].DATA[0]=0;
            //   _msg[3].DATA[1]=100;
         // }
        // if(tick-_last_can_uptime<500)
         {
             std::lock_guard<std::mutex> lg(_last_can_lock); 
             for(int32_t i=0;i<sizeof(_msg)/sizeof(TPCANMsg);i++)
             {
                if(_msg[i].ID!=0)
                {

                  Write(_msg[i]);
                  
                // _can->Write(&_msg[i]);
                   
                }

             }
         }
         std::this_thread::sleep_for(std::chrono::milliseconds(20)); 
        
     }
}
void CCarSensor::Run()
{
      _run=true;
      
     
       
      while(_run)
      {
        std::this_thread::sleep_for(std::chrono::milliseconds(20));  
        /* for(int32_t i=0;i<sizeof(frames)/sizeof(cannet_frame);i++)
         {
           frames[i].canid=0;
         }
         */
         TPCANMsg CANMsg;
         TPCANTimestamp CANTimeStamp;
         TPCANStatus ret=PCAN_ERROR_OK;
         do {
              if(_can_ready==true)
             {
	       //printf("111\r\n");
               ret=_can->Read(&CANMsg,&CANTimeStamp);
              if(ret!=PCAN_ERROR_QRCVEMPTY)
              {
                  Notify(CANMsg);
              }
               
               memset(&CANMsg,0,sizeof(CANMsg));
               memset(&CANTimeStamp,0,sizeof(CANTimeStamp));
             }
           
          
               else
             {
               std::this_thread::sleep_for(std::chrono::milliseconds(20));
             } 
              if(_run==false) break;
         }while(!(ret&PCAN_ERROR_QRCVEMPTY));
             std::this_thread::sleep_for(std::chrono::milliseconds(50));
        if(!_automous)
        { 
          /*
          RemoNet::CCCanMsg req;
          for(int32_t i=0;i<sizeof(frames)/sizeof(cannet_frame);i++)
          {
               if(frames[i].canid!=0)
               {
                    auto frame=req.add_frams();
                    frame->set_canid(frames[i].canid);
                    frame->set_dlc(frames[i].dlc);
                    frame->set_data(frames[i].data,frames[i].dlc);
               }
          }
          */
    //       std::cout<<_data.engine_rpm<<","<<_data.brake_pressure<<","<<_data.gear<<","<<_data.gearbox_oil_pressure<<","<<_data.gearbox_oil_temp<<","<<_data.speed<<","<<_data.work_pressure<<std::endl;
         RemoNet::State req;
         req.set_brake_pressure(_data.brake_pressure);
         req.set_engine_rpm(_data.engine_rpm);
         req.set_engine_pressure(_data.engine_pressure);
         //req.set_gear(_data.gear);
         req.set_gearbox_oil_pressure(_data.gearbox_oil_pressure);
         req.set_gearbox_oil_temp(_data.gearbox_oil_temp);
         req.set_speed(_data.speed);
         req.set_work_pressure(_data.work_pressure);
         req.set_cold_water(_data.cold_water);
         req.set_steer_angle(_data.steer_angle);
         req.set_left_lock(_data.left_lock_status);
         req.set_right_lock(_data.right_lock_status);
         
          MessageHead Head;
          CIOBuffer pBuffer;
          Head.Command = RemoNet::CC_STATE;
          Head.Length = req.ByteSizeLong();
          Head.Serialize(pBuffer.Buffer);
          auto ptr = pBuffer.Buffer + MessageHead::Size();
          req.SerializeToArray(ptr, Head.Length);
          pBuffer.Length = MessageHead::Size() + Head.Length;
          if(_channelReady)
               _message->WriteBuffer(ChannelType::CHANNEL_CAR,pBuffer);
        }
       

      }
      
}
void CCarSensor::SetAutomous(bool value,int32_t delay)
{    if(delay==0)
         _automous=value;
     else
     {
          _delayseconds=delay;
          _delayset=value;
     _delay=std::thread(&CCarSensor::DelayFn,this);
     _delay.detach();
     }
}
void CCarSensor::DelayFn()
{
   std::this_thread::sleep_for(std::chrono::seconds(_delayseconds));
   _automous=_delayset;
}
 /*
bool CCarSensor::DriverAnalog(int32_t steer,int32_t throttle,int32_t brake,bool onoff)
{
      CIOBuffer pBuffer;
      cannet_frame* frame=reinterpret_cast<cannet_frame *>(pBuffer.Buffer);
      memset(frame->data,0,sizeof(frame->data));
      frame->canid=htonl(0X386);
      frame->dlc=8;
      frame->data[0]=throttle&0XFF;
      frame->data[1]=brake&0XFF;
      frame->data[2]=steer&0xFF;
      frame->data[6]=onoff?1:0;
      _socket->Write(&pBuffer);
      return true;

}
bool CCarSensor::EmbraceAnalog(int32_t tip,int32_t zoomL,int32_t zoomR,bool onoff)
{
     CIOBuffer pBuffer;
     cannet_frame* frame=reinterpret_cast<cannet_frame *>(pBuffer.Buffer);
     memset(frame->data,0,sizeof(frame->data));
     frame->canid=htonl(0x486);
     frame->dlc=8;
     frame->data[0]=tip&0xFF;
     frame->data[2]=zoomL&0xFF;
     frame->data[4]=zoomR&0xFF;
     frame->data[6]=onoff?1:0;
     _socket->Write(&pBuffer);
     return true;
}
*/
void CCarSensor::SetSensorSocket(SensorSocket<CCarSensor> *s)
{
    _socket=s; 
}
  
void CCarSensor::SetChannelReady(bool ret)
{
     _channelReady=ret;
}
inline int8_t hi_byte(int16_t value)
{
	int8_t hi =(int8_t)((value & 0xFF00) >> 8);
	return hi;
}
inline int8_t lo_byte(int16_t value)
{
	int8_t lo = (int8_t)(value & 0xff);
	return lo;
}
void CCarSensor::OnMessage(cannet_frame* frames,int32_t count)
{
    
    std::lock_guard<std::mutex> lg(_last_can_lock);
     _last_can_uptime= std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
   
    
     for(int32_t i=0;i<count;i++)
     {
      cannet_frame& frame=frames[i];
     // TPCANMsg msg;
     // msg.LEN=frame.dlc;
     // msg.MSGTYPE=PCAN_MESSAGE_STANDARD;
       int32_t canid=frame.canid;
        switch(canid)
        {
          /*case 0x701:
             
             if(frame.data[5]==0)
             {
               _automous=_finished=false;
             }
             else
              {
               _automous=frame.data[5]!=0&&!_finished;
              }
             
              break; */
         
          case 0x181:
           {
            
              if(frame.data[0]==0x08)
              {
                    if(_message->GetParkingState()!=ParkingState::None)
                    {
                         _message->SetParkingState(ParkingState::None);
                    }
                   // if(_automous)   	
                    {
                         _automous=false;
                         _message->StopAutomous();
                    }
                    
               }
              
              

                _msg[0].ID=0x186;
                _msg[0].LEN=frame.dlc;
                _msg[0].MSGTYPE=PCAN_MESSAGE_STANDARD;
               memcpy(_msg[0].DATA,frame.data,frame.dlc);
               _front_view= (_msg[0].DATA[1]&0x01)==1;

               _message->SwitchCamera(_front_view);
              

             
               if((_msg[0].DATA[2]==0x01||_msg[0].DATA[2]==0x02)&&std::abs(_rise_down_value)>100)
               {
                    
                         _message->SetParkingState(ParkingState::RiseDown);
                         
               } 
              /*  std::cout<<std::hex<<"ID:"<<msg.ID<<std::endl;
               for(int i=0;i<msg.LEN;i++)
                    {
                         int16_t t=msg.DATA[i];
                         std::cout<<std::hex<<t<<" ";
                    }
                    std::cout<<std::endl;	     
                */  
             
           } 
             break;

          case 0x185:
            if(_message->GetParkingState()!=ParkingState::None) return;
             _msg[1].ID=_automous?0:0x586;
             _msg[1].LEN=frame.dlc;
             _msg[1].MSGTYPE=PCAN_MESSAGE_STANDARD;
            _steer=make_int16(frame.data[2],frame.data[3]);
             if(_message->GetCarType()==CarType::KRESS)
             {
           
                int16_t value=_steer;
               value/=2;
               frame.data[2] = hi_byte(value);
			frame.data[3] = lo_byte(value);
             }
             /*
             if(_front_view==false)
             {
                
                 frame.data[4]^=1;
                


             }
             */
             
            //  for(int i=0;i<frame.dlc;i++)
            //        {
             //            int16_t t=frame.data[i];
             //            std::cout<<std::hex<<t<<" ";
            //        }
           //         std::cout<<std::endl;	
            // frame.data[0]=0xFF;
             memcpy(_msg[1].DATA,frame.data,frame.dlc);
            
             break;
          case 0x281:
            {
                if(_message->GetParkingState()!=ParkingState::None) return;
               _msg[2].ID=_automous?0:0x286; 
               _msg[2].LEN=frame.dlc;
               _msg[2].MSGTYPE=PCAN_MESSAGE_STANDARD;
             /* if(_automous)
              {
                  frame.data[0]=0x08;
                  frame.data[1]=0;
                  memcpy(msg.DATA,frame.data,frame.dlc);       
              }
              else
              */
              {
                
                 memcpy(_msg[2].DATA,frame.data,frame.dlc);
              }
           

            } 
             break;
             
          case 0x381:
             _brake=frame.data[1];
           if(_message->GetParkingState()!=ParkingState::None) return;
             _msg[3].ID=_automous?0:0x386;
             _msg[3].LEN=frame.dlc;
             _msg[3].MSGTYPE=PCAN_MESSAGE_STANDARD;
             memcpy(_msg[3].DATA,frame.data,frame.dlc);
             if(_msg[3].DATA[6]==0x0F&&std::abs(_tip_value)>2000)//&&_data.left_lock_status!=0&&_data.right_lock_status!=0)
             {
                      _message->SetParkingState(_tip_value>0?ParkingState::TipIn:ParkingState::TipRise);
             }

             break;
          case 0x481:
           if(_message->GetParkingState()!=ParkingState::None) return;
             _msg[4].ID=_automous?0:0x486;
             _msg[4].LEN=frame.dlc;
             _msg[4].MSGTYPE=PCAN_MESSAGE_STANDARD;
             _rise_down_value=make_int16(_msg[4].DATA[5],_msg[4].DATA[4]);
             if(_rise_down_value==0)
                  _rise_down_value=make_int16(_msg[4].DATA[3],_msg[4].DATA[2]);
               
          //     std::cout<<std::hex<<_rise_down_value<<std::endl;

              _tip_value=make_int16(_msg[4].DATA[0],_msg[4].DATA[1]);

             memcpy(_msg[4].DATA,frame.data,frame.dlc);
             break;
        }
    //    if(msg.ID!=0)
    //      _can->Write(&msg);
        
     }
}



void CCarSensor::Write(TPCANMsg& msg)
{
     if(_can_ready==false)
     {
          auto  ret= _can->Uninitialize();
          std::cout<<ret<<std::endl;
           ret= _can->Initialize();
           _can_ready=ret==PCAN_ERROR_OK;
             std::cout<<ret<<std::endl;
           return;
     }
      auto ret= _can->Write(&msg);
        
      if(ret!=PCAN_ERROR_OK)
      {
           std::cout<<ret<<std::endl;

          
         ret= _can->Uninitialize();
          std::cout<<ret<<std::endl;
           ret= _can->Initialize();
            _can_ready=ret==PCAN_ERROR_OK;
          
           
             std::cout<<ret<<std::endl;

      }
}


void CCarSensor::Notify(int8_t * data,int32_t size)
{
     ////if(!_automous) return;
     MessageHead head;
    head.Deserialize(data);
    auto ptr=data+sizeof(MessageHead);
    switch (head.Command)
    {
case AR::AR_Finish:
     {
           ARFinish * node=(ARFinish *)ptr;
           _message->OnGoalFinish((WorkArea)node->area,node->no);
           RemoNet::CSMoveEnd Req;
           Req.set_area(node->area);
           Req.set_no(node->no);
           Req.set_uid(0);
           CIOBuffer buffer;
           MessageHead Head;
           Head.Command=RemoNet::CS_MoveEnd;
           Head.Length=Req.ByteSize();
           Head.Serialize(buffer.Buffer);
           auto ptr=buffer.Buffer+MessageHead::Size();
           Req.SerializeToArray(ptr,Head.Length);
           buffer.Length=MessageHead::Size()+Head.Length;
           _message->WriteBuffer(buffer);
          _message->SetAutomous(false,0);
     }
     break;
     case AR::AR_NDTPos:
     {
          ARNdtPos* node=(ARNdtPos *)ptr;
          _message->SetNdtPos(node);
     }
     break;
     case AR::AR_CTRL:
     {
          if(_automous)
          {
               _last_can_uptime= std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
   
              //   int64_t tick= std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
             
          if(_last_can_uptime-_last_encode_uptime>5000)
          {
               _message->StopAutomous();
               RemoNet::MoveRet Req;
               Req.set_desc(RemoNet::MoveDesc::Move_Encode_Fail);
               Req.set_peer(_message->GetPeer());
               CIOBuffer pBuffer;
               MessageHead Head;
          
               Head.Command = RemoNet::CS_MoveRet;
               Head.Length = Req.ByteSizeLong();
               Head.Serialize(pBuffer.Buffer);
               auto ptr = pBuffer.Buffer + MessageHead::Size();
               Req.SerializeToArray(ptr, Head.Length);
               pBuffer.Length = MessageHead::Size() + Head.Length;
               _message->WriteBuffer(pBuffer);
               _automous=false;
               return;
          }
           can_array* array=reinterpret_cast<can_array *>(ptr);
           for(auto i=0;i<array->count;i++)
               {
                    // TPCANMsg msg;
                    cannet_frame& frame=array->frames[i];
                    if(frame.canid==0x386)
                    {
                    
                    
                         int16_t accel=frame.data[0];
                         int16_t brake=frame.data[1];
                         std::cout<<"accel :"<<accel<<"  brake :"<<brake<< std::endl;
                         auto * node=_message->GetRadar();
                         // std::cout<<"tick :"<<tick-node[0].radar_tick<<"  range :"<<node[0].range<< std::endl;
                         if((_last_can_uptime-node[0].radar_tick>5000)|| node[0].range<250) 
                         {
                              //std::cout << "tick :"<<_last_can_uptime-node[0].radar_tick<<"  range :"<<node[0].range<< std::endl;
                              _range_count++;
                         }
                         else
                         {
                              _range_count=0;
                         }
                         if(_range_count>=100||_brake>100)
                         {
                               frame.data[0]=0;
                               frame.data[1]=100;
                            
                         }
                         _msg[3].ID=frame.canid;
                         _msg[3].LEN=frame.dlc;
                         memcpy(_msg[3].DATA,frame.data,frame.dlc);
                    }
                    else if (frame.canid==0x586) {
                         _msg[1].ID=frame.canid;
                         _msg[1].LEN=frame.dlc;
                         _msg[1].MSGTYPE=PCAN_MESSAGE_STANDARD;

                         
                         memcpy(_msg[1].DATA,frame.data,frame.dlc);
                    }
                    else if(frame.canid==0x286)
                    {
                         _msg[2].ID=frame.canid;
                         _msg[2].LEN=frame.dlc;
                         _msg[2].MSGTYPE=PCAN_MESSAGE_STANDARD;

                         
                         memcpy(_msg[2].DATA,frame.data,frame.dlc);
                    }

               }
          }
     }
     break;

    }
   
     
}
void CCarSensor::Notify(TPCANMsg& canmsg)
{
   //   int32_t receivedCnt = size/CAN_MSG_LEN;
	//    cannet_frame* p = (cannet_frame*)buffer;
      
   //  for(int i=0; i<receivedCnt; i++)
     //{
     //    p->canid=htonl(p->canid);
        int32_t index=-1;   
        int32_t canid=canmsg.ID;
        //std::cout<<std::hex<<canid<<std::endl;
	//printf("---%x---\r\n",canid);
        
        switch (canid)
        {
        case 0x1F0:
             index=0;
             break;
        case 0x1F1:
             _data.work_pressure=make_int16(canmsg.DATA[1],canmsg.DATA[0]);//工作系统压力
             _data.brake_pressure=make_int16(canmsg.DATA[5],canmsg.DATA[4]);//制动压力
            
             break;;
        case 0x1F2:
             index=3;
             break;
        case 0x1F3:
             _data.engine_rpm=make_int16(canmsg.DATA[1],canmsg.DATA[0]);
             _data.engine_pressure=canmsg.DATA[4]+canmsg.DATA[5]*256;
             _data.cold_water=make_int16(canmsg.DATA[7],canmsg.DATA[6]);
             
             break;
        case 0x1F4:
             _data.gearbox_oil_temp=make_int16(canmsg.DATA[1],canmsg.DATA[0]);
             _data.gearbox_oil_pressure=make_int16(canmsg.DATA[3],canmsg.DATA[2]);
             //_data.gear=make_int16(canmsg.DATA[5],canmsg.DATA[4]);

             break;
        case 0x1F5:
             index=6;
             break;
        case 0x1F6:
             index=7;
             break;
        case 0x283:
            _data.left_lock_status= (canmsg.DATA[0]&0x2)!=0;
            _data.right_lock_status=(canmsg.DATA[0]&0x4)!=0;
            break;
        case 0x2F2:
             index=8;
             break;
        case 0x2F3:
             index=9;
             break;
        case 0x2F4:
             index=10;
             break;
        
        case 0x3F0:
             index=11;
             break;
        case 0x3F1:
             index=12;
             break;
        case 0x3F2:
             index=13;
             break;
        case 0x3F3:
             index=14;
             break;
        case 0x3F4:
             index=15;
             break;
 
        case 0x4F0:
             index=16;
             break; 
        case 0x4F1:
             index=17;
             break;
        case 0x4F2:
             index=18;
             break;
        case 0x4F3:
             _data.speed=make_int16(canmsg.DATA[4],canmsg.DATA[5]);
	  //   std::cout << "speed00000" << _data.speed << std::endl;
	     break;
        case 0x4F4:
             index=20;
             break;
         
        
     
	 case 0x189:
         {   
            
             _last_encode_uptime=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
             
           
             int32_t l0=canmsg.DATA[0]&0x000000ff;
             int32_t l1=canmsg.DATA[1]&0x000000ff;
             int32_t l2=canmsg.DATA[2]&0x000000ff;
             int32_t l3=canmsg.DATA[3]&0x000000ff;
             int16_t last_steer_angle=  (l1<<8)|l0;
             
             _data.steer_angle=last_steer_angle;
	     std::cout << last_steer_angle << "angle" << std::endl; 
             _message->SetSteerAngle(last_steer_angle);
              
          //   if(_automous) // 可以随时发
	     { 
        	     CIOBuffer pBuffer;
               MessageHead Head;
               Head.Command=RA::RA_Encode,
               Head.Length=sizeof(cannet_frame);
               Head.Serialize(pBuffer.Buffer);
               auto ptr=pBuffer.Buffer+sizeof(MessageHead);
               cannet_frame * data=reinterpret_cast<cannet_frame *>(ptr);
               
               data->canid=canmsg.ID;
               data->dlc=canmsg.LEN;
               memcpy(data->data,canmsg.DATA,canmsg.LEN);
               pBuffer.Length=Head.Length+sizeof(MessageHead);
               _socket->Write(pBuffer.Buffer, pBuffer.Length);
             }
             
              
         }
          break;
     
        default:
          break;
        }
         
       /*  
         if(index!=-1)
         {
          frames[index].canid=canid;
          frames[index].dlc=canmsg.LEN;
          memcpy(frames[index].data,canmsg.DATA,sizeof(canmsg.DATA)); 
         }
         */

}
int64_t CCarSensor::GetLastEncodeTime()  
{
     return _last_encode_uptime;
}
 
bool CCarSensor::IsFrontView()
{
      return _front_view;
}
int16_t CCarSensor::GetRiseDownValue() const
{
    return _rise_down_value;
}
int16_t CCarSensor::GetTipValue() const
{
     return _tip_value;
}