detect_zero_vel.c 21 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011
  1. //#include "detect_zero_vel.h"
  2. //#include "hal_mt.h"
  3. //#include "hal_flash.h"
  4. //#define MAG_THRESHHOLD 1000.f
  5. //int front_zero_tmp = 0;
  6. //int back_zero_tmp = 0;
  7. //void start_cal_step(int16_t front[3], int16_t back[3], int16_t acc[3])
  8. //{
  9. // int16_t front_zero = 0;
  10. // int16_t back_zero = 0;
  11. // int16_t acc_zero = 0;
  12. //
  13. // detect_zero_vel( front, back, acc, &front_zero, &back_zero, &acc_zero);
  14. //}
  15. //void cal_step(int16_t front_zero, int16_t back_zero)
  16. //{
  17. // static int step_calm_down;
  18. //
  19. // if(front_zero || back_zero)
  20. // {
  21. // if(step_calm_down == 0)
  22. // {
  23. // mFlash.mStep.stepCur[0] ++;
  24. // }
  25. // step_calm_down = 10;
  26. // }
  27. //
  28. // if(step_calm_down > 0)
  29. // {
  30. // step_calm_down --;
  31. // }
  32. //}
  33. // void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
  34. // int16_t *front_zero, int16_t *back_zero, int16_t *acc_zero)
  35. //{
  36. // static float front_mag_window[WINDOW_SIZE];
  37. //
  38. // static float back_mag_window[WINDOW_SIZE];
  39. //
  40. // static float acc_x_window[WINDOW_SIZE];
  41. // static float acc_y_window[WINDOW_SIZE];
  42. // static float acc_z_window[WINDOW_SIZE];
  43. //
  44. // static int last_front_zupt;
  45. //
  46. // static int last_back_zupt;
  47. //
  48. // static int front_zupt_wait;
  49. //
  50. // static int back_zupt_wait;
  51. //
  52. //
  53. // float front_val = sqrt((float) (front[0] * front[0] + front[1] * front[1] + front[2] * front[2]));
  54. //
  55. // float back_val = sqrt((float) (back[0] * back[0] + back[1] * back[1] + back[2] * back[2]));
  56. //
  57. //
  58. // //滑动窗口更新数据
  59. //
  60. // memcpy(front_mag_window, front_mag_window + 1, (WINDOW_SIZE - 1) * sizeof(float));
  61. //
  62. // memcpy(back_mag_window, back_mag_window + 1, (WINDOW_SIZE - 1) * sizeof(float));
  63. //
  64. // memcpy(acc_x_window, acc_x_window + 1, (WINDOW_SIZE - 1) * sizeof(float));
  65. //
  66. // memcpy(acc_y_window, acc_y_window + 1, (WINDOW_SIZE - 1) * sizeof(float));
  67. //
  68. // memcpy(acc_z_window, acc_z_window + 1, (WINDOW_SIZE - 1) * sizeof(float));
  69. //
  70. // front_mag_window[WINDOW_SIZE - 1] = front_val;
  71. //
  72. // back_mag_window[WINDOW_SIZE - 1] = back_val;
  73. //
  74. // acc_x_window[WINDOW_SIZE - 1] = acc[0] / 2048.f;
  75. // acc_y_window[WINDOW_SIZE - 1] = acc[1] / 2048.f;
  76. // acc_z_window[WINDOW_SIZE - 1] = acc[2] / 2048.f;
  77. // float front_max_val = front_val;
  78. // float front_min_val = front_val;
  79. //
  80. // uint16_t front_max_index = WINDOW_SIZE - 1;
  81. // uint16_t front_min_index = WINDOW_SIZE - 1;
  82. //
  83. // float back_max_val = back_val;
  84. // float back_min_val = back_val;
  85. //
  86. // uint16_t back_max_index = WINDOW_SIZE - 1;
  87. // uint16_t back_min_index = WINDOW_SIZE - 1;
  88. //
  89. // for(int i = 0 ; i < WINDOW_SIZE; i++ )
  90. // {
  91. // if(front_mag_window[i] > front_max_val)
  92. // {
  93. // front_max_val = front_mag_window[i];
  94. // front_max_index = i;
  95. // }
  96. //
  97. // if(front_mag_window[i] < front_min_val)
  98. // {
  99. // front_min_val = front_mag_window[i];
  100. // front_min_index = i;
  101. // }
  102. //
  103. // if(back_mag_window[i] > back_max_val)
  104. // {
  105. // back_max_val = back_mag_window[i];
  106. // back_max_index = i;
  107. // }
  108. //
  109. // if(back_mag_window[i] < back_min_val)
  110. // {
  111. // back_min_val = back_mag_window[i];
  112. // back_min_index = i;
  113. // }
  114. //
  115. // }
  116. //
  117. // /*
  118. // * 计算稳定的状态
  119. // */
  120. //
  121. // float acc_max_val_x = acc_x_window[WINDOW_SIZE - 1] ;
  122. // float acc_min_val_x = acc_x_window[WINDOW_SIZE - 1] ;
  123. //
  124. // float acc_max_val_y = acc_y_window[WINDOW_SIZE - 1] ;
  125. // float acc_min_val_y = acc_y_window[WINDOW_SIZE - 1] ;
  126. //
  127. // float acc_max_val_z = acc_z_window[WINDOW_SIZE - 1];
  128. // float acc_min_val_z = acc_z_window[WINDOW_SIZE - 1];
  129. //
  130. // for(int i = WINDOW_SIZE - 5 ; i < WINDOW_SIZE; i++ )
  131. // {
  132. // if(acc_x_window[i] < acc_min_val_x)
  133. // {
  134. // acc_min_val_x = acc_x_window[i];
  135. // }
  136. //
  137. // if(acc_x_window[i] > acc_max_val_x)
  138. // {
  139. // acc_max_val_x = acc_x_window[i];
  140. // }
  141. //
  142. // if(acc_y_window[i] < acc_min_val_y)
  143. // {
  144. // acc_min_val_y = acc_y_window[i];
  145. // }
  146. //
  147. // if(acc_y_window[i] > acc_max_val_y)
  148. // {
  149. // acc_max_val_y = acc_y_window[i];
  150. // }
  151. //
  152. // if(acc_z_window[i] < acc_min_val_z)
  153. // {
  154. // acc_min_val_z = acc_z_window[i];
  155. // }
  156. //
  157. // if(acc_z_window[i] > acc_max_val_z)
  158. // {
  159. // acc_max_val_z = acc_z_window[i];
  160. // }
  161. //
  162. // }
  163. //
  164. // if(front_max_index > front_min_index && front_max_val > front_min_val + MAG_THRESHHOLD)
  165. // {
  166. // front_zero_tmp = 1;
  167. //
  168. // front_zupt_wait = 10;
  169. // }
  170. // else if(front_max_index < front_min_index && front_max_val > front_min_val + MAG_THRESHHOLD)
  171. // {
  172. // front_zero_tmp = 0;
  173. // }
  174. // else if(front_zero_tmp == 1)
  175. // {
  176. // front_zero_tmp = 2;
  177. // }
  178. //
  179. //
  180. // /*
  181. // * 判断后脚往下压
  182. // */
  183. //
  184. // if(back_max_index > back_min_index && back_max_val > back_min_val + MAG_THRESHHOLD)
  185. // {
  186. // back_zero_tmp = 1;
  187. //
  188. // back_zupt_wait = 10;
  189. // }
  190. //
  191. // else if(back_max_index < back_min_index && back_max_val > back_min_val + MAG_THRESHHOLD)
  192. // {
  193. // back_zero_tmp = 0;
  194. // }
  195. // else if(back_zero_tmp == 1)
  196. // {
  197. // back_zero_tmp = 2;
  198. // }
  199. //
  200. // /*
  201. // * 判断仅前踮 或 后垫
  202. // */
  203. // if(back_zero_tmp == 2 && front_zero_tmp == 0)
  204. // {
  205. // back_zero_tmp = 0;
  206. // }
  207. //
  208. // if(front_zero_tmp == 2 && back_zero_tmp == 0)
  209. // {
  210. // front_zero_tmp = 0;
  211. // }
  212. //
  213. // //延迟等待稳定状态
  214. //// if(*front_zero == 0 && front_zupt_wait > 0 && acc_max_val - acc_min_val < 0.08f)
  215. //// {
  216. //// *front_zero = 1;
  217. //// }
  218. //
  219. // if(front_zero_tmp == 1)
  220. // {
  221. // *front_zero = 1;
  222. //
  223. // }
  224. // else if(front_zupt_wait > 0 && acc_max_val_x - acc_min_val_x < 0.07f && acc_max_val_y - acc_min_val_y < 0.07f && acc_max_val_z - acc_min_val_z < 0.07f)
  225. // {
  226. // *front_zero = 1;
  227. // front_zupt_wait = 20;
  228. // }
  229. // else
  230. // {
  231. // *front_zero = 0;
  232. // }
  233. //
  234. //
  235. //// if(*back_zero == 0 && back_zupt_wait > 0 && acc_max_val - acc_min_val < 0.08f)
  236. //// {
  237. //// *back_zero = 1;
  238. //// }
  239. //
  240. // if(back_zero_tmp == 1)
  241. // {
  242. // *back_zero = 1;
  243. // }
  244. // else if(back_zupt_wait > 0 && acc_max_val_x - acc_min_val_x < 0.07f && acc_max_val_y - acc_min_val_y < 0.07f && acc_max_val_z - acc_min_val_z < 0.07f )
  245. // {
  246. // *back_zero = 1;
  247. //
  248. // back_zupt_wait = 20;
  249. // }
  250. // else
  251. // {
  252. // *back_zero = 0;
  253. // }
  254. //
  255. //
  256. // for(int i = 0 ; i < WINDOW_SIZE - 5; i++ )
  257. // {
  258. // if(acc_x_window[i] < acc_min_val_x)
  259. // {
  260. // acc_min_val_x = acc_x_window[i];
  261. // }
  262. //
  263. // if(acc_x_window[i] > acc_max_val_x)
  264. // {
  265. // acc_max_val_x = acc_x_window[i];
  266. // }
  267. //
  268. // if(acc_y_window[i] < acc_min_val_y)
  269. // {
  270. // acc_min_val_y = acc_y_window[i];
  271. // }
  272. //
  273. // if(acc_y_window[i] > acc_max_val_y)
  274. // {
  275. // acc_max_val_y = acc_y_window[i];
  276. // }
  277. //
  278. // if(acc_z_window[i] < acc_min_val_z)
  279. // {
  280. // acc_min_val_z = acc_z_window[i];
  281. // }
  282. //
  283. // if(acc_z_window[i] > acc_max_val_z)
  284. // {
  285. // acc_max_val_z = acc_z_window[i];
  286. // }
  287. //
  288. // }
  289. //
  290. //
  291. // if(acc_max_val_x - acc_min_val_x < 0.05f && acc_max_val_y - acc_min_val_y < 0.05f && acc_max_val_z - acc_min_val_z < 0.05f)
  292. // {
  293. // *acc_zero = 1;
  294. // }
  295. // else
  296. // {
  297. // *acc_zero = 0;
  298. // }
  299. //
  300. //
  301. // if(*front_zero == 0)
  302. // {
  303. // *front_zero = *back_zero;
  304. // }
  305. //
  306. // if(front_zupt_wait > 0)
  307. // {
  308. // front_zupt_wait --;
  309. // }
  310. //
  311. // if(back_zupt_wait > 0)
  312. // {
  313. // back_zupt_wait --;
  314. // }
  315. //
  316. //
  317. // //利用加速度延续
  318. // if((last_front_zupt == 1|| last_back_zupt == 1)
  319. // && fabsf(acc_x_window[WINDOW_SIZE - 2] - acc_x_window[WINDOW_SIZE - 1]) < 0.05f
  320. // && fabsf(acc_y_window[WINDOW_SIZE - 2] - acc_y_window[WINDOW_SIZE - 1]) < 0.05f
  321. // && fabsf(acc_z_window[WINDOW_SIZE - 2] - acc_z_window[WINDOW_SIZE - 1]) < 0.05f)
  322. // {
  323. // *front_zero = 1;
  324. // *back_zero = 1;
  325. //
  326. // front_zero_tmp = 1;
  327. // back_zero_tmp = 1;
  328. //
  329. // front_zupt_wait = 15;
  330. // back_zupt_wait = 15;
  331. // }
  332. //
  333. //
  334. // last_front_zupt = front_zero_tmp;
  335. //
  336. // last_back_zupt = back_zero_tmp;
  337. //
  338. // /*
  339. // * 直接用这个来记步好了
  340. // */
  341. // cal_step(*front_zero, *back_zero );
  342. //}
  343. #include "detect_zero_vel.h"
  344. #include "hal_mt.h"
  345. #include "app_flash.h"
  346. //#include "hal_imu.h"
  347. #define MAG_THRESHHOLD 2000.f
  348. int front_zero_tmp = 0;
  349. int back_zero_tmp = 0;
  350. void start_cal_step(int16_t front[3], int16_t back[3], int16_t acc[3])
  351. {
  352. int16_t front_zero = 0;
  353. int16_t back_zero = 0;
  354. int16_t acc_zero = 0;
  355. SEGGER_RTT_printf(0,"detect_zero_vel( front, back, acc, &front_zero, &back_zero, &acc_zero); \n");
  356. detect_zero_vel( front, back, acc, &front_zero, &back_zero, &acc_zero);
  357. }
  358. void cal_step(int16_t front_zero, int16_t back_zero, float acc_x, float acc_y, float acc_z)
  359. {
  360. static int step_calm_down;
  361. static float acc_min[3];
  362. static float acc_max[3];
  363. if(!(front_zero || back_zero))
  364. {
  365. acc_max[2] = 1.0f;
  366. acc_min[2] = 1.0f;
  367. if(acc_x > acc_max[0])
  368. {
  369. acc_max[0] = acc_x;
  370. }
  371. if(acc_y > acc_max[1])
  372. {
  373. acc_max[1] = acc_y;
  374. }
  375. if(acc_z > acc_max[2])
  376. {
  377. acc_max[2] = acc_z;
  378. }
  379. if(acc_x < acc_min[0])
  380. {
  381. acc_min[0] = acc_x;
  382. }
  383. if(acc_y < acc_min[1])
  384. {
  385. acc_min[1] = acc_y;
  386. }
  387. if(acc_z < acc_min[2])
  388. {
  389. acc_min[2] = acc_z;
  390. }
  391. }
  392. if(front_zero || back_zero)
  393. {
  394. if(step_calm_down == 0 && (acc_max[0] - acc_min[0] > 0.8f || acc_max[1] - acc_min[1] > 0.8f || acc_max[2] - acc_min[2] > 0.8f))
  395. {
  396. mFlash.mStep.stepCur[0] ++;
  397. }
  398. //假设20ms相见为极限
  399. step_calm_down = 5;
  400. acc_max[0] = acc_x;
  401. acc_max[1] = acc_y;
  402. acc_max[2] = acc_z;
  403. acc_min[0] = acc_x;
  404. acc_min[1] = acc_y;
  405. acc_min[2] = acc_z;
  406. }
  407. if(step_calm_down > 0)
  408. {
  409. step_calm_down --;
  410. }
  411. }
  412. void setLongTimeUpTrend(int max_index, int min_index, float max_val, float min_val, float *longTime_maxVal, float *longTime_minVal)
  413. {
  414. if(max_index > min_index && max_val > min_val + 800.f)
  415. {
  416. *longTime_maxVal = *longTime_maxVal > max_val ? *longTime_maxVal : max_val;
  417. *longTime_minVal = *longTime_minVal < min_val ? *longTime_minVal : min_val;
  418. }
  419. else if(max_index < min_index && max_val > min_val + 800.f)
  420. {
  421. *longTime_maxVal = 0.0f;
  422. *longTime_minVal = 50000.0f;
  423. }
  424. }
  425. void setLongTimeDownTrend(int max_index, int min_index, float max_val, float min_val, float *longTime_maxVal, float *longTime_minVal)
  426. {
  427. if(max_index > min_index && max_val > min_val + 800.f)
  428. {
  429. *longTime_maxVal = 0.0f;
  430. *longTime_minVal = 50000.0f;
  431. }
  432. else if(max_index < min_index && max_val > min_val + 800.f)
  433. {
  434. *longTime_maxVal = *longTime_maxVal > max_val ? *longTime_maxVal : max_val;
  435. *longTime_minVal = *longTime_minVal < min_val ? *longTime_minVal : min_val;
  436. }
  437. }
  438. int press_down_front(float *press_buff, int length)
  439. {
  440. static float max_val = 0.0f;
  441. if(press_buff[0] - press_buff[length - 1] > 100.0f)
  442. {
  443. max_val = press_buff[0] > max_val ? press_buff[0] : max_val;
  444. }
  445. else
  446. {
  447. max_val = 0;
  448. }
  449. if(max_val > press_buff[length - 1] + 2000.0f)
  450. {
  451. return 1;
  452. }
  453. return 0;
  454. }
  455. int press_down_back(float *press_buff, int length)
  456. {
  457. static float max_val = 0.0f;
  458. if(press_buff[0] - press_buff[length - 1] > 100.0f)
  459. {
  460. max_val = press_buff[0] > max_val ? press_buff[0] : max_val;
  461. }
  462. else
  463. {
  464. max_val = 0;
  465. }
  466. if(max_val > press_buff[length - 1] + 2000.0f)
  467. {
  468. return 1;
  469. }
  470. return 0;
  471. }
  472. //2020/01/02
  473. //长趋势判断压力上升
  474. int isLongTimeUpTrend(float *mag_window, int length, float up_threshhold, float *min_val)
  475. {
  476. // float prev_window_sum = (mag_window[0] + mag_window[1] + mag_window[2]) * 0.333f;
  477. //
  478. // float back_window_sum = (mag_window[length - 1] + mag_window[length - 2] + mag_window[length - 3]) * 0.333f;
  479. //
  480. // float back_window_sum1 = (mag_window[length - 4] + mag_window[length - 5] + mag_window[length - 6]) * 0.333f;
  481. // float back_window_sum2 = (mag_window[length - 7] + mag_window[length - 8] + mag_window[length - 9]) * 0.333f;
  482. //
  483. // if(back_window_sum > prev_window_sum + 50.f || back_window_sum1 > prev_window_sum + 50.f || back_window_sum2 > prev_window_sum + 50.f )
  484. // {
  485. // //触发判断, 先替换掉最小值
  486. // float min_val_temp = mag_window[0];
  487. //
  488. // min_val_temp = min_val_temp < mag_window[1] ? min_val_temp : mag_window[1];
  489. //
  490. // min_val_temp = min_val_temp < mag_window[2] ? min_val_temp : mag_window[2];
  491. //
  492. // *min_val = min_val_temp < *min_val ? min_val_temp : *min_val;
  493. //
  494. // float max_val_temp = mag_window[length - 6];
  495. //
  496. // for(int i = length - 5; i < length; i++)
  497. // {
  498. // if(max_val_temp < mag_window[i])
  499. // {
  500. // max_val_temp = mag_window[i];
  501. // }
  502. // }
  503. //
  504. // if(max_val_temp > *min_val + 1500.0f)
  505. // {
  506. // return 1;
  507. // }
  508. // }
  509. // else
  510. // {
  511. // *min_val = mag_window[length - 1];
  512. // }
  513. //上下沿判断
  514. int max_index = 0; int min_index = 0;
  515. float window_max_val = mag_window[0]; float window_min_val = mag_window[0];
  516. for(int i = 1; i < length; i++)
  517. {
  518. if(window_max_val < mag_window[i])
  519. {
  520. window_max_val = mag_window[i];
  521. max_index = i;
  522. }
  523. if(window_min_val > mag_window[i])
  524. {
  525. window_min_val = mag_window[i];
  526. min_index = i;
  527. }
  528. }
  529. *min_val = window_min_val;
  530. if((max_index > min_index && window_max_val - window_min_val > 1500.0f && window_max_val - mag_window[length - 1] < 1000.f )
  531. || (mag_window[length - 1] - window_min_val > 2000.0f))
  532. {
  533. return 1;
  534. }
  535. return 0;
  536. }
  537. int isLongTimeDownTrend(float *mag_window, int length, float up_threshhold, float *max_val)
  538. {
  539. // float prev_window_sum = (mag_window[0] + mag_window[1] + mag_window[2]) * 0.333f;
  540. //
  541. // float back_window_sum = (mag_window[length - 1] + mag_window[length - 2] + mag_window[length - 3]) * 0.333f;
  542. //
  543. // float back_window_sum1 = (mag_window[length - 4] + mag_window[length - 5] + mag_window[length - 6]) * 0.333f;
  544. // float back_window_sum2 = (mag_window[length - 7] + mag_window[length - 8] + mag_window[length - 9]) * 0.333f;
  545. //
  546. // if(prev_window_sum > back_window_sum + 50.f || prev_window_sum > back_window_sum1 + 50.f || prev_window_sum > back_window_sum2 + 50.f )
  547. // {
  548. // float max_val_temp = mag_window[0];
  549. //
  550. // max_val_temp = max_val_temp > mag_window[1] ? max_val_temp : mag_window[1];
  551. //
  552. // max_val_temp = max_val_temp > mag_window[2] ? max_val_temp : mag_window[2];
  553. //
  554. // *max_val = *max_val > max_val_temp ? *max_val : max_val_temp;
  555. //
  556. //
  557. //
  558. // float min_val_temp = mag_window[length - 6];
  559. //
  560. // for(int i = length - 5; i < length; i++)
  561. // {
  562. // if(min_val_temp > mag_window[i])
  563. // {
  564. // min_val_temp = mag_window[i];
  565. // }
  566. // }
  567. // if(*max_val > min_val_temp + 1500.0f)
  568. // {
  569. // return 1;
  570. // }
  571. // }
  572. // else
  573. // {
  574. // *max_val = mag_window[length - 1];
  575. // }
  576. //上下沿判断
  577. int max_index = 0; int min_index = 0;
  578. float window_max_val = mag_window[0]; float window_min_val = mag_window[0];
  579. for(int i = 1; i < length; i++)
  580. {
  581. if(window_max_val < mag_window[i])
  582. {
  583. window_max_val = mag_window[i];
  584. max_index = i;
  585. }
  586. if(window_min_val > mag_window[i])
  587. {
  588. window_min_val = mag_window[i];
  589. min_index = i;
  590. }
  591. }
  592. *max_val = window_max_val;
  593. if((max_index < min_index && window_max_val - window_min_val > 2000.0f && mag_window[length - 1] - window_min_val < 1000.f)
  594. || window_max_val - mag_window[length - 1] > 1000.f)
  595. {
  596. return 1;
  597. }
  598. return 0;
  599. }
  600. void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
  601. int16_t *front_zero, int16_t *back_zero, int16_t *acc_zero)
  602. {
  603. static float front_mag_window[WINDOW_SIZE];
  604. static float back_mag_window[WINDOW_SIZE];
  605. static float acc_x_window[WINDOW_SIZE];
  606. static float acc_y_window[WINDOW_SIZE];
  607. static float acc_z_window[WINDOW_SIZE];
  608. static int last_front_zupt;
  609. static int last_back_zupt;
  610. static int front_zupt_wait;
  611. static int back_zupt_wait;
  612. static int acc_zero_count;
  613. static float front_min_val;
  614. static float back_min_val;
  615. static float front_max_val;
  616. static float back_max_val;
  617. float front_val = (float) (abs(front[2]));
  618. float back_val = (float) (abs(back[2]));
  619. //滑动窗口更新数据
  620. memcpy(front_mag_window, front_mag_window + 1, (WINDOW_SIZE - 1) * sizeof(float));
  621. memcpy(back_mag_window, back_mag_window + 1, (WINDOW_SIZE - 1) * sizeof(float));
  622. memcpy(acc_x_window, acc_x_window + 1, (WINDOW_SIZE - 1) * sizeof(float));
  623. memcpy(acc_y_window, acc_y_window + 1, (WINDOW_SIZE - 1) * sizeof(float));
  624. memcpy(acc_z_window, acc_z_window + 1, (WINDOW_SIZE - 1) * sizeof(float));
  625. front_mag_window[WINDOW_SIZE - 1] = front_val;
  626. back_mag_window[WINDOW_SIZE - 1] = back_val;
  627. acc_x_window[WINDOW_SIZE - 1] = acc[0] / 2048.f;
  628. acc_y_window[WINDOW_SIZE - 1] = acc[1] / 2048.f;
  629. acc_z_window[WINDOW_SIZE - 1] = acc[2] / 2048.f;
  630. /*
  631. * 计算稳定的状态
  632. */
  633. float acc_max_val_x = acc_x_window[WINDOW_SIZE - 1] ;
  634. float acc_min_val_x = acc_x_window[WINDOW_SIZE - 1] ;
  635. float acc_max_val_y = acc_y_window[WINDOW_SIZE - 1] ;
  636. float acc_min_val_y = acc_y_window[WINDOW_SIZE - 1] ;
  637. float acc_max_val_z = acc_z_window[WINDOW_SIZE - 1];
  638. float acc_min_val_z = acc_z_window[WINDOW_SIZE - 1];
  639. for(int i = WINDOW_SIZE - 10 ; i < WINDOW_SIZE; i++ )
  640. {
  641. if(acc_x_window[i] < acc_min_val_x)
  642. {
  643. acc_min_val_x = acc_x_window[i];
  644. }
  645. if(acc_x_window[i] > acc_max_val_x)
  646. {
  647. acc_max_val_x = acc_x_window[i];
  648. }
  649. if(acc_y_window[i] < acc_min_val_y)
  650. {
  651. acc_min_val_y = acc_y_window[i];
  652. }
  653. if(acc_y_window[i] > acc_max_val_y)
  654. {
  655. acc_max_val_y = acc_y_window[i];
  656. }
  657. if(acc_z_window[i] < acc_min_val_z)
  658. {
  659. acc_min_val_z = acc_z_window[i];
  660. }
  661. if(acc_z_window[i] > acc_max_val_z)
  662. {
  663. acc_max_val_z = acc_z_window[i];
  664. }
  665. }
  666. int front_up_trend = isLongTimeUpTrend(front_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &front_min_val);
  667. int front_down_trend = isLongTimeDownTrend(front_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &front_max_val);
  668. int back_up_trend = isLongTimeUpTrend(back_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &back_min_val);
  669. int back_down_trend = isLongTimeDownTrend(back_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &back_max_val);
  670. if (back_down_trend == 1 && front_down_trend == 1)
  671. {
  672. front_up_trend = 0;
  673. back_up_trend = 0;
  674. }
  675. if(back_mag_window[WINDOW_SIZE - 1] - back_min_val > 1000.f && front_mag_window[WINDOW_SIZE-1] - front_min_val > 500.f
  676. && fabsf(front_max_val - front_mag_window[WINDOW_SIZE-1]) < 0.001f)
  677. {
  678. front_up_trend = 1;
  679. }
  680. // if (back_up_trend == 1 && acc_z_window[WINDOW_SIZE - 1] > 0.7f)
  681. // {
  682. // front_up_trend = 1;
  683. // }
  684. //if(back_down_trend == 1 && front_up_trend == 1)
  685. //{
  686. // printf("back_down_trend == 1 && front_up_trend == 1 \n");
  687. // front_up_trend = 0;
  688. //}
  689. if(front_up_trend)
  690. {
  691. front_zero_tmp = 1;
  692. }
  693. else if(front_down_trend)
  694. {
  695. front_zero_tmp = 0;
  696. }
  697. else if(front_zero_tmp == 1)
  698. {
  699. front_zero_tmp = 2;
  700. }
  701. //当触发了压力上升及平稳的时候,需要用加速度倒计时延续状态
  702. if(front_zero_tmp > 0)
  703. {
  704. front_zupt_wait = 20;
  705. }
  706. /*
  707. * 判断后脚往下压
  708. */
  709. if(back_up_trend)
  710. {
  711. back_zero_tmp = 1;
  712. }
  713. else if(back_down_trend)
  714. {
  715. back_zero_tmp = 0;
  716. }
  717. else if(back_zero_tmp == 1)
  718. {
  719. back_zero_tmp = 2;
  720. }
  721. //当触发了压力上升及平稳的时候,需要用加速度倒计时延续状态
  722. if(back_zero_tmp > 0)
  723. {
  724. back_zupt_wait = 20;
  725. }
  726. /*过滤一下类似于穿拖鞋,后鞋垫与磁力计传感器之间的距离在振荡*/
  727. if( back_zero_tmp == 2 && front_down_trend)
  728. {
  729. back_zero_tmp = 0;
  730. }
  731. if(front_zero_tmp == 2 && back_down_trend)
  732. {
  733. front_zero_tmp = 0;
  734. }
  735. if((front_zupt_wait > 0 || back_zupt_wait > 0) && acc_max_val_x - acc_min_val_x < 0.2f && acc_max_val_y - acc_min_val_y < 0.2f && acc_max_val_z - acc_min_val_z < 0.2f)
  736. {
  737. *back_zero = 1;
  738. *front_zero = 1;
  739. }
  740. if(front_zero_tmp > 0)
  741. {
  742. /*
  743. * 如IMU放在前脚掌,就不用front_calm_down_wait == 0的判断了
  744. */
  745. *front_zero = 1;
  746. }
  747. else
  748. {
  749. *front_zero = 0;
  750. }
  751. if(back_zero_tmp > 0)
  752. {
  753. *back_zero = 1;
  754. }
  755. else
  756. {
  757. *back_zero = 0;
  758. }
  759. for(int i = 0 ; i < WINDOW_SIZE - 5; i++ )
  760. {
  761. if(acc_x_window[i] < acc_min_val_x)
  762. {
  763. acc_min_val_x = acc_x_window[i];
  764. }
  765. if(acc_x_window[i] > acc_max_val_x)
  766. {
  767. acc_max_val_x = acc_x_window[i];
  768. }
  769. if(acc_y_window[i] < acc_min_val_y)
  770. {
  771. acc_min_val_y = acc_y_window[i];
  772. }
  773. if(acc_y_window[i] > acc_max_val_y)
  774. {
  775. acc_max_val_y = acc_y_window[i];
  776. }
  777. if(acc_z_window[i] < acc_min_val_z)
  778. {
  779. acc_min_val_z = acc_z_window[i];
  780. }
  781. if(acc_z_window[i] > acc_max_val_z)
  782. {
  783. acc_max_val_z = acc_z_window[i];
  784. }
  785. }
  786. if(acc_max_val_x - acc_min_val_x < 0.03f && acc_max_val_y - acc_min_val_y < 0.03f && acc_max_val_z - acc_min_val_z < 0.03f )
  787. {
  788. *acc_zero = 1;
  789. acc_zero_count ++;
  790. }
  791. else
  792. {
  793. *acc_zero = 0;
  794. acc_zero_count = 0;
  795. }
  796. // /*
  797. // * 强制大于1秒的零速均视为触地
  798. // */
  799. if(acc_zero_count > 100)
  800. {
  801. *front_zero = 1;
  802. *back_zero = 1;
  803. front_zero_tmp = 1;
  804. back_zero_tmp = 1;
  805. }
  806. //利用加速度延续
  807. if((last_front_zupt == 1|| last_back_zupt == 1)
  808. && fabsf(acc_x_window[WINDOW_SIZE - 2] - acc_x_window[WINDOW_SIZE - 1]) < 0.1f
  809. && fabsf(acc_y_window[WINDOW_SIZE - 2] - acc_y_window[WINDOW_SIZE - 1]) < 0.1f
  810. && fabsf(acc_z_window[WINDOW_SIZE - 2] - acc_z_window[WINDOW_SIZE - 1]) < 0.1f)
  811. {
  812. *front_zero = 1;
  813. *back_zero = 1;
  814. }
  815. if(front_zupt_wait > 0)
  816. {
  817. front_zupt_wait --;
  818. }
  819. if(back_zupt_wait > 0)
  820. {
  821. back_zupt_wait --;
  822. }
  823. if(*front_zero || *back_zero)
  824. {
  825. front_zupt_wait = 20;
  826. back_zupt_wait = 20;
  827. }
  828. last_front_zupt = *front_zero;
  829. last_back_zupt = *back_zero;
  830. /*
  831. * 直接用这个来记步好了
  832. */
  833. cal_step(*front_zero, *back_zero, acc_x_window[WINDOW_SIZE - 1], acc_y_window[WINDOW_SIZE - 1], acc_z_window[WINDOW_SIZE - 1]);
  834. }