main.cpp 8.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267
  1. #include <QCoreApplication>
  2. #include <QTime>
  3. #include <QMutex>
  4. #include <QTimer>
  5. #include "xmlparam.h"
  6. #include "modulecomm.h"
  7. #include "ivbacktrace.h"
  8. #include "ivversion.h"
  9. #include "decition.pb.h"
  10. #include "chassis.pb.h"
  11. #include <thread>
  12. #include "include/car_control.h"
  13. CarControl car_control_module;
  14. void * gpadecition;
  15. iv::brain::decition gdecition_def;
  16. iv::brain::decition gdecition;
  17. int gnDecitionNum = 0; //when is zero,send default;
  18. const int gnDecitionNumMax = 100;
  19. static QMutex gMutex;
  20. bool gstatus;
  21. GearPrkgAssistReq ggearSetVal;
  22. GearLevelIndicate ggearRealVal;
  23. ChassisErrCode gchassErr;
  24. StsMach gstsMach;
  25. float gsteerDeg, gspeed;
  26. double lastspeedSetVal = 0;
  27. double lastEpsSetVal = 0;
  28. GearPrkgAssistReq lastgearSetVal= GearPrkgAssistReq::kNoRequest;
  29. static void ShareChassis(void * pa,iv::chassis * pchassis)
  30. {
  31. char * str;
  32. int ndatasize = pchassis->ByteSize();
  33. str = new char[ndatasize];
  34. std::shared_ptr<char> pstr;
  35. pstr.reset(str);
  36. if(!pchassis->SerializeToArray(str,ndatasize))
  37. {
  38. std::cout<<"ShareChassis Error."<<std::endl;
  39. return;
  40. }
  41. iv::modulecomm::ModuleSendMsg(pa,str,ndatasize);
  42. }
  43. void executeDecition(const iv::brain::decition decition)
  44. {
  45. std::cout<<"Command: Acc is "<<decition.accelerator()<<" Angle is "<<decition.wheelangle()<<" DangWei is "<<decition.gear()<<std::endl;
  46. GearPrkgAssistReq gearSetVal= GearPrkgAssistReq::kNoRequest;
  47. double speedSetVal = 0;
  48. double EpsSetVal = 0;
  49. if(decition.has_gear()){
  50. switch (decition.gear()) {
  51. case 1:
  52. //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
  53. gearSetVal=GearPrkgAssistReq::kTargetGearP;
  54. break;
  55. case 2:
  56. //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearR);
  57. gearSetVal=GearPrkgAssistReq::kTargetGearR;
  58. break;
  59. case 3:
  60. //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
  61. gearSetVal=GearPrkgAssistReq::kTargetGearD;
  62. break;
  63. default:
  64. //car_control_module.set_target_gear(GearPrkgAssistReq::kNoRequest);
  65. gearSetVal=GearPrkgAssistReq::kNoRequest;;
  66. break;
  67. }
  68. lastgearSetVal=gearSetVal;
  69. }
  70. else
  71. {
  72. gearSetVal=lastgearSetVal;
  73. }
  74. if(decition.has_accelerator()){
  75. speedSetVal=decition.accelerator();
  76. lastspeedSetVal=speedSetVal;
  77. //speedSetVal=0.1;
  78. // car_control_module.set_target_acc_mps2(decition.accelerator());
  79. // car_control_module.set_target_acc_mps2(0.1);
  80. }
  81. else
  82. {
  83. speedSetVal=lastspeedSetVal;
  84. }
  85. if(decition.has_wheelangle())
  86. {
  87. // car_control_module.set_target_pinion_ag_in_deg(0.0);
  88. EpsSetVal=decition.wheelangle();
  89. lastEpsSetVal=EpsSetVal;
  90. // EpsSetVal=0.0;//
  91. // car_control_module.set_target_pinion_ag_in_deg(decition.wheelangle());
  92. }
  93. else
  94. {
  95. EpsSetVal=lastEpsSetVal;
  96. }
  97. // gearSetVal=GearPrkgAssistReq::kTargetGearD;
  98. car_control_module.set_target_gear(gearSetVal);
  99. car_control_module.set_target_acc_mps2(speedSetVal);
  100. car_control_module.set_target_pinion_ag_in_deg(EpsSetVal);
  101. if(decition.has_leftlamp() && decition.leftlamp()==true)
  102. car_control_module.set_turn_light_status(TurnLightIndicReq::kLeft);
  103. else if(decition.has_rightlamp() && decition.rightlamp()==true)
  104. car_control_module.set_turn_light_status(TurnLightIndicReq::kRight);
  105. else
  106. car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);
  107. /*
  108. void set_lat_lgt_ctrl_req(bool req); // 握手请求, true:请求握手, false:退出握手
  109. void set_target_gear(GearPrkgAssistReq tar); // 目标档位请求
  110. bool is_lat_lgt_ctrl_active(); // 底盘控制状态反馈, true: 底盘可线控, false: 底盘不可控
  111. void set_target_pinion_ag_in_deg(float32_t ang_req);// 设置目标方向盘角度请求, -450°至+450°(车辆实测范围值),方向盘左正,右负。
  112. void set_target_acc_mps2(float32_t tar_acc);// 目标加减速度值。
  113. void set_turn_light_status(TurnLightIndicReq req);// 转向灯控制请求
  114. */
  115. }
  116. void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  117. {
  118. qint64 oldtime;
  119. iv::brain::decition xdecition;
  120. if(!xdecition.ParseFromArray(strdata,nSize))
  121. {
  122. std::cout<<"ListenDecition parse error."<<std::endl;
  123. return;
  124. }
  125. if(xdecition.gear() != 3)
  126. {
  127. qDebug("not D");
  128. }
  129. if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)
  130. // qDebug("dection time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
  131. oldtime = QDateTime::currentMSecsSinceEpoch();
  132. gMutex.lock();
  133. gdecition.CopyFrom(xdecition);
  134. gMutex.unlock();
  135. gnDecitionNum = gnDecitionNumMax;
  136. std::cout<<"update decision. "<<std::endl;
  137. }
  138. void sendthread()
  139. {
  140. iv::brain::decition xdecition;
  141. // car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
  142. // car_control_module.sm_task_20ms(); // 线控状态机函数
  143. while(1)
  144. {
  145. if(gnDecitionNum <= 0)
  146. {
  147. xdecition.CopyFrom(gdecition_def);
  148. }
  149. else
  150. {
  151. gMutex.lock();
  152. xdecition.CopyFrom(gdecition);
  153. gMutex.unlock();
  154. gnDecitionNum--;
  155. }
  156. bool bstatus = car_control_module.is_lat_lgt_ctrl_active();
  157. if(bstatus == true)
  158. {
  159. // std::cout<<"di pan ke kong "<<std::endl;
  160. executeDecition(xdecition);
  161. }
  162. else
  163. {
  164. std::cout<<" lat lgt req. status: "<<(int)car_control_module.get_chassis_err_state()
  165. <<" machine state: "<<(int)car_control_module.get_chassis_statemachine_sts()<< std::endl;
  166. car_control_module.set_lat_lgt_ctrl_req(true);
  167. }
  168. car_control_module.sm_task_20ms(); // 线控状态机函数
  169. std::this_thread::sleep_for(std::chrono::milliseconds(20));
  170. }
  171. }
  172. void * gpa;
  173. void recvthread()
  174. {
  175. iv::chassis xchassis;
  176. int tmp1[10] = {0,1,2,3,4,5,6,7,8,9};
  177. while(1)
  178. {
  179. gstatus = car_control_module.is_lat_lgt_ctrl_active();
  180. ggearSetVal = car_control_module.get_setted_tar_gear();
  181. ggearRealVal = car_control_module.get_cur_disp_gear();
  182. gchassErr = car_control_module.get_chassis_err_state();
  183. gstsMach = car_control_module.get_chassis_statemachine_sts();
  184. gspeed = car_control_module.get_current_vehicle_spd_in_ms();
  185. gsteerDeg = car_control_module.get_current_steer_ang_in_deg();
  186. std::cout<<"FeedBack: current_vehicle_spd_in_ms is "<<gspeed<<"err code :"<<(int)gchassErr<<std::endl;
  187. xchassis.set_angle_feedback(gsteerDeg);
  188. ShareChassis(gpa,&xchassis);
  189. std::this_thread::sleep_for(std::chrono::milliseconds(20));
  190. /*
  191. bool is_lat_lgt_ctrl_active(); // 底盘控制状态反馈, true: 底盘可线控, false: 底盘不可控
  192. ChassisErrCode get_chassis_err_state();// 底盘错误状态码
  193. StsMach get_chassis_statemachine_sts(); // 内部状态机运行状态
  194. float32_t get_current_steer_ang_in_deg();// 当前方向盘实际角度
  195. float32_t get_current_vehicle_spd_in_ms();// 当前车辆实际车速,单位m/s
  196. GearPrkgAssistReq get_setted_tar_gear(); // 获取当前设定目标档位值
  197. GearLevelIndicate get_cur_disp_gear(); // 当前实际显示档位状态
  198. */
  199. }
  200. }
  201. int main(int argc, char *argv[])
  202. {
  203. RegisterIVBackTrace();
  204. showversion("controller_Geely_xingyueL");
  205. QCoreApplication a(argc, argv);
  206. QString strpath = QCoreApplication::applicationDirPath();
  207. if(argc < 2)
  208. strpath = strpath + "/controller_Geely_xingyueL.xml";
  209. else
  210. strpath = argv[1];
  211. std::cout<<strpath.toStdString()<<std::endl;
  212. // car_control_module.set_lat_lgt_ctrl_req(true);
  213. // car_control_module.sm_task_20ms(); // 线控状态机函数
  214. //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
  215. iv::xmlparam::Xmlparam xp(strpath.toStdString());
  216. std::string gstrmemdecition = xp.GetParam("dection","deciton");
  217. std::string strchassismsgname = xp.GetParam("chassismsgname","chassis");
  218. gpa = iv::modulecomm::RegisterSend(strchassismsgname.data(),1000,1);
  219. gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
  220. std::thread xthread(sendthread);
  221. // std::thread myxthread(recvthread);
  222. return a.exec();
  223. }