#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= 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= 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= 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); }