jumpH5.cpp 6.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297
  1. #include "jumpH5.h"
  2. int detect_sharp_trend(unsigned int *press_window, int length)
  3. {
  4. unsigned int max_val = press_window[0];
  5. unsigned int min_val = press_window[0];
  6. int max_index = 0;
  7. int min_index = 0;
  8. for(int i = 0; i < length; i++)
  9. {
  10. if (max_val < press_window[i])
  11. {
  12. max_val = press_window[i];
  13. max_index = i;
  14. }
  15. if (min_val > press_window[i])
  16. {
  17. min_val = press_window[i];
  18. min_index = i;
  19. }
  20. }
  21. if (max_index > min_index && max_val > min_val + 3000)
  22. {
  23. std::cout << "max_val - min_val : " << max_val - min_val << endl;
  24. return 1;
  25. }
  26. return 0;
  27. }
  28. void jumpH5::Process(int* right_pos, int* right_att, int* right_acc, int right_press,int right_zupt,
  29. int* left_pos, int* left_att, int* left_acc, int left_press, int left_zupt,
  30. int jump, int time_s, int rssi)
  31. {
  32. static int right_stand_count = 0;
  33. static int left_stand_count = 0;
  34. int right_pos_offset[3];
  35. int left_pos_offset[3];
  36. memcpy(left_press_window, left_press_window + 1, 9 * sizeof(unsigned int));
  37. left_press_window[9] = (unsigned int)(left_press);
  38. //std::cout << "left_acc[2] : " << left_acc[2] << std::endl;
  39. memcpy(right_press_window, right_press_window + 1, 9 * sizeof(unsigned int));
  40. right_press_window[9] = (unsigned int)(right_press);
  41. if (detect_sharp_trend(left_press_window, 10) == 1)
  42. {
  43. std::cout << "detect_sharp_trend == 1, left" << std::endl;
  44. left_press_zupt_wait = 20;
  45. }
  46. if (detect_sharp_trend(right_press_window, 10) == 1)
  47. {
  48. right_press_zupt_wait = 20;
  49. }
  50. if ((right_att[0] == 1 || right_att[1] == 1 || right_att[2] == 1)
  51. && (right_press_zupt_wait > 0 || last_right_zupt == 1))
  52. {
  53. right_zupt = 1;
  54. can_detect_acc_zero_right = 0;
  55. }
  56. else
  57. {
  58. right_zupt = 0;
  59. }
  60. if ((left_att[0] == 1 || left_att[1] == 1 || left_att[2] == 1)
  61. && (left_press_zupt_wait > 0 || last_left_zupt == 1))
  62. {
  63. left_zupt = 1;
  64. can_detect_acc_zero_left = 0;
  65. }
  66. else
  67. {
  68. left_zupt = 0;
  69. }
  70. //计算时间戳
  71. if (H5_receive_time == -256)
  72. {
  73. H5_receive_time = time_s;
  74. }
  75. H5_time_stamp += (time_s - H5_receive_time > 0 ? time_s - H5_receive_time : time_s - H5_receive_time + 256);
  76. //std::cout << "H5_time_stamp : " << H5_time_stamp << std::endl;
  77. //清空命令, 命令缓存160ms
  78. if(H5_time_stamp - last_result_time_stamp > 16)
  79. memset(H5_result, 0, 2 * sizeof(int));
  80. for (int i = 0; i < 3; i++)
  81. {
  82. right_pos_offset[i] = right_pos[i] - last_right_pos[i];
  83. left_pos_offset[i] = left_pos[i] - last_left_pos[i];
  84. }
  85. if ((right_zupt == 0 && left_zupt == 0))
  86. {
  87. report_tag = 1;
  88. }
  89. //当左右脚都不是贴地面,输出高度,同时统计时间
  90. if (report_tag == 1)
  91. {
  92. int jump_time = time_s - not_on_floor_time;
  93. if (time_s - not_on_floor_time < 0)
  94. {
  95. jump_time += 256;
  96. }
  97. int high = 0;
  98. if (abs(left_pos_offset[2]) < abs(right_pos_offset[2]))
  99. {
  100. high = left_pos_offset[2] > 0 ? left_pos_offset[2] : 0;
  101. }
  102. else
  103. {
  104. high = right_pos_offset[2] > 0 ? right_pos_offset[2] : 0;
  105. }
  106. //std::cout << "空中持续时间: " << jump_time << "/ms" << ",高度为: "<< high << endl;
  107. H5_result[0] = 3; H5_result[1] = jump_time;
  108. }
  109. else
  110. {
  111. not_on_floor_time = time_s;
  112. }
  113. //添加一个计时器,用以探测所有触发点,来判断是否触底
  114. if (left_zupt == 1 && last_left_zupt == 0 && left_wait_time == 0)
  115. {
  116. left_wait_time = 10;
  117. memcpy(left_pos_wait, left_pos_offset, 3 * sizeof(int));
  118. }
  119. if (right_zupt == 1 && last_right_zupt == 0 && right_wait_time == 0)
  120. {
  121. right_wait_time = 10;
  122. memcpy(right_pos_wait, right_pos_offset, 3 * sizeof(int));
  123. }
  124. if ((left_wait_time > 0 && right_wait_time > 0) && report_tag == 1)
  125. {
  126. //证明双脚踏地, 暂时双脚踏地了,输出双个格子
  127. if (next_result_wait == 0)
  128. {
  129. std::cout << "dual foot on floor" << std::endl;
  130. next_result_wait = 5;
  131. int distance_rssi = 0;
  132. //利用rssi来判断距离,太近,近, 适中, 远,很远 的等级
  133. if (rssi < 20)
  134. {
  135. std::cout << "双脚接地 距离过近" << endl;
  136. distance_rssi = 0;
  137. }
  138. else if (rssi < 25)
  139. {
  140. std::cout << "双脚接地,距离稍微近" << endl;
  141. distance_rssi = 1;
  142. }
  143. else if (rssi < 30)
  144. {
  145. std::cout << "双脚接地,距离合适" << endl;
  146. distance_rssi = 2;
  147. }
  148. else if (rssi < 35)
  149. {
  150. std::cout << "双脚接地,距离稍微远" << endl;
  151. distance_rssi = 3;
  152. }
  153. else
  154. {
  155. std::cout << "双脚接地,距离过远" << endl;
  156. distance_rssi = 4;
  157. }
  158. H5_result[0] = 1; H5_result[1] = distance_rssi; last_result_time_stamp = H5_time_stamp;
  159. }
  160. left_wait_time = 0;
  161. right_wait_time = 0;
  162. report_tag = 0;
  163. }
  164. else if (left_wait_time == 0 && left_zupt == 1 && right_zupt == 0 && report_tag == 1 )
  165. {
  166. if (next_result_wait == 0)
  167. {
  168. std::cout << "only left foot on floor" << std::endl;
  169. next_result_wait = 5;
  170. H5_result[0] = 2;
  171. H5_result[1] = 1;
  172. last_result_time_stamp = H5_time_stamp;
  173. }
  174. report_tag = 0;
  175. }
  176. else if (right_wait_time == 0 && right_zupt == 1 && left_zupt == 0 && report_tag == 1)
  177. {
  178. if (next_result_wait == 0)
  179. {
  180. std::cout << "only right foot on floor" << std::endl;
  181. next_result_wait = 5;
  182. H5_result[0] = 2;
  183. H5_result[1] = 2;
  184. last_result_time_stamp = H5_time_stamp;
  185. }
  186. report_tag = 0;
  187. }
  188. //直接存高度双脚高度
  189. H5_result[2] = left_pos_offset[2] > 0 ? left_pos_offset[2] : 0;
  190. H5_result[2] = H5_result[2] > 10 ? 10 : H5_result[2];
  191. H5_result[3] = right_pos_offset[2] > 0 ? right_pos_offset[2] : 0;
  192. H5_result[3] = H5_result[3] > 10 ? 10 : H5_result[3];
  193. //计算偏移量, 检测到left_zupt
  194. if(left_zupt == 1)
  195. memcpy(last_left_pos, left_pos, 3 * sizeof(int));
  196. if (right_zupt == 1)
  197. memcpy(last_right_pos, right_pos, 3 * sizeof(int));
  198. if (left_wait_time > 0)
  199. {
  200. left_wait_time--;
  201. }
  202. if (right_wait_time > 0)
  203. {
  204. right_wait_time--;
  205. }
  206. if (next_result_wait > 0)
  207. {
  208. next_result_wait--;
  209. }
  210. if (left_press_zupt_wait > 0)
  211. {
  212. left_press_zupt_wait--;
  213. }
  214. if (right_press_zupt_wait > 0)
  215. {
  216. right_press_zupt_wait--;
  217. }
  218. //std::cout << "result: " << H5_result[0] << " " << H5_result[1] << " " << H5_result[2] << " " << H5_result[3] << " " << std::endl;
  219. last_right_zupt = right_zupt;
  220. last_left_zupt = left_zupt;
  221. H5_receive_time = time_s;
  222. }
  223. void jumpH5::getResult(int* result)
  224. {
  225. memcpy(result, H5_result, 4 * sizeof(int));
  226. }