#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>1100) { FKEY = 1; if(mag_buf_front[2] - FrontLiftPoint>FrontLiftDistance) { FrontLiftDistance = mag_buf_front[2] - FrontLiftPoint; } } if(mag_buf_back[2] - BackLiftPoint>1100) { 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<=750)||((FrontDropPoint-mag_buf_front[2]>0.4*FrontLiftDistance)&&(abs(mag_buf_front[2]-mag_buf_front[1])<100))) //当磁力计回降到小于上升起点+1000的数值时，FKEY归零，预示前脚波形结束。 { FKEY=0; FrontLiftDistance = 1100; } if((mag_buf_back[2] - BackLiftPoint<=750)||((BackDropPoint-mag_buf_back[2]>0.4*BackLiftDistance)&&(abs(mag_buf_back[2]-mag_buf_back[1])<100)))//当磁力计回降到小于上升起点+1000的数值时，BKEY归零，预示后脚波形结束。 { BKEY=0; BackLiftDistance = 1100; } /*跳出死循环*/ 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 up_count = 0; static int max_acc_z = 0; static int min_acc_z = 0; uint8_t 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) { //寻找上升沿 // DEBUG_LOG("mag_front[3] : %d, mag_front[4] : %d , acc_front_z[4] : %d \n", mag_front[3], mag_front[4], acc_front_z[4]); if (mag_front[4] >= mag_front[3]) { // DEBUG_LOG("mag_front[4] >= mag_front[3] \n"); mag_cur_min = mag_cur_min > mag_front[3] ? mag_front[3] : mag_cur_min; // DEBUG_LOG("mag_cur_min : %d, mag_front[4] : %d \n", mag_cur_min, mag_front[4]); } else { // DEBUG_LOG("else ------------------------> mag_front[4] >= mag_front[3] \n"); mag_cur_min = 40000; } if (mag_front[4] > mag_cur_min + 500) { up_count ++; } else { up_count = 0; } // DEBUG_LOG("up_count : %d \n", up_count); if(up_count > 0) { on_floor = 1; } if (last_floor_status == 1 && abs(acc_front_z[4] - acc_front_z[3]) < 203) { 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--; // } // DEBUG_LOG("on_floor : %d \n", on_floor); } else { time_count++; } uint8_t step = 0; if(last_floor_status == 0 && on_floor == 1 && max_acc_z - min_acc_z > 512) { step = 1; DEBUG_LOG("step + 1, mag_cur_min : %d ,mag_front[4]: %d \n", mag_cur_min, mag_front[4]) } if(on_floor == 0) { max_acc_z = max_acc_z > acc_z ? max_acc_z : acc_z; min_acc_z = min_acc_z < acc_z ? min_acc_z : acc_z; } else { max_acc_z = acc_z; min_acc_z = acc_z; } last_floor_status = on_floor; return step; }