process_result.c 7.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285
  1. #include "process_result.h"
  2. #include "hal_mt.h"
  3. #include "app_flash.h"
  4. #include "app_switchimu.h"
  5. #include "math.h"
  6. float gyr[3];
  7. float acc[3];
  8. float mag[3];
  9. int16_t accel[3];
  10. int16_t gyro[3];
  11. int16_t front_magn[3];
  12. int16_t back_magn[3];
  13. int32_t pos[3];
  14. int16_t att[3];
  15. int16_t zupt;
  16. int8_t slave_rssi;
  17. uint8_t foot_data_buf[64];
  18. uint8_t foot_data_len;
  19. int16_t front_zero;
  20. int16_t back_zero;
  21. int16_t acc_zero;
  22. uint16_t front_mag_norm;
  23. uint16_t back_mag_norm;
  24. int32_t step_count;
  25. float gyr[3];
  26. float acc[3];
  27. float mag[3];
  28. int low_power_dectect;
  29. int low_power_step;
  30. uint16_t low_power_mag;
  31. int zupt_wait_time;
  32. int nan_flag = 0;
  33. //left
  34. //float acc_bias[3] = {0.091139f, 0.101830f, -0.048524f};
  35. //float acc_scale[3] = {0.980197f, 1.003513f, 0.987654f};
  36. //right
  37. float acc_bias[3] = {0.047714f, 0.026054f, -0.008199f};
  38. float acc_scale[3] = {1.003853f, 1.007566f, 0.986378f};
  39. void foot_data_to_package()
  40. {
  41. static uint16_t package_time = 0;
  42. foot_data_len = 0;
  43. foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>24);
  44. foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>16);
  45. foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>8);
  46. foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>0);
  47. foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>24);
  48. foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>16);
  49. foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>8);
  50. foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>0);
  51. foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>24);
  52. foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>16);
  53. foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>8);
  54. foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>0);
  55. foot_data_buf[foot_data_len++] = (uint8_t)(att[0]>>8);
  56. foot_data_buf[foot_data_len++] = (uint8_t)(att[0]>>0);
  57. foot_data_buf[foot_data_len++] = (uint8_t)(att[1]>>8);
  58. foot_data_buf[foot_data_len++] = (uint8_t)(att[1]>>0);
  59. foot_data_buf[foot_data_len++] = (uint8_t)(att[2]>>8);
  60. foot_data_buf[foot_data_len++] = (uint8_t)(att[2]>>0);
  61. foot_data_buf[foot_data_len++] = (uint8_t)(front_mag_norm>>8);
  62. foot_data_buf[foot_data_len++] = (uint8_t)(front_mag_norm>>0);
  63. foot_data_buf[foot_data_len++] = (uint8_t)(back_mag_norm>>8);
  64. foot_data_buf[foot_data_len++] = (uint8_t)(back_mag_norm>>0);
  65. foot_data_buf[foot_data_len++] = (uint8_t)(zupt>>0);
  66. foot_data_buf[foot_data_len++] = (uint8_t) slave_rssi;
  67. foot_data_buf[foot_data_len++] = (uint8_t)(package_time>>8);
  68. foot_data_buf[foot_data_len++] = (uint8_t)(package_time>>0);
  69. foot_data_buf[foot_data_len++] = (uint8_t)(accel[0]>>8);
  70. foot_data_buf[foot_data_len++] = (uint8_t)(accel[0]>>0);
  71. foot_data_buf[foot_data_len++] = (uint8_t)(accel[1]>>8);
  72. foot_data_buf[foot_data_len++] = (uint8_t)(accel[1]>>0);
  73. foot_data_buf[foot_data_len++] = (uint8_t)(accel[2]>>8);
  74. foot_data_buf[foot_data_len++] = (uint8_t)(accel[2]>>0);
  75. foot_data_buf[foot_data_len++] = (uint8_t)(step_count>>24);
  76. foot_data_buf[foot_data_len++] = (uint8_t)(step_count>>16);
  77. foot_data_buf[foot_data_len++] = (uint8_t)(step_count>>8);
  78. foot_data_buf[foot_data_len++] = (uint8_t)(step_count>>0);
  79. // SEGGER_RTT_printf(0,"test footPDR \n");
  80. package_time ++;
  81. }
  82. void imu_data_to_package()
  83. {
  84. static uint16_t package_time = 0;
  85. foot_data_len = 0;
  86. foot_data_buf[foot_data_len++] = (uint8_t)(accel[0]>>8);
  87. foot_data_buf[foot_data_len++] = (uint8_t)(accel[0]>>0);
  88. foot_data_buf[foot_data_len++] = (uint8_t)(accel[1]>>8);
  89. foot_data_buf[foot_data_len++] = (uint8_t)(accel[1]>>0);
  90. foot_data_buf[foot_data_len++] = (uint8_t)(accel[2]>>8);
  91. foot_data_buf[foot_data_len++] = (uint8_t)(accel[2]>>0);
  92. foot_data_buf[foot_data_len++] = (uint8_t)(gyro[0]>>8);
  93. foot_data_buf[foot_data_len++] = (uint8_t)(gyro[0]>>0);
  94. foot_data_buf[foot_data_len++] = (uint8_t)(gyro[1]>>8);
  95. foot_data_buf[foot_data_len++] = (uint8_t)(gyro[1]>>0);
  96. foot_data_buf[foot_data_len++] = (uint8_t)(gyro[2]>>8);
  97. foot_data_buf[foot_data_len++] = (uint8_t)(gyro[2]>>0);
  98. foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[0]>>8);
  99. foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[0]>>0);
  100. foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[1]>>8);
  101. foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[1]>>0);
  102. foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[2]>>8);
  103. foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[2]>>0);
  104. foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[0]>>8);
  105. foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[0]>>0);
  106. foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[1]>>8);
  107. foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[1]>>0);
  108. foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[2]>>8);
  109. foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[2]>>0);
  110. // SEGGER_RTT_printf(0," left_sh , foot_data_len = %d\n", foot_data_len);
  111. package_time ++;
  112. }
  113. void get_foot_data(uint8_t *buf, uint8_t *buff_len)
  114. {
  115. memcpy(buf, foot_data_buf, foot_data_len);
  116. *buff_len = foot_data_len;
  117. }
  118. void process_motion(int32_t time_stamp, int16_t _acc[3], int16_t _gry[3], int16_t front_mag[3], int16_t back_mag[3], uint8_t _rssi)
  119. {
  120. /*
  121. 解析数据
  122. */
  123. if(isnan(_acc[0]) || isnan(_acc[1]) || isnan(_acc[2]) || isnan(_gry[0]) || isnan(_gry[1]) || isnan(_gry[2]) )
  124. {
  125. nan_flag = 1;
  126. }
  127. if(nan_flag == 1)
  128. {
  129. SEGGER_RTT_printf(0,"read nan from IMU", foot_data_len);
  130. }
  131. memcpy(accel, _acc, 3 * sizeof(int16_t));
  132. memcpy(gyro , _gry, 3 * sizeof(int16_t));
  133. memcpy(front_magn , front_mag, 3 * sizeof(int16_t));
  134. memcpy(back_magn, back_mag, 3 * sizeof(int16_t));
  135. slave_rssi = _rssi - 8;
  136. if(slave_rssi < 0)
  137. {
  138. slave_rssi = 0;
  139. }
  140. for(int i = 0; i < 3; i++)
  141. {
  142. gyr[i] = gyro[i] / GYR_LSB;
  143. }
  144. for(int i = 0; i < 3; i++)
  145. {
  146. acc[i] = accel[i] / ACC_LSB;
  147. // acc[i] = (acc[i] + acc_bias[i]) * acc_scale[i];
  148. }
  149. // SEGGER_RTT_printf(0,"PDR gyro[0]:%d ;gyro[1] : %d ;gyro[2]: %d \n", gyro[0], gyro[1],gyro[2]);
  150. // SEGGER_RTT_printf(0,"PDR acc[0]:%d ;acc[1] : %d ;acc[2]: %d \n", _acc[0], _acc[1], _acc[2]);
  151. front_mag_norm = (uint16_t)(abs(front_magn[2]));
  152. back_mag_norm = (uint16_t)(abs(back_magn[2]));
  153. //SEGGER_RTT_printf(0,"front_mag_norm : %d; back_mag_norm : %d; front_magn[0] : %d; front_magn[1] : %d; front_magn[2] : %d \r\n", front_mag_norm, back_mag_norm,front_magn[0],front_magn[1],front_magn[2]);
  154. //检测零速
  155. detect_zero_vel(front_magn, back_magn, accel, &front_zero, &back_zero, &acc_zero);
  156. //轨迹计算
  157. if(front_zero > 0 || back_zero > 0 )
  158. {
  159. zupt = 1;
  160. }
  161. else
  162. {
  163. zupt = 0;
  164. }
  165. footPDR(time_stamp, gyr, acc, 0 , zupt, acc_zero, pos, att);
  166. //SEGGER_RTT_printf(0,"pos[0] : %d; pos[1] : %d ; pos[2] : %d \r\n", pos[0] , pos[1], pos[2]);
  167. //SEGGER_RTT_printf(0,"gyro[0] : %d; gyro[1] : %d ; gyro[2] : %d \r\n", gyro[0] , gyro[1], gyro[2]);
  168. //35号左鞋安装有误差,软件来补锅
  169. // float pos_tmp[3];
  170. //
  171. // pos_tmp[0] = -pos[1] * 0.01f;
  172. // pos_tmp[1] = pos[0] * 0.01f;
  173. //
  174. // pos[0] = (int32_t)((0.9397f * pos_tmp[0] + 0.3420f * pos_tmp[1])*100.0f);
  175. // pos[1] = (int32_t)((-0.3420f * pos_tmp[0] + 0.9397f * pos_tmp[1])*100.0f);
  176. int32_t pos_tmp[3];
  177. memcpy(pos_tmp, pos, 3 * sizeof(int32_t));
  178. pos[0] = -pos_tmp[1];
  179. pos[1] = pos_tmp[0];
  180. accel[2] = -accel[2];
  181. step_count = mFlash.mStep.stepCur[0];
  182. //SEGGER_RTT_printf(0,"step_count: %d \r\n", step_count);
  183. foot_data_to_package();
  184. }