hal_wearshoes.c 4.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153
  1. #include "hal_wearshoes.h"
  2. #include "bsp_time.h"
  3. #include "system.h"
  4. #include "hal_ser_imu_mode_manage.h"
  5. #include "tool.h"
  6. #include "exception.h"
  7. #define HAL_WEARSHOES_TIMEOUT 60000
  8. #define Bufflength 6
  9. static int32_t buff[Bufflength]={0};
  10. static uint8_t refreshFlag =1;
  11. enum{
  12. WEARSHOES_NO=0,
  13. WEARSHOES_YES
  14. };
  15. static uint8_t isWearShoes = WEARSHOES_NO;
  16. static uint8_t IsWearShoes(int16_t mag[])
  17. {
  18. uint8_t flag = 0;
  19. int32_t mag_norm =0;
  20. int32_t LvBobuff[Bufflength]={0};
  21. int32_t temp =0;
  22. uint8_t i=0;
  23. mag_norm = (int32_t)(sqrt((float) (mag[0] *mag[0] + mag[1] * mag[1] + mag[2] *mag[2])));
  24. if(1 == refreshFlag){refreshFlag =0;
  25. for(i=0;i< Bufflength;i++){
  26. buff[i]=mag_norm;
  27. }
  28. }else{
  29. for(i=0;i<(Bufflength-1);i++){
  30. buff[i]=buff[i+1];
  31. }
  32. buff[Bufflength-1] = mag_norm;
  33. }
  34. memcpy(LvBobuff,buff,sizeof(buff));
  35. for(uint8_t d=0;d<(Bufflength-1);d++){
  36. for(uint8_t b=0;b<(Bufflength-1-d);b++){
  37. if(LvBobuff[b]>LvBobuff[b+1]){
  38. temp=LvBobuff[b+1];
  39. LvBobuff[b+1]=LvBobuff[b];
  40. LvBobuff[b]=temp;
  41. }
  42. }
  43. }
  44. if((LvBobuff[Bufflength-1] - LvBobuff[0]) >= 1000)flag = 1;
  45. return flag;
  46. }
  47. static void hal_wearshoes_determine(void)
  48. {
  49. static uint32_t tim = 0;
  50. ser_imu_data_t data={0};
  51. SER_IMU_DIR_e SER_IMU_Set =SER_IMU_DIR_BACK;
  52. //选择前脚还是后脚的磁力计
  53. if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY)){
  54. SER_IMU_Set =SER_IMU_DIR_BACK;
  55. }
  56. else if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL)){
  57. SER_IMU_Set =SER_IMU_DIR_FRONT;
  58. }
  59. else return;
  60. if(hal_ser_imu_mode_manage_get_data_num(SER_IMU_Set) >= 1){
  61. hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_BACK, 0, &data);
  62. if(1 == IsWearShoes(data.mag)){
  63. tim = TIME_GetTicks();
  64. if(isWearShoes == WEARSHOES_NO){
  65. isWearShoes = WEARSHOES_YES;
  66. DEBUG_LOG("I am WearShoes\r\n");
  67. }
  68. }else{
  69. if(isWearShoes && (TIME_GetTicks() -tim >= HAL_WEARSHOES_TIMEOUT)) {
  70. isWearShoes = WEARSHOES_NO;
  71. DEBUG_LOG("I am not WearShoes\r\n");
  72. }
  73. }
  74. }
  75. }
  76. static void hal_wearshoes_Process(void)
  77. {
  78. static uint8_t state =0;
  79. static uint32_t tim =0;
  80. switch(state){
  81. case 0://配置完成
  82. if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL)){
  83. if(isWearShoes == WEARSHOES_NO){
  84. hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_NORMAL, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF);
  85. hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_STANDBY, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
  86. state =1;
  87. tim = TIME_GetTicks();
  88. }
  89. else{
  90. hal_wearshoes_determine();
  91. }
  92. }
  93. else if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY)){
  94. if(isWearShoes == WEARSHOES_YES){
  95. hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_NORMAL, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
  96. hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_STANDBY, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF);
  97. state =1;
  98. tim = TIME_GetTicks();
  99. }
  100. else{
  101. hal_wearshoes_determine();
  102. }
  103. }
  104. break;
  105. case 1://配置中
  106. if(isWearShoes == WEARSHOES_NO){
  107. if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY) &&
  108. 0 != hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL))state =0;
  109. }else{
  110. if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL) &&
  111. 0 != hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY))state =0;
  112. }
  113. if(TIME_GetTicks()-tim >= 4000){tim = TIME_GetTicks();//配置10秒超时
  114. state =0;
  115. }
  116. break;
  117. default:state =0;break;
  118. }
  119. }
  120. uint8_t hal_wearshoes_is_wearshoes(void)
  121. {
  122. return isWearShoes;
  123. }
  124. void hal_wearshoes_Init(void)
  125. {
  126. if(isWearShoes == WEARSHOES_NO){
  127. hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_STANDBY, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
  128. }else if(isWearShoes == WEARSHOES_YES){
  129. hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_NORMAL, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
  130. }
  131. Process_Start(100,"hal_wearshoes_Process",hal_wearshoes_Process);
  132. }