//#include "detect_zero_vel.h" //#include "hal_mt.h" //#include "hal_flash.h" //#define MAG_THRESHHOLD 1000.f //int front_zero_tmp = 0; //int back_zero_tmp = 0; //void start_cal_step(int16_t front[3], int16_t back[3], int16_t acc[3]) //{ // int16_t front_zero = 0; // int16_t back_zero = 0; // int16_t acc_zero = 0; // // detect_zero_vel( front, back, acc, &front_zero, &back_zero, &acc_zero); //} //void cal_step(int16_t front_zero, int16_t back_zero) //{ // static int step_calm_down; // // if(front_zero || back_zero) // { // if(step_calm_down == 0) // { // mFlash.mStep.stepCur[0] ++; // } // step_calm_down = 10; // } // // if(step_calm_down > 0) // { // step_calm_down --; // } //} // void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3], // int16_t *front_zero, int16_t *back_zero, int16_t *acc_zero) //{ // static float front_mag_window[WINDOW_SIZE]; // // static float back_mag_window[WINDOW_SIZE]; // // static float acc_x_window[WINDOW_SIZE]; // static float acc_y_window[WINDOW_SIZE]; // static float acc_z_window[WINDOW_SIZE]; // // static int last_front_zupt; // // static int last_back_zupt; // // static int front_zupt_wait; // // static int back_zupt_wait; // // // float front_val = sqrt((float) (front[0] * front[0] + front[1] * front[1] + front[2] * front[2])); // // float back_val = sqrt((float) (back[0] * back[0] + back[1] * back[1] + back[2] * back[2])); // // // //滑动窗口更新数据 // // memcpy(front_mag_window, front_mag_window + 1, (WINDOW_SIZE - 1) * sizeof(float)); // // memcpy(back_mag_window, back_mag_window + 1, (WINDOW_SIZE - 1) * sizeof(float)); // // memcpy(acc_x_window, acc_x_window + 1, (WINDOW_SIZE - 1) * sizeof(float)); // // memcpy(acc_y_window, acc_y_window + 1, (WINDOW_SIZE - 1) * sizeof(float)); // // memcpy(acc_z_window, acc_z_window + 1, (WINDOW_SIZE - 1) * sizeof(float)); // // front_mag_window[WINDOW_SIZE - 1] = front_val; // // back_mag_window[WINDOW_SIZE - 1] = back_val; // // acc_x_window[WINDOW_SIZE - 1] = acc[0] / 2048.f; // acc_y_window[WINDOW_SIZE - 1] = acc[1] / 2048.f; // acc_z_window[WINDOW_SIZE - 1] = acc[2] / 2048.f; // float front_max_val = front_val; // float front_min_val = front_val; // // uint16_t front_max_index = WINDOW_SIZE - 1; // uint16_t front_min_index = WINDOW_SIZE - 1; // // float back_max_val = back_val; // float back_min_val = back_val; // // uint16_t back_max_index = WINDOW_SIZE - 1; // uint16_t back_min_index = WINDOW_SIZE - 1; // // for(int i = 0 ; i < WINDOW_SIZE; i++ ) // { // if(front_mag_window[i] > front_max_val) // { // front_max_val = front_mag_window[i]; // front_max_index = i; // } // // if(front_mag_window[i] < front_min_val) // { // front_min_val = front_mag_window[i]; // front_min_index = i; // } // // if(back_mag_window[i] > back_max_val) // { // back_max_val = back_mag_window[i]; // back_max_index = i; // } // // if(back_mag_window[i] < back_min_val) // { // back_min_val = back_mag_window[i]; // back_min_index = i; // } // // } // // /* // * 计算稳定的状态 // */ // // float acc_max_val_x = acc_x_window[WINDOW_SIZE - 1] ; // float acc_min_val_x = acc_x_window[WINDOW_SIZE - 1] ; // // float acc_max_val_y = acc_y_window[WINDOW_SIZE - 1] ; // float acc_min_val_y = acc_y_window[WINDOW_SIZE - 1] ; // // float acc_max_val_z = acc_z_window[WINDOW_SIZE - 1]; // float acc_min_val_z = acc_z_window[WINDOW_SIZE - 1]; // // for(int i = WINDOW_SIZE - 5 ; i < WINDOW_SIZE; i++ ) // { // if(acc_x_window[i] < acc_min_val_x) // { // acc_min_val_x = acc_x_window[i]; // } // // if(acc_x_window[i] > acc_max_val_x) // { // acc_max_val_x = acc_x_window[i]; // } // // if(acc_y_window[i] < acc_min_val_y) // { // acc_min_val_y = acc_y_window[i]; // } // // if(acc_y_window[i] > acc_max_val_y) // { // acc_max_val_y = acc_y_window[i]; // } // // if(acc_z_window[i] < acc_min_val_z) // { // acc_min_val_z = acc_z_window[i]; // } // // if(acc_z_window[i] > acc_max_val_z) // { // acc_max_val_z = acc_z_window[i]; // } // // } // // if(front_max_index > front_min_index && front_max_val > front_min_val + MAG_THRESHHOLD) // { // front_zero_tmp = 1; // // front_zupt_wait = 10; // } // else if(front_max_index < front_min_index && front_max_val > front_min_val + MAG_THRESHHOLD) // { // front_zero_tmp = 0; // } // else if(front_zero_tmp == 1) // { // front_zero_tmp = 2; // } // // // /* // * 判断后脚往下压 // */ // // if(back_max_index > back_min_index && back_max_val > back_min_val + MAG_THRESHHOLD) // { // back_zero_tmp = 1; // // back_zupt_wait = 10; // } // // else if(back_max_index < back_min_index && back_max_val > back_min_val + MAG_THRESHHOLD) // { // back_zero_tmp = 0; // } // else if(back_zero_tmp == 1) // { // back_zero_tmp = 2; // } // // /* // * 判断仅前踮 或 后垫 // */ // if(back_zero_tmp == 2 && front_zero_tmp == 0) // { // back_zero_tmp = 0; // } // // if(front_zero_tmp == 2 && back_zero_tmp == 0) // { // front_zero_tmp = 0; // } // // //延迟等待稳定状态 //// if(*front_zero == 0 && front_zupt_wait > 0 && acc_max_val - acc_min_val < 0.08f) //// { //// *front_zero = 1; //// } // // if(front_zero_tmp == 1) // { // *front_zero = 1; // // } // else if(front_zupt_wait > 0 && acc_max_val_x - acc_min_val_x < 0.07f && acc_max_val_y - acc_min_val_y < 0.07f && acc_max_val_z - acc_min_val_z < 0.07f) // { // *front_zero = 1; // front_zupt_wait = 20; // } // else // { // *front_zero = 0; // } // // //// if(*back_zero == 0 && back_zupt_wait > 0 && acc_max_val - acc_min_val < 0.08f) //// { //// *back_zero = 1; //// } // // if(back_zero_tmp == 1) // { // *back_zero = 1; // } // else if(back_zupt_wait > 0 && acc_max_val_x - acc_min_val_x < 0.07f && acc_max_val_y - acc_min_val_y < 0.07f && acc_max_val_z - acc_min_val_z < 0.07f ) // { // *back_zero = 1; // // back_zupt_wait = 20; // } // else // { // *back_zero = 0; // } // // // for(int i = 0 ; i < WINDOW_SIZE - 5; i++ ) // { // if(acc_x_window[i] < acc_min_val_x) // { // acc_min_val_x = acc_x_window[i]; // } // // if(acc_x_window[i] > acc_max_val_x) // { // acc_max_val_x = acc_x_window[i]; // } // // if(acc_y_window[i] < acc_min_val_y) // { // acc_min_val_y = acc_y_window[i]; // } // // if(acc_y_window[i] > acc_max_val_y) // { // acc_max_val_y = acc_y_window[i]; // } // // if(acc_z_window[i] < acc_min_val_z) // { // acc_min_val_z = acc_z_window[i]; // } // // if(acc_z_window[i] > acc_max_val_z) // { // acc_max_val_z = acc_z_window[i]; // } // // } // // // if(acc_max_val_x - acc_min_val_x < 0.05f && acc_max_val_y - acc_min_val_y < 0.05f && acc_max_val_z - acc_min_val_z < 0.05f) // { // *acc_zero = 1; // } // else // { // *acc_zero = 0; // } // // // if(*front_zero == 0) // { // *front_zero = *back_zero; // } // // if(front_zupt_wait > 0) // { // front_zupt_wait --; // } // // if(back_zupt_wait > 0) // { // back_zupt_wait --; // } // // // //利用加速度延续 // if((last_front_zupt == 1|| last_back_zupt == 1) // && fabsf(acc_x_window[WINDOW_SIZE - 2] - acc_x_window[WINDOW_SIZE - 1]) < 0.05f // && fabsf(acc_y_window[WINDOW_SIZE - 2] - acc_y_window[WINDOW_SIZE - 1]) < 0.05f // && fabsf(acc_z_window[WINDOW_SIZE - 2] - acc_z_window[WINDOW_SIZE - 1]) < 0.05f) // { // *front_zero = 1; // *back_zero = 1; // // front_zero_tmp = 1; // back_zero_tmp = 1; // // front_zupt_wait = 15; // back_zupt_wait = 15; // } // // // last_front_zupt = front_zero_tmp; // // last_back_zupt = back_zero_tmp; // // /* // * 直接用这个来记步好了 // */ // cal_step(*front_zero, *back_zero ); //} #include "detect_zero_vel.h" #include "hal_mt.h" #include "app_flash.h" #include "hal_imu.h" #define MAG_THRESHHOLD 2000.f int front_zero_tmp = 0; int back_zero_tmp = 0; void start_cal_step(int16_t front[3], int16_t back[3], int16_t acc[3]) { int16_t front_zero = 0; int16_t back_zero = 0; int16_t acc_zero = 0; SEGGER_RTT_printf(0,"detect_zero_vel( front, back, acc, &front_zero, &back_zero, &acc_zero); \n"); detect_zero_vel( front, back, acc, &front_zero, &back_zero, &acc_zero); } void cal_step(int16_t front_zero, int16_t back_zero, float acc_x, float acc_y, float acc_z) { static int step_calm_down; static float acc_min[3]; static float acc_max[3]; if(!(front_zero || back_zero)) { acc_max[2] = 1.0f; acc_min[2] = 1.0f; if(acc_x > acc_max[0]) { acc_max[0] = acc_x; } if(acc_y > acc_max[1]) { acc_max[1] = acc_y; } if(acc_z > acc_max[2]) { acc_max[2] = acc_z; } if(acc_x < acc_min[0]) { acc_min[0] = acc_x; } if(acc_y < acc_min[1]) { acc_min[1] = acc_y; } if(acc_z < acc_min[2]) { acc_min[2] = acc_z; } } if(front_zero || back_zero) { if(step_calm_down == 0 && (acc_max[0] - acc_min[0] > 0.8f || acc_max[1] - acc_min[1] > 0.8f || acc_max[2] - acc_min[2] > 0.8f)) { mFlash.mStep.stepCur[0] ++; } //假设20ms相见为极限 step_calm_down = 5; acc_max[0] = acc_x; acc_max[1] = acc_y; acc_max[2] = acc_z; acc_min[0] = acc_x; acc_min[1] = acc_y; acc_min[2] = acc_z; } if(step_calm_down > 0) { step_calm_down --; } } void setLongTimeUpTrend(int max_index, int min_index, float max_val, float min_val, float *longTime_maxVal, float *longTime_minVal) { if(max_index > min_index && max_val > min_val + 800.f) { *longTime_maxVal = *longTime_maxVal > max_val ? *longTime_maxVal : max_val; *longTime_minVal = *longTime_minVal < min_val ? *longTime_minVal : min_val; } else if(max_index < min_index && max_val > min_val + 800.f) { *longTime_maxVal = 0.0f; *longTime_minVal = 50000.0f; } } void setLongTimeDownTrend(int max_index, int min_index, float max_val, float min_val, float *longTime_maxVal, float *longTime_minVal) { if(max_index > min_index && max_val > min_val + 800.f) { *longTime_maxVal = 0.0f; *longTime_minVal = 50000.0f; } else if(max_index < min_index && max_val > min_val + 800.f) { *longTime_maxVal = *longTime_maxVal > max_val ? *longTime_maxVal : max_val; *longTime_minVal = *longTime_minVal < min_val ? *longTime_minVal : min_val; } } int press_down_front(float *press_buff) { static float max_val = 0.0f; if(press_buff[0] - press_buff[4] > 100.0f) { max_val = press_buff[0] > max_val ? press_buff[0] : max_val; } else { max_val = 0; } if(max_val > press_buff[4] + 2000.0f) { return 1; } return 0; } int press_down_back(float *press_buff) { static float max_val = 0.0f; if(press_buff[0] - press_buff[4] > 100.0f) { max_val = press_buff[0] > max_val ? press_buff[0] : max_val; } else { max_val = 0; } if(max_val > press_buff[4] + 2000.0f) { return 1; } return 0; } //2020/01/02 //长趋势判断压力上升 int isLongTimeUpTrend(float *mag_window, int length, float up_threshhold, float *min_val) { float prev_window_sum = (mag_window[0] + mag_window[1] + mag_window[2]) * 0.333f; float back_window_sum = (mag_window[length - 1] + mag_window[length - 2] + mag_window[length - 3]) * 0.333f; float back_window_sum1 = (mag_window[length - 4] + mag_window[length - 5] + mag_window[length - 6]) * 0.333f; float back_window_sum2 = (mag_window[length - 7] + mag_window[length - 8] + mag_window[length - 9]) * 0.333f; if(back_window_sum > prev_window_sum + 50.f || back_window_sum1 > prev_window_sum + 50.f || back_window_sum2 > prev_window_sum + 50.f ) { //触发判断， 先替换掉最小值 float min_val_temp = mag_window[0]; min_val_temp = min_val_temp < mag_window[1] ? min_val_temp : mag_window[1]; min_val_temp = min_val_temp < mag_window[2] ? min_val_temp : mag_window[2]; *min_val = min_val_temp < *min_val ? min_val_temp : *min_val; float max_val_temp = mag_window[length - 6]; for(int i = length - 5; i < length; i++) { if(max_val_temp < mag_window[i]) { max_val_temp = mag_window[i]; } } if(max_val_temp > *min_val + 2000.0f) { return 1; } } else { *min_val = mag_window[length - 1]; } //上下沿判断 int max_index = 0; int min_index = 0; float window_max_val = mag_window[0]; float window_min_val = mag_window[0]; for(int i = 1; i < length; i++) { if(window_max_val < mag_window[i]) { window_max_val = mag_window[i]; max_index = i; } if(window_min_val > mag_window[i]) { window_min_val = mag_window[i]; min_index = i; } } if((max_index > min_index && window_max_val - window_min_val > 2000.0f) || (mag_window[length - 1] - window_min_val > 2000.0f)) { return 1; } return 0; } int isLongTimeDownTrend(float *mag_window, int length, float up_threshhold, float *max_val) { float prev_window_sum = (mag_window[0] + mag_window[1] + mag_window[2]) * 0.333f; float back_window_sum = (mag_window[length - 1] + mag_window[length - 2] + mag_window[length - 3]) * 0.333f; float back_window_sum1 = (mag_window[length - 4] + mag_window[length - 5] + mag_window[length - 6]) * 0.333f; float back_window_sum2 = (mag_window[length - 7] + mag_window[length - 8] + mag_window[length - 9]) * 0.333f; if(prev_window_sum > back_window_sum + 50.f || prev_window_sum > back_window_sum1 + 50.f || prev_window_sum > back_window_sum2 + 50.f ) { float max_val_temp = mag_window[0]; max_val_temp = max_val_temp > mag_window[1] ? max_val_temp : mag_window[1]; max_val_temp = max_val_temp > mag_window[2] ? max_val_temp : mag_window[2]; *max_val = *max_val > max_val_temp ? *max_val : max_val_temp; float min_val_temp = mag_window[length - 6]; for(int i = length - 5; i < length; i++) { if(min_val_temp > mag_window[i]) { min_val_temp = mag_window[i]; } } if(*max_val > min_val_temp + 2000.0f) { return 1; } } else { *max_val = mag_window[length - 1]; } //上下沿判断 int max_index = 0; int min_index = 0; float window_max_val = mag_window[0]; float window_min_val = mag_window[0]; for(int i = 1; i < length; i++) { if(window_max_val < mag_window[i]) { window_max_val = mag_window[i]; max_index = i; } if(window_min_val > mag_window[i]) { window_min_val = mag_window[i]; min_index = i; } } if(max_index < min_index && window_max_val - window_min_val > + 2000.0f) { return 1; } return 0; } void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3], int16_t *front_zero, int16_t *back_zero, int16_t *acc_zero) { static float front_mag_window[WINDOW_SIZE]; static float back_mag_window[WINDOW_SIZE]; static float acc_x_window[WINDOW_SIZE]; static float acc_y_window[WINDOW_SIZE]; static float acc_z_window[WINDOW_SIZE]; static int last_front_zupt; static int last_back_zupt; static int front_zupt_wait; static int back_zupt_wait; static int front_calm_down_wait; static int acc_zero_count; static float back_mag_max_up = 0.0f; static float back_mag_min_up = 50000.f;//磁力计均方和大小小于32000 static float back_mag_max_down = 0.0f; static float back_mag_min_down = 50000.f;//磁力计均方和大小小于32000 static float front_mag_max_down = 0.0f; static float front_mag_min_down = 50000.f;//磁力计均方和大小小于32000 static float front_min_val; static float back_min_val; static float front_max_val; static float back_max_val; float front_val = (float) (abs(front[2])); float back_val = (float) (abs(back[2])); //滑动窗口更新数据 memcpy(front_mag_window, front_mag_window + 1, (WINDOW_SIZE - 1) * sizeof(float)); memcpy(back_mag_window, back_mag_window + 1, (WINDOW_SIZE - 1) * sizeof(float)); memcpy(acc_x_window, acc_x_window + 1, (WINDOW_SIZE - 1) * sizeof(float)); memcpy(acc_y_window, acc_y_window + 1, (WINDOW_SIZE - 1) * sizeof(float)); memcpy(acc_z_window, acc_z_window + 1, (WINDOW_SIZE - 1) * sizeof(float)); front_mag_window[WINDOW_SIZE - 1] = front_val; back_mag_window[WINDOW_SIZE - 1] = back_val; acc_x_window[WINDOW_SIZE - 1] = acc[0] / 2048.f; acc_y_window[WINDOW_SIZE - 1] = acc[1] / 2048.f; acc_z_window[WINDOW_SIZE - 1] = acc[2] / 2048.f; // float front_max_val = front_val; // float front_min_val = front_val; // // uint16_t front_max_index = WINDOW_SIZE - 1; // uint16_t front_min_index = WINDOW_SIZE - 1; // // float back_max_val = back_val; // float back_min_val = back_val; // // uint16_t back_max_index = WINDOW_SIZE - 1; // uint16_t back_min_index = WINDOW_SIZE - 1; // for(int i = 0 ; i < WINDOW_SIZE; i++ ) // { // if(front_mag_window[i] > front_max_val) // { // front_max_val = front_mag_window[i]; // front_max_index = i; // } // // if(front_mag_window[i] < front_min_val) // { // front_min_val = front_mag_window[i]; // front_min_index = i; // } // // if(back_mag_window[i] > back_max_val) // { // back_max_val = back_mag_window[i]; // back_max_index = i; // } // // if(back_mag_window[i] < back_min_val) // { // back_min_val = back_mag_window[i]; // back_min_index = i; // } // // } /* * 计算稳定的状态 */ float acc_max_val_x = acc_x_window[WINDOW_SIZE - 1] ; float acc_min_val_x = acc_x_window[WINDOW_SIZE - 1] ; float acc_max_val_y = acc_y_window[WINDOW_SIZE - 1] ; float acc_min_val_y = acc_y_window[WINDOW_SIZE - 1] ; float acc_max_val_z = acc_z_window[WINDOW_SIZE - 1]; float acc_min_val_z = acc_z_window[WINDOW_SIZE - 1]; for(int i = WINDOW_SIZE - 10 ; i < WINDOW_SIZE; i++ ) { if(acc_x_window[i] < acc_min_val_x) { acc_min_val_x = acc_x_window[i]; } if(acc_x_window[i] > acc_max_val_x) { acc_max_val_x = acc_x_window[i]; } if(acc_y_window[i] < acc_min_val_y) { acc_min_val_y = acc_y_window[i]; } if(acc_y_window[i] > acc_max_val_y) { acc_max_val_y = acc_y_window[i]; } if(acc_z_window[i] < acc_min_val_z) { acc_min_val_z = acc_z_window[i]; } if(acc_z_window[i] > acc_max_val_z) { acc_max_val_z = acc_z_window[i]; } } int front_up_trend = isLongTimeUpTrend(front_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &front_min_val); int front_down_trend = isLongTimeDownTrend(front_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &front_max_val); int back_up_trend = isLongTimeUpTrend(back_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &back_min_val); int back_down_trend = isLongTimeDownTrend(back_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &back_max_val); if(front_up_trend) { front_zero_tmp = 1; } else if(front_down_trend) { front_zero_tmp = 0; } else if(front_zero_tmp == 1) { front_zero_tmp = 2; } //当触发了压力上升及平稳的时候，需要用加速度倒计时延续状态 if(front_zero_tmp > 0) { front_zupt_wait = 20; } /* * 判断后脚往下压 */ if(back_up_trend) { back_zero_tmp = 1; } else if(back_down_trend) { back_zero_tmp = 0; } else if(back_zero_tmp == 1) { back_zero_tmp = 2; } //当触发了压力上升及平稳的时候，需要用加速度倒计时延续状态 if(back_zero_tmp > 0) { back_zupt_wait = 20; } /*过滤一下类似于穿拖鞋，后鞋垫与磁力计传感器之间的距离在振荡*/ if( back_zero_tmp == 2 && front_down_trend) { back_zero_tmp = 0; } if(front_zero_tmp == 2 && back_down_trend) { front_zero_tmp = 0; } /* * 规避先脚掌下地，后脚掌在下地；接着是后脚先离地，再前脚离地的情况 */ // setLongTimeUpTrend(back_max_index, back_min_index, back_val, back_min_val, &back_mag_max_up, &back_mag_min_up); // // setLongTimeDownTrend(back_max_index, back_min_index, back_val, back_min_val, &back_mag_max_down, &back_mag_min_down); // // setLongTimeDownTrend(front_max_index, front_min_index, front_val, front_min_val, &front_mag_max_down, &front_mag_min_down); if(back_zero_tmp == 2 && back_mag_max_down > back_mag_min_down + 3000.f) { back_zero_tmp = 0; back_mag_max_down = 0.0f; back_mag_min_down = 50000.0f; } if(front_zero_tmp == 2 && front_mag_max_down > front_mag_min_down + 3000.f) { front_zero_tmp = 0; front_mag_max_down = 0.0f; front_mag_min_down = 50000.0f; } if((front_zupt_wait > 0 || back_zupt_wait > 0) && acc_max_val_x - acc_min_val_x < 0.1f && acc_max_val_y - acc_min_val_y < 0.1f && acc_max_val_z - acc_min_val_z < 0.1f) { *back_zero = 1; *front_zero = 1; } /* * 判断仅前踮 或 后垫， IMU往前移动，就不需要这个了 */ // if(back_zero_tmp == 2 && front_zero_tmp == 0) // { // back_zero_tmp = 0; // } // // if(front_zero_tmp == 2 && back_zero_tmp == 0) // { // front_zero_tmp = 0; // } //延迟等待稳定状态 // if(*front_zero == 0 && front_zupt_wait > 0 && acc_max_val - acc_min_val < 0.08f) // { // *front_zero = 1; // } if(front_zero_tmp > 0) { /* * 如IMU放在前脚掌，就不用front_calm_down_wait == 0的判断了 */ *front_zero = 1; } else { *front_zero = 0; } // if(*back_zero == 0 && back_zupt_wait > 0 && acc_max_val - acc_min_val < 0.08f) // { // *back_zero = 1; // } if(back_zero_tmp > 0) { *back_zero = 1; } else { *back_zero = 0; } /* * 会出现缓慢离地，但是没有检测到离地，补个逻辑，后面整段检测触地逻辑需要重写 */ if(acc_max_val_x - acc_min_val_x > 0.25f || acc_max_val_y - acc_min_val_y > 0.25f || acc_max_val_z - acc_min_val_z > 0.25f) { if(press_down_front(front_mag_window + 10) || press_down_back(back_mag_window + 10)) { *back_zero = 0; *front_zero = 0; front_zero_tmp = 0; back_zero_tmp = 0; } } for(int i = 0 ; i < WINDOW_SIZE - 5; i++ ) { if(acc_x_window[i] < acc_min_val_x) { acc_min_val_x = acc_x_window[i]; } if(acc_x_window[i] > acc_max_val_x) { acc_max_val_x = acc_x_window[i]; } if(acc_y_window[i] < acc_min_val_y) { acc_min_val_y = acc_y_window[i]; } if(acc_y_window[i] > acc_max_val_y) { acc_max_val_y = acc_y_window[i]; } if(acc_z_window[i] < acc_min_val_z) { acc_min_val_z = acc_z_window[i]; } if(acc_z_window[i] > acc_max_val_z) { acc_max_val_z = acc_z_window[i]; } } if(acc_max_val_x - acc_min_val_x < 0.03f && acc_max_val_y - acc_min_val_y < 0.03f && acc_max_val_z - acc_min_val_z < 0.03f ) { *acc_zero = 1; acc_zero_count ++; } else { *acc_zero = 0; acc_zero_count = 0; } // /* // * 强制大于1秒的零速均视为触地 // */ if(acc_zero_count > 100) { *front_zero = 1; *back_zero = 1; front_zero_tmp = 1; back_zero_tmp = 1; } //利用加速度延续 if((last_front_zupt == 1|| last_back_zupt == 1) && fabsf(acc_x_window[WINDOW_SIZE - 2] - acc_x_window[WINDOW_SIZE - 1]) < 0.1f && fabsf(acc_y_window[WINDOW_SIZE - 2] - acc_y_window[WINDOW_SIZE - 1]) < 0.1f && fabsf(acc_z_window[WINDOW_SIZE - 2] - acc_z_window[WINDOW_SIZE - 1]) < 0.1f) { *front_zero = 1; *back_zero = 1; } if(front_zupt_wait > 0) { front_zupt_wait --; } if(back_zupt_wait > 0) { back_zupt_wait --; } if(*front_zero || *back_zero) { front_zupt_wait = 20; back_zupt_wait = 20; } last_front_zupt = *front_zero; last_back_zupt = *back_zero; /* * 直接用这个来记步好了 */ cal_step(*front_zero, *back_zero, acc_x_window[WINDOW_SIZE - 1], acc_y_window[WINDOW_SIZE - 1], acc_z_window[WINDOW_SIZE - 1]); }