#include "detect_step_by_mag.h" #include "math.h" #include "stdlib.h" #include "system.h" #include "nrf_delay.h" uint16_t mag_sqrt =0; uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3]) { static int16_t FKEY=0; static int16_t BKEY=0; static int FrontLiftPoint; static int FrontDropPoint; static int BackLiftPoint; static int BackDropPoint; static uint8_t mag_index_front = 0; static uint8_t mag_index_back = 0; static int16_t Rcounter=0; static int16_t Ucounter=0;//离地计时器 static int16_t ShoeTouchFlag=1; static uint8_t acc_index = 0; static int calculate_flag = 0; int32_t mag_temp_front[3]; int32_t mag_temp_back[3]; static int16_t accZ_buf[4]; static int mag_buf_front[3]; static int mag_buf_back[3]; int32_t mag_sqrt_front =0; int32_t mag_sqrt_back =0; int16_t step=0; static int FrontLiftDistance; static int BackLiftDistance; //DEBUG_LOG("Enter RealTimeStep!\n"); //Tcounter++; for(int i = 0; i < 3; i ++) { mag_temp_front[i] = (int32_t) (front[i]); } mag_sqrt_front = abs(mag_temp_front[2]); for(int i = 0; i < 3; i ++) { mag_temp_back[i] = (int32_t) (back[i]); } mag_sqrt_back = abs(mag_temp_back[2]); /*读入磁力计数据*/ if(mag_index_front >= 3) { mag_buf_front[0] = mag_buf_front[1]; mag_buf_front[1] = mag_buf_front[2]; mag_index_front = 2; calculate_flag = 1; } mag_buf_front[mag_index_front++] = mag_sqrt_front;//前脚掌数据 if(mag_index_back >= 3) { mag_buf_back[0] = mag_buf_back[1]; mag_buf_back[1] = mag_buf_back[2]; mag_index_back = 2; calculate_flag = 1; } mag_buf_back[mag_index_back++] = mag_sqrt_back;//后脚掌数据 /*读入xyZ轴加速度数据*/ if(acc_index >= 3) { accZ_buf[0] = accZ_buf[1]; accZ_buf[1] = accZ_buf[2]; acc_index = 2; calculate_flag = 1; } accZ_buf[acc_index++] = -acc[2]; if(calculate_flag==1) { // DEBUG_LOG("mag_front3==:%d...\n",mag_buf_front[2]); // DEBUG_LOG("mag_back3==:%d...\n",mag_buf_back[2]); // DEBUG_LOG("FrontLiftPoint==:%d...\n",FrontLiftPoint); // DEBUG_LOG("BackLiftPoint==:%d...\n",BackLiftPoint); // DEBUG_LOG("FrontDropPoint==:%d...\n",FrontDropPoint); // DEBUG_LOG("BackDropPoint==:%d...\n",BackDropPoint); // DEBUG_LOG("FrontLiftDistance==:%d...\n",FrontLiftDistance); // DEBUG_LOG("BackLiftDistance==:%d...\n",BackLiftDistance); /*不断更新前后脚掌磁力计的上升起始点或下降起始点*/ if(mag_buf_front[2] - mag_buf_front[1]>=0) { if(FKEY==0) { FrontDropPoint = mag_buf_front[2]; } else { if(mag_buf_front[2]>FrontDropPoint) { FrontDropPoint = mag_buf_front[2]; } } } else { if(FKEY==0)//FKEY为前脚波形状态变量，1代表波形进行中，0代表波形结束。该语句意思是前脚掌磁力计不处于波形进行阶段时，如常更新上升起点。 { FrontLiftPoint = mag_buf_front[2]; } else { ;//当波形进行中，不改变初始上升起点，有助于过滤杂波。 } } if(mag_buf_back[2] - mag_buf_back[1]>=0) { if(BKEY==0) { BackDropPoint = mag_buf_back[2]; } else { if(mag_buf_back[2]>BackDropPoint) { BackDropPoint = mag_buf_back[2]; } } } else { if(BKEY==0)//BKEY为后脚波形状态变量，1代表波形进行中，0代表波形结束。该语句意思是后脚掌磁力计不处于波形进行阶段时，如常更新上升起点 { BackLiftPoint = mag_buf_back[2]; } else { ; } } /*判断前后脚掌的状态*/ if(mag_buf_front[2] - FrontLiftPoint>2200) { FKEY = 1; if(mag_buf_front[2] - FrontLiftPoint>FrontLiftDistance) { FrontLiftDistance = mag_buf_front[2] - FrontLiftPoint; } } if(mag_buf_back[2] - BackLiftPoint>2200) { BKEY = 1; if(mag_buf_back[2] - BackLiftPoint>BackLiftDistance) { BackLiftDistance = mag_buf_back[2] - BackLiftPoint; } } /*判断是否满足FKEY=0和BKEY=0，即是否满足一个升降波形*/ if((mag_buf_front[2] - FrontLiftPoint<=1500)||((FrontDropPoint-mag_buf_front[2]>0.4*FrontLiftDistance)&&(abs(mag_buf_front[2]-mag_buf_front[1])<400))) //当磁力计回降到小于上升起点+1000的数值时，FKEY归零，预示前脚波形结束。 { FKEY=0; FrontLiftDistance = 2200; } if((mag_buf_back[2] - BackLiftPoint<=1500)||((BackDropPoint-mag_buf_back[2]>0.4*BackLiftDistance)&&(abs(mag_buf_back[2]-mag_buf_back[1])<400)))//当磁力计回降到小于上升起点+1000的数值时，BKEY归零，预示后脚波形结束。 { BKEY=0; BackLiftDistance = 2200; } /*跳出死循环*/ if(FrontLiftPoint==0) //当磁力计回降到小于上升起点+1000的数值时，FKEY归零，预示前脚波形结束。 { FKEY=0; } if(BackLiftPoint==0)//当磁力计回降到小于上升起点+1000的数值时，BKEY归零，预示后脚波形结束。 { BKEY=0; } /*判断鞋子触地或离地*/ if(ShoeTouchFlag==0) { if(Ucounter<300) { Ucounter++; } Rcounter=0; if(((FKEY==1)||(BKEY==1))&&(Ucounter>=8)) { step=1; ShoeTouchFlag=1; //DEBUG_LOG("You have one step!\n"); } else { step=0; ShoeTouchFlag=0; } } else { step = 0; if(Rcounter<300) { Rcounter++; } Ucounter=0; if((Rcounter>=6)&&(FKEY==0)&&((abs(accZ_buf[0]-2000)>300)||(abs(accZ_buf[1]-2000)>300)||(abs(accZ_buf[2]-2000)>300))&&(BKEY==0)) { ShoeTouchFlag=0; } else { ShoeTouchFlag=1; } } } else { step=0; FrontLiftPoint = mag_buf_front[2]; FrontDropPoint = mag_buf_front[2]; BackLiftPoint = mag_buf_back[2]; BackDropPoint = mag_buf_back[2]; } return step; } uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z) { //利用前脚磁力计以及后脚来判断 static int32_t mag_front[5] = {40000}; static int16_t acc_front_z[5]; static int32_t mag_cur_min = 40000; static int time_count; static int last_floor_status = 1; static int press_up_wait_acc_time = 0; int on_floor = 0; memcpy(mag_front, mag_front + 1, 4 * sizeof(int32_t)); memcpy(acc_front_z, acc_front_z + 1, 4 * sizeof(int16_t)); mag_front[4] = abs(mag[2]); acc_front_z[4] = acc_z; if (time_count > 4) { int16_t max_acc_z = acc_front_z[0], min_acc_z = acc_front_z[0]; int32_t max_mag_front = mag_front[0]; int32_t min_mag_front = mag_front[0]; int16_t max_mag_index = 0, min_mag_index = 0; for (int i = 1; i < 5; i++) { if (min_mag_front > mag_front[i]) { min_mag_front = mag_front[i]; min_mag_index = i; } if (max_mag_front < mag_front[i]) { max_mag_front = mag_front[i]; max_mag_index = i; } if (max_acc_z < acc_front_z[i]) { max_acc_z = acc_front_z[i]; } if (min_acc_z > acc_front_z[i]) { min_acc_z = acc_front_z[i]; } } //寻找上升沿 if (min_mag_index < max_mag_index && max_mag_front > min_mag_front + 100) { mag_cur_min = mag_cur_min > min_mag_front ? min_mag_front : mag_cur_min; } else if (min_mag_index > max_mag_index && max_mag_front > min_mag_front + 100) { mag_cur_min = 40000; } if (mag_front[4] > mag_cur_min + 1000) { on_floor = 1; } if (last_floor_status == 1 && abs(acc_front_z[4] - acc_front_z[3]) < 105) { on_floor = 1; } if (on_floor) { press_up_wait_acc_time = 3; //拖延3个触地状态 } else if (press_up_wait_acc_time > 0) { on_floor = 1; } if (press_up_wait_acc_time > 0) { press_up_wait_acc_time--; } } else { time_count++; } uint8_t step = 0; if(last_floor_status == 0 && on_floor == 1) { step = 1; } last_floor_status = on_floor; return step; }