#include #include #include #include #include #include #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 #include //#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(); _log=std::make_unique(); 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<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::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 :"<5000)|| node[0].range<100) { std::cout << "tick :"<=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 lg(_last_can_lock); for(int32_t i=0;iWrite(&_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;iRead(&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;iset_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<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(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(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 *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 lg(_last_can_lock); _last_can_uptime= std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); for(int32_t i=0;iGetParkingState()!=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<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;iGetParkingState()!=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<Write(&msg); } } void CCarSensor::Write(TPCANMsg& msg) { if(_can_ready==false) { auto ret= _can->Uninitialize(); std::cout<Initialize(); _can_ready=ret==PCAN_ERROR_OK; std::cout<Write(&msg); if(ret!=PCAN_ERROR_OK) { std::cout<Uninitialize(); std::cout<Initialize(); _can_ready=ret==PCAN_ERROR_OK; std::cout<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::system_clock::now().time_since_epoch()).count(); // int64_t tick= std::chrono::duration_cast(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(ptr); for(auto i=0;icount;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 :"<GetRadar(); // std::cout<<"tick :"<5000)|| node[0].range<250) { std::cout << "tick :"<<_last_can_uptime-node[0].radar_tick<<" range :"<=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; icanid=htonl(p->canid); int32_t index=-1; int32_t canid=canmsg.ID; //std::cout<(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; // std::cout<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(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); } /*index=21; 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; int32_t value= (l3<<24)|(l2<<16)|(l1<<8)|l0; */ // std::cout<