|
|
@@ -0,0 +1,1451 @@
|
|
|
+
|
|
|
+#include "press_down_detect.h"
|
|
|
+
|
|
|
+#define down_thresh 2000.0f
|
|
|
+
|
|
|
+int back_jump_stage = 0;
|
|
|
+
|
|
|
+int cancel_down = 0;
|
|
|
+int virus_flag = 0;
|
|
|
+
|
|
|
+float real_front_min_left = 50000.f;
|
|
|
+float real_front_min_right = 50000.f;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void detect_up_trend(float *mag_up_min, float *mag_window, int window_size)
|
|
|
+{
|
|
|
+ //1、寻找最大值
|
|
|
+ float max_val = mag_window[0];
|
|
|
+
|
|
|
+ for(int i = 1; i < window_size; i++)
|
|
|
+ {
|
|
|
+ if(max_val < mag_window[i])
|
|
|
+ {
|
|
|
+ max_val = mag_window[i];
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(max_val - mag_window[0] > 100.0f)
|
|
|
+ {
|
|
|
+ if(mag_window[0] < *mag_up_min)
|
|
|
+ {
|
|
|
+ *mag_up_min = mag_window[0];
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ *mag_up_min = 40000.f;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void avoid_down_apeear_dual_foot_run(float *mag_window,float *another_mag_window, int window_size, float *trend_min, float *trend_max, float *down_max, float *down_min)
|
|
|
+{
|
|
|
+ float max_val = mag_window[0];
|
|
|
+
|
|
|
+ for(int i = 1; i < window_size; i++)
|
|
|
+ {
|
|
|
+ if(max_val < mag_window[i])
|
|
|
+ {
|
|
|
+ max_val = mag_window[i];
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(max_val - mag_window[0] > 200.0f)
|
|
|
+ {
|
|
|
+ if(*trend_min > mag_window[0])
|
|
|
+ {
|
|
|
+ *trend_min = mag_window[0];
|
|
|
+ }
|
|
|
+
|
|
|
+ if( *trend_max < max_val)
|
|
|
+ {
|
|
|
+ *trend_max = max_val;
|
|
|
+ }
|
|
|
+
|
|
|
+ *down_max = another_mag_window[window_size - 1];
|
|
|
+ *down_min = another_mag_window[window_size - 1];
|
|
|
+ }
|
|
|
+ else if(mag_window[window_size - 1] - max_val < -200.0f)
|
|
|
+ {
|
|
|
+ if(mag_window[window_size - 1] < *trend_max)
|
|
|
+ {
|
|
|
+ *trend_max = mag_window[window_size - 1];
|
|
|
+ }
|
|
|
+
|
|
|
+ if(max_val > *trend_min)
|
|
|
+ {
|
|
|
+ *trend_min = max_val;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(*down_max < another_mag_window[window_size - 1])
|
|
|
+ {
|
|
|
+ *down_max = another_mag_window[window_size - 1];
|
|
|
+ }
|
|
|
+
|
|
|
+ if(*down_min > another_mag_window[window_size - 1])
|
|
|
+ {
|
|
|
+ *down_min = another_mag_window[window_size - 1];
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+int station_acc(float* acc_buff, int buff_size)
|
|
|
+{
|
|
|
+ float max_val = acc_buff[0];
|
|
|
+ float min_val = acc_buff[0];
|
|
|
+
|
|
|
+ for(int i = 1; i < buff_size; i++)
|
|
|
+ {
|
|
|
+ if(max_val < acc_buff[i] )
|
|
|
+ {
|
|
|
+ max_val = acc_buff[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ if(min_val > acc_buff[i] )
|
|
|
+ {
|
|
|
+ min_val = acc_buff[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ if(max_val - min_val < 0.3f)
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+int check_dual_front_press_down(float *left_mag_window, float *right_mag_window, int length)
|
|
|
+{
|
|
|
+ float left_max_val = left_mag_window[0];
|
|
|
+
|
|
|
+ float right_max_val = right_mag_window[0];
|
|
|
+
|
|
|
+ float left_min_val = left_mag_window[0];
|
|
|
+
|
|
|
+ float right_min_val = right_mag_window[0];
|
|
|
+
|
|
|
+ int left_max_index = 0; int right_max_index = 0;
|
|
|
+
|
|
|
+ int left_min_index = 0; int right_min_index = 0;
|
|
|
+
|
|
|
+ for(int i = 1; i < length; i++)
|
|
|
+ {
|
|
|
+ if(left_max_val < left_mag_window[i])
|
|
|
+ {
|
|
|
+ left_max_val = left_mag_window[i];
|
|
|
+
|
|
|
+ left_max_index = i;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(right_max_val < right_mag_window[i])
|
|
|
+ {
|
|
|
+ right_max_val = right_mag_window[i];
|
|
|
+
|
|
|
+ right_max_index = i;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(left_min_val > left_mag_window[i])
|
|
|
+ {
|
|
|
+ left_min_val = left_mag_window[i];
|
|
|
+
|
|
|
+ left_min_index = i;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(right_min_val > right_mag_window[i])
|
|
|
+ {
|
|
|
+ right_min_val = right_mag_window[i];
|
|
|
+
|
|
|
+ right_min_index = i;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(right_min_index > right_max_index && left_min_index > left_max_index
|
|
|
+ && left_max_val > left_min_val + 2000.0f && right_max_val > right_min_val + 2000.0f)
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+int back_mag_different_trend(float *left_back_buff, float * right_back_buff, int length)
|
|
|
+{
|
|
|
+ int left_max_index = 0; int right_max_index = 0;
|
|
|
+
|
|
|
+ int left_min_index = 0; int right_min_index = 0;
|
|
|
+
|
|
|
+ float left_max_val = left_back_buff[0]; float right_max_val = right_back_buff[0];
|
|
|
+
|
|
|
+ float left_min_val = left_back_buff[0]; float right_min_val = right_back_buff[0];
|
|
|
+
|
|
|
+ for(int i = 0; i < length; i++)
|
|
|
+ {
|
|
|
+ if(left_max_val < left_back_buff[i])
|
|
|
+ {
|
|
|
+ left_max_val = left_back_buff[i];
|
|
|
+ left_max_index = i;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(left_min_val > left_back_buff[i])
|
|
|
+ {
|
|
|
+ left_min_val = left_back_buff[i];
|
|
|
+ left_min_index = i;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(right_max_val < right_back_buff[i])
|
|
|
+ {
|
|
|
+ right_max_val = right_back_buff[i];
|
|
|
+ right_max_index = i;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(right_min_val > right_back_buff[i])
|
|
|
+ {
|
|
|
+ right_min_val = right_back_buff[i];
|
|
|
+ right_min_index = i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(left_max_index > left_min_index && left_max_val > left_min_val + 2000.0f
|
|
|
+ && right_max_index < right_min_index && right_max_val > right_min_val + 2000.0f)
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(left_max_index < left_min_index && left_max_val > left_min_val + 2000.0f
|
|
|
+ && right_max_index > right_min_index && right_max_val > right_min_val + 2000.0f)
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+}
|
|
|
+//快速排序
|
|
|
+void quick_sork(float arr[], int start, int end)
|
|
|
+{
|
|
|
+ if(start < end)
|
|
|
+ {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ int i = start;
|
|
|
+ int j = end;
|
|
|
+ float baseval = arr[start];
|
|
|
+
|
|
|
+ while(i < j)
|
|
|
+ {
|
|
|
+ while(i < j && arr[j] >= baseval)
|
|
|
+ {
|
|
|
+ j--;
|
|
|
+ }
|
|
|
+ if(i < j)
|
|
|
+ {
|
|
|
+ arr[i] = arr[j];
|
|
|
+ i++;
|
|
|
+ }
|
|
|
+
|
|
|
+ while(i < j && arr[i] < baseval)
|
|
|
+ {
|
|
|
+ i++;
|
|
|
+ }
|
|
|
+ if(i < j)
|
|
|
+ {
|
|
|
+ arr[j] = arr[i];
|
|
|
+ j--;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ arr[i] = baseval;
|
|
|
+ quick_sork(arr, start, i-1);
|
|
|
+ quick_sork(arr, i+1, end);
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+float mid_val(float *mag_buff)
|
|
|
+{
|
|
|
+ static float src[10];
|
|
|
+
|
|
|
+
|
|
|
+ memcpy(src, mag_buff, 10 * sizeof(float));
|
|
|
+
|
|
|
+ //快速排序
|
|
|
+ quick_sork(mag_buff, 0 , 9);
|
|
|
+
|
|
|
+ return 0.5f *(mag_buff[4] + mag_buff[5]);
|
|
|
+}
|
|
|
+
|
|
|
+int special_stay_by_back_mag_down(float *left_back_mag, float *right_back_mag, int left_zupt, int right_zupt)
|
|
|
+{
|
|
|
+ static float last_left_back_mag_val = 0.0f;
|
|
|
+ static float last_right_back_mag_val = 0.0f;
|
|
|
+
|
|
|
+ static float max_left_back;
|
|
|
+ static float min_left_back;
|
|
|
+
|
|
|
+ static float max_right_back;
|
|
|
+ static float min_right_back;
|
|
|
+
|
|
|
+ float cur_left_back_mag_val = mid_val(left_back_mag);
|
|
|
+ float cur_right_back_mag_val = mid_val(right_back_mag);
|
|
|
+
|
|
|
+ if(last_left_back_mag_val < cur_left_back_mag_val - 0.00001f)
|
|
|
+ {
|
|
|
+ max_left_back = cur_left_back_mag_val;
|
|
|
+ min_left_back = cur_left_back_mag_val;
|
|
|
+
|
|
|
+ max_right_back = cur_right_back_mag_val;
|
|
|
+ min_right_back = cur_right_back_mag_val;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ min_left_back = cur_left_back_mag_val;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(last_right_back_mag_val < cur_right_back_mag_val - 0.00001f)
|
|
|
+ {
|
|
|
+ max_left_back = cur_left_back_mag_val;
|
|
|
+ min_left_back = cur_left_back_mag_val;
|
|
|
+
|
|
|
+ max_right_back = cur_right_back_mag_val;
|
|
|
+ min_right_back = cur_right_back_mag_val;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ min_right_back = cur_right_back_mag_val;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ last_left_back_mag_val = cur_left_back_mag_val;
|
|
|
+ last_right_back_mag_val = cur_right_back_mag_val;
|
|
|
+
|
|
|
+ if(max_left_back > min_left_back +2000.f && max_right_back > min_right_back +2000.f
|
|
|
+ && left_zupt && right_zupt)
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+int special_stay_by_long_time_down_process(float *front_mag_left_buff, float *front_mag_right_buff, float *back_mag_left_buff, float *back_mag_right_buff)
|
|
|
+{
|
|
|
+ int left_index = 9;
|
|
|
+ int right_index = 9;
|
|
|
+
|
|
|
+ /*
|
|
|
+ * 寻找第一个递增的点
|
|
|
+ */
|
|
|
+ for(left_index = 9; left_index > 0 ; left_index --)
|
|
|
+ {
|
|
|
+ if(front_mag_left_buff[left_index] < front_mag_left_buff[left_index - 1])
|
|
|
+ {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ for(right_index = 9; right_index > 0 ; right_index --)
|
|
|
+ {
|
|
|
+ if(front_mag_right_buff[right_index] < front_mag_right_buff[right_index - 1])
|
|
|
+ {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ float right_max_front_val = front_mag_right_buff[0];
|
|
|
+ float right_min_front_val = front_mag_right_buff[0];
|
|
|
+
|
|
|
+ for(int i = 0; i < 10; i++)
|
|
|
+ {
|
|
|
+ right_max_front_val = right_max_front_val > front_mag_right_buff[i] ? right_max_front_val : front_mag_right_buff[i];
|
|
|
+
|
|
|
+ right_min_front_val = right_min_front_val < front_mag_right_buff[i] ? right_min_front_val : front_mag_right_buff[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ /*
|
|
|
+ * 有些特殊的点，左脚前掌往下压，但是右脚前掌没啥变化, 只靠后脚掌数据递降来判断
|
|
|
+ */
|
|
|
+ if(left_index < 6 && front_mag_left_buff[9] - front_mag_left_buff[left_index] > 2000.0f
|
|
|
+ && right_max_front_val - right_min_front_val < 500.f && right_min_front_val > 15000.f
|
|
|
+ && back_mag_right_buff[left_index] - back_mag_right_buff[9] > 2000.f)
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ /*
|
|
|
+ * 左脚前掌往下压，右脚前掌往下压
|
|
|
+ */
|
|
|
+
|
|
|
+ if(left_index < 6 && front_mag_left_buff[9] - front_mag_left_buff[left_index] > 2000.0f
|
|
|
+ && right_index < 6 && front_mag_right_buff[9] - front_mag_right_buff[right_index] > 2000.0f)
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+int special_stay_by_long_time_down(float* front_mag_left, float* front_mag_right, int length)
|
|
|
+{
|
|
|
+
|
|
|
+ float front_mag_left_max = front_mag_left[length - 1];
|
|
|
+
|
|
|
+ float front_mag_right_max = front_mag_right[length - 1];
|
|
|
+
|
|
|
+ int up_count = 0;
|
|
|
+
|
|
|
+ for(int i = length - 1; i > 1; i--)
|
|
|
+ {
|
|
|
+
|
|
|
+ if(front_mag_left[i] - front_mag_left[i-1] > 10.f && front_mag_right[i] - front_mag_right[i-1] > 10.f)
|
|
|
+ {
|
|
|
+ up_count ++;
|
|
|
+
|
|
|
+ if (front_mag_left[i] > front_mag_left_max)
|
|
|
+ {
|
|
|
+ front_mag_left_max = front_mag_left[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ if (front_mag_right[i] > front_mag_right_max)
|
|
|
+ {
|
|
|
+ front_mag_right_max = front_mag_right[i];
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(up_count > 7 && front_mag_left_max - front_mag_left[i] > 2000.f && front_mag_right_max - front_mag_right[i] >2000.f
|
|
|
+ && front_mag_left_max - front_mag_left[length - 1] < 1000.f && front_mag_right_max - front_mag_right[length - 1] < 1000.f)
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+int stage_class_up_or_down(float *mag_buff, int length)
|
|
|
+{
|
|
|
+ if(((mag_buff[length - 10] + mag_buff[length - 9] + mag_buff[length - 8])*0.334f + 50.f < (mag_buff[length - 6] + mag_buff[length - 5] + mag_buff[length - 4])*0.334f)
|
|
|
+ || ((mag_buff[length - 10] + mag_buff[length - 9] + mag_buff[length - 8])*0.334f + 50.f < (mag_buff[length - 3] + mag_buff[length - 2] + mag_buff[length - 1])*0.334f))
|
|
|
+
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(((mag_buff[length - 10] + mag_buff[length - 9] + mag_buff[length - 8])*0.334f + 100.f > (mag_buff[length - 6] + mag_buff[length - 5] + mag_buff[length - 4])*0.334f)
|
|
|
+ || ((mag_buff[length - 10] + mag_buff[length - 9] + mag_buff[length - 8])*0.334f + 100.f > (mag_buff[length - 3] + mag_buff[length - 2] + mag_buff[length - 1])*0.334f))
|
|
|
+
|
|
|
+ {
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+int back_down_by_long_time_big_trend(float* left_mag, float* right_mag, int length)
|
|
|
+{
|
|
|
+ float left_down_sum = 0.0f;
|
|
|
+ float left_up_sum = 0.0f;
|
|
|
+
|
|
|
+ float right_down_sum = 0.0f;
|
|
|
+ float right_up_sum = 0.0f;
|
|
|
+
|
|
|
+ //先寻找到最小值下标
|
|
|
+ int left_mag_min_index = length - 1;
|
|
|
+ int right_mag_min_index = length - 1;
|
|
|
+
|
|
|
+ float left_mag_min_val = left_mag[length - 1];
|
|
|
+ float right_mag_min_val = right_mag[length - 1];
|
|
|
+
|
|
|
+ int left_mag_max_index = length - 1;
|
|
|
+ int right_mag_max_index = length - 1;
|
|
|
+
|
|
|
+ float left_mag_max_val = left_mag[length - 1];
|
|
|
+ float right_mag_max_val = right_mag[length - 1];
|
|
|
+
|
|
|
+
|
|
|
+ for(int i = length - 1; i > 0; i--)
|
|
|
+ {
|
|
|
+ if(left_mag_min_val > left_mag[i])
|
|
|
+ {
|
|
|
+ left_mag_min_val = left_mag[i];
|
|
|
+ left_mag_min_index = i;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(right_mag_min_val > right_mag[i])
|
|
|
+ {
|
|
|
+ right_mag_min_val = right_mag[i];
|
|
|
+ right_mag_min_index = i;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(left_mag_max_val < left_mag[i])
|
|
|
+ {
|
|
|
+ left_mag_max_val = left_mag[i];
|
|
|
+ left_mag_max_index = i;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(right_mag_max_val < right_mag[i])
|
|
|
+ {
|
|
|
+ right_mag_max_val = right_mag[i];
|
|
|
+ right_mag_max_index = i;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ int index = left_mag_min_index > right_mag_min_index ? left_mag_min_index : right_mag_min_index;
|
|
|
+
|
|
|
+ //寻找差异值
|
|
|
+ float left_diff;
|
|
|
+
|
|
|
+ float right_diff;
|
|
|
+
|
|
|
+ for(int i = index; i < length - 1; i++)
|
|
|
+ {
|
|
|
+ left_diff = left_mag[i] - left_mag[i + 1];
|
|
|
+
|
|
|
+ if(left_diff > 0.0f)
|
|
|
+ {
|
|
|
+ left_down_sum += left_diff;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ left_up_sum -= left_diff;
|
|
|
+ }
|
|
|
+
|
|
|
+ right_diff = right_mag[i] - right_mag[i + 1];
|
|
|
+
|
|
|
+ if(right_diff > 0.0f)
|
|
|
+ {
|
|
|
+ right_down_sum += right_diff;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ right_up_sum -= right_diff;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ if(left_mag_min_val + 2000.f < left_mag[length - 1] && right_mag_min_val + 2000.f < right_mag[length - 1]
|
|
|
+ && right_down_sum < 0.25f *right_up_sum && left_down_sum < 0.25f * left_up_sum)
|
|
|
+ //if(left_mag_min_val + 2000.f < left_mag[length - 1] && right_mag_min_val + 2000.f < right_mag[length - 1] )
|
|
|
+
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+}
|
|
|
+int back_down_by_long_time(float* front_mag_left, float* front_mag_right, float* back_mag_left, float* back_mag_right, int length)
|
|
|
+{
|
|
|
+ static float max_front_left_val;
|
|
|
+ static float max_front_right_val;
|
|
|
+
|
|
|
+ static float min_back_left_val;
|
|
|
+ static float min_back_right_val;
|
|
|
+
|
|
|
+ static int up_count;
|
|
|
+
|
|
|
+ up_count ++;
|
|
|
+
|
|
|
+ static int continue_up_count = 0;
|
|
|
+
|
|
|
+ if(up_count == 0)
|
|
|
+ {
|
|
|
+ max_front_left_val = front_mag_left[length - 1];
|
|
|
+ max_front_right_val = front_mag_right[length - 1];
|
|
|
+
|
|
|
+ min_back_left_val = back_mag_left[length - 1];
|
|
|
+ min_back_right_val = back_mag_right[length - 1];
|
|
|
+ }
|
|
|
+
|
|
|
+// if(stage_class_up_or_down(front_mag_left, 15) == -1 && stage_class_up_or_down(front_mag_right, 15) == -1)
|
|
|
+//// if( stage_class_up_or_down(back_mag_left, 15) == 1 && stage_class_up_or_down(back_mag_right, 15) == 1 )
|
|
|
+// {
|
|
|
+// float tmp = (front_mag_left[0] + front_mag_left[1] + front_mag_left[2]) *0.334f;
|
|
|
+//
|
|
|
+// max_front_left_val = tmp > max_front_left_val ? tmp : max_front_left_val;
|
|
|
+//
|
|
|
+// tmp = (front_mag_right[0] + front_mag_right[1] + front_mag_right[2]) *0.334f;
|
|
|
+//
|
|
|
+// max_front_right_val = tmp > max_front_right_val ? tmp : max_front_left_val;
|
|
|
+//
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// max_front_left_val = front_mag_left[length - 1];
|
|
|
+// max_front_right_val = front_mag_right[length - 1];
|
|
|
+//
|
|
|
+// }
|
|
|
+
|
|
|
+ if(stage_class_up_or_down(back_mag_left, length) == 1 && stage_class_up_or_down(back_mag_right, length) == 1)
|
|
|
+ {
|
|
|
+ float tmp = (back_mag_left[0] + back_mag_left[1] + back_mag_left[2]) *0.334f;
|
|
|
+
|
|
|
+ min_back_left_val = tmp > min_back_left_val ? min_back_left_val : tmp;
|
|
|
+
|
|
|
+ tmp = (back_mag_right[0] + back_mag_right[1] + back_mag_right[2]) *0.334f;
|
|
|
+
|
|
|
+ min_back_right_val = tmp > min_back_right_val ? min_back_right_val : tmp;
|
|
|
+
|
|
|
+ continue_up_count ++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ min_back_left_val = back_mag_left[length - 1];
|
|
|
+ min_back_right_val = back_mag_right[length - 1];
|
|
|
+
|
|
|
+ continue_up_count = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(back_mag_left[length - 1] - min_back_left_val > 2000.f && back_mag_right[length - 1] - min_back_right_val > 2000.f && continue_up_count > 10)
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(back_mag_left[length - 1] - min_back_left_val > 4000.f && back_mag_right[length - 1] - min_back_right_val > 4000.f)
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+#define BIG_WINDOW_SIZE 20
|
|
|
+
|
|
|
+int press_down_detect_new(int index, float front_mag_left, float back_mag_left,
|
|
|
+ float front_mag_right, float back_mag_right,int left_zupt, int right_zupt,
|
|
|
+ float left_acc_x,float left_acc_y, float left_acc_z, float right_acc_x, float right_acc_y, float right_acc_z,
|
|
|
+ int *front_down, int *back_down)
|
|
|
+{
|
|
|
+ static float front_mag_norm_left[PRESS_COUNT_MAX];
|
|
|
+ static float front_mag_norm_right[PRESS_COUNT_MAX];
|
|
|
+
|
|
|
+ static float back_mag_norm_left[PRESS_COUNT_MAX];
|
|
|
+ static float back_mag_norm_right[PRESS_COUNT_MAX];
|
|
|
+
|
|
|
+ static float front_mag_left_big_buff[BIG_WINDOW_SIZE];
|
|
|
+ static float front_mag_right_big_buff[BIG_WINDOW_SIZE];
|
|
|
+
|
|
|
+ static float back_mag_left_big_buff[BIG_WINDOW_SIZE];
|
|
|
+ static float back_mag_right_big_buff[BIG_WINDOW_SIZE];
|
|
|
+
|
|
|
+
|
|
|
+ static float acc_norm_left[10];
|
|
|
+ static float acc_norm_right[10];
|
|
|
+
|
|
|
+ static float left_front_up_min = 40000.0f;
|
|
|
+ static float left_back_up_min = 40000.0f;
|
|
|
+
|
|
|
+ static float right_front_up_min = 40000.0f;
|
|
|
+ static float right_back_up_min = 40000.0f;
|
|
|
+
|
|
|
+
|
|
|
+ memcpy(front_mag_norm_left, front_mag_norm_left + 1, 4 * sizeof(float));
|
|
|
+ memcpy(front_mag_norm_right, front_mag_norm_right + 1, 4 * sizeof(float));
|
|
|
+
|
|
|
+ memcpy(front_mag_left_big_buff, front_mag_left_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));
|
|
|
+ memcpy(front_mag_right_big_buff, front_mag_right_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));
|
|
|
+
|
|
|
+ memcpy(back_mag_norm_left, back_mag_norm_left + 1, 4 * sizeof(float));
|
|
|
+ memcpy(back_mag_norm_right, back_mag_norm_right + 1, 4 * sizeof(float));
|
|
|
+
|
|
|
+ memcpy(acc_norm_left, acc_norm_left + 1, 9 * sizeof(float));
|
|
|
+ memcpy(acc_norm_right, acc_norm_right + 1, 9 * sizeof(float));
|
|
|
+
|
|
|
+ memcpy(back_mag_left_big_buff, back_mag_left_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));
|
|
|
+ memcpy(back_mag_right_big_buff, back_mag_right_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));
|
|
|
+
|
|
|
+ front_mag_norm_left[PRESS_COUNT_MAX - 1] = front_mag_left;
|
|
|
+ front_mag_norm_right[PRESS_COUNT_MAX - 1] = front_mag_right;
|
|
|
+
|
|
|
+ front_mag_left_big_buff[(BIG_WINDOW_SIZE - 1)] = front_mag_left;
|
|
|
+ front_mag_right_big_buff[(BIG_WINDOW_SIZE - 1)] = front_mag_right;
|
|
|
+
|
|
|
+ back_mag_norm_left[PRESS_COUNT_MAX - 1] = back_mag_left;
|
|
|
+ back_mag_norm_right[PRESS_COUNT_MAX - 1] = back_mag_right;
|
|
|
+
|
|
|
+
|
|
|
+ back_mag_left_big_buff[(BIG_WINDOW_SIZE - 1)] = back_mag_left;
|
|
|
+ back_mag_right_big_buff[(BIG_WINDOW_SIZE - 1)] = back_mag_right;
|
|
|
+
|
|
|
+ acc_norm_left[9] = sqrt(left_acc_x * left_acc_x + left_acc_y * left_acc_y + left_acc_z * left_acc_z);
|
|
|
+ acc_norm_right[9] = sqrt(right_acc_x * right_acc_x + right_acc_y * right_acc_y + right_acc_z * right_acc_z);
|
|
|
+
|
|
|
+
|
|
|
+ detect_up_trend(&left_front_up_min, front_mag_norm_left, PRESS_COUNT_MAX);
|
|
|
+ detect_up_trend(&right_front_up_min, front_mag_norm_right, PRESS_COUNT_MAX);
|
|
|
+
|
|
|
+ detect_up_trend(&left_back_up_min, back_mag_norm_left, PRESS_COUNT_MAX);
|
|
|
+ detect_up_trend(&right_back_up_min, back_mag_norm_right, PRESS_COUNT_MAX);
|
|
|
+
|
|
|
+ if(left_zupt == 0)
|
|
|
+ {
|
|
|
+ left_front_up_min = front_mag_left;
|
|
|
+ left_back_up_min = back_mag_left;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(right_zupt == 0)
|
|
|
+ {
|
|
|
+ right_front_up_min = front_mag_right;
|
|
|
+ right_back_up_min = back_mag_right;
|
|
|
+ }
|
|
|
+ //当检测到前脚掌的压力下降同时较快，将不会触发后脚掌下蹲的判断
|
|
|
+ if(check_dual_front_press_down(front_mag_left_big_buff, front_mag_right_big_buff, BIG_WINDOW_SIZE))
|
|
|
+ {
|
|
|
+ left_back_up_min = back_mag_left;
|
|
|
+ right_back_up_min = back_mag_right;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(front_mag_left - left_front_up_min > 3000.f && front_mag_right - right_front_up_min > 3000.f)
|
|
|
+ {
|
|
|
+ *front_down = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(back_mag_different_trend(back_mag_left_big_buff, back_mag_right_big_buff, BIG_WINDOW_SIZE))
|
|
|
+ {
|
|
|
+ *front_down = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ //新增一个长时间判断蹲的，2020/01/08
|
|
|
+ if (special_stay_by_long_time_down(front_mag_left_big_buff, front_mag_right_big_buff, BIG_WINDOW_SIZE))
|
|
|
+ {
|
|
|
+ *front_down = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(back_mag_left - left_back_up_min > 2000.0f && back_mag_right - right_back_up_min > 2000.0f )
|
|
|
+ {
|
|
|
+ *back_down = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(!station_acc(acc_norm_left, 10) || !station_acc(acc_norm_right, 10))
|
|
|
+ {
|
|
|
+ *front_down = 0;
|
|
|
+ *back_down = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+// if(back_down_by_long_time(front_mag_left_big_buff, front_mag_right_big_buff, back_mag_left_big_buff, back_mag_right_big_buff, BIG_WINDOW_SIZE))
|
|
|
+// {
|
|
|
+// *back_down = 1;
|
|
|
+// }
|
|
|
+
|
|
|
+ //2022/01/17
|
|
|
+ /* if(back_down_by_long_time_big_trend(back_mag_left_big_buff, back_mag_right_big_buff, BIG_WINDOW_SIZE))
|
|
|
+
|
|
|
+ {
|
|
|
+ *back_down = 1;
|
|
|
+ }*/
|
|
|
+
|
|
|
+ if (special_stay_by_long_time_down(back_mag_left_big_buff, back_mag_right_big_buff, BIG_WINDOW_SIZE))
|
|
|
+ {
|
|
|
+ *back_down = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+short pos_jump_detect(int *h_pos, int *s_pos, int left_zupt, int right_zupt)
|
|
|
+{
|
|
|
+ static int last_h_z;
|
|
|
+ static int last_s_z;
|
|
|
+ static int left_up;
|
|
|
+ static int right_up;
|
|
|
+
|
|
|
+ static int left_up_count;
|
|
|
+ static int right_up_count;
|
|
|
+
|
|
|
+
|
|
|
+ static int left_zupt_count;
|
|
|
+ static int right_zupt_count;
|
|
|
+
|
|
|
+ if(left_zupt)
|
|
|
+ {
|
|
|
+ left_zupt_count = 20;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ left_zupt_count --;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(right_zupt)
|
|
|
+ {
|
|
|
+ right_zupt_count = 20;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ right_zupt_count --;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(h_pos[2] - last_h_z > 0 && h_pos[2]> 0)
|
|
|
+ {
|
|
|
+ left_up = 1;
|
|
|
+ }
|
|
|
+ else if((h_pos[2] - last_h_z < 0) || h_pos[2] <= 0)
|
|
|
+ {
|
|
|
+ left_up = -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(left_up == 1)
|
|
|
+ {
|
|
|
+ left_up_count ++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ left_up_count = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(s_pos[2] - last_s_z > 0 && s_pos[2] > 0)
|
|
|
+ {
|
|
|
+ right_up = 1;
|
|
|
+ }
|
|
|
+ else if((s_pos[2] - last_s_z < 0) || s_pos[2] <= 0)
|
|
|
+ {
|
|
|
+ right_up = -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(right_up == 1)
|
|
|
+ {
|
|
|
+ right_up_count ++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ right_up_count = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ last_h_z = h_pos[2];
|
|
|
+ last_s_z = s_pos[2];
|
|
|
+
|
|
|
+ if(left_up == 1 && right_up == 1 && right_up_count < 15 && left_up_count < 15
|
|
|
+ && right_zupt_count > 0 && left_zupt_count > 0 &&
|
|
|
+ ((right_up_count > 2 && left_up_count > 2) ||
|
|
|
+ (right_up_count > 0 && left_up_count > 0 && h_pos[2] >1 && s_pos[2] > 1)))
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+//由于仅靠IMU来探测触底了，高度估计得不太对，所以用加速度来算起跳动作
|
|
|
+
|
|
|
+void max_min_window(float *data, int length, float *max_val , int *max_index, float * min_val, int *min_index)
|
|
|
+{
|
|
|
+ *max_val = data[0];
|
|
|
+
|
|
|
+ *max_index = 0;
|
|
|
+
|
|
|
+ *min_val = data[0];
|
|
|
+
|
|
|
+ *min_index = 0;
|
|
|
+
|
|
|
+ for(int i = 0; i < length; i++)
|
|
|
+ {
|
|
|
+ if(*max_val < data[i])
|
|
|
+ {
|
|
|
+ *max_val = data[i];
|
|
|
+ *max_index = i;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(*min_val > data[i])
|
|
|
+ {
|
|
|
+ *min_val = data[i];
|
|
|
+ *min_index = i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+short press_jump_detect(float left_acc_z, float right_acc_z, int left_zupt, int right_zupt, int left_front_press, int right_front_press)
|
|
|
+{
|
|
|
+
|
|
|
+ static float right_data_z_buff[10];
|
|
|
+ static float left_data_z_buff[10];
|
|
|
+
|
|
|
+ static int last_right_press;
|
|
|
+ static int last_left_press;
|
|
|
+
|
|
|
+ static int left_max_press;
|
|
|
+ static int right_max_press;
|
|
|
+
|
|
|
+ static int wait;
|
|
|
+
|
|
|
+ /*
|
|
|
+ * 存储数据
|
|
|
+ */
|
|
|
+ memcpy(right_data_z_buff, right_data_z_buff + 1, 9 * sizeof(float));
|
|
|
+
|
|
|
+ memcpy(left_data_z_buff, left_data_z_buff + 1, 9 * sizeof(float));
|
|
|
+
|
|
|
+ right_data_z_buff[9] = right_acc_z;
|
|
|
+
|
|
|
+ left_data_z_buff[9] = left_acc_z;
|
|
|
+
|
|
|
+ if (left_zupt && right_zupt)
|
|
|
+ {
|
|
|
+ wait = 20;
|
|
|
+ }
|
|
|
+
|
|
|
+ /*
|
|
|
+ * 检测压力下降用的逻辑
|
|
|
+ */
|
|
|
+ if (last_left_press - left_front_press > 200)
|
|
|
+ {
|
|
|
+ if (last_left_press > left_max_press)
|
|
|
+ {
|
|
|
+ left_max_press = last_left_press;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (last_left_press - left_front_press < 50)
|
|
|
+ {
|
|
|
+ left_max_press = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ last_left_press = left_front_press;
|
|
|
+
|
|
|
+ if (last_right_press - right_front_press > 200)
|
|
|
+ {
|
|
|
+ if (last_right_press > right_max_press)
|
|
|
+ {
|
|
|
+ right_max_press = last_right_press;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (last_right_press - right_front_press < 50)
|
|
|
+ {
|
|
|
+ right_max_press = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ last_right_press = right_front_press;
|
|
|
+
|
|
|
+ /*
|
|
|
+ * 加速变化估计
|
|
|
+ */
|
|
|
+
|
|
|
+ float max_val_right, min_val_right, max_val_left, min_val_left;
|
|
|
+
|
|
|
+ int max_index_right, min_index_right, max_index_left, min_index_left;
|
|
|
+
|
|
|
+
|
|
|
+ max_min_window(right_data_z_buff, 10, &max_val_right, &max_index_right, &min_val_right, &min_index_right);
|
|
|
+
|
|
|
+ max_min_window(left_data_z_buff, 10, &max_val_left, &max_index_left, &min_val_left, &min_index_left);
|
|
|
+
|
|
|
+ if (wait > 0)
|
|
|
+ {
|
|
|
+ wait--;
|
|
|
+ }
|
|
|
+
|
|
|
+ /*
|
|
|
+ * 1.0f的阈值轻轻跳就可以达到了,现在改为1.3f的阈值
|
|
|
+ */
|
|
|
+ if (max_index_right < min_index_right && max_index_left < min_index_left
|
|
|
+
|
|
|
+ && max_val_right - min_val_right > 1.0f && min_val_right < 0.f && max_val_left-min_val_left > 1.0f && min_val_left < 0.f
|
|
|
+
|
|
|
+ && (wait > 0 && left_zupt == 0 && right_zupt == 0))
|
|
|
+ //if(left_max_press - left_front_press > 3000 && right_max_press - right_front_press > 3000)
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+int att_jump_detect(float left_pitch, float right_pitch, int left_zupt, int right_zupt)
|
|
|
+{
|
|
|
+
|
|
|
+ static int num;
|
|
|
+
|
|
|
+ static float left_pitch_window[8];
|
|
|
+ static float right_pitch_window[8];
|
|
|
+
|
|
|
+ //用俯仰角判断好了, 现在的IMU方向 下标1为俯仰角
|
|
|
+ memcpy(left_pitch_window, left_pitch_window + 1, 7 * sizeof(float));
|
|
|
+ memcpy(right_pitch_window, right_pitch_window + 1, 7 * sizeof(float));
|
|
|
+
|
|
|
+ left_pitch_window[7] = left_pitch;
|
|
|
+ right_pitch_window[7] = right_pitch;
|
|
|
+
|
|
|
+ num ++;
|
|
|
+
|
|
|
+ if(num < 8)
|
|
|
+ {
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ num = 8;
|
|
|
+ }
|
|
|
+ //要求起跳要同步
|
|
|
+
|
|
|
+ static int zupt_count = 0;
|
|
|
+ if(left_zupt && right_zupt)
|
|
|
+ {
|
|
|
+ zupt_count = 21;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(zupt_count > 0)
|
|
|
+ {
|
|
|
+ zupt_count --;
|
|
|
+ }
|
|
|
+
|
|
|
+ for(int i = 7; i > 1; i--)
|
|
|
+ {
|
|
|
+ if((left_pitch_window[i] > left_pitch_window[i-1] || left_pitch_window[i] > left_pitch_window[i-2])
|
|
|
+ && (right_pitch_window[i] > right_pitch_window[i-1] || right_pitch_window[i] > right_pitch_window[i-2]))
|
|
|
+ {
|
|
|
+ if((left_pitch > left_pitch_window[i - 1] + 0.1f || left_pitch > left_pitch_window[i - 2] + 0.1f )
|
|
|
+ && (right_pitch > right_pitch_window[i - 1] + 0.1f || right_pitch > right_pitch_window[i - 2] + 0.1f )
|
|
|
+ && zupt_count > 0 )
|
|
|
+ {
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+//int little_jump_detect_process(int press, int *press_max, int*press_min, int *last_press)
|
|
|
+//{
|
|
|
+//
|
|
|
+// if(*last_press - press > 200)
|
|
|
+// {
|
|
|
+// if(*press_max < press)
|
|
|
+// {
|
|
|
+// *press_max = press;
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(*press_min > press)
|
|
|
+// {
|
|
|
+// *press_min = press;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else if(*last_press - press < -200)
|
|
|
+// {
|
|
|
+// if(*press_max > press)
|
|
|
+// {
|
|
|
+// *press_max = press;
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(*press_min < press)
|
|
|
+// {
|
|
|
+// *press_min = press;
|
|
|
+// }
|
|
|
+// }
|
|
|
+//
|
|
|
+// *last_press = press;
|
|
|
+//
|
|
|
+// if(*press_max - press > 3000)
|
|
|
+// {
|
|
|
+// return 1;
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(*press_max - press < -1000)
|
|
|
+// {
|
|
|
+// return -1;
|
|
|
+// }
|
|
|
+//
|
|
|
+// return 0;
|
|
|
+//}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+//int little_jump_detect( int left_front_press, int right_front_press, int left_back_press, int right_back_press, int left_zupt, int right_zupt)
|
|
|
+//{
|
|
|
+// static int left_front_max = 0;
|
|
|
+// static int right_front_max = 0;
|
|
|
+// static int left_back_max = 0;
|
|
|
+// static int right_back_max = 0;
|
|
|
+//
|
|
|
+// static int left_front_min = 40000;
|
|
|
+// static int right_front_min = 40000;
|
|
|
+// static int left_back_min = 40000;
|
|
|
+// static int right_back_min = 40000;
|
|
|
+//
|
|
|
+// static int last_left_front_press = 0;
|
|
|
+// static int last_right_front_press = 0;
|
|
|
+//
|
|
|
+// static int last_left_back_press = 0;
|
|
|
+// static int last_right_back_press = 0;
|
|
|
+
|
|
|
+// int left_front_down = little_jump_detect_process(left_front_press, &left_front_max, &left_front_min, &last_left_front_press);
|
|
|
+//
|
|
|
+// int right_front_down = little_jump_detect_process(right_front_press, &right_front_max, &right_front_min, &last_right_front_press);
|
|
|
+//
|
|
|
+// int left_back_down = little_jump_detect_process(left_back_press, &left_back_max, &left_back_min, &last_left_back_press);
|
|
|
+//
|
|
|
+// int right_back_down = little_jump_detect_process(right_back_press, &right_back_max, &right_back_min, &last_right_back_press);
|
|
|
+//
|
|
|
+//// if( left_back_down == -1)
|
|
|
+//// {
|
|
|
+//// left_front_max = left_front_press;
|
|
|
+////
|
|
|
+//// left_front_min = left_front_press;
|
|
|
+
|
|
|
+//// }
|
|
|
+////
|
|
|
+//// if( right_back_down == -1)
|
|
|
+//// {
|
|
|
+//// right_front_max = right_front_press;
|
|
|
+////
|
|
|
+//// right_front_min = right_front_press;
|
|
|
+
|
|
|
+//// }
|
|
|
+//
|
|
|
+// if(left_front_down != 1)
|
|
|
+// {
|
|
|
+// right_front_max = right_front_press;
|
|
|
+//
|
|
|
+// right_front_min = right_front_press;
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(right_front_down != 1)
|
|
|
+// {
|
|
|
+// left_front_max = left_front_press;
|
|
|
+//
|
|
|
+// left_front_min = left_front_press;
|
|
|
+// }
|
|
|
+
|
|
|
+//
|
|
|
+// if(left_front_down==1 && right_front_down==1 && left_zupt==0 && right_zupt==0)
|
|
|
+// {
|
|
|
+// return 1;
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(left_back_down==1 && right_back_down==1 && left_zupt==0 && right_zupt==0)
|
|
|
+// {
|
|
|
+// return 1;
|
|
|
+// }
|
|
|
+//
|
|
|
+// return 0;
|
|
|
+//
|
|
|
+//}
|
|
|
+
|
|
|
+//int detect_L_line(float *press_buff, int length)
|
|
|
+//{
|
|
|
+//
|
|
|
+// float max_diff_val = press_buff[0] - press_buff[1];
|
|
|
+//
|
|
|
+// float temp_diff_val = 0.0f;
|
|
|
+//
|
|
|
+// int max_index = 0;
|
|
|
+//
|
|
|
+// for(int i = 1; i < length; i++)
|
|
|
+// {
|
|
|
+// if(press_buff[i-1] - press_buff[i] > -50.f)
|
|
|
+// {
|
|
|
+// temp_diff_val += (press_buff[i-1] - press_buff[i]);
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+//
|
|
|
+// temp_diff_val = 0.0f;
|
|
|
+// }
|
|
|
+//
|
|
|
+
|
|
|
+// if(max_diff_val < temp_diff_val)
|
|
|
+// {
|
|
|
+// max_diff_val = temp_diff_val;
|
|
|
+//
|
|
|
+// if(max_diff_val > 4000.f)
|
|
|
+// {
|
|
|
+// max_index = i;
|
|
|
+// }
|
|
|
+// }
|
|
|
+//
|
|
|
+// }
|
|
|
+//
|
|
|
+// float stable_max_val = press_buff[length - 4];
|
|
|
+// float stable_min_val = press_buff[length - 4];
|
|
|
+//
|
|
|
+// for(int i= length - 4; i< length; i++)
|
|
|
+// {
|
|
|
+// if(stable_max_val < press_buff[i])
|
|
|
+// {
|
|
|
+// stable_max_val = press_buff[i];
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(stable_min_val > press_buff[i])
|
|
|
+// {
|
|
|
+// stable_min_val = press_buff[i];
|
|
|
+// }
|
|
|
+// }
|
|
|
+//
|
|
|
+// if( stable_max_val - stable_min_val < 1000.f && max_diff_val > 3000.f
|
|
|
+// && max_index > length - 5)
|
|
|
+// {
|
|
|
+// return 1;
|
|
|
+// }
|
|
|
+//
|
|
|
+// return 0;
|
|
|
+//}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+//int acc_z_down_trend(float *left_acc_buff, float *right_acc_buff, int length)
|
|
|
+//{
|
|
|
+// int left_max_index = 0;
|
|
|
+// int left_min_index = 0;
|
|
|
+//
|
|
|
+// int right_max_index = 0;
|
|
|
+// int right_min_index = 0;
|
|
|
+//
|
|
|
+//
|
|
|
+// float left_max_val = left_acc_buff[0];
|
|
|
+// float left_min_val = left_acc_buff[0];
|
|
|
+//
|
|
|
+// float right_max_val = right_acc_buff[0];
|
|
|
+// float right_min_val = right_acc_buff[0];
|
|
|
+//
|
|
|
+//
|
|
|
+// for(int i = 0; i < length; i++)
|
|
|
+// {
|
|
|
+// if(left_max_val < left_acc_buff[i])
|
|
|
+// {
|
|
|
+// left_max_val = left_acc_buff[i];
|
|
|
+// left_max_index = i;
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(left_min_val > left_acc_buff[i])
|
|
|
+// {
|
|
|
+// left_min_val = left_acc_buff[i];
|
|
|
+// left_min_index = i;
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(left_max_val < left_acc_buff[i])
|
|
|
+// {
|
|
|
+// left_max_val = left_acc_buff[i];
|
|
|
+// left_max_index = i;
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(right_min_val > right_acc_buff[i])
|
|
|
+// {
|
|
|
+// right_min_val = right_acc_buff[i];
|
|
|
+// right_min_index = i;
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(right_max_val < right_acc_buff[i])
|
|
|
+// {
|
|
|
+// right_max_val = right_acc_buff[i];
|
|
|
+// right_max_index = i;
|
|
|
+// }
|
|
|
+//
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(right_max_index < right_min_index && right_max_val > right_min_val + 1.5f
|
|
|
+// && left_max_index < left_min_index && left_max_val > left_min_val + 1.5f
|
|
|
+// && abs(right_max_index - left_max_index < 4) && abs(left_max_index - left_min_index < 4))
|
|
|
+// {
|
|
|
+// return 1;
|
|
|
+// }
|
|
|
+//
|
|
|
+// return 0;
|
|
|
+//
|
|
|
+//}
|
|
|
+
|
|
|
+//int acc_little_shake(float *left_acc, float *right_acc, int length)
|
|
|
+//{
|
|
|
+// float left_max_val = left_acc[0];
|
|
|
+// float right_max_val = right_acc[0];
|
|
|
+//
|
|
|
+// float left_min_val = left_acc[0];
|
|
|
+// float right_min_val = right_acc[0];
|
|
|
+//
|
|
|
+// for(int i = 0; i < length; i++)
|
|
|
+// {
|
|
|
+// if(left_max_val < left_acc[i])
|
|
|
+// {
|
|
|
+// left_max_val = left_acc[i];
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(left_min_val > left_acc[i])
|
|
|
+// {
|
|
|
+// left_min_val = left_acc[i];
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(right_max_val < left_acc[i])
|
|
|
+// {
|
|
|
+// right_max_val = left_acc[i];
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(right_min_val > left_acc[i])
|
|
|
+// {
|
|
|
+// right_min_val = left_acc[i];
|
|
|
+// }
|
|
|
+
|
|
|
+//
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(left_max_val - left_min_val > 0.4f && right_max_val - right_min_val > 0.4f)
|
|
|
+// {
|
|
|
+// return 1;
|
|
|
+// }
|
|
|
+//
|
|
|
+// return 0;
|
|
|
+//
|
|
|
+//
|
|
|
+//}
|
|
|
+
|
|
|
+//int normal_jump_detect(float left_acc_z, float right_acc_z, int left_zupt, int right_zupt,
|
|
|
+// float left_front_press, float right_front_press, float left_back_press, float right_back_press)
|
|
|
+//{
|
|
|
+
|
|
|
+//// static float right_data_press[15];
|
|
|
+//// static float left_data_press[15];
|
|
|
+//
|
|
|
+// static float right_front_data_press[20];
|
|
|
+// static float left_front_data_press[20];
|
|
|
+//
|
|
|
+// static float left_acc_z_buff[15];
|
|
|
+// static float right_acc_z_buff[15];
|
|
|
+//
|
|
|
+//
|
|
|
+// static int left_wait;
|
|
|
+// static int right_wait;
|
|
|
+// static int wait;
|
|
|
+//
|
|
|
+// static int last_left_zupt;
|
|
|
+// static int last_right_zupt;
|
|
|
+//
|
|
|
+// static int press_jump_count = 0;
|
|
|
+//
|
|
|
+// /*
|
|
|
+// * 存储数据
|
|
|
+// */
|
|
|
+//
|
|
|
+//// memcpy(right_data_press, right_data_press + 1, 14 * sizeof(float));
|
|
|
+////
|
|
|
+//// memcpy(left_data_press, left_data_press + 1, 14 * sizeof(float));
|
|
|
+//
|
|
|
+// memcpy(right_front_data_press, right_front_data_press + 1, 19 * sizeof(float));
|
|
|
+//
|
|
|
+// memcpy(left_front_data_press, left_front_data_press + 1, 19 * sizeof(float));
|
|
|
+//
|
|
|
+// memcpy(left_acc_z_buff, left_acc_z_buff + 1, 14 * sizeof(float));
|
|
|
+//
|
|
|
+// memcpy(right_acc_z_buff, right_acc_z_buff + 1, 14 * sizeof(float));
|
|
|
+//
|
|
|
+//
|
|
|
+//// right_data_press[14] = right_back_press; left_data_press[14] = left_back_press;
|
|
|
+//
|
|
|
+// right_front_data_press[19] = right_front_press; left_front_data_press[19] = left_front_press;
|
|
|
+//
|
|
|
+// left_acc_z_buff[14] = left_acc_z; right_acc_z_buff[14] = right_acc_z;
|
|
|
+//
|
|
|
+//// if(left_zupt == 0 && last_left_zupt == 1)
|
|
|
+//// {
|
|
|
+//// left_wait = 10;
|
|
|
+//// }
|
|
|
+////
|
|
|
+//// if(right_zupt == 0 && last_right_zupt == 1)
|
|
|
+//// {
|
|
|
+//// right_wait = 10;
|
|
|
+//// }
|
|
|
+////
|
|
|
+//
|
|
|
+//// if(left_wait > 0)
|
|
|
+//// {
|
|
|
+//// left_wait --;
|
|
|
+//// }
|
|
|
+////
|
|
|
+//// if(right_wait > 0)
|
|
|
+//// {
|
|
|
+//// right_wait --;
|
|
|
+//// }
|
|
|
+
|
|
|
+// if(left_zupt == 1 && right_zupt == 1)
|
|
|
+// {
|
|
|
+// wait = 15;
|
|
|
+// }
|
|
|
+//
|
|
|
+// if(wait > 0)
|
|
|
+// {
|
|
|
+// wait --;
|
|
|
+// }
|
|
|
+//
|
|
|
+// last_left_zupt = left_zupt;
|
|
|
+// last_right_zupt = right_zupt;
|
|
|
+//
|
|
|
+
|
|
|
+//// if(detect_L_line(right_data_press, 15) && detect_L_line(left_data_press, 15) && wait > 0 && wait < 12 && left_zupt == 0 && right_zupt == 0)
|
|
|
+//// {
|
|
|
+//// return 1;
|
|
|
+//// }
|
|
|
+//
|
|
|
+// if(detect_L_line(right_front_data_press, 20) && detect_L_line(left_front_data_press, 20) && wait > 0 && wait < 14 && left_zupt == 0 && right_zupt == 0)
|
|
|
+// {
|
|
|
+// press_jump_count ++;
|
|
|
+// if(press_jump_count > 2)
|
|
|
+// {
|
|
|
+// return 1;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// press_jump_count =0;
|
|
|
+// }
|
|
|
+////
|
|
|
+//// if(little_up_jump_by_four_mag(right_data_press+7, left_data_press+7, right_front_data_press+7, left_front_data_press+7, 8) && wait > 0 && wait < 12)
|
|
|
+//// {
|
|
|
+//// return 1;
|
|
|
+//// }
|
|
|
+//
|
|
|
+// if(acc_z_down_trend(left_acc_z_buff, right_acc_z_buff, 15)&& wait > 0 && wait < 14 && left_zupt == 0 && right_zupt == 0 )
|
|
|
+// {
|
|
|
+// return 1;
|
|
|
+// }
|
|
|
+//
|
|
|
+//
|
|
|
+// return 0;
|
|
|
+//
|
|
|
+//
|
|
|
+//}
|
|
|
+//short press_jump_detect(float left_acc_z, float right_acc_z, int left_zupt, int right_zupt,
|
|
|
+// float left_front_press, float right_front_press, float left_back_press, float right_back_press)
|
|
|
+//{
|
|
|
+//
|
|
|
+//// return little_jump_detect( left_front_press, right_front_press, left_back_press, right_back_press, left_zupt, right_zupt) ||
|
|
|
+//// normal_jump_detect( left_acc_z, right_acc_z, left_zupt, right_zupt, left_front_press, right_front_press, left_back_press, right_back_press);
|
|
|
+//
|
|
|
+// if(normal_jump_detect( left_acc_z, right_acc_z, left_zupt, right_zupt, left_front_press, right_front_press, left_back_press, right_back_press))
|
|
|
+// {
|
|
|
+// return 1;
|
|
|
+// }
|
|
|
+
|
|
|
+//
|
|
|
+// return 0;
|
|
|
+//
|
|
|
+//}
|
|
|
+
|
|
|
+
|
liang