p900.cpp 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570
  1. #include "p900.h"
  2. #define CONNECTED true
  3. #define DISCONNECTED false
  4. //#define DEBUG
  5. extern iv::Ivlog * givlog;
  6. extern iv::Ivfault *gfault;
  7. radio * gradio;
  8. VehicleStatus back_car;
  9. radio::radio(const char * struartdev,const char * strmsgradio_name_send, const char * strmsgradio_car_id, \
  10. const char * strmemgps, const char * strmemmap_index, const char * strcollision_index)
  11. {
  12. mTime.start();
  13. mset_cmd_mode = 0;
  14. virtual_gps_index_counter = 0;
  15. mbSerialOpen = false;
  16. mnNotRecvCount = 0;
  17. mnLastOpenTime = 0;
  18. mnLastServerDataTime = 0;
  19. mserial_recv_data.clear();
  20. gradio = this;
  21. mpmem_radio_gps_addr = iv::modulecomm::RegisterRecv(strmemgps,Listen_gpsimu);
  22. mpmem_radio_send_addr = iv::modulecomm::RegisterSend(strmsgradio_name_send,100,1);
  23. mpmem_map_addr = iv::modulecomm::RegisterSend(strmemmap_index,5,1);
  24. ModuleFun funswitchmap = std::bind(&radio::recvCurMapIndex,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  25. iv::modulecomm::RegisterRecvPlus("current_map_index",funswitchmap);
  26. ModuleFun recBrainCmd = std::bind(&radio::recvBrainCMD,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  27. iv::modulecomm::RegisterRecvPlus("current_map_index",recBrainCmd);
  28. mcar_id = atoi(strmsgradio_car_id);
  29. mcollision_point = atoi(strcollision_index);
  30. mcar_map_index = 1;
  31. mserver_map_index = 1;
  32. mserver_status = DISCONNECTED;
  33. mcar_control_speed = 0;
  34. givlog->debug("init","[%s]car id %d",__func__, mcar_id);
  35. mapInit();
  36. serialPortInit(struartdev);
  37. mradio_protobuf_send.set_server_status(0);
  38. mradio_protobuf_send.set_car_control_speed(0);
  39. mradio_protobuf_send.set_cmd_mode(0);
  40. mradio_protobuf_send.set_cmd_control(0);
  41. mcar_status_data = new CarStatusData;
  42. mcar_status_data->mmutex.lock();
  43. mcar_status_data->loadMapFromFile(mvmap_name.at(0));
  44. micur_maptrace_index = 0;
  45. mcar_status_data->mmutex.unlock();
  46. mcar_status_data->frameDataInit(mcar_id);
  47. QTimer * timer = new QTimer(); //car数据发送定时器,每隔30ms发送一次
  48. connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
  49. timer->setTimerType(Qt::PreciseTimer);
  50. timer->start(30);
  51. QTimer * timer_server = new QTimer(); //服务器数据接收定时器,每隔300ms检查一次
  52. connect(timer_server,SIGNAL(timeout()),this,SLOT(onTimerServer()));
  53. timer_server->setTimerType(Qt::PreciseTimer);
  54. timer_server->start(30);
  55. }
  56. radio::~radio()
  57. {
  58. m_serialPort_Radio->close();
  59. delete gradio;
  60. givlog->debug("init","[%s] radio bye",__func__);
  61. }
  62. void Listen_gpsimu(const char * _strdata,const unsigned int _nSize,const unsigned int _index,const QDateTime * _dt,const char * _strmemname)
  63. {
  64. iv::gps::gpsimu proto_gpsimu;
  65. if(false == proto_gpsimu.ParseFromArray(_strdata,_nSize))
  66. {
  67. givlog->error("GPS","[%s] Listencansend Parse fail.",__func__);
  68. return;
  69. }
  70. gradio->mserial_send_data = gradio->mcar_status_data->fill_frame(proto_gpsimu);
  71. // givlog->verbose("listen gps data");
  72. }
  73. void radio::recvCurMapIndex(const char * _strdata,const unsigned int _nSize,const unsigned int _index,const QDateTime * _dt,const char * _strmemname)
  74. {
  75. iv::formation_map_index::map map_index;
  76. if(false == map_index.ParseFromArray(_strdata,_nSize))
  77. {
  78. givlog->error("p900","[%s] Listencansend Parse fail.",__func__);
  79. return;
  80. }
  81. if(map_index.has_currentindex())
  82. {
  83. micur_maptrace_index = map_index.currentindex();
  84. givlog->info("map","[%s] Rec Current map index from maptrace,Index:%d",micur_maptrace_index);
  85. mcar_status_data->mimaptrace_index = micur_maptrace_index;
  86. }
  87. }
  88. void radio::recvBrainCMD(const char *pdata, const int ndatasize,const unsigned int _index,const QDateTime * _dt,const char * _strmemname)
  89. {
  90. if(ndatasize < sizeof(iv::hmi::HMIBasic))
  91. return;
  92. iv::hmi::hmimsg xhmimsg;
  93. if(!xhmimsg.ParseFromArray(pdata,ndatasize))
  94. {
  95. givlog->error("iv::decition::BrainDecition::UpdateHMI parse error");
  96. return;
  97. }
  98. mbRunPause = xhmimsg.mbpause();
  99. givlog->info("collision","[%s] rec brain state from HMI");
  100. }
  101. bool radio::serialPortInit(const char * struartdev)
  102. {
  103. m_serialPort_Radio = new QSerialPort();
  104. m_serialPort_Radio->setPortName(struartdev);
  105. m_serialPort_Radio->setBaudRate(QSerialPort::Baud115200);
  106. m_serialPort_Radio->setParity(QSerialPort::NoParity);
  107. m_serialPort_Radio->setDataBits(QSerialPort::Data8);
  108. m_serialPort_Radio->setStopBits(QSerialPort::OneStop);
  109. m_serialPort_Radio->setFlowControl(QSerialPort::NoFlowControl);
  110. m_serialPort_Radio->setReadBufferSize(0);
  111. connect(m_serialPort_Radio,SIGNAL(readyRead()),this,SLOT(receiveData()));
  112. if(m_serialPort_Radio->open(QSerialPort::ReadWrite))
  113. {
  114. givlog->debug("init","[%s] open serial port success",__func__);
  115. mbSerialOpen = true;
  116. return true;
  117. }
  118. else
  119. {
  120. givlog->error("init","[%s]open serial faile",__func__);
  121. mbSerialOpen = false;
  122. return false;
  123. }
  124. }
  125. void radio::run()
  126. {
  127. while(!QThread::isInterruptionRequested())
  128. {
  129. sleep(1);
  130. }
  131. }
  132. //定时发送车辆状态到服务器
  133. void radio::onTimer()
  134. {
  135. #ifdef DEBUG
  136. mbSerialOpen = true;
  137. #endif
  138. if(mbSerialOpen)
  139. {
  140. #ifdef DEBUG
  141. //模拟发送数据
  142. mserial_send_data = mcar_status_data->test_use_map_gps(virtual_gps_index_counter);
  143. if(mserial_send_data.isEmpty())
  144. {
  145. givlog->debug("sendData","[%s] test data error",__func__);
  146. return;
  147. }
  148. if(virtual_gps_index_counter<mcar_status_data->navigation_data.size())
  149. virtual_gps_index_counter++;
  150. else
  151. virtual_gps_index_counter = 0;
  152. qDebug()<<virtual_gps_index_counter;
  153. #endif
  154. m_serialPort_Radio->write(mserial_send_data);
  155. givlog->verbose("sendData","[%s] send car statue frame: %s",__func__,mserial_send_data.toHex().data());
  156. }
  157. else{
  158. if((QDateTime::currentMSecsSinceEpoch() - mnLastOpenTime)>=100)
  159. {
  160. if(m_serialPort_Radio->open(QSerialPort::ReadWrite))
  161. {
  162. givlog->debug("state","[%s] open serial port success",__func__);
  163. mbSerialOpen = true;
  164. }
  165. else
  166. {
  167. givlog->debug("state","[%s] open faile",__func__);
  168. }
  169. mnLastOpenTime = QDateTime::currentMSecsSinceEpoch();
  170. }
  171. }
  172. }
  173. // 服务器状态监控
  174. void radio::onTimerServer()
  175. {
  176. if((QDateTime::currentMSecsSinceEpoch() - mnLastServerDataTime)>=900)
  177. {
  178. mserver_status = DISCONNECTED;
  179. mradio_protobuf_send.set_server_status(mserver_status);
  180. protoMsgSend();
  181. givlog->error("state","[%s] server lose connect",__func__);
  182. }
  183. #ifdef DEBUG_REC
  184. //模拟接受服务器数据
  185. QByteArray _ba;
  186. static int count = 1;
  187. int sum =0;
  188. _ba.clear();
  189. _ba[0] = 0xAA;
  190. _ba[1] = 0x26;
  191. _ba[2] = 0x09;
  192. _ba[3] = 0x11;
  193. _ba[4] = 0;
  194. _ba[5] = 0;
  195. _ba[6] = count;
  196. if(count < 10)
  197. count++;
  198. else
  199. count = 1;
  200. _ba[7] = 0;
  201. _ba[8] = 0;
  202. for(int i =0; i<9; i++)
  203. sum = sum+_ba[i];
  204. _ba[9] = sum &0x00ff;
  205. givlog->debug("virtual","[%s] %s",__func__, _ba.toHex().data());
  206. virtual_gps_index_counter = 0;
  207. serialData_Decode(_ba);
  208. #endif
  209. }
  210. // 预处理数据,截取完整数据包
  211. bool radio::preprocData()
  212. {
  213. int32_t index = 0;
  214. while((mserial_recv_data.mid(index,2).toHex() != "aa26") && \
  215. (mserial_recv_data.mid(index,2).toHex() != "aa28") && \
  216. (index<mserial_recv_data.length()))
  217. {
  218. index = index + 1;
  219. }
  220. givlog->verbose("recData","[%s] mserial_recv_data:[rec:] %s index: %d",__func__,mserial_recv_data.toHex().data(),index);
  221. if(index)
  222. mserial_recv_data.remove(0,index);
  223. givlog->verbose("recData","[%s] mserial_recv_data:[after:] %s",__func__, mserial_recv_data.toHex().data());
  224. if(checkSum(0))
  225. {
  226. return true;
  227. }
  228. return false;
  229. }
  230. void radio::receiveData()
  231. {
  232. if(mbSerialOpen)
  233. {
  234. mnLastServerDataTime = QDateTime::currentMSecsSinceEpoch();
  235. QByteArray _ba;
  236. _ba.clear();
  237. _ba = m_serialPort_Radio->readAll();
  238. serialData_Decode(_ba);
  239. givlog->verbose("recData","[%s] %s",__func__,_ba.toHex().data());
  240. #if 0
  241. mserial_recv_data.append(_ba);
  242. qDebug()<<"ba"<<_ba.toHex();
  243. radioServerDataDecode();
  244. #endif
  245. }
  246. }
  247. void radio::radioServerDataDecode()
  248. {
  249. uint8_t _length;
  250. uint8_t _data[15];
  251. if(!preprocData())
  252. {
  253. return;
  254. }
  255. _length = (mserial_recv_data.data()[2]&0xff) + 1;
  256. memcpy(_data,mserial_recv_data.data(),_length);
  257. // qDebug()<<"_data"<<QByteArray((char *)_data,_length).toHex();
  258. if(_data[3] == mcar_id)
  259. {
  260. mserver_status = CONNECTED;
  261. radioServerDataDecodeSen(_data);
  262. }
  263. //清除此次已处理的数据段
  264. mserial_recv_data.remove(0,_length);
  265. if(!mserial_recv_data.isEmpty())
  266. {
  267. radioServerDataDecode();
  268. }
  269. return;
  270. }
  271. void radio::radioServerDataDecodeSen(const uint8_t *_data)
  272. {
  273. //处理服务器决策信息
  274. mcar_control_speed =(double)(_data[4]<< 8 | _data[5])/10.0;
  275. mradio_protobuf_send.set_car_control_speed(mcar_control_speed);
  276. mradio_protobuf_send.set_server_status(mserver_status);
  277. mradio_protobuf_send.set_cmd_mode(mset_cmd_mode);
  278. mradio_protobuf_send.set_cmd_control(0);
  279. givlog->verbose("recData","[%s:] 0x26 id: %d speed: %f cmd_mode: %d",__func__,_data[3],mcar_control_speed,mset_cmd_mode);
  280. protoMsgSend();
  281. }
  282. void radio::protoMsgSend()
  283. {
  284. char * strser;
  285. bool bser;
  286. int nbytesize;
  287. nbytesize = mradio_protobuf_send.ByteSize();
  288. strser = new char[nbytesize];
  289. bser = mradio_protobuf_send.SerializeToArray(strser,nbytesize);
  290. if(bser)
  291. iv::modulecomm::ModuleSendMsg(mpmem_radio_send_addr,strser,nbytesize);
  292. else
  293. {
  294. givlog->error("sendData","[%s:] radio serialize error.",__func__);
  295. // gfault->SetFaultState(1, 0, "radio serialize err");
  296. }
  297. delete strser;
  298. }
  299. void radio::mapProtoMsgSend()
  300. {
  301. char * strser;
  302. bool bser;
  303. int nbytesize;
  304. nbytesize = mmap_protobuf_index.ByteSize();
  305. strser = new char[nbytesize];
  306. bser = mmap_protobuf_index.SerializeToArray(strser,nbytesize);
  307. if(bser)
  308. iv::modulecomm::ModuleSendMsg(mpmem_map_addr,strser,nbytesize);
  309. else
  310. {
  311. givlog->error("sendData","[%s:] map index error.",__func__);
  312. // gfault->SetFaultState(1, 0, "radio serialize err");
  313. }
  314. delete strser;
  315. }
  316. bool radio::checkSum(const uint32_t _index)
  317. {
  318. if( mserial_recv_data.length() > 9 && mserial_recv_data.length() > (mserial_recv_data.data()[2]&0xff) )
  319. {
  320. uint8_t _len = (mserial_recv_data.data()[_index+2]&0xff) + _index;
  321. uint8_t _sum = 0;
  322. for(int i = _index; i < _len; i++)
  323. {
  324. _sum = _sum + (mserial_recv_data.data()[i]&0xff);
  325. }
  326. _sum = _sum & 0xFF;
  327. if(_sum == (uint8_t)mserial_recv_data.data()[_len])
  328. {
  329. givlog->debug("recData","check sum success");
  330. return true;
  331. }
  332. }
  333. givlog->debug("recData","[%s],check sum fail",__func__);
  334. return false;
  335. }
  336. //串口数据解析
  337. void radio::serialData_Decode(QByteArray rx_data)
  338. {
  339. uint8_t active_char=0;
  340. VehicleStatus vehicle_status;
  341. static uint8_t protocal_flag=0,protocal_index=0;//协议解析标记
  342. static uint8_t receive_data[30];
  343. int nbytes=rx_data.length();
  344. uint16_t check_sum=0;
  345. double car_control_speed;
  346. for(int nbytes_index=0;nbytes_index<nbytes;nbytes_index++)
  347. {
  348. active_char=rx_data[nbytes_index];
  349. switch(protocal_flag)
  350. {
  351. case 0:
  352. if(active_char == 0xAA)
  353. {
  354. protocal_index=0;
  355. protocal_flag=1;
  356. receive_data[protocal_index]=active_char;
  357. }
  358. break;
  359. case 1:
  360. protocal_index++;
  361. receive_data[protocal_index]=active_char;
  362. if(active_char == 0x16)
  363. {
  364. protocal_flag=2;
  365. }
  366. break;
  367. case 2:
  368. protocal_index++;
  369. receive_data[protocal_index]=active_char;
  370. if(protocal_index>=24) //计算校验和
  371. {
  372. check_sum=0;
  373. for(uint8_t check_i=0;check_i<protocal_index;check_i++)
  374. {
  375. check_sum+=receive_data[check_i];
  376. }
  377. if(receive_data[protocal_index]==(check_sum&0x00ff))
  378. {
  379. protocal_flag=3; //校验通过
  380. }else{
  381. protocal_flag=0;
  382. }
  383. }
  384. if(protocal_flag==3)
  385. {
  386. // 解析数据
  387. vehicle_status.vehicleID=receive_data[3];
  388. vehicle_status.vehicleLat=((receive_data[4]<<24)|(receive_data[5]<<16)|(receive_data[6]<<8)|(receive_data[7]));
  389. vehicle_status.vehicleLon=((receive_data[8]<<24)|(receive_data[9]<<16)|(receive_data[10]<<8)|(receive_data[11]));
  390. vehicle_status.vehicleSpeed=((receive_data[12]<<8)|(receive_data[13]));
  391. vehicle_status.vehicleAng=((receive_data[14]<<8)|(receive_data[15]));
  392. vehicle_status.vehicleDistance=((receive_data[16]<<8)|(receive_data[17]));
  393. vehicle_status.vehicleProcess=((receive_data[18]<<8)|(receive_data[19]));
  394. vehicle_status.map_type=receive_data[20];
  395. vehicle_status.status_flag=receive_data[23];
  396. car_control_speed=((receive_data[21]<<8)|(receive_data[22]))/10;
  397. if(CAR_TYPE==1)
  398. {
  399. back_car.vehicleSpeed=vehicle_status.vehicleSpeed;
  400. back_car.vehicleProcess=vehicle_status.vehicleProcess;
  401. back_car.vehicleDistance=vehicle_status.vehicleDistance;
  402. if(back_car.vehicleProcess<4000)
  403. {
  404. formationcontrol(front_car,back_car,&back_car,2);
  405. mradio_protobuf_send.set_car_control_speed(400);
  406. }else{
  407. formationcontrol(front_car,back_car,&back_car,1);
  408. mradio_protobuf_send.set_car_control_speed(600);
  409. }
  410. }else if(CAR_TYPE==2)
  411. {
  412. mradio_protobuf_send.set_car_control_speed(car_control_speed);
  413. }
  414. mradio_protobuf_send.set_server_status(true);
  415. mradio_protobuf_send.set_cmd_mode(0);
  416. mradio_protobuf_send.set_cmd_control(0);
  417. givlog->verbose("recData","[%s] receive 0x26 : car_id %d,speed %f,map:%d",__func__,mcar_id, \
  418. car_control_speed, mserver_map_index);
  419. protocal_flag=0;
  420. protocal_index=0;
  421. }
  422. break;
  423. default:
  424. break;
  425. }
  426. }
  427. protoMsgSend();
  428. }
  429. void radio::mapInit()
  430. {
  431. char * strx = getenv("HOME");
  432. std::string strmap_dir = strx;
  433. strmap_dir = strmap_dir + "/map/";
  434. mvmap_name.append(strmap_dir + "map.txt");
  435. for(int i = 0; i < mvmap_name.length(); i++)
  436. givlog->debug("map","[%s] map_name: %s", __func__, mvmap_name.at(i).data());
  437. }
  438. void formationcontrol(VehicleStatus Master,VehicleStatus Slave,VehicleStatus *formationD,unsigned char controlMode)
  439. {
  440. float kp=3.0,kd=0.6; //10cm,error=1,kp=2,+0.2km/h
  441. float error=0,speed_out=0;
  442. if(controlMode==1){ //进度控制模式
  443. kp=3.0;
  444. kd=0.6; //10cm,error=1,kp=2,+0.2km/h
  445. error=0;
  446. speed_out=0;
  447. if((Slave.vehicleProcess>2)&&(abs(Master.vehicleProcess-Slave.vehicleProcess)<1000))
  448. {
  449. error=Master.vehicleProcess-Slave.vehicleProcess;
  450. speed_out=kp*error+kd*(error-Slave.last_error);
  451. formationD->vehicleDesiredSpeed=Slave.INIT_SPEED+(int)speed_out;
  452. qDebug() <<"error:"<<error<<"last_error:"<<Slave.last_error<<"speed_out:"<<speed_out<<endl;
  453. qDebug() <<"Slave_speed:"<<Slave.vehicleSpeed<<"Desired_speed:"<<formationD->vehicleDesiredSpeed<<endl;
  454. Slave.last_error=error;
  455. }else{
  456. formationD->vehicleDesiredSpeed=Slave.INIT_SPEED;
  457. }
  458. if((formationD->vehicleDesiredSpeed)<Slave.INIT_SPEED-8)
  459. {
  460. formationD->vehicleDesiredSpeed=Slave.INIT_SPEED-8;
  461. }
  462. else if((formationD->vehicleDesiredSpeed)>Slave.INIT_SPEED+8)
  463. {
  464. formationD->vehicleDesiredSpeed=Slave.INIT_SPEED+8;
  465. }
  466. }else if(controlMode==2){ //相对位移控制模式
  467. kp=3.0;
  468. kd=0.6; //10cm,error=1,kp=2,+0.2km/h
  469. error=0;
  470. speed_out=0;
  471. if((Slave.vehicleDistanceRel>2)&&(abs(Master.vehicleDistanceRel-Slave.vehicleDistanceRel)<200))
  472. {
  473. error=Master.vehicleDistanceRel-Slave.vehicleDistanceRel;
  474. speed_out=kp*error+kd*(error-Slave.last_error);
  475. formationD->vehicleDesiredSpeed=Slave.INIT_SPEED+(int)speed_out;
  476. qDebug() <<"error:"<<error<<"last_error:"<<Slave.last_error<<"speed_out:"<<speed_out<<endl;
  477. qDebug() <<"Slave_speed:"<<Slave.vehicleSpeed<<"Desired_speed:"<<formationD->vehicleDesiredSpeed<<endl;
  478. Slave.last_error=error;
  479. }else{
  480. formationD->vehicleDesiredSpeed=Slave.INIT_SPEED;
  481. }
  482. if((formationD->vehicleDesiredSpeed)<Slave.INIT_SPEED-8)
  483. {
  484. formationD->vehicleDesiredSpeed=Slave.INIT_SPEED-8;
  485. }
  486. else if((formationD->vehicleDesiredSpeed)>Slave.INIT_SPEED+8)
  487. {
  488. formationD->vehicleDesiredSpeed=Slave.INIT_SPEED+8;
  489. }
  490. }
  491. }