|
@@ -249,6 +249,8 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
|
|
|
static int16_t back_up_wait;
|
|
|
static int16_t press_up_wait;
|
|
|
|
|
|
+ static int16_t special_press_up_wait;
|
|
|
+
|
|
|
|
|
|
static int16_t front_min_val;
|
|
|
static int16_t back_min_val;
|
|
@@ -325,7 +327,16 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
|
|
|
(front_mag_window[WINDOW_SIZE - 1] - front_min_val > 2000 && front_max_val - front_mag_window[WINDOW_SIZE - 1] < 1000))
|
|
|
{
|
|
|
press_up_wait = 20 * SAMPLE_C;
|
|
|
+
|
|
|
+ special_press_up_wait = 20 * SAMPLE_C;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(front_down_trend == 1)
|
|
|
+ {
|
|
|
+ special_press_up_wait = 0;
|
|
|
}
|
|
|
+
|
|
|
+
|
|
|
|
|
|
if (back_mag_window[WINDOW_SIZE - 1] - back_min_val > 2000 && front_mag_window[WINDOW_SIZE - 1] - front_min_val > 2000)
|
|
|
{
|
|
@@ -421,12 +432,9 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
|
|
|
}
|
|
|
|
|
|
}
|
|
|
- //利用后脚跟压力和抖动来判断
|
|
|
- if(press_up_wait > 0 && press_up_wait < 11 * SAMPLE_C && var_acc_temp > 3.0f)
|
|
|
+ //利用特殊的压力上升等待时间和抖动来判断
|
|
|
+ if(special_press_up_wait > 0 && var_acc_temp > 3.0f)
|
|
|
{
|
|
|
-
|
|
|
- press_up_wait = 20 * SAMPLE_C;
|
|
|
-
|
|
|
*front_zero = 1;
|
|
|
|
|
|
if (front_zero_tmp == 0)
|
|
@@ -435,8 +443,6 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-
|
|
|
-
|
|
|
//累加平稳加速度的数目,如果超过一秒,则强制归位触地状态
|
|
|
if(acc_max_val_x - acc_min_val_x < 103 && acc_max_val_y - acc_min_val_y < 103 && acc_max_val_z - acc_min_val_z < 103)
|
|
|
{
|
|
@@ -463,15 +469,19 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
|
|
|
|
|
|
//利用加速度延续
|
|
|
int16_t continue_thresh = 103;
|
|
|
-
|
|
|
- if (shake_acc_wait > 0)
|
|
|
+
|
|
|
+ if(special_press_up_wait > 0 )
|
|
|
{
|
|
|
continue_thresh = 1030;
|
|
|
- }else if(MT_GetState())
|
|
|
- {
|
|
|
- //震动,导致加速度数据产生抖动
|
|
|
- continue_thresh = 250;
|
|
|
}
|
|
|
+// if (shake_acc_wait > 0)
|
|
|
+// {
|
|
|
+// continue_thresh = 1030;
|
|
|
+// }else if(MT_GetState())
|
|
|
+// {
|
|
|
+// //震动,导致加速度数据产生抖动
|
|
|
+// continue_thresh = 250;
|
|
|
+// }
|
|
|
|
|
|
|
|
|
if(last_front_zupt == 1
|
|
@@ -521,6 +531,11 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
|
|
|
{
|
|
|
front_zupt_wait = 20 * SAMPLE_C;
|
|
|
}
|
|
|
+
|
|
|
+ if(special_press_up_wait > 0)
|
|
|
+ {
|
|
|
+ special_press_up_wait --;
|
|
|
+ }
|
|
|
|
|
|
|
|
|
|