app_detectIsHost.c 9.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308
  1. /*********************************************************************
  2. * INCLUDES
  3. */
  4. #include "ble_comm.h"
  5. #include "app_detectIsHost.h"
  6. #include "hal_ble_client.h"
  7. #include "app_flash.h"
  8. #include "hal_qma.h"
  9. #include "hal_mt.h"
  10. #include "tool.h"
  11. #include "bll_imu.h"
  12. #include "hal_charge.h"
  13. #include "bsp_time.h"
  14. #include "hal_mahonyAHRS.h"
  15. #include "hal_ano.h"
  16. #include "hal_led.h"
  17. //全功率模式
  18. static const bll_imu_one_way_param_t all_front_param={
  19. .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //前脚 - 加速度正常模式
  20. .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //前脚 - 陀螺仪正常模式
  21. .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //前脚 - 时间戳25US精度
  22. .timestamp_switch = FML_IMU_TIMESTAMP_ON, //前脚 - 时间戳开启
  23. .acc_fs = FML_IMU_ACC_FS_16G, //前脚 - 加速度量程 - 16G
  24. .gry_fs = FML_IMU_GRY_FS_2000DPS, //前脚 - 陀螺仪量程 - 2000DPS
  25. .mag_fs = FML_IMU_MAG_FS_30GS, //前脚 - 地磁计量程 - 30GS
  26. .acc_odr = FML_IMU_ACC_ODR_104HZ, //前脚 - 加速度采样频率 - 104HZ
  27. .gry_odr = FML_IMU_GRY_ODR_OFF, //前脚 - 陀螺仪采样频率 - 104HZ
  28. .mag_odr = FML_IMU_MAG_ODR_OFF, //前脚 - 地磁计采样频率 - 200HZ
  29. .fifo_odr = FML_IMU_FIFO_ODR_104HZ,
  30. };
  31. static const bll_imu_one_way_param_t all_back_param={
  32. .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //后脚 - 加速度正常模式
  33. .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //后脚 - 陀螺仪正常模式
  34. .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //后脚 - 时间戳25US精度
  35. .timestamp_switch = FML_IMU_TIMESTAMP_OFF, //后脚 - 时间戳关闭
  36. .acc_fs = FML_IMU_ACC_FS_16G, //后脚 - 加速度量程 - 16G
  37. .gry_fs = FML_IMU_GRY_FS_2000DPS, //后脚 - 陀螺仪量程 - 2000DPS
  38. .mag_fs = FML_IMU_MAG_FS_30GS, //后脚 - 地磁计量程 - 30GS
  39. .acc_odr = FML_IMU_ACC_ODR_OFF, //后脚 - 加速度采样频率 - 关闭
  40. .gry_odr = FML_IMU_GRY_ODR_OFF, //后脚 - 陀螺仪采样频率 - 关闭
  41. .mag_odr = FML_IMU_MAG_ODR_OFF, //后脚 - 地磁计采样频率 - 200HZ
  42. .fifo_odr = FML_IMU_FIFO_ODR_OFF,
  43. };
  44. static const bll_imu_param_t all_bll_imu_param_t={
  45. .config_param[FML_IMU_DIR_FRONT] = &all_front_param,
  46. .config_param[FML_IMU_DIR_BACK] = &all_back_param,
  47. };
  48. static uint8_t SetDeviceNameFlag = 0;
  49. void app_ReSetDeviceName(void){
  50. SetDeviceNameFlag = 1;
  51. }
  52. static void app_SetDeviceName_Porcess(void){
  53. static uint8_t state =0;
  54. char buf[16];
  55. memset(buf,0,16);
  56. switch(state){
  57. case 0:
  58. if(1 == SetDeviceNameFlag){state =1;}
  59. break;
  60. case 1:
  61. if(host_isconnect()){host_disconnect();}
  62. else{
  63. if(slave_isconnect()) slave_disconnect();
  64. else{
  65. advertising_stop();
  66. ST_scan_stop();
  67. state =2;
  68. }
  69. }
  70. break;
  71. case 2:
  72. if(app_Get_isHost()){ //
  73. #if BleNameHoldOn_ENANBLE
  74. slave_set_adv_name((char *) LEFT_NAME,sizeof(LEFT_NAME));
  75. DEBUG_LOG("AdvName(%d):%s\n",sizeof(LEFT_NAME),LEFT_NAME);
  76. host_set_scan_name((char *)RIGHT_NAME,sizeof(RIGHT_NAME));
  77. DEBUG_LOG("scanName(%d):%s\n",sizeof(RIGHT_NAME),RIGHT_NAME);
  78. #else
  79. if(mFlash.mClient.isConfig == 'C'){
  80. sprintf(buf,"%02X%02X%02X%02X%02X%02X",mFlash.macHost[0],mFlash.macHost[1],mFlash.macHost[2],mFlash.mClient.macAddr[3],mFlash.mClient.macAddr[4],mFlash.mClient.macAddr[5]);
  81. DEBUG_LOG("scanName(%d):%s\n",strlen(buf),buf);
  82. host_set_scan_name(buf,strlen(buf));
  83. memset(buf,0,sizeof(buf));
  84. sprintf(buf,"SH_%02X%02X",mFlash.macHost[1], mFlash.macHost[0]);
  85. slave_set_adv_name(buf,strlen(buf));
  86. }
  87. #endif
  88. }else{
  89. #if BleNameHoldOn_ENANBLE
  90. slave_set_adv_name((char *)RIGHT_NAME,sizeof(RIGHT_NAME));
  91. DEBUG_LOG("AdvName(%d):%s\n",sizeof(RIGHT_NAME),RIGHT_NAME);
  92. #else
  93. if(mFlash.mClient.isConfig=='C'){ //
  94. sprintf(buf,"%02X%02X%02X%02X%02X%02X",mFlash.mClient.macAddr[0],mFlash.mClient.macAddr[1],mFlash.mClient.macAddr[2],mFlash.macHost[3],mFlash.macHost[4],mFlash.macHost[5]);
  95. DEBUG_LOG("advName(%d):%s\n",strlen(buf),buf);
  96. slave_set_adv_name(buf,strlen(buf));
  97. }
  98. host_set_scan_name((char *)"12321321312",sizeof("12321321312"));
  99. ST_scan_stop();
  100. #endif
  101. }
  102. slave_adv_init();
  103. advertising_start();
  104. state =0;
  105. SetDeviceNameFlag = 0;
  106. break;
  107. default:state =0;SetDeviceNameFlag = 0;break;
  108. }
  109. }
  110. static uint8_t app_get_Front_Number(int *number){
  111. uint8_t front_CS =0,back_CS =0;
  112. front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&all_bll_imu_param_t);
  113. back_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&all_bll_imu_param_t);
  114. // DEBUG_LOG("front_CS:%d back_CS:%d\r\n",front_CS,back_CS);
  115. if(BLL_IMU_CONFIG_FINISH == front_CS && BLL_IMU_CONFIG_FINISH == back_CS){
  116. *number = bll_imu_get_data_num(BLL_IMU_DIR_FRONT);
  117. if(*number >= 1)return 1;
  118. else return 0;
  119. }
  120. else return 0;
  121. }
  122. /**
  123. @brief 返回主机标志位
  124. @param 无
  125. @return 主机标志位
  126. */
  127. uint8_t app_Get_isHost(void)
  128. {
  129. return mFlash.isHost;
  130. }
  131. static MahonyAHRS_t Front_Mahony={0};
  132. static MahonyAHRS_t Mind_Mahony={0};
  133. float Mahony_M_GetRoll(void) {return Mind_Mahony.roll;}
  134. float Mahony_M_GetPitch(void) {return Mind_Mahony.pitch;}
  135. float Mahony_M_GetYaw(void) {return Mind_Mahony.yaw;}
  136. //void My_Send_Senser(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz, int16_t mx, int16_t my, int16_t mz)
  137. //{
  138. // uint8_t buf[32];
  139. // uint8_t L=0;
  140. //
  141. // buf[L++] = (uint8_t)(ax>>8);
  142. // buf[L++] = (uint8_t)(ax>>0);
  143. // buf[L++] = (uint8_t)(ay>>8);
  144. // buf[L++] = (uint8_t)(ay>>0);
  145. // buf[L++] = (uint8_t)(az>>8);
  146. // buf[L++] = (uint8_t)(az>>0);
  147. //
  148. // buf[L++] = (uint8_t)(gx>>8);
  149. // buf[L++] = (uint8_t)(gx>>0);
  150. // buf[L++] = (uint8_t)(gy>>8);
  151. // buf[L++] = (uint8_t)(gy>>0);
  152. // buf[L++] = (uint8_t)(gz>>8);
  153. // buf[L++] = (uint8_t)(gz>>0);
  154. //
  155. // buf[L++] = (uint8_t)(mx>>8);
  156. // buf[L++] = (uint8_t)(mx>>0);
  157. // buf[L++] = (uint8_t)(my>>8);
  158. // buf[L++] = (uint8_t)(my>>0);
  159. // buf[L++] = (uint8_t)(mz>>8);
  160. // buf[L++] = (uint8_t)(mz>>0);
  161. // BLE_Client_Tx_Send(0,BLE_HEART,buf,L);
  162. //}
  163. static volatile uint8_t LRFlagReady =0;
  164. uint8_t get_LR_readly(void)
  165. {
  166. return LRFlagReady;
  167. }
  168. static int8_t isLeftOrRight(int16_t* imuAcc, int16_t* midAcc)
  169. { //10ms读一次imu和中间加速度数据,调用一次
  170. static int16_t LR = 0;
  171. static int16_t imuAcc_update[3];
  172. static int16_t midAcc_update[3];
  173. imuAcc_update[0] = imuAcc[0];
  174. imuAcc_update[1] = imuAcc[1];
  175. imuAcc_update[2] = imuAcc[2];
  176. midAcc_update[0] = midAcc[1];
  177. midAcc_update[1] = midAcc[0];
  178. midAcc_update[2] = -midAcc[2];
  179. Mahony_update(&Front_Mahony,0,0,0,imuAcc_update[0],imuAcc_update[1],imuAcc_update[2],0,0,0);
  180. Mahony_update(&Mind_Mahony, 0,0,0,midAcc_update[0],midAcc_update[1],midAcc_update[2],0,0,0);
  181. int16_t d_roll = abs((int16_t)(Front_Mahony.roll*1) - (int16_t)(Mind_Mahony.roll*1));
  182. if(d_roll>300) d_roll = 360 - d_roll;
  183. int16_t pit = abs((int16_t)(Mahony_M_GetPitch()*1));
  184. if(pit<60){
  185. if(d_roll>150){ //反着放,右鞋
  186. if(LR<50){
  187. LR++;
  188. if(LR == 50) LED_Start(LED_LRCHECK,COLOR_CYAN);
  189. }
  190. }else{ //正着放,左鞋
  191. if(LR>-50){
  192. LR--;
  193. if(LR == -50) LED_Start(LED_LRCHECK,COLOR_PURPLE);
  194. }
  195. }
  196. }
  197. // ANO_Send_Status(Mind_Mahony.roll,Mind_Mahony.pitch,Front_Mahony.roll,0,0,0,0);
  198. // ANO_Send_Senser(midAcc[0],midAcc[1],midAcc[2],imuAcc[0],imuAcc[1],imuAcc[2],d_roll,LR,0);
  199. if(LR > 0) return 0; //右鞋
  200. else return 1; //左鞋
  201. }
  202. static void DetectLr_notify_cb(uint32_t dir_bit){
  203. int number =0;
  204. int ret =0;
  205. bll_imu_data_t f_data={0};
  206. qma_data_t qma_data={0};
  207. if(!((dir_bit >> BLL_IMU_DIR_FRONT) & 0x01))return;
  208. //中间加速度没有设置成功
  209. if(QMA_104HZ != hal_get_QmaFrequency()){
  210. return ;
  211. }
  212. //获取前脚角速度值的值
  213. if(!app_get_Front_Number(&number)){
  214. return;
  215. }
  216. ret = drv_qma_get_acc_data(&qma_data);
  217. if(-1 == ret || (0 == qma_data.acc[0] && 0 == qma_data.acc[1] && 0== qma_data.acc[2])){
  218. return;
  219. }
  220. if(0 == LRFlagReady){//开机6秒不检测
  221. if(TIME_GetTicks() >= 5000){
  222. LRFlagReady =1;
  223. }
  224. }
  225. uint8_t result =0;
  226. if(-1 != bll_imu_get_data(BLL_IMU_DIR_FRONT, number-1, &f_data)){
  227. result = isLeftOrRight(f_data.acc,qma_data.acc);
  228. if(LRFlagReady){
  229. mFlash.isHost = result;
  230. //测试
  231. // if(mFlash.isHost)LED_Start(LED_LRCHECK,COLOR_PURPLE);
  232. // else LED_Start(LED_LRCHECK,COLOR_CYAN);
  233. }
  234. }
  235. }
  236. static void app_detect_LR_Porcess(void){
  237. uint8_t front_CS =0,back_CS =0;
  238. front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&all_bll_imu_param_t);
  239. back_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&all_bll_imu_param_t);
  240. if(BLL_IMU_CONFIG_DOING != front_CS || BLL_IMU_CONFIG_DOING != back_CS){
  241. bll_imu_Resume_config_param(&all_bll_imu_param_t);
  242. }
  243. }
  244. void app_detect_LR_process_stop(void){
  245. SetDeviceNameFlag =1;//设置一次蓝牙名字
  246. bll_imu_Resume_unregister_config_param(&all_bll_imu_param_t);
  247. Process_Stop(app_detect_LR_Porcess);
  248. LED_Stop(LED_LRCHECK);
  249. hal_qma_setFrequency(QMA_OFF);
  250. LED_Stop(LED_LRCHECK);
  251. }
  252. void app_detect_Init(void)
  253. {
  254. SetDeviceNameFlag =1;//开机设置一次蓝牙名字
  255. Process_Start(100,"SetDeviceName",app_SetDeviceName_Porcess);
  256. if(LR_FLAG_VALUE != mFlash.LR_FLAG){
  257. // Flash_DeleteAllInfor();
  258. // Flash_DeleteAllStep();
  259. // Flash_DeleteAllBackup();
  260. Mahony_Init(&Front_Mahony,104);
  261. Mahony_Init(&Mind_Mahony,104);
  262. hal_qma_setFrequency(QMA_104HZ);
  263. bll_imu_Resume_config_param(&all_bll_imu_param_t);
  264. Process_Start(100,"app_detect_LR",app_detect_LR_Porcess);
  265. bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, DetectLr_notify_cb);
  266. Process_SetHoldOn(app_detect_LR_Porcess,1);
  267. }
  268. if(mFlash.isHost){DEBUG_LOG("======= Left shooe ======= \n");}
  269. else {DEBUG_LOG("======= Right shooe ======= \n");}
  270. DEBUG_LOG("======= mFlash.mClient.isConfig:%d ======= \n",mFlash.mClient.isConfig);
  271. }