ivpark_simple.cpp 17 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626
  1. #include "ivpark_simple.h"
  2. #include "adc_tools/compute_00.h"
  3. #include "adc_tools/transfer.h"
  4. #include <chrono>
  5. using namespace iv;
  6. ivpark_simple::ivpark_simple()
  7. {
  8. }
  9. bool ivpark_simple::IsBocheEnable(double fLon, double fLat, double fHeading)
  10. {
  11. GPS_INS nowgps,aimgps;
  12. std::vector<std::vector<GPS_INS>> xvectordTPoint;
  13. std::vector<double> xvectorRearDis;
  14. std::vector<double> xvectorAngle;
  15. std::vector<int> xvectortype;
  16. std::vector<GPS_INS> xvectoraimgps;
  17. std::vector<iv::simpleparkspace> xvectorsimpleparkspace = GetParkSpace();
  18. xvectordTPoint.clear();
  19. xvectorRearDis.clear();
  20. xvectorAngle.clear();
  21. unsigned int i;
  22. unsigned int nsize = static_cast<unsigned int >(xvectorsimpleparkspace.size());
  23. nowgps.gps_lat = fLat;
  24. nowgps.gps_lng = fLon;
  25. nowgps.ins_heading_angle = fHeading;
  26. for(i=0;i<nsize;i++)
  27. {
  28. iv::simpleparkspace xpark = xvectorsimpleparkspace[i];
  29. iv::GPS_INS aimgps;
  30. aimgps.gps_lat = xpark.mfLat;
  31. aimgps.gps_lng = xpark.mfLon;
  32. aimgps.ins_heading_angle = xpark.mfHeading;
  33. std::vector<GPS_INS> TPoints;
  34. double fRearDis,fAngle;
  35. if(xpark.mnParkType == 1) //side park
  36. {
  37. if(iv::decition::Compute00().bocheDirectCompute(nowgps,aimgps,TPoints,fAngle,fRearDis) == 1)
  38. {
  39. xvectordTPoint.push_back(TPoints);
  40. xvectorAngle.push_back(fAngle);
  41. xvectorRearDis.push_back(fRearDis);
  42. xvectoraimgps.push_back(aimgps);
  43. xvectortype.push_back(1);
  44. }
  45. }
  46. if(xpark.mnParkType == 0)
  47. {
  48. if(iv::decition::Compute00().bocheCompute(nowgps,aimgps,TPoints,fAngle,fRearDis) == 1)
  49. {
  50. xvectordTPoint.push_back(TPoints);
  51. xvectorAngle.push_back(fAngle);
  52. xvectorRearDis.push_back(fRearDis);
  53. xvectoraimgps.push_back(aimgps);
  54. xvectortype.push_back(0);
  55. }
  56. }
  57. }
  58. nsize = static_cast<unsigned int >(xvectordTPoint.size());
  59. if(nsize<1)
  60. {
  61. return false;
  62. }
  63. unsigned int nsel = 0;
  64. //Select rear dis >1.0 and small
  65. for(i =1;i<nsize;i++)
  66. {
  67. if((xvectorRearDis[i] < xvectorRearDis[nsel])&&(xvectorRearDis[i]>1.0))
  68. {
  69. nsel = i;
  70. }
  71. else
  72. {
  73. if(xvectorRearDis[i]<1.0)
  74. {
  75. if(xvectorRearDis[nsel]>5.0)
  76. {
  77. nsel = i;
  78. }
  79. }
  80. else
  81. {
  82. if((xvectorRearDis[i]<3.0)&&(xvectorRearDis[nsel]<0.5))
  83. {
  84. nsel = i;
  85. }
  86. }
  87. }
  88. }
  89. if(xvectortype[nsel] == 0)
  90. {
  91. iv::decition::Compute00().nearTpoint = xvectordTPoint[nsel].at(0);
  92. iv::decition::Compute00().farTpoint = xvectordTPoint[nsel].at(1);
  93. iv::decition::Compute00().bocheAngle = xvectorAngle[nsel];
  94. iv::decition::Compute00().nParkType = 0;
  95. maimgps = xvectoraimgps[nsel];
  96. }
  97. if(xvectortype[nsel] == 1)
  98. {
  99. iv::decition::Compute00().dTpoint0 = xvectordTPoint[nsel].at(0);
  100. iv::decition::Compute00().dTpoint1 = xvectordTPoint[nsel].at(0);
  101. iv::decition::Compute00().dTpoint2 = xvectordTPoint[nsel].at(0);
  102. iv::decition::Compute00().dTpoint3 = xvectordTPoint[nsel].at(0);
  103. iv::decition::Compute00().bocheAngle = xvectorAngle[nsel];
  104. iv::decition::Compute00().nParkType = 1;
  105. maimgps = xvectoraimgps[nsel];
  106. }
  107. GaussProjCal(maimgps.gps_lng,maimgps.gps_lat,&maimgps.gps_x,&maimgps.gps_y);
  108. return true;
  109. }
  110. double ivpark_simple::GetDistance(iv::GPS_INS p1,iv::GPS_INS p2)
  111. {
  112. return sqrt(pow(p1.gps_x - p2.gps_x,2)+pow(p1.gps_y-p2.gps_y,2));
  113. }
  114. int ivpark_simple::GetBocheDecision(double fLon,double fLat,double fHeading,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate,bool bbocheMode)
  115. {
  116. (void)fHeading;
  117. if(bbocheMode == false)
  118. {
  119. return 0; //Not in boche mode
  120. }
  121. static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  122. int64_t nstoptime_ms = 1000; //every stop time is 3
  123. if(xvehstate!=dRever && xvehstate!=dRever0 && xvehstate!=dRever1 && xvehstate!=dRever2
  124. && xvehstate!=dRever3 && xvehstate!=dRever4 && xvehstate!=reverseArr
  125. && xvehstate!=reverseCar && xvehstate!=reversing && xvehstate!=reverseCircle && xvehstate!=reverseDirect)
  126. {
  127. if(fSpeed>0.3)
  128. {
  129. fdSpeed = 0;fdSecSpeed = fdSpeed/3.6;
  130. fAcc = -0.5;
  131. fWheel = 0.0;
  132. nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  133. }
  134. else
  135. {
  136. fdSpeed = 0;fdSecSpeed = fdSpeed/3.6;
  137. fAcc = -0.5;
  138. fWheel = 0.0;
  139. nshift = 2; //rear
  140. int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  141. if(abs(nnow - nstoptiming_ms) >= nstoptime_ms)
  142. {
  143. if(iv::decition::Compute00().nParkType == 0)
  144. {
  145. xvehstate = reversing;
  146. mlastvehstate = xvehstate;
  147. }
  148. else
  149. {
  150. xvehstate = dRever;
  151. mlastvehstate = xvehstate;
  152. }
  153. }
  154. }
  155. return 1;
  156. }
  157. iv::GPS_INS nowgps;
  158. nowgps.gps_lat = fLat;
  159. nowgps.gps_lng = fLon;
  160. GaussProjCal(nowgps.gps_lng,nowgps.gps_lat,&nowgps.gps_x,&nowgps.gps_y);
  161. switch (xvehstate) {
  162. case reversing:
  163. reversingcarFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
  164. break;
  165. case reverseCircle:
  166. reverseCircleFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
  167. break;
  168. case reverseDirect:
  169. reverseDirectFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
  170. break;
  171. case reverseArr:
  172. reverseArrFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
  173. break;
  174. case dRever0:
  175. dRever0Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
  176. break;
  177. case dRever1:
  178. dRever1Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
  179. break;
  180. case dRever2:
  181. dRever2Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
  182. break;
  183. case dRever3:
  184. dRever3Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
  185. break;
  186. default:
  187. return 0;
  188. }
  189. return 0;
  190. }
  191. void ivpark_simple::reversingcarFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
  192. {
  193. mlastvehstate = xvehstate;
  194. double fdistonear = sqrt(pow(nowgps.gps_x - iv::decition::Compute00().nearTpoint.gps_x,2)+pow(nowgps.gps_y - iv::decition::Compute00().nearTpoint.gps_y,2));
  195. Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
  196. Point2D ptnear = iv::decition::Coordinate_Transfer(iv::decition::Compute00().nearTpoint.gps_x,iv::decition::Compute00().nearTpoint.gps_y, maimgps);
  197. fdistonear = fabs(pt.x - ptnear.x);
  198. nshift = 2;
  199. if(fdistonear>1.0)
  200. {
  201. fAcc = 0.0; //acc calcutale by pid
  202. fWheel = 0.0;
  203. fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
  204. }
  205. else
  206. {
  207. if((fSpeed>0.3)&&(fdistonear>0.3))
  208. {
  209. fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fdistonear);
  210. }
  211. else
  212. {
  213. fAcc = -0.5;
  214. xvehstate = reverseCircle;
  215. }
  216. }
  217. return;
  218. }
  219. void ivpark_simple::reverseCircleFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
  220. {
  221. (void)fSpeed;
  222. static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  223. int64_t nstoptime_ms = 3000; //every stop time is 3
  224. if(mlastvehstate != xvehstate)
  225. {
  226. nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  227. }
  228. mlastvehstate = xvehstate;
  229. int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  230. nshift = 2;
  231. if(abs(nnow - nstoptiming_ms) < nstoptime_ms) //stop 3s, change wheel when stop
  232. {
  233. fdSpeed = 0;
  234. fdSecSpeed = 0;
  235. fAcc = -0.5;
  236. fWheel = iv::decition::Compute00().bocheAngle*16.5 *(-1.05);
  237. mCircleWheel = fWheel;
  238. return;
  239. }
  240. fWheel = mCircleWheel;
  241. Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
  242. double angdis =fabs(nowgps.ins_heading_angle - maimgps.ins_heading_angle);
  243. if((fabs(pt.x)<2.0)&&(((angdis<5)||(angdis>355))))
  244. {
  245. xvehstate = reverseDirect;
  246. fAcc = -0.5;
  247. fdSpeed = 0.0;
  248. fWheel = 0.0;
  249. fdSecSpeed = 0.0;
  250. }
  251. else
  252. {
  253. fAcc = 0.0;
  254. fdSpeed = 2;
  255. fdSecSpeed = fdSecSpeed / 3.6;
  256. }
  257. return;
  258. }
  259. void ivpark_simple::reverseDirectFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
  260. {
  261. (void)fSpeed;
  262. static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  263. int64_t nstoptime_ms = 3000; //every stop time is 3
  264. if(mlastvehstate != xvehstate)
  265. {
  266. nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  267. }
  268. mlastvehstate = xvehstate;
  269. int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  270. nshift = 2;
  271. if(abs(nnow - nstoptiming_ms) < nstoptime_ms) //stop 3s, change wheel when stop
  272. {
  273. fdSpeed = 0;
  274. fdSecSpeed = 0;
  275. fAcc = -0.5;
  276. fWheel = 0.0;
  277. return;
  278. }
  279. Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
  280. if(pt.y<0.5)
  281. {
  282. xvehstate = reverseArr;
  283. fAcc = -0.5;
  284. fdSpeed = 0.0;
  285. fWheel = 0.0;
  286. fdSecSpeed = 0.0;
  287. }
  288. else
  289. {
  290. fAcc = 0.0;
  291. fdSpeed = 2;
  292. fdSecSpeed = fdSecSpeed / 3.6;
  293. fWheel = 0.0;
  294. }
  295. return;
  296. }
  297. void ivpark_simple::reverseArrFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
  298. {
  299. (void)nowgps;
  300. (void)fSpeed;
  301. static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  302. int64_t nstoptime_ms = 3000; //every stop time is 3
  303. if(mlastvehstate != xvehstate)
  304. {
  305. nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  306. }
  307. mlastvehstate = xvehstate;
  308. int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  309. nshift = 2;
  310. if(abs(nnow - nstoptiming_ms) < nstoptime_ms) //stop 3s, change wheel when stop
  311. {
  312. fdSpeed = 0;
  313. fdSecSpeed = 0;
  314. fAcc = -0.5;
  315. fWheel = 0.0;
  316. return;
  317. }
  318. nshift = 1; //P shift
  319. fdSpeed = 0;
  320. fdSecSpeed = 0;
  321. fAcc = -0.5;
  322. fWheel = 0.0;
  323. xvehstate = normalRun;
  324. }
  325. void ivpark_simple::dRever0Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
  326. {
  327. mlastvehstate = xvehstate;
  328. Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
  329. Point2D ptnear = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint1.gps_x,iv::decition::Compute00().dTpoint1.gps_y, maimgps);
  330. double fdistonear = fabs(pt.y - ptnear.y);
  331. nshift = 2;
  332. if(fdistonear>1.0)
  333. {
  334. fAcc = 0.0; //acc calcutale by pid
  335. fWheel = 0.0;
  336. fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
  337. }
  338. else
  339. {
  340. if((fSpeed>0.3)&&(fdistonear>0.3))
  341. {
  342. fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fdistonear);
  343. fdSpeed = 0.0;
  344. fdSecSpeed = fdSpeed/3.6;
  345. }
  346. else
  347. {
  348. fAcc = -0.5;
  349. xvehstate = dRever2;
  350. fdSpeed = 0.0;
  351. fdSecSpeed = fdSpeed/3.6;
  352. }
  353. }
  354. return;
  355. }
  356. void ivpark_simple::dRever1Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
  357. {
  358. (void)fSpeed;
  359. static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  360. int64_t nstoptime_ms = 3000; //every stop time is 3
  361. if(mlastvehstate != xvehstate)
  362. {
  363. nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  364. }
  365. mlastvehstate = xvehstate;
  366. int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  367. nshift = 2;
  368. if(abs(nnow - nstoptiming_ms) < nstoptime_ms) //stop 3s, change wheel when stop
  369. {
  370. fdSpeed = 0;
  371. fdSecSpeed = 0;
  372. fAcc = -0.5;
  373. fWheel = iv::decition::Compute00().bocheAngle*16.5 *(-1.0);
  374. mCircleWheel = fWheel;
  375. return;
  376. }
  377. Point2D pt1 = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
  378. Point2D pt2 = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint1.gps_x,iv::decition::Compute00().dTpoint1.gps_y, maimgps);
  379. double fdis = fabs(pt1.x - pt2.x);
  380. if(fdis>1.0)
  381. {
  382. fAcc = 0.0; //acc calcutale by pid
  383. fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
  384. }
  385. else
  386. {
  387. if((fSpeed>0.3)&&(fdis>0.3))
  388. {
  389. fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fdis);
  390. fdSpeed = 0.0;
  391. fdSecSpeed = fdSpeed/3.6;
  392. }
  393. else
  394. {
  395. fAcc = -0.5;
  396. fdSpeed = 0.0;
  397. fdSecSpeed = fdSpeed/3.6;
  398. xvehstate = dRever2;
  399. }
  400. }
  401. fWheel = mCircleWheel;
  402. }
  403. void ivpark_simple::dRever2Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
  404. {
  405. (void)fSpeed;
  406. static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  407. int64_t nstoptime_ms = 3000; //every stop time is 3
  408. if(mlastvehstate != xvehstate)
  409. {
  410. nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  411. }
  412. mlastvehstate = xvehstate;
  413. int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  414. nshift = 2;
  415. if(abs(nnow - nstoptiming_ms) < nstoptime_ms) //stop 3s, change wheel when stop
  416. {
  417. fdSpeed = 0;
  418. fdSecSpeed = 0;
  419. fAcc = -0.5;
  420. fWheel = 0;
  421. return;
  422. }
  423. Point2D pt1 = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
  424. Point2D pt2 = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint2.gps_x,iv::decition::Compute00().dTpoint2.gps_y, maimgps);
  425. double fdis = pt1.x - pt2.x;
  426. if(fdis>-1.0)
  427. {
  428. fAcc = 0.0; //acc calcutale by pid
  429. fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
  430. }
  431. else
  432. {
  433. if((fSpeed>0.3)&&(fabs(fdis)>0.3))
  434. {
  435. fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fabs(fdis));
  436. fdSpeed = 0.0;
  437. fdSecSpeed = fdSpeed/3.6;
  438. }
  439. else
  440. {
  441. fAcc = -0.5;
  442. fdSpeed = 0.0;
  443. fdSecSpeed = fdSpeed/3.6;
  444. xvehstate = dRever3;
  445. }
  446. }
  447. fWheel = 0;
  448. }
  449. void ivpark_simple::dRever3Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
  450. {
  451. (void)fSpeed;
  452. static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  453. int64_t nstoptime_ms = 3000; //every stop time is 3
  454. if(mlastvehstate != xvehstate)
  455. {
  456. nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  457. }
  458. mlastvehstate = xvehstate;
  459. int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  460. nshift = 2;
  461. if(abs(nnow - nstoptiming_ms) < nstoptime_ms) //stop 3s, change wheel when stop
  462. {
  463. fdSpeed = 0;
  464. fdSecSpeed = 0;
  465. fAcc = -0.5;
  466. fWheel = iv::decition::Compute00().bocheAngle*16.5 *1.0;
  467. mCircleWheel = fWheel;
  468. return;
  469. }
  470. Point2D pt1 = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
  471. Point2D pt2 = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint3.gps_x,iv::decition::Compute00().dTpoint3.gps_y, maimgps);
  472. double fdis = fabs(pt1.x - pt2.x);
  473. if(fdis>1.0)
  474. {
  475. fAcc = 0.0; //acc calcutale by pid
  476. fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
  477. }
  478. else
  479. {
  480. if((fSpeed>0.3)&&(fabs(fdis)>0.3))
  481. {
  482. fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fabs(fdis));
  483. fdSpeed = 0.0;
  484. fdSecSpeed = fdSpeed/3.6;
  485. }
  486. else
  487. {
  488. fAcc = -0.5;
  489. fdSpeed = 0.0;
  490. fdSecSpeed = fdSpeed/3.6;
  491. xvehstate = reverseDirect;
  492. }
  493. }
  494. fWheel = mCircleWheel;
  495. }