123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570 |
- #include "p900.h"
- #define CONNECTED true
- #define DISCONNECTED false
- //#define DEBUG
- extern iv::Ivlog * givlog;
- extern iv::Ivfault *gfault;
- radio * gradio;
- VehicleStatus back_car;
- radio::radio(const char * struartdev,const char * strmsgradio_name_send, const char * strmsgradio_car_id, \
- const char * strmemgps, const char * strmemmap_index, const char * strcollision_index)
- {
- mTime.start();
- mset_cmd_mode = 0;
- virtual_gps_index_counter = 0;
- mbSerialOpen = false;
- mnNotRecvCount = 0;
- mnLastOpenTime = 0;
- mnLastServerDataTime = 0;
- mserial_recv_data.clear();
- gradio = this;
- mpmem_radio_gps_addr = iv::modulecomm::RegisterRecv(strmemgps,Listen_gpsimu);
- mpmem_radio_send_addr = iv::modulecomm::RegisterSend(strmsgradio_name_send,100,1);
- mpmem_map_addr = iv::modulecomm::RegisterSend(strmemmap_index,5,1);
- ModuleFun funswitchmap = std::bind(&radio::recvCurMapIndex,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- iv::modulecomm::RegisterRecvPlus("current_map_index",funswitchmap);
- ModuleFun recBrainCmd = std::bind(&radio::recvBrainCMD,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- iv::modulecomm::RegisterRecvPlus("current_map_index",recBrainCmd);
- mcar_id = atoi(strmsgradio_car_id);
- mcollision_point = atoi(strcollision_index);
- mcar_map_index = 1;
- mserver_map_index = 1;
- mserver_status = DISCONNECTED;
- mcar_control_speed = 0;
- givlog->debug("init","[%s]car id %d",__func__, mcar_id);
- mapInit();
- serialPortInit(struartdev);
- mradio_protobuf_send.set_server_status(0);
- mradio_protobuf_send.set_car_control_speed(0);
- mradio_protobuf_send.set_cmd_mode(0);
- mradio_protobuf_send.set_cmd_control(0);
- mcar_status_data = new CarStatusData;
- mcar_status_data->mmutex.lock();
- mcar_status_data->loadMapFromFile(mvmap_name.at(0));
- micur_maptrace_index = 0;
- mcar_status_data->mmutex.unlock();
- mcar_status_data->frameDataInit(mcar_id);
- QTimer * timer = new QTimer(); //car数据发送定时器,每隔30ms发送一次
- connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
- timer->setTimerType(Qt::PreciseTimer);
- timer->start(30);
- QTimer * timer_server = new QTimer(); //服务器数据接收定时器,每隔300ms检查一次
- connect(timer_server,SIGNAL(timeout()),this,SLOT(onTimerServer()));
- timer_server->setTimerType(Qt::PreciseTimer);
- timer_server->start(30);
- }
- radio::~radio()
- {
- m_serialPort_Radio->close();
- delete gradio;
- givlog->debug("init","[%s] radio bye",__func__);
- }
- void Listen_gpsimu(const char * _strdata,const unsigned int _nSize,const unsigned int _index,const QDateTime * _dt,const char * _strmemname)
- {
- iv::gps::gpsimu proto_gpsimu;
- if(false == proto_gpsimu.ParseFromArray(_strdata,_nSize))
- {
- givlog->error("GPS","[%s] Listencansend Parse fail.",__func__);
- return;
- }
- gradio->mserial_send_data = gradio->mcar_status_data->fill_frame(proto_gpsimu);
- // givlog->verbose("listen gps data");
- }
- void radio::recvCurMapIndex(const char * _strdata,const unsigned int _nSize,const unsigned int _index,const QDateTime * _dt,const char * _strmemname)
- {
- iv::formation_map_index::map map_index;
- if(false == map_index.ParseFromArray(_strdata,_nSize))
- {
- givlog->error("p900","[%s] Listencansend Parse fail.",__func__);
- return;
- }
- if(map_index.has_currentindex())
- {
- micur_maptrace_index = map_index.currentindex();
- givlog->info("map","[%s] Rec Current map index from maptrace,Index:%d",micur_maptrace_index);
- mcar_status_data->mimaptrace_index = micur_maptrace_index;
- }
- }
- void radio::recvBrainCMD(const char *pdata, const int ndatasize,const unsigned int _index,const QDateTime * _dt,const char * _strmemname)
- {
- if(ndatasize < sizeof(iv::hmi::HMIBasic))
- return;
- iv::hmi::hmimsg xhmimsg;
- if(!xhmimsg.ParseFromArray(pdata,ndatasize))
- {
- givlog->error("iv::decition::BrainDecition::UpdateHMI parse error");
- return;
- }
- mbRunPause = xhmimsg.mbpause();
- givlog->info("collision","[%s] rec brain state from HMI");
- }
- bool radio::serialPortInit(const char * struartdev)
- {
- m_serialPort_Radio = new QSerialPort();
- m_serialPort_Radio->setPortName(struartdev);
- m_serialPort_Radio->setBaudRate(QSerialPort::Baud115200);
- m_serialPort_Radio->setParity(QSerialPort::NoParity);
- m_serialPort_Radio->setDataBits(QSerialPort::Data8);
- m_serialPort_Radio->setStopBits(QSerialPort::OneStop);
- m_serialPort_Radio->setFlowControl(QSerialPort::NoFlowControl);
- m_serialPort_Radio->setReadBufferSize(0);
- connect(m_serialPort_Radio,SIGNAL(readyRead()),this,SLOT(receiveData()));
- if(m_serialPort_Radio->open(QSerialPort::ReadWrite))
- {
- givlog->debug("init","[%s] open serial port success",__func__);
- mbSerialOpen = true;
- return true;
- }
- else
- {
- givlog->error("init","[%s]open serial faile",__func__);
- mbSerialOpen = false;
- return false;
- }
- }
- void radio::run()
- {
- while(!QThread::isInterruptionRequested())
- {
- sleep(1);
- }
- }
- //定时发送车辆状态到服务器
- void radio::onTimer()
- {
- #ifdef DEBUG
- mbSerialOpen = true;
- #endif
- if(mbSerialOpen)
- {
- #ifdef DEBUG
- //模拟发送数据
- mserial_send_data = mcar_status_data->test_use_map_gps(virtual_gps_index_counter);
- if(mserial_send_data.isEmpty())
- {
- givlog->debug("sendData","[%s] test data error",__func__);
- return;
- }
- if(virtual_gps_index_counter<mcar_status_data->navigation_data.size())
- virtual_gps_index_counter++;
- else
- virtual_gps_index_counter = 0;
- qDebug()<<virtual_gps_index_counter;
- #endif
- m_serialPort_Radio->write(mserial_send_data);
- givlog->verbose("sendData","[%s] send car statue frame: %s",__func__,mserial_send_data.toHex().data());
- }
- else{
- if((QDateTime::currentMSecsSinceEpoch() - mnLastOpenTime)>=100)
- {
- if(m_serialPort_Radio->open(QSerialPort::ReadWrite))
- {
- givlog->debug("state","[%s] open serial port success",__func__);
- mbSerialOpen = true;
- }
- else
- {
- givlog->debug("state","[%s] open faile",__func__);
- }
- mnLastOpenTime = QDateTime::currentMSecsSinceEpoch();
- }
- }
- }
- // 服务器状态监控
- void radio::onTimerServer()
- {
- if((QDateTime::currentMSecsSinceEpoch() - mnLastServerDataTime)>=900)
- {
- mserver_status = DISCONNECTED;
- mradio_protobuf_send.set_server_status(mserver_status);
- protoMsgSend();
- givlog->error("state","[%s] server lose connect",__func__);
- }
- #ifdef DEBUG_REC
- //模拟接受服务器数据
- QByteArray _ba;
- static int count = 1;
- int sum =0;
- _ba.clear();
- _ba[0] = 0xAA;
- _ba[1] = 0x26;
- _ba[2] = 0x09;
- _ba[3] = 0x11;
- _ba[4] = 0;
- _ba[5] = 0;
- _ba[6] = count;
- if(count < 10)
- count++;
- else
- count = 1;
- _ba[7] = 0;
- _ba[8] = 0;
- for(int i =0; i<9; i++)
- sum = sum+_ba[i];
- _ba[9] = sum &0x00ff;
- givlog->debug("virtual","[%s] %s",__func__, _ba.toHex().data());
- virtual_gps_index_counter = 0;
- serialData_Decode(_ba);
- #endif
- }
- // 预处理数据,截取完整数据包
- bool radio::preprocData()
- {
- int32_t index = 0;
- while((mserial_recv_data.mid(index,2).toHex() != "aa26") && \
- (mserial_recv_data.mid(index,2).toHex() != "aa28") && \
- (index<mserial_recv_data.length()))
- {
- index = index + 1;
- }
- givlog->verbose("recData","[%s] mserial_recv_data:[rec:] %s index: %d",__func__,mserial_recv_data.toHex().data(),index);
- if(index)
- mserial_recv_data.remove(0,index);
- givlog->verbose("recData","[%s] mserial_recv_data:[after:] %s",__func__, mserial_recv_data.toHex().data());
- if(checkSum(0))
- {
- return true;
- }
- return false;
- }
- void radio::receiveData()
- {
- if(mbSerialOpen)
- {
- mnLastServerDataTime = QDateTime::currentMSecsSinceEpoch();
- QByteArray _ba;
- _ba.clear();
- _ba = m_serialPort_Radio->readAll();
- serialData_Decode(_ba);
- givlog->verbose("recData","[%s] %s",__func__,_ba.toHex().data());
- #if 0
- mserial_recv_data.append(_ba);
- qDebug()<<"ba"<<_ba.toHex();
- radioServerDataDecode();
- #endif
- }
- }
- void radio::radioServerDataDecode()
- {
- uint8_t _length;
- uint8_t _data[15];
- if(!preprocData())
- {
- return;
- }
- _length = (mserial_recv_data.data()[2]&0xff) + 1;
- memcpy(_data,mserial_recv_data.data(),_length);
- // qDebug()<<"_data"<<QByteArray((char *)_data,_length).toHex();
- if(_data[3] == mcar_id)
- {
- mserver_status = CONNECTED;
- radioServerDataDecodeSen(_data);
- }
- //清除此次已处理的数据段
- mserial_recv_data.remove(0,_length);
- if(!mserial_recv_data.isEmpty())
- {
- radioServerDataDecode();
- }
- return;
- }
- void radio::radioServerDataDecodeSen(const uint8_t *_data)
- {
- //处理服务器决策信息
- mcar_control_speed =(double)(_data[4]<< 8 | _data[5])/10.0;
- mradio_protobuf_send.set_car_control_speed(mcar_control_speed);
- mradio_protobuf_send.set_server_status(mserver_status);
- mradio_protobuf_send.set_cmd_mode(mset_cmd_mode);
- mradio_protobuf_send.set_cmd_control(0);
- givlog->verbose("recData","[%s:] 0x26 id: %d speed: %f cmd_mode: %d",__func__,_data[3],mcar_control_speed,mset_cmd_mode);
- protoMsgSend();
- }
- void radio::protoMsgSend()
- {
- char * strser;
- bool bser;
- int nbytesize;
- nbytesize = mradio_protobuf_send.ByteSize();
- strser = new char[nbytesize];
- bser = mradio_protobuf_send.SerializeToArray(strser,nbytesize);
- if(bser)
- iv::modulecomm::ModuleSendMsg(mpmem_radio_send_addr,strser,nbytesize);
- else
- {
- givlog->error("sendData","[%s:] radio serialize error.",__func__);
- // gfault->SetFaultState(1, 0, "radio serialize err");
- }
- delete strser;
- }
- void radio::mapProtoMsgSend()
- {
- char * strser;
- bool bser;
- int nbytesize;
- nbytesize = mmap_protobuf_index.ByteSize();
- strser = new char[nbytesize];
- bser = mmap_protobuf_index.SerializeToArray(strser,nbytesize);
- if(bser)
- iv::modulecomm::ModuleSendMsg(mpmem_map_addr,strser,nbytesize);
- else
- {
- givlog->error("sendData","[%s:] map index error.",__func__);
- // gfault->SetFaultState(1, 0, "radio serialize err");
- }
- delete strser;
- }
- bool radio::checkSum(const uint32_t _index)
- {
- if( mserial_recv_data.length() > 9 && mserial_recv_data.length() > (mserial_recv_data.data()[2]&0xff) )
- {
- uint8_t _len = (mserial_recv_data.data()[_index+2]&0xff) + _index;
- uint8_t _sum = 0;
- for(int i = _index; i < _len; i++)
- {
- _sum = _sum + (mserial_recv_data.data()[i]&0xff);
- }
- _sum = _sum & 0xFF;
- if(_sum == (uint8_t)mserial_recv_data.data()[_len])
- {
- givlog->debug("recData","check sum success");
- return true;
- }
- }
- givlog->debug("recData","[%s],check sum fail",__func__);
- return false;
- }
- //串口数据解析
- void radio::serialData_Decode(QByteArray rx_data)
- {
- uint8_t active_char=0;
- VehicleStatus vehicle_status;
- static uint8_t protocal_flag=0,protocal_index=0;//协议解析标记
- static uint8_t receive_data[30];
- int nbytes=rx_data.length();
- uint16_t check_sum=0;
- double car_control_speed;
- for(int nbytes_index=0;nbytes_index<nbytes;nbytes_index++)
- {
- active_char=rx_data[nbytes_index];
- switch(protocal_flag)
- {
- case 0:
- if(active_char == 0xAA)
- {
- protocal_index=0;
- protocal_flag=1;
- receive_data[protocal_index]=active_char;
- }
- break;
- case 1:
- protocal_index++;
- receive_data[protocal_index]=active_char;
- if(active_char == 0x16)
- {
- protocal_flag=2;
- }
- break;
- case 2:
- protocal_index++;
- receive_data[protocal_index]=active_char;
- if(protocal_index>=24) //计算校验和
- {
- check_sum=0;
- for(uint8_t check_i=0;check_i<protocal_index;check_i++)
- {
- check_sum+=receive_data[check_i];
- }
- if(receive_data[protocal_index]==(check_sum&0x00ff))
- {
- protocal_flag=3; //校验通过
- }else{
- protocal_flag=0;
- }
- }
- if(protocal_flag==3)
- {
- // 解析数据
- vehicle_status.vehicleID=receive_data[3];
- vehicle_status.vehicleLat=((receive_data[4]<<24)|(receive_data[5]<<16)|(receive_data[6]<<8)|(receive_data[7]));
- vehicle_status.vehicleLon=((receive_data[8]<<24)|(receive_data[9]<<16)|(receive_data[10]<<8)|(receive_data[11]));
- vehicle_status.vehicleSpeed=((receive_data[12]<<8)|(receive_data[13]));
- vehicle_status.vehicleAng=((receive_data[14]<<8)|(receive_data[15]));
- vehicle_status.vehicleDistance=((receive_data[16]<<8)|(receive_data[17]));
- vehicle_status.vehicleProcess=((receive_data[18]<<8)|(receive_data[19]));
- vehicle_status.map_type=receive_data[20];
- vehicle_status.status_flag=receive_data[23];
- car_control_speed=((receive_data[21]<<8)|(receive_data[22]))/10;
- if(CAR_TYPE==1)
- {
- back_car.vehicleSpeed=vehicle_status.vehicleSpeed;
- back_car.vehicleProcess=vehicle_status.vehicleProcess;
- back_car.vehicleDistance=vehicle_status.vehicleDistance;
- if(back_car.vehicleProcess<4000)
- {
- formationcontrol(front_car,back_car,&back_car,2);
- mradio_protobuf_send.set_car_control_speed(400);
- }else{
- formationcontrol(front_car,back_car,&back_car,1);
- mradio_protobuf_send.set_car_control_speed(600);
- }
- }else if(CAR_TYPE==2)
- {
- mradio_protobuf_send.set_car_control_speed(car_control_speed);
- }
- mradio_protobuf_send.set_server_status(true);
- mradio_protobuf_send.set_cmd_mode(0);
- mradio_protobuf_send.set_cmd_control(0);
- givlog->verbose("recData","[%s] receive 0x26 : car_id %d,speed %f,map:%d",__func__,mcar_id, \
- car_control_speed, mserver_map_index);
- protocal_flag=0;
- protocal_index=0;
- }
- break;
- default:
- break;
- }
- }
- protoMsgSend();
- }
- void radio::mapInit()
- {
- char * strx = getenv("HOME");
- std::string strmap_dir = strx;
- strmap_dir = strmap_dir + "/map/";
- mvmap_name.append(strmap_dir + "map.txt");
- for(int i = 0; i < mvmap_name.length(); i++)
- givlog->debug("map","[%s] map_name: %s", __func__, mvmap_name.at(i).data());
- }
- void formationcontrol(VehicleStatus Master,VehicleStatus Slave,VehicleStatus *formationD,unsigned char controlMode)
- {
- float kp=3.0,kd=0.6; //10cm,error=1,kp=2,+0.2km/h
- float error=0,speed_out=0;
- if(controlMode==1){ //进度控制模式
- kp=3.0;
- kd=0.6; //10cm,error=1,kp=2,+0.2km/h
- error=0;
- speed_out=0;
- if((Slave.vehicleProcess>2)&&(abs(Master.vehicleProcess-Slave.vehicleProcess)<1000))
- {
- error=Master.vehicleProcess-Slave.vehicleProcess;
- speed_out=kp*error+kd*(error-Slave.last_error);
- formationD->vehicleDesiredSpeed=Slave.INIT_SPEED+(int)speed_out;
- qDebug() <<"error:"<<error<<"last_error:"<<Slave.last_error<<"speed_out:"<<speed_out<<endl;
- qDebug() <<"Slave_speed:"<<Slave.vehicleSpeed<<"Desired_speed:"<<formationD->vehicleDesiredSpeed<<endl;
- Slave.last_error=error;
- }else{
- formationD->vehicleDesiredSpeed=Slave.INIT_SPEED;
- }
- if((formationD->vehicleDesiredSpeed)<Slave.INIT_SPEED-8)
- {
- formationD->vehicleDesiredSpeed=Slave.INIT_SPEED-8;
- }
- else if((formationD->vehicleDesiredSpeed)>Slave.INIT_SPEED+8)
- {
- formationD->vehicleDesiredSpeed=Slave.INIT_SPEED+8;
- }
- }else if(controlMode==2){ //相对位移控制模式
- kp=3.0;
- kd=0.6; //10cm,error=1,kp=2,+0.2km/h
- error=0;
- speed_out=0;
- if((Slave.vehicleDistanceRel>2)&&(abs(Master.vehicleDistanceRel-Slave.vehicleDistanceRel)<200))
- {
- error=Master.vehicleDistanceRel-Slave.vehicleDistanceRel;
- speed_out=kp*error+kd*(error-Slave.last_error);
- formationD->vehicleDesiredSpeed=Slave.INIT_SPEED+(int)speed_out;
- qDebug() <<"error:"<<error<<"last_error:"<<Slave.last_error<<"speed_out:"<<speed_out<<endl;
- qDebug() <<"Slave_speed:"<<Slave.vehicleSpeed<<"Desired_speed:"<<formationD->vehicleDesiredSpeed<<endl;
- Slave.last_error=error;
- }else{
- formationD->vehicleDesiredSpeed=Slave.INIT_SPEED;
- }
- if((formationD->vehicleDesiredSpeed)<Slave.INIT_SPEED-8)
- {
- formationD->vehicleDesiredSpeed=Slave.INIT_SPEED-8;
- }
- else if((formationD->vehicleDesiredSpeed)>Slave.INIT_SPEED+8)
- {
- formationD->vehicleDesiredSpeed=Slave.INIT_SPEED+8;
- }
- }
- }
|