|
|
@@ -241,6 +241,8 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
|
|
|
|
|
|
static int16_t press_wait;
|
|
|
static int16_t shake_acc_wait;
|
|
|
+
|
|
|
+ static int16_t continue_up_min_val;
|
|
|
|
|
|
|
|
|
static int acc_zero_count;
|
|
|
@@ -291,6 +293,7 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
|
|
|
|
|
|
int16_t acc_max_val_z , acc_min_val_z ;
|
|
|
|
|
|
+ //寻找加速度窗口的最大值、最小值
|
|
|
find_acc_max_and_min_val(acc_x_window, WINDOW_SIZE - SAMPLE_C * 10, WINDOW_SIZE, SAMPLE_C , &acc_max_val_x, &acc_min_val_x);
|
|
|
|
|
|
find_acc_max_and_min_val(acc_y_window, WINDOW_SIZE - SAMPLE_C * 10, WINDOW_SIZE, SAMPLE_C, &acc_max_val_y, &acc_min_val_y);
|
|
|
@@ -312,6 +315,7 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
|
|
|
//当还处于后脚压力上升的余热中，降低前脚的判断阈值
|
|
|
int front_up_trend;
|
|
|
|
|
|
+ //当back_up_wait 后脚跟压力上升的等待时间 大于0时候， 阈值调低为2000，否则为3000
|
|
|
if (back_up_wait > 0)
|
|
|
{
|
|
|
front_up_trend = isLongTimeUpTrend(front_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &front_min_val);
|
|
|
@@ -320,24 +324,47 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
|
|
|
{
|
|
|
front_up_trend = isLongTimeUpTrend(front_mag_window, WINDOW_SIZE, 3000, &front_min_val);
|
|
|
}
|
|
|
+
|
|
|
+ //缓慢踩地
|
|
|
+ if(front_mag_window[WINDOW_SIZE - 1] - front_mag_window[WINDOW_SIZE - 1 - SAMPLE_C] >-200)
|
|
|
+ {
|
|
|
+ if(continue_up_min_val > front_mag_window[WINDOW_SIZE - 1])
|
|
|
+ {
|
|
|
+ continue_up_min_val = front_mag_window[WINDOW_SIZE - 1];
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ continue_up_min_val = front_mag_window[WINDOW_SIZE - 1];
|
|
|
+ }
|
|
|
+
|
|
|
+ if(front_mag_window[WINDOW_SIZE - 1] - continue_up_min_val > 2000)
|
|
|
+ {
|
|
|
+ front_up_trend = 1;
|
|
|
+ }
|
|
|
|
|
|
+ //front_down_trend 为 1时候， 意味着前脚掌压力下降
|
|
|
int front_down_trend = isLongTimeDownTrend(front_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &front_max_val);
|
|
|
|
|
|
+ // 自定义前后脚压力均上升，则重置压力等待时间，press_up_wait用于判断产生剧烈抖动时，则为触地
|
|
|
+ // special_press_up_wait 用于判断延续触地逻辑，使其阈值变大(解决触地剧烈抖动的情况)
|
|
|
if ((back_mag_window[WINDOW_SIZE - 1] - back_min_val > 2000 && back_max_val - back_mag_window[WINDOW_SIZE - 1] < 1000) ||
|
|
|
- (front_mag_window[WINDOW_SIZE - 1] - front_min_val > 2000 && front_max_val - front_mag_window[WINDOW_SIZE - 1] < 1000))
|
|
|
+ (front_mag_window[WINDOW_SIZE - 1] - front_min_val > 2000 && front_max_val - front_mag_window[WINDOW_SIZE - 1] < 1000) ||
|
|
|
+ (front_mag_window[WINDOW_SIZE - 1] - continue_up_min_val > 2000))
|
|
|
{
|
|
|
press_up_wait = 20 * SAMPLE_C;
|
|
|
|
|
|
special_press_up_wait = 20 * SAMPLE_C;
|
|
|
}
|
|
|
|
|
|
+ //发现前脚压力向下降的时候，置special_press_up_wait为0，避免离地时候，该倒计时还要生效
|
|
|
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)
|
|
|
{
|
|
|
front_up_trend = 1;
|
