detect_step_by_mag.c 7.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386
  1. #include "detect_step_by_mag.h"
  2. #include "math.h"
  3. #include "stdlib.h"
  4. //#include "hal_imu.h"
  5. #include "nrf_delay.h"
  6. uint16_t mag_sqrt =0;
  7. uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
  8. {
  9. static int16_t FKEY=0;
  10. static int16_t BKEY=0;
  11. static int FrontLiftPoint;
  12. static int FrontDropPoint;
  13. static int BackLiftPoint;
  14. static int BackDropPoint;
  15. static uint8_t mag_index_front = 0;
  16. static uint8_t mag_index_back = 0;
  17. static int16_t Rcounter=0;
  18. static int16_t Ucounter=0;//离地计时器
  19. static int16_t ShoeTouchFlag=1;
  20. static uint8_t acc_index = 0;
  21. static int calculate_flag = 0;
  22. int32_t mag_temp_front[3];
  23. int32_t mag_temp_back[3];
  24. static int16_t accZ_buf[4];
  25. static int mag_buf_front[3];
  26. static int mag_buf_back[3];
  27. int32_t mag_sqrt_front =0;
  28. int32_t mag_sqrt_back =0;
  29. int16_t step=0;
  30. static int FrontLiftDistance;
  31. static int BackLiftDistance;
  32. //SEGGER_RTT_printf(0,"Enter RealTimeStep!\n");
  33. //Tcounter++;
  34. for(int i = 0; i < 3; i ++)
  35. {
  36. mag_temp_front[i] = (int32_t) (front[i]);
  37. }
  38. mag_sqrt_front = abs(mag_temp_front[2]);
  39. for(int i = 0; i < 3; i ++)
  40. {
  41. mag_temp_back[i] = (int32_t) (back[i]);
  42. }
  43. mag_sqrt_back = abs(mag_temp_back[2]);
  44. /*读入磁力计数据*/
  45. if(mag_index_front >= 3)
  46. {
  47. mag_buf_front[0] = mag_buf_front[1];
  48. mag_buf_front[1] = mag_buf_front[2];
  49. mag_index_front = 2;
  50. calculate_flag = 1;
  51. }
  52. mag_buf_front[mag_index_front++] = mag_sqrt_front;//前脚掌数据
  53. if(mag_index_back >= 3)
  54. {
  55. mag_buf_back[0] = mag_buf_back[1];
  56. mag_buf_back[1] = mag_buf_back[2];
  57. mag_index_back = 2;
  58. calculate_flag = 1;
  59. }
  60. mag_buf_back[mag_index_back++] = mag_sqrt_back;//后脚掌数据
  61. /*读入xyZ轴加速度数据*/
  62. if(acc_index >= 3)
  63. {
  64. accZ_buf[0] = accZ_buf[1];
  65. accZ_buf[1] = accZ_buf[2];
  66. acc_index = 2;
  67. calculate_flag = 1;
  68. }
  69. accZ_buf[acc_index++] = -acc[2];
  70. if(calculate_flag==1)
  71. {
  72. SEGGER_RTT_printf(0,"mag_front3==:%d...\n",mag_buf_front[2]);
  73. SEGGER_RTT_printf(0,"mag_back3==:%d...\n",mag_buf_back[2]);
  74. SEGGER_RTT_printf(0,"FrontLiftPoint==:%d...\n",FrontLiftPoint);
  75. SEGGER_RTT_printf(0,"BackLiftPoint==:%d...\n",BackLiftPoint);
  76. SEGGER_RTT_printf(0,"FrontDropPoint==:%d...\n",FrontDropPoint);
  77. SEGGER_RTT_printf(0,"BackDropPoint==:%d...\n",BackDropPoint);
  78. SEGGER_RTT_printf(0,"FrontLiftDistance==:%d...\n",FrontLiftDistance);
  79. SEGGER_RTT_printf(0,"BackLiftDistance==:%d...\n",BackLiftDistance);
  80. /*不断更新前后脚掌磁力计的上升起始点或下降起始点*/
  81. if(mag_buf_front[2] - mag_buf_front[1]>=0)
  82. {
  83. if(FKEY==0)
  84. {
  85. FrontDropPoint = mag_buf_front[2];
  86. }
  87. else
  88. {
  89. if(mag_buf_front[2]>FrontDropPoint)
  90. {
  91. FrontDropPoint = mag_buf_front[2];
  92. }
  93. }
  94. }
  95. else
  96. {
  97. if(FKEY==0)//FKEY为前脚波形状态变量,1代表波形进行中,0代表波形结束。该语句意思是前脚掌磁力计不处于波形进行阶段时,如常更新上升起点。
  98. {
  99. FrontLiftPoint = mag_buf_front[2];
  100. }
  101. else
  102. {
  103. ;//当波形进行中,不改变初始上升起点,有助于过滤杂波。
  104. }
  105. }
  106. if(mag_buf_back[2] - mag_buf_back[1]>=0)
  107. {
  108. if(BKEY==0)
  109. {
  110. BackDropPoint = mag_buf_back[2];
  111. }
  112. else
  113. {
  114. if(mag_buf_back[2]>BackDropPoint)
  115. {
  116. BackDropPoint = mag_buf_back[2];
  117. }
  118. }
  119. }
  120. else
  121. {
  122. if(BKEY==0)//BKEY为后脚波形状态变量,1代表波形进行中,0代表波形结束。该语句意思是后脚掌磁力计不处于波形进行阶段时,如常更新上升起点
  123. {
  124. BackLiftPoint = mag_buf_back[2];
  125. }
  126. else
  127. {
  128. ;
  129. }
  130. }
  131. /*判断前后脚掌的状态*/
  132. if(mag_buf_front[2] - FrontLiftPoint>2200)
  133. {
  134. FKEY = 1;
  135. if(mag_buf_front[2] - FrontLiftPoint>FrontLiftDistance)
  136. {
  137. FrontLiftDistance = mag_buf_front[2] - FrontLiftPoint;
  138. }
  139. }
  140. if(mag_buf_back[2] - BackLiftPoint>2200)
  141. {
  142. BKEY = 1;
  143. if(mag_buf_back[2] - BackLiftPoint>BackLiftDistance)
  144. {
  145. BackLiftDistance = mag_buf_back[2] - BackLiftPoint;
  146. }
  147. }
  148. /*判断是否满足FKEY=0和BKEY=0,即是否满足一个升降波形*/
  149. if((mag_buf_front[2] - FrontLiftPoint<=1500)||((FrontDropPoint-mag_buf_front[2]>0.4*FrontLiftDistance)&&(abs(mag_buf_front[2]-mag_buf_front[1])<400))) //当磁力计回降到小于上升起点+1000的数值时,FKEY归零,预示前脚波形结束。
  150. {
  151. FKEY=0;
  152. FrontLiftDistance = 2200;
  153. }
  154. if((mag_buf_back[2] - BackLiftPoint<=1500)||((BackDropPoint-mag_buf_back[2]>0.4*BackLiftDistance)&&(abs(mag_buf_back[2]-mag_buf_back[1])<400)))//当磁力计回降到小于上升起点+1000的数值时,BKEY归零,预示后脚波形结束。
  155. {
  156. BKEY=0;
  157. BackLiftDistance = 2200;
  158. }
  159. /*跳出死循环*/
  160. if(FrontLiftPoint==0) //当磁力计回降到小于上升起点+1000的数值时,FKEY归零,预示前脚波形结束。
  161. {
  162. FKEY=0;
  163. }
  164. if(BackLiftPoint==0)//当磁力计回降到小于上升起点+1000的数值时,BKEY归零,预示后脚波形结束。
  165. {
  166. BKEY=0;
  167. }
  168. /*判断鞋子触地或离地*/
  169. if(ShoeTouchFlag==0)
  170. {
  171. if(Ucounter<300)
  172. {
  173. Ucounter++;
  174. }
  175. Rcounter=0;
  176. if(((FKEY==1)||(BKEY==1))&&(Ucounter>=8))
  177. {
  178. step=1;
  179. ShoeTouchFlag=1;
  180. //SEGGER_RTT_printf(0,"You have one step!\n");
  181. }
  182. else
  183. {
  184. step=0;
  185. ShoeTouchFlag=0;
  186. }
  187. }
  188. else
  189. {
  190. step = 0;
  191. if(Rcounter<300)
  192. {
  193. Rcounter++;
  194. }
  195. Ucounter=0;
  196. if((Rcounter>=6)&&(FKEY==0)&&((abs(accZ_buf[0]-2000)>300)||(abs(accZ_buf[1]-2000)>300)||(abs(accZ_buf[2]-2000)>300))&&(BKEY==0))
  197. {
  198. ShoeTouchFlag=0;
  199. }
  200. else
  201. {
  202. ShoeTouchFlag=1;
  203. }
  204. }
  205. }
  206. else
  207. {
  208. step=0;
  209. FrontLiftPoint = mag_buf_front[2];
  210. FrontDropPoint = mag_buf_front[2];
  211. BackLiftPoint = mag_buf_back[2];
  212. BackDropPoint = mag_buf_back[2];
  213. }
  214. return step;
  215. }
  216. uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
  217. {
  218. static uint8_t up_flag = 1;
  219. int32_t mag_temp[3];
  220. static int16_t accZ_buf[3];
  221. static uint8_t acc_index = 0;
  222. static int mag_buf[3];
  223. static uint8_t mag_index = 0;
  224. int step;
  225. int calculate_flag = 0;
  226. static int16_t LiftPoint;
  227. static int16_t DropPoint;
  228. static int16_t FrontTouchFlag=0;
  229. static int16_t LiftDistance;
  230. static int16_t counter1=0;
  231. static int16_t counter2=0;
  232. for(int i = 0; i < 3; i ++)
  233. {
  234. mag_temp[i] = (int32_t) (mag[i]);
  235. }
  236. mag_sqrt = abs(mag_temp[2]);
  237. /*读入磁力计数据*/
  238. if(mag_index >= 3)
  239. {
  240. mag_buf[0] = mag_buf[1];
  241. mag_buf[1] = mag_buf[2];
  242. mag_index = 2;
  243. calculate_flag = 1;
  244. }
  245. mag_buf[mag_index++] = mag_sqrt;
  246. /*读入xyZ轴加速度数据*/
  247. if(acc_index >= 3)
  248. {
  249. accZ_buf[0] = accZ_buf[1];
  250. accZ_buf[1] = accZ_buf[2];
  251. acc_index = 2;
  252. }
  253. accZ_buf[acc_index++] = -acc_z;
  254. /*开始计算*/
  255. if (calculate_flag == 1)
  256. {
  257. SEGGER_RTT_printf(0,"LiftPoint==:%d...\n",LiftPoint);
  258. SEGGER_RTT_printf(0,"DropPoint==:%d...\n",DropPoint);
  259. SEGGER_RTT_printf(0,"LiftDistance==:%d...\n",LiftDistance);
  260. if(mag_buf[2] - mag_buf[1]>=0)
  261. {
  262. if(FrontTouchFlag==0)
  263. {
  264. DropPoint = mag_buf[2];
  265. }
  266. else
  267. {
  268. if(mag_buf[2]>DropPoint)
  269. {
  270. DropPoint = mag_buf[2];
  271. }
  272. }
  273. }
  274. else
  275. {
  276. if(FrontTouchFlag==0)
  277. {
  278. LiftPoint = mag_buf[2];
  279. }
  280. else
  281. {
  282. ;
  283. }
  284. }
  285. /*判断前后脚掌的状态*/
  286. if(mag_buf[2] - LiftPoint>1000)
  287. {
  288. FrontTouchFlag = 1;
  289. if(mag_buf[2] - LiftPoint>LiftDistance)
  290. {
  291. LiftDistance = mag_buf[2] - LiftPoint;
  292. }
  293. }
  294. if((mag_buf[2] - LiftPoint<=1000)||((DropPoint-mag_buf[2]>0.4*LiftDistance)&&(abs(mag_buf[2]-mag_buf[1])<400)))
  295. {
  296. FrontTouchFlag = 0;
  297. LiftDistance = 1000;
  298. }
  299. /*计算步数*/
  300. if (up_flag == 0)
  301. {
  302. counter1=0;
  303. if(counter2<300)
  304. {
  305. counter2++;
  306. }
  307. if((FrontTouchFlag==1)&&(counter2>=6))
  308. {
  309. step = 1;
  310. up_flag = 1;
  311. SEGGER_RTT_printf(0,"step==%d\n",step);
  312. }
  313. else
  314. {
  315. step = 0;
  316. up_flag = 0;
  317. }
  318. }
  319. else
  320. {
  321. step = 0;
  322. counter2=0;
  323. if(counter1<300)
  324. {
  325. counter1++;
  326. }
  327. if((FrontTouchFlag==0)&&((abs(accZ_buf[0]-2000)>300)||(abs(accZ_buf[1]-2000)>300)||(abs(accZ_buf[2]-2000)>300))&&(counter1>=6))
  328. {
  329. up_flag=0;
  330. }
  331. }
  332. }
  333. else
  334. {
  335. step = 0;
  336. LiftPoint = mag_buf[2];
  337. DropPoint = mag_buf[2];
  338. }
  339. return step;
  340. }