main.cpp 5.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210
  1. #include <QCoreApplication>
  2. #include <QTime>
  3. #include "control/control_status.h"
  4. #include "control/controller.h"
  5. #include "xmlparam.h"
  6. #include "modulecomm.h"
  7. #include "ivversion.h"
  8. #include "ivbacktrace.h"
  9. #include "canmsg.pb.h"
  10. #include "decition.pb.h"
  11. #include <thread>
  12. #include <QMutex>
  13. void * gpacansend;
  14. void * gpadecition;
  15. std::string gstrmemdecition;
  16. std::string gstrmemcansend;
  17. bool gbSendRun = true;
  18. iv::brain::decition gdecition_def;
  19. iv::brain::decition gdecition;
  20. QMutex gMutexDecision;
  21. QTime gTime;
  22. int gnLastSendTime = 0;
  23. int gnLastRecvDecTime = -1000;
  24. int gnDecitionNum = 0; //when is zero,send default;
  25. const int gnDecitionNumMax = 100;
  26. int gnIndex = 0;
  27. boost::shared_ptr<iv::control::Controller> gcontroller; //实际车辆控制器
  28. void executeDecition(const iv::brain::decition decition)
  29. {
  30. // std::cout<<"acc is "<<decition.accelerator()<<" ang is "<<decition.wheelangle()<<std::endl;
  31. // std::cout<<"brake is "<<decition.brake()<<std::endl;
  32. gcontroller->inialize();
  33. gcontroller->control_wheel(decition.wheelangle());
  34. gcontroller->control_accelerate(decition.accelerator());
  35. // gcontroller->control_braking(decition.brake()); //vv7不用brake
  36. gcontroller->control_accmode(true);
  37. gcontroller->control_turnmode(true);
  38. gcontroller->control_drivemode(1);
  39. #if 0
  40. optional bool air_ac = 37;
  41. optional bool air_circle = 38;
  42. optional bool air_auto = 39;
  43. optional bool air_off = 40;
  44. optional uint32 window_fl = 41;
  45. optional uint32 window_fr = 42;
  46. optional uint32 window_rl = 43;
  47. optional uint32 window_rr = 44;
  48. #endif
  49. if(decition.has_air_ac())
  50. gcontroller->control_air_AC_ctrl(decition.air_ac());
  51. if(decition.has_air_auto())
  52. gcontroller->control_air_auto_ctrl(decition.air_auto());
  53. if(decition.has_air_circle())
  54. gcontroller->control_air_inloop_ctrl(decition.air_circle());
  55. if(decition.has_air_off())
  56. gcontroller->control_air_close(decition.air_off());
  57. if(decition.has_window_fl())
  58. gcontroller->control_air_lf_win(decition.window_fl());
  59. if(decition.has_window_fr())
  60. gcontroller->control_air_rf_win(decition.window_fr());
  61. if(decition.has_window_rl())
  62. gcontroller->control_air_lr_win(decition.window_rl());
  63. if(decition.has_window_rr())
  64. gcontroller->control_air_rr_win(decition.window_rr());
  65. bool leftlampSig = false;
  66. bool rightlampSig = false;
  67. if(decition.has_leftlamp())
  68. leftlampSig = decition.leftlamp();
  69. if(decition.has_rightlamp())
  70. rightlampSig = decition.rightlamp();
  71. gcontroller->control_turnsignals(leftlampSig,rightlampSig);
  72. // if(decition.has_engine())
  73. // gcontroller->control_engine(decition.engine());
  74. if(decition.has_speak())
  75. gcontroller->control_speaker(decition.speak());
  76. }
  77. void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  78. {
  79. iv::brain::decition xdecition;
  80. if(!xdecition.ParseFromArray(strdata,nSize))
  81. {
  82. std::cout<<"ListenDecition parse error."<<std::endl;
  83. return;
  84. }
  85. gMutexDecision.lock();
  86. gdecition.CopyFrom(xdecition);
  87. gMutexDecision.unlock();
  88. gnDecitionNum = gnDecitionNumMax;
  89. }
  90. void ExecSend()
  91. {
  92. iv::can::canmsg xmsg;
  93. iv::can::canraw xraw;
  94. xraw.set_id(ServiceControlStatus.command_ID);
  95. xraw.set_data(ServiceControlStatus.command.byte,8);
  96. // qDebug("%02x %02x",ServiceControlStatus.command.byte[2],ServiceControlStatus.command.byte[3]);
  97. xraw.set_bext(false);
  98. xraw.set_bremote(false);
  99. xraw.set_len(8);
  100. iv::can::canraw * pxraw = xmsg.add_rawmsg();
  101. pxraw->CopyFrom(xraw);
  102. xmsg.set_channel(0);
  103. xmsg.set_index(gnIndex);
  104. gnIndex++;
  105. xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
  106. int ndatasize = xmsg.ByteSize();
  107. char * strser = new char[ndatasize];
  108. std::shared_ptr<char> pstrser;
  109. pstrser.reset(strser);
  110. if(xmsg.SerializePartialToArray(strser,ndatasize))
  111. {
  112. iv::modulecomm::ModuleSendMsg(gpacansend,strser,ndatasize);
  113. }
  114. else
  115. {
  116. std::cout<<"MainWindow::onTimer serialize error."<<std::endl;
  117. }
  118. }
  119. void sendthread()
  120. {
  121. iv::brain::decition xdecition;
  122. while(gbSendRun)
  123. {
  124. if(gnDecitionNum <= 0)
  125. {
  126. xdecition.CopyFrom(gdecition_def);
  127. }
  128. else
  129. {
  130. gMutexDecision.lock();
  131. xdecition.CopyFrom(gdecition);
  132. gMutexDecision.unlock();
  133. gnDecitionNum--;
  134. }
  135. executeDecition(xdecition);
  136. ExecSend();
  137. std::this_thread::sleep_for(std::chrono::milliseconds(10));
  138. }
  139. }
  140. int main(int argc, char *argv[])
  141. {
  142. RegisterIVBackTrace();
  143. showversion("controller_tju_vv7");
  144. QCoreApplication a(argc, argv);
  145. QString strpath = QCoreApplication::applicationDirPath();
  146. if(argc < 2)
  147. strpath = strpath + "/controller_vv7.xml";
  148. else
  149. strpath = argv[1];
  150. std::cout<<strpath.toStdString()<<std::endl;
  151. gdecition_def.set_accelerator(-0.5);
  152. gdecition_def.set_brake(1.0);
  153. gdecition_def.set_torque(0);
  154. gdecition_def.set_rightlamp(true);
  155. // gdecition_def.set_doublespark(true);
  156. gTime.start();
  157. gcontroller = boost::shared_ptr<iv::control::Controller>(new iv::control::Controller());
  158. iv::xmlparam::Xmlparam xp(strpath.toStdString());
  159. gstrmemcansend = xp.GetParam("cansend","cansend0");
  160. gstrmemdecition = xp.GetParam("dection","deciton");
  161. const char * strx = gstrmemcansend.data();
  162. gpacansend = iv::modulecomm::RegisterSend(gstrmemcansend.data(),10000,3);
  163. gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
  164. std::thread xthread(sendthread);
  165. return a.exec();
  166. }