|
|
@@ -8,7 +8,7 @@
|
|
|
|
|
|
//#include "hal_imu.h"
|
|
|
|
|
|
-#define MAG_THRESHHOLD 500.f
|
|
|
+#define MAG_THRESHHOLD 500
|
|
|
#define SAMPLE_C 4
|
|
|
|
|
|
int16_t front_zero_tmp = 0;
|
|
|
@@ -165,7 +165,7 @@ int isLongTimeDownTrend(int16_t *mag_window, int16_t length, int16_t down_thresh
|
|
|
void find_acc_max_and_min_val(int16_t*src, int16_t start_index, int16_t end_index, int16_t sample, int16_t* max_val, int16_t*min_val)
|
|
|
{
|
|
|
*max_val = src[end_index - 1];
|
|
|
- *min_val = src[start_index];
|
|
|
+ *min_val = src[end_index - 1];
|
|
|
|
|
|
for(int i = end_index - 1; i >= start_index; i -= sample)
|
|
|
{
|
|
|
@@ -318,11 +318,11 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
|
|
|
//当back_up_wait 后脚跟压力上升的等待时间 大于0时候， 阈值调低为2000，否则为3000
|
|
|
if (back_up_wait > 0)
|
|
|
{
|
|
|
- front_up_trend = isLongTimeUpTrend(front_mag_window, WINDOW_SIZE, 1000, &front_min_val);
|
|
|
+ front_up_trend = isLongTimeUpTrend(front_mag_window, WINDOW_SIZE, MAG_THRESHHOLD * 2, &front_min_val);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- front_up_trend = isLongTimeUpTrend(front_mag_window, WINDOW_SIZE, 2000, &front_min_val);
|
|
|
+ front_up_trend = isLongTimeUpTrend(front_mag_window, WINDOW_SIZE, MAG_THRESHHOLD * 4, &front_min_val);
|
|
|
}
|
|
|
|
|
|
//缓慢踩地
|
|
|
@@ -365,7 +365,7 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
|
|
|
|
|
|
|
|
|
//当前脚掌压力上升，后脚压力上升，则意味全脚掌着地，那就意味是真正的触地
|
|
|
- if (back_mag_window[WINDOW_SIZE - 1] - back_min_val > 500 && front_mag_window[WINDOW_SIZE - 1] - front_min_val > 500)
|
|
|
+ if (back_mag_window[WINDOW_SIZE - 1] - back_min_val > MAG_THRESHHOLD * 2 && front_mag_window[WINDOW_SIZE - 1] - front_min_val > MAG_THRESHHOLD * 2)
|
|
|
{
|
|
|
front_up_trend = 1;
|
|
|
}
|
liang