press_down_detect.c 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623
  1. #include "press_down_detect.h"
  2. #define down_thresh 2000.0f
  3. int back_jump_stage = 0;
  4. int cancel_down = 0;
  5. int virus_flag = 0;
  6. float real_front_min_left = 50000.f;
  7. float real_front_min_right = 50000.f;
  8. void dual_foot_detect_up_trend(float* left_mag_up_min, int *left_up_count, float* left_mag_window,
  9. float* right_mag_up_min, int *right_up_count ,float* right_mag_window, int window_size)
  10. {
  11. float left_max_val = left_mag_window[0];
  12. float left_min_val = left_mag_window[0];
  13. int left_max_index = 0;
  14. int left_min_index = 0;
  15. float right_max_val = right_mag_window[0];
  16. float right_min_val = right_mag_window[0];
  17. int right_max_index = 0;
  18. int right_min_index = 0;
  19. for (int i = 1; i < window_size; i++)
  20. {
  21. if (left_max_val < left_mag_window[i])
  22. {
  23. left_max_val = left_mag_window[i];
  24. left_max_index = i;
  25. }
  26. if (left_min_val > left_mag_window[i])
  27. {
  28. left_min_val = left_mag_window[i];
  29. left_min_index = i;
  30. }
  31. if (right_max_val < right_mag_window[i])
  32. {
  33. right_max_val = right_mag_window[i];
  34. right_max_index = i;
  35. }
  36. if (right_min_val > right_mag_window[i])
  37. {
  38. right_min_val = right_mag_window[i];
  39. right_min_index = i;
  40. }
  41. }
  42. if (((left_max_val - left_min_val > 200.0f && left_max_index > left_min_index) && left_max_val - left_mag_window[window_size - 1] < 1000.f) &&
  43. ((right_max_val - right_min_val > 200.0f && right_max_index > right_min_index) && right_max_val - right_mag_window[window_size - 1] < 1000.f))
  44. {
  45. if (left_min_val < *left_mag_up_min)
  46. {
  47. *left_mag_up_min = left_min_val;
  48. }
  49. if (right_min_val < *right_mag_up_min)
  50. {
  51. *right_mag_up_min = right_min_val;
  52. }
  53. if (*left_up_count > 0)
  54. {
  55. *left_up_count = *left_up_count + 1;
  56. }
  57. else
  58. {
  59. int index = left_min_index > right_min_index ? left_min_index : right_min_index;
  60. *left_mag_up_min = left_mag_window[index];
  61. for (int i = 0; i <= index; i++)
  62. {
  63. left_mag_window[i] = left_mag_window[index];
  64. }
  65. *left_up_count = window_size - index;
  66. }
  67. if (*right_up_count > 0)
  68. {
  69. *right_up_count = *right_up_count + 1;
  70. }
  71. else
  72. {
  73. int index = left_min_index > right_min_index ? left_min_index : right_min_index;
  74. *right_mag_up_min = right_mag_window[index];
  75. for (int i = 0; i <= index; i++)
  76. {
  77. right_mag_window[i] = right_mag_window[index];
  78. }
  79. *right_up_count = window_size - index;
  80. }
  81. }
  82. else
  83. {
  84. *left_mag_up_min = 40000.f;
  85. *right_mag_up_min = 40000.f;
  86. *left_up_count = 0;
  87. *right_up_count = 0;
  88. }
  89. }
  90. int avoid_down_during_change_road(float* left_mag_window, float* right_mag_window, int window_size)
  91. {
  92. float left_max_val = left_mag_window[0];
  93. float left_min_val = left_mag_window[0];
  94. int left_max_index = 0;
  95. int left_min_index = 0;
  96. float right_max_val = right_mag_window[0];
  97. float right_min_val = right_mag_window[0];
  98. int right_max_index = 0;
  99. int right_min_index = 0;
  100. for (int i = 1; i < window_size; i++)
  101. {
  102. if (left_max_val < left_mag_window[i])
  103. {
  104. left_max_val = left_mag_window[i];
  105. left_max_index = i;
  106. }
  107. if (left_min_val > left_mag_window[i])
  108. {
  109. left_min_val = left_mag_window[i];
  110. left_min_index = i;
  111. }
  112. if (right_max_val < right_mag_window[i])
  113. {
  114. right_max_val = right_mag_window[i];
  115. right_max_index = i;
  116. }
  117. if (right_min_val > right_mag_window[i])
  118. {
  119. right_min_val = right_mag_window[i];
  120. right_min_index = i;
  121. }
  122. }
  123. if (left_max_index > left_min_index && right_max_index < right_min_index
  124. && left_max_val - left_min_val > 2000.f && right_max_val - right_min_val > 2000.f)
  125. {
  126. return 1;
  127. }
  128. if (left_max_index < left_min_index && right_max_index > right_min_index
  129. && left_max_val - left_min_val > 2000.f && right_max_val - right_min_val > 2000.f)
  130. {
  131. return 1;
  132. }
  133. return 0;
  134. }
  135. int avoid_down_during_change_road_by_acc(float* left_acc_window, float* right_acc_window, int window_size)
  136. {
  137. float left_max_val = left_acc_window[0];
  138. float left_min_val = left_acc_window[0];
  139. float right_max_val = right_acc_window[0];
  140. float right_min_val = right_acc_window[0];
  141. for (int i = 1; i < window_size; i++)
  142. {
  143. if (left_max_val < left_acc_window[i])
  144. {
  145. left_max_val = left_acc_window[i];
  146. }
  147. if (left_min_val > left_acc_window[i])
  148. {
  149. left_min_val = left_acc_window[i];
  150. }
  151. if (right_max_val < right_acc_window[i])
  152. {
  153. right_max_val = right_acc_window[i];
  154. }
  155. if (right_min_val > right_acc_window[i])
  156. {
  157. right_min_val = right_acc_window[i];
  158. }
  159. }
  160. if (left_max_val > left_min_val + 0.3f || right_max_val > right_min_val + 0.3f)
  161. {
  162. return 1;
  163. }
  164. return 0;
  165. }
  166. #define BIG_WINDOW_SIZE 20
  167. int press_down_detect_new(int index, float front_mag_left, float back_mag_left,
  168. float front_mag_right, float back_mag_right, int left_zupt, int right_zupt,
  169. float left_acc_x, float left_acc_y, float left_acc_z, float right_acc_x, float right_acc_y, float right_acc_z,
  170. int* front_down, int* back_down)
  171. {
  172. static float front_mag_left_big_buff[BIG_WINDOW_SIZE];
  173. static float front_mag_right_big_buff[BIG_WINDOW_SIZE];
  174. static float back_mag_left_big_buff[BIG_WINDOW_SIZE];
  175. static float back_mag_right_big_buff[BIG_WINDOW_SIZE];
  176. static float left_acc_z_big_buff[BIG_WINDOW_SIZE];
  177. static float right_acc_z_big_buff[BIG_WINDOW_SIZE];
  178. static float left_front_up_min = 40000.0f;
  179. static float left_back_up_min = 40000.0f;
  180. static float right_front_up_min = 40000.0f;
  181. static float right_back_up_min = 40000.0f;
  182. static int left_front_up_count = 0;
  183. static int right_front_up_count = 0;
  184. static int left_back_up_count = 0;
  185. static int right_back_up_count = 0;
  186. memcpy(front_mag_left_big_buff, front_mag_left_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));
  187. memcpy(front_mag_right_big_buff, front_mag_right_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));
  188. memcpy(back_mag_left_big_buff, back_mag_left_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));
  189. memcpy(back_mag_right_big_buff, back_mag_right_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));
  190. memcpy(left_acc_z_big_buff, left_acc_z_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));
  191. memcpy(right_acc_z_big_buff, right_acc_z_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));
  192. front_mag_left_big_buff[(BIG_WINDOW_SIZE - 1)] = front_mag_left;
  193. front_mag_right_big_buff[(BIG_WINDOW_SIZE - 1)] = front_mag_right;
  194. back_mag_left_big_buff[(BIG_WINDOW_SIZE - 1)] = back_mag_left;
  195. back_mag_right_big_buff[(BIG_WINDOW_SIZE - 1)] = back_mag_right;
  196. left_acc_z_big_buff[(BIG_WINDOW_SIZE - 1)] = left_acc_z;
  197. right_acc_z_big_buff[(BIG_WINDOW_SIZE - 1)] = right_acc_z;
  198. dual_foot_detect_up_trend(&left_front_up_min, &left_front_up_count, front_mag_left_big_buff,
  199. &right_front_up_min, &right_front_up_count,front_mag_right_big_buff, BIG_WINDOW_SIZE);
  200. dual_foot_detect_up_trend(&left_back_up_min,&left_back_up_count, back_mag_left_big_buff,
  201. &right_back_up_min, &right_back_up_count, back_mag_right_big_buff, BIG_WINDOW_SIZE);
  202. if (front_mag_left - left_front_up_min > 2000.f && front_mag_right - right_front_up_min > 2000.f)
  203. {
  204. *front_down = 1;
  205. //printf( "left_front_up_count: %d , right_front_up_count : %d \n" , left_front_up_count , right_front_up_count);
  206. }
  207. if (back_mag_left - left_back_up_min > 2000.0f && back_mag_right - right_back_up_min > 2000.0f)
  208. {
  209. *back_down = 1;
  210. //printf("left_front_up_count: %d , right_front_up_count : %d \n", left_front_up_count , right_front_up_count);
  211. }
  212. if (avoid_down_during_change_road_by_acc(left_acc_z_big_buff, right_acc_z_big_buff, BIG_WINDOW_SIZE))
  213. {
  214. if (*front_down == 1)
  215. {
  216. //printf("delete front_down when change road \n");
  217. }
  218. if (*back_down == 1)
  219. {
  220. //printf("delete back_down when change road \n");
  221. }
  222. *front_down = 0;
  223. }
  224. return 0;
  225. }
  226. short pos_jump_detect(int* h_pos, int* s_pos, int left_zupt, int right_zupt)
  227. {
  228. static int last_h_z;
  229. static int last_s_z;
  230. static int left_up;
  231. static int right_up;
  232. static int left_up_count;
  233. static int right_up_count;
  234. static int left_zupt_count;
  235. static int right_zupt_count;
  236. if (left_zupt)
  237. {
  238. left_zupt_count = 10;
  239. }
  240. else
  241. {
  242. left_zupt_count--;
  243. }
  244. if (right_zupt)
  245. {
  246. right_zupt_count = 10;
  247. }
  248. else
  249. {
  250. right_zupt_count--;
  251. }
  252. if (h_pos[2] - last_h_z > 0 && h_pos[2] > 0)
  253. {
  254. left_up = 1;
  255. }
  256. else if ((h_pos[2] - last_h_z < 0) || h_pos[2] <= 0)
  257. {
  258. left_up = -1;
  259. }
  260. if (left_up == 1)
  261. {
  262. left_up_count++;
  263. }
  264. else
  265. {
  266. left_up_count = 0;
  267. }
  268. if (s_pos[2] - last_s_z > 0 && s_pos[2] > 0)
  269. {
  270. right_up = 1;
  271. }
  272. else if ((s_pos[2] - last_s_z < 0) || s_pos[2] <= 0)
  273. {
  274. right_up = -1;
  275. }
  276. if (right_up == 1)
  277. {
  278. right_up_count++;
  279. }
  280. else
  281. {
  282. right_up_count = 0;
  283. }
  284. last_h_z = h_pos[2];
  285. last_s_z = s_pos[2];
  286. if (left_up == 1 && right_up == 1 && right_up_count < 15 && left_up_count < 15
  287. && right_zupt_count > 0 && left_zupt_count > 0 &&
  288. ((right_up_count > 2 && left_up_count > 2) ||
  289. (right_up_count > 0 && left_up_count > 0 && h_pos[2] > 1 && s_pos[2] > 1)))
  290. {
  291. return 1;
  292. }
  293. return 0;
  294. }
  295. //由于仅靠IMU来探测触底了,高度估计得不太对,所以用加速度来算起跳动作
  296. void max_min_window(float* data, int length, float* max_val, int* max_index, float* min_val, int* min_index)
  297. {
  298. *max_val = data[0];
  299. *max_index = 0;
  300. *min_val = data[0];
  301. *min_index = 0;
  302. for (int i = 0; i < length; i++)
  303. {
  304. if (*max_val < data[i])
  305. {
  306. *max_val = data[i];
  307. *max_index = i;
  308. }
  309. if (*min_val > data[i])
  310. {
  311. *min_val = data[i];
  312. *min_index = i;
  313. }
  314. }
  315. }
  316. int avoid_error_jump_by_press(float* mag_window, int window_size)
  317. {
  318. float min_val = mag_window[0];
  319. float max_val = mag_window[0];
  320. int max_index = 0;
  321. for (int i = 0; i < window_size; i++)
  322. {
  323. if (min_val > mag_window[0])
  324. {
  325. min_val = mag_window[0];
  326. }
  327. if (max_val < mag_window[0])
  328. {
  329. max_val = mag_window[0];
  330. max_index = i;
  331. }
  332. }
  333. if (max_val - min_val > 2000.f && max_index == window_size - 1)
  334. {
  335. return 1;
  336. }
  337. return 0;
  338. }
  339. short press_jump_detect(float left_acc_z, float right_acc_z, int left_zupt, int right_zupt, int left_front_press, int right_front_press)
  340. {
  341. static float right_data_z_buff[10];
  342. static float left_data_z_buff[10];
  343. static float left_front_mag_buff[10];
  344. static float right_front_mag_buff[10];
  345. static int last_right_press;
  346. static int last_left_press;
  347. static int left_max_press;
  348. static int right_max_press;
  349. static int wait;
  350. /*
  351. * 存储数据
  352. */
  353. memcpy(right_data_z_buff, right_data_z_buff + 1, 9 * sizeof(float));
  354. memcpy(left_data_z_buff, left_data_z_buff + 1, 9 * sizeof(float));
  355. right_data_z_buff[9] = right_acc_z;
  356. left_data_z_buff[9] = left_acc_z;
  357. memcpy(left_front_mag_buff, left_front_mag_buff + 1, 9 * sizeof(float));
  358. memcpy(right_front_mag_buff, right_front_mag_buff + 1, 9 * sizeof(float));
  359. left_front_mag_buff[9] = left_front_press;
  360. right_front_mag_buff[9] = right_front_press;
  361. if (left_zupt && right_zupt)
  362. {
  363. wait = 25;
  364. }
  365. /*
  366. * 检测压力下降用的逻辑
  367. */
  368. if (last_left_press - left_front_press > 200)
  369. {
  370. if (last_left_press > left_max_press)
  371. {
  372. left_max_press = last_left_press;
  373. }
  374. }
  375. else if (last_left_press - left_front_press < 50)
  376. {
  377. left_max_press = 0;
  378. }
  379. last_left_press = left_front_press;
  380. if (last_right_press - right_front_press > 200)
  381. {
  382. if (last_right_press > right_max_press)
  383. {
  384. right_max_press = last_right_press;
  385. }
  386. }
  387. else if (last_right_press - right_front_press < 50)
  388. {
  389. right_max_press = 0;
  390. }
  391. last_right_press = right_front_press;
  392. /*
  393. * 加速变化估计
  394. */
  395. float max_val_right, min_val_right, max_val_left, min_val_left;
  396. int max_index_right, min_index_right, max_index_left, min_index_left;
  397. max_min_window(right_data_z_buff, 10, &max_val_right, &max_index_right, &min_val_right, &min_index_right);
  398. max_min_window(left_data_z_buff, 10, &max_val_left, &max_index_left, &min_val_left, &min_index_left);
  399. if (wait > 0)
  400. {
  401. wait--;
  402. }
  403. /*
  404. * 1.0f的阈值轻轻跳就可以达到了,现在改为1.3f的阈值
  405. */
  406. int jump = 0;
  407. if (max_index_right < min_index_right && max_index_left < min_index_left
  408. && max_val_right - min_val_right > 0.8f && right_acc_z < 0.2f && max_val_left-min_val_left > 0.8f && left_acc_z < 0.2f
  409. && (wait > 0 && left_zupt == 0 && right_zupt == 0))
  410. //if(left_max_press - left_front_press > 3000 && right_max_press - right_front_press > 3000)
  411. {
  412. jump = 1;
  413. }
  414. /*if (avoid_error_jump_by_press(left_front_mag_buff, 10))
  415. {
  416. jump = 0;
  417. }
  418. if (avoid_error_jump_by_press(right_front_mag_buff, 10))
  419. {
  420. jump = 0;
  421. }*/
  422. return jump;
  423. }
  424. int att_jump_detect(float left_pitch, float right_pitch, int left_zupt, int right_zupt)
  425. {
  426. static int num;
  427. static float left_pitch_window[8];
  428. static float right_pitch_window[8];
  429. //用俯仰角判断好了, 现在的IMU方向 下标1为俯仰角
  430. memcpy(left_pitch_window, left_pitch_window + 1, 7 * sizeof(float));
  431. memcpy(right_pitch_window, right_pitch_window + 1, 7 * sizeof(float));
  432. left_pitch_window[7] = left_pitch;
  433. right_pitch_window[7] = right_pitch;
  434. num++;
  435. if (num < 8)
  436. {
  437. return 0;
  438. }
  439. else
  440. {
  441. num = 8;
  442. }
  443. //要求起跳要同步
  444. static int zupt_count = 0;
  445. if (left_zupt && right_zupt)
  446. {
  447. zupt_count = 21;
  448. }
  449. if (zupt_count > 0)
  450. {
  451. zupt_count--;
  452. }
  453. for (int i = 7; i > 1; i--)
  454. {
  455. if ((left_pitch_window[i] > left_pitch_window[i - 1] || left_pitch_window[i] > left_pitch_window[i - 2])
  456. && (right_pitch_window[i] > right_pitch_window[i - 1] || right_pitch_window[i] > right_pitch_window[i - 2]))
  457. {
  458. if ((left_pitch > left_pitch_window[i - 1] + 0.1f || left_pitch > left_pitch_window[i - 2] + 0.1f)
  459. && (right_pitch > right_pitch_window[i - 1] + 0.1f || right_pitch > right_pitch_window[i - 2] + 0.1f)
  460. && zupt_count > 0)
  461. {
  462. return 1;
  463. }
  464. }
  465. else
  466. {
  467. return 0;
  468. }
  469. }
  470. return 0;
  471. }