123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256 |
- #include "app_math.h"
- #include "bsp_time.h"
- #include "system.h"
- #include "hal_ser_imu_mode_manage.h"
- #include "math.h"
- #include "ble_comm.h"
- #include "app_flash.h"
- #include "detect_zero_vel.h"
- #include "tool.h"
- #include "app_switchimu.h"
- #include "detect_step_by_mag.h"
- #include "pdrStatus.h"
- #include "detect_step_by_mag.h"
- #include "app_client_step.h"
- #define IMU_MAX_GROUP_NUM 20
- static int16_t acc_front[IMU_MAX_GROUP_NUM][3];
- static int16_t gry_front[IMU_MAX_GROUP_NUM][3];
- static int16_t mag6310_front[IMU_MAX_GROUP_NUM][3];
- static int16_t mag6310_back[3];
- static int32_t timestamp_front[IMU_MAX_GROUP_NUM];
- static uint8_t rssi;
- static int16_t IMU_STATUS; //记录状态用来重新记录时间戳
- static int32_t timestamp;
- static int32_t last_timestamp;
- void process_imu_data_front(int front_index)
- {
- if(IMU_STATUS != 1)
- {
- IMU_STATUS = 1;
-
- last_timestamp = timestamp_front[0];
-
- timestamp = 0;
- }
-
- if(front_index >IMU_MAX_GROUP_NUM)return;
- for(int i = 0; i < front_index; i++)
- {
- int32_t dt = timestamp_front[i] - last_timestamp;
-
- if(dt > 20000 || dt < 0)
- {
- dt = 10000;
- }
-
- timestamp += dt;
-
- SEGGER_RTT_printf(0,"timestamp_front[i] - last_timestamp : %d; i = %d NRF_RTC0->COUNTER:%d\r\n", timestamp_front[i] - last_timestamp, i,NRF_RTC0->COUNTER);
-
- last_timestamp = timestamp_front[i];
-
- // SEGGER_RTT_printf(0,"timestamp_front[i] : %d; i = %d\r\n", timestamp_front[i], i);
-
- IMU_Process_motion_queue(mFlash.isHost, timestamp, acc_front[i],
- gry_front[i],mag6310_front[i], mag6310_back, rssi);
- }
- }
- void app_math_TimerCounter(int front_num, int back_num)
- {
- ser_imu_data_t data;
- int16_t group_num = 0;
-
- //游戏模式
- if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_GAME) != -1){
-
- rssi = 0-host_get_rssi();
- group_num = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT);
- if(group_num >IMU_MAX_GROUP_NUM || front_num == 0)return;
- for(int i=0;i<group_num;i++){
- hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, i, &data);
- gry_front[i][0] = data.gry[0];gry_front[i][1] = data.gry[1];gry_front[i][2] = data.gry[2];
- acc_front[i][0] = data.acc[0];acc_front[i][1] = data.acc[1];acc_front[i][2] = data.acc[2];
- mag6310_front[i][0] = data.mag[0];mag6310_front[i][1] = data.mag[1];mag6310_front[i][2] = data.mag[2];
- timestamp_front[i] = data.fifo_timestamp;
- // JS_RTT_Print_06(acc_front[i][0],acc_front[i][1],acc_front[i][2],gry_front[i][0],gry_front[i][1],gry_front[i][2]);
- // JS_RTT_Print_06(mag6310_front[i][0],mag6310_front[i][1],mag6310_front[i][2],timestamp_front[i],0,0);
- }
-
- if(hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_BACK) >= 1){
- hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_BACK, 0, &data);
- mag6310_back[0] = data.mag[0];mag6310_back[1] = data.mag[1];mag6310_back[2] = data.mag[2];
- }
-
- if(mFlash.isHost){
- process_imu_data_front(group_num);
- }else if(Slave_Get7_5ms_interval()){
- process_imu_data_front(group_num);
- }
-
- }else{
- //将状态重设为0
- IMU_STATUS = 0;
- set_pdr_status();
- }
-
-
- //实时计步模式
- if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_REALSTEP) != -1){
- rssi = 0-host_get_rssi();
- group_num = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT);
- if(group_num >IMU_MAX_GROUP_NUM || front_num == 0)return;
- for(int i=0;i<group_num;i++){
- hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, i, &data);
- gry_front[i][0] = data.gry[0];gry_front[i][1] = data.gry[1];gry_front[i][2] = data.gry[2];
- acc_front[i][0] = data.acc[0];acc_front[i][1] = data.acc[1];acc_front[i][2] = data.acc[2];
- mag6310_front[i][0] = data.mag[0];mag6310_front[i][1] = data.mag[1];mag6310_front[i][2] = data.mag[2];
- timestamp_front[i] = data.fifo_timestamp;
- }
- if(hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_BACK) >= 1){
- hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_BACK, 0, &data);
- mag6310_back[0] = data.mag[0];mag6310_back[1] = data.mag[1];mag6310_back[2] = data.mag[2];
- }
-
- // SEGGER_RTT_printf(0,"======RealTimeStep=====%d\r\n",TIME_GetTicks());
- if(RealTimeStep((int16_t*)mag6310_front, (int16_t*)mag6310_back, (int16_t*)acc_front)){
- mFlash.mStep.stepCur[0]++;
- SEGGER_RTT_printf(0,"======RealTimeStep=====step %d\r\n",mFlash.mStep.stepCur[0]);
- }
- }
- }
- static void app_math_DailyStep_Process(void)
- {
- int16_t acc[3];
- int16_t mag6310[3];
- int16_t group_num = 0;
- ser_imu_data_t data;
-
- if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL) != -1 && hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_REALSTEP) == -1){
- group_num = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT);
- for(int i=0; i < group_num; i++){
- hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, i, &data);
- mag6310[0] = data.mag[0];mag6310[1] = data.mag[1];mag6310[2] = data.mag[2];
- acc[0] = data.acc[0];acc[1] = data.acc[1];acc[2] = data.acc[2];
-
- // SEGGER_RTT_printf(0,"f_mx=%d\r,f_my=%d\r,f_mz=%d\r\n",mag6310[0],mag6310[1],mag6310[2]);
- if(1 == detect_step_by_mag(mag6310,acc[2])){
- mFlash.mStep.stepCur[0]++;
- SEGGER_RTT_printf(0,"DailyStep current step:%d\r\n",mFlash.mStep.stepCur[0]);
- break;
- }
- }
- }
- }
- static uint32_t timeCNT = 0;
- static uint8_t FlagFix_process = 0;
- //一小时计时
- static void app_math_Hour_process(void){
- uint32_t sec = 0;
- static uint32_t cnt_b=0;
- uint32_t cnt = NRF_RTC0->COUNTER;
- if(cnt<cnt_b) cnt += 16777216;
- timeCNT += cnt - cnt_b;
- sec = timeCNT/32768;
- if(sec >= 130000 )SEGGER_RTT_printf(0,"!!!!!!!!!!!!!!! sec over,%d,%d,%d\n",cnt,cnt_b,sec);
- if(cnt >16777216)cnt_b = cnt - 16777216;
- else cnt_b = cnt;
-
- if(sec>=60*60){
- app_client_step_SetIsScan();
- FlagFix_process =1;
- timeCNT =0;
- SEGGER_RTT_printf(0,"timeCNT(%d)(%d)\n",timeCNT,sec);
-
- }
- }
- static void app_gyro_Fix_process(void){//陀螺仪零偏矫正
- static uint8_t state =0;
- static int16_t sample_count =0;
- static uint32_t tim =0;
- int16_t f_gry[3];
- ser_imu_data_t data;
-
- switch(state){
- case 0:
- if(-1 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_GAME) && 1 == FlagFix_process){
- Process_SetHoldOn(app_gyro_Fix_process,1);
- hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_CALIBRATION, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
- state =1;
- FlagFix_process =0;
- tim = TIME_GetTicks();
- // SEGGER_RTT_printf(0,"====>>>>HAL_SER_IMU_MODE_MANAGE_CALIBRATION\r\n");
- }
- break;
- case 1:
- if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_CALIBRATION)){
- state =2;
- Process_UpdatePeroid(app_gyro_Fix_process,10);
- sample_count =0;
- tim = TIME_GetTicks();
- // SEGGER_RTT_printf(0,"====INTO HAL_SER_IMU_MODE_MANAGE_CALIBRATION\r\n");
- }else{
- if(TIME_GetTicks()-tim>=30000){tim = TIME_GetTicks();//进入游戏模式30秒失败
- // SEGGER_RTT_printf(0,"====cnt >= 3000\r\n");
- state =3;
- }
- }
- if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_GAME))state =3;//游戏模式下退出
- break;
- case 2:
- //读取ACC值
- if(hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT) >= 1){
- hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, 0, &data);
- f_gry[0] = data.gry[0];f_gry[1] = data.gry[1];f_gry[2] = data.gry[2];
- estimate_gyr_bias_interface(f_gry,sample_count);
- // SEGGER_RTT_printf(0,"====>>>>gry:%d,%d,%d,tim:%d\r\n",f_gry[0],f_gry[1],f_gry[2],sample_count);
- sample_count++;
- }
- if(TIME_GetTicks()-tim>=10000){ //跑完10秒退出
- state =3;
- // SEGGER_RTT_printf(0,"====>>>>HAL_SER_IMU_MODE_MANAGE_CALIBRATION\r\n");
- }
- if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_GAME))state =3;//游戏模式下退出
- break;
- case 3:
- Process_UpdatePeroid(app_gyro_Fix_process,1000);
- hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_CALIBRATION, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF);
- Process_SetHoldOn(app_gyro_Fix_process,0);
- state =0;
- // SEGGER_RTT_printf(0,"====OUT app_gyro_Fix_process\r\n");
- break;
- }
-
- }
- void app_math_Init(void)
- {
- Process_Start(100,"app_math_DailyStep_Process",app_math_DailyStep_Process);
- // Process_Start(10,"app_math_TimerCounter",app_math_TimerCounter);
-
- hal_ser_imu_data_update_notify(app_math_TimerCounter);
-
- Process_Start(500,"app_gyro_Fix_process",app_gyro_Fix_process);
- Process_Start(1000,"app_math_Hour",app_math_Hour_process);
- }
|