|
@@ -246,140 +246,113 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
|
|
|
|
|
|
uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
|
|
|
{
|
|
|
- static uint8_t up_flag = 1;
|
|
|
- int32_t mag_temp[3];
|
|
|
- static int16_t accZ_buf[3];
|
|
|
- static uint8_t acc_index = 0;
|
|
|
- static int mag_buf[3];
|
|
|
- static uint8_t mag_index = 0;
|
|
|
- int step;
|
|
|
- int calculate_flag = 0;
|
|
|
- static int16_t LiftPoint;
|
|
|
- static int16_t DropPoint;
|
|
|
- static int16_t FrontTouchFlag=0;
|
|
|
- static int16_t LiftDistance;
|
|
|
- static int16_t counter1=0;
|
|
|
- static int16_t counter2=0;
|
|
|
-
|
|
|
-
|
|
|
- for(int i = 0; i < 3; i ++)
|
|
|
- {
|
|
|
- mag_temp[i] = (int32_t) (mag[i]);
|
|
|
- }
|
|
|
- mag_sqrt = abs(mag_temp[2]);
|
|
|
- /*读入磁力计数据*/
|
|
|
- if(mag_index >= 3)
|
|
|
- {
|
|
|
- mag_buf[0] = mag_buf[1];
|
|
|
- mag_buf[1] = mag_buf[2];
|
|
|
- mag_index = 2;
|
|
|
- calculate_flag = 1;
|
|
|
- }
|
|
|
- mag_buf[mag_index++] = mag_sqrt;
|
|
|
+ //利用前脚磁力计以及后脚来判断
|
|
|
+
|
|
|
+ static int32_t mag_front[5] = {40000};
|
|
|
+ static int16_t acc_front_z[5];
|
|
|
+
|
|
|
+ static int32_t mag_cur_min = 40000;
|
|
|
|
|
|
- /*读入xyZ轴加速度数据*/
|
|
|
- if(acc_index >= 3)
|
|
|
- {
|
|
|
- accZ_buf[0] = accZ_buf[1];
|
|
|
- accZ_buf[1] = accZ_buf[2];
|
|
|
- acc_index = 2;
|
|
|
- }
|
|
|
- accZ_buf[acc_index++] = -acc_z;
|
|
|
+ static int time_count;
|
|
|
+
|
|
|
+ static int last_floor_status = 1;
|
|
|
|
|
|
+ static int press_up_wait_acc_time = 0;
|
|
|
+
|
|
|
+ int on_floor = 0;
|
|
|
|
|
|
- /*开始计算*/
|
|
|
- if (calculate_flag == 1)
|
|
|
+ memcpy(mag_front, mag_front + 1, 4 * sizeof(int32_t));
|
|
|
+ memcpy(acc_front_z, acc_front_z + 1, 4 * sizeof(int16_t));
|
|
|
+
|
|
|
+ mag_front[4] = abs(mag[2]);
|
|
|
+ acc_front_z[4] = acc_z;
|
|
|
+
|
|
|
+
|
|
|
+ if (time_count > 4)
|
|
|
{
|
|
|
-
|
|
|
-// DEBUG_LOG("LiftPoint==:%d...\n",LiftPoint);
|
|
|
-// DEBUG_LOG("DropPoint==:%d...\n",DropPoint);
|
|
|
-// DEBUG_LOG("LiftDistance==:%d...\n",LiftDistance);
|
|
|
-
|
|
|
- if(mag_buf[2] - mag_buf[1]>=0)
|
|
|
- {
|
|
|
- if(FrontTouchFlag==0)
|
|
|
+ int16_t max_acc_z = acc_front_z[0], min_acc_z = acc_front_z[0];
|
|
|
+ int32_t max_mag_front = mag_front[0];
|
|
|
+ int32_t min_mag_front = mag_front[0];
|
|
|
+ int16_t max_mag_index = 0, min_mag_index = 0;
|
|
|
+
|
|
|
+ for (int i = 1; i < 5; i++)
|
|
|
+ {
|
|
|
+ if (min_mag_front > mag_front[i])
|
|
|
{
|
|
|
- DropPoint = mag_buf[2];
|
|
|
+ min_mag_front = mag_front[i];
|
|
|
+ min_mag_index = i;
|
|
|
}
|
|
|
- else
|
|
|
+
|
|
|
+ if (max_mag_front < mag_front[i])
|
|
|
{
|
|
|
- if(mag_buf[2]>DropPoint)
|
|
|
- {
|
|
|
- DropPoint = mag_buf[2];
|
|
|
- }
|
|
|
+ max_mag_front = mag_front[i];
|
|
|
+ max_mag_index = i;
|
|
|
}
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- if(FrontTouchFlag==0)
|
|
|
+
|
|
|
+ if (max_acc_z < acc_front_z[i])
|
|
|
{
|
|
|
- LiftPoint = mag_buf[2];
|
|
|
+ max_acc_z = acc_front_z[i];
|
|
|
}
|
|
|
- else
|
|
|
+
|
|
|
+ if (min_acc_z > acc_front_z[i])
|
|
|
{
|
|
|
- ;
|
|
|
+ min_acc_z = acc_front_z[i];
|
|
|
}
|
|
|
- }
|
|
|
-
|
|
|
- /*判断前后脚掌的状态*/
|
|
|
- if(mag_buf[2] - LiftPoint>800)
|
|
|
- {
|
|
|
- FrontTouchFlag = 1;
|
|
|
- if(mag_buf[2] - LiftPoint>LiftDistance)
|
|
|
- {
|
|
|
- LiftDistance = mag_buf[2] - LiftPoint;
|
|
|
- }
|
|
|
- }
|
|
|
- if((mag_buf[2] - LiftPoint<=800)||((DropPoint-mag_buf[2]>0.4*LiftDistance)&&(abs(mag_buf[2]-mag_buf[1])<400)))
|
|
|
- {
|
|
|
- FrontTouchFlag = 0;
|
|
|
- LiftDistance = 800;
|
|
|
- }
|
|
|
|
|
|
- /*计算步数*/
|
|
|
- if (up_flag == 0)
|
|
|
- {
|
|
|
- counter1=0;
|
|
|
- if(counter2<300)
|
|
|
- {
|
|
|
- counter2++;
|
|
|
- }
|
|
|
- if((FrontTouchFlag==1)&&(counter2>=8))
|
|
|
- {
|
|
|
- step = 1;
|
|
|
- up_flag = 1;
|
|
|
- DEBUG_LOG("step==%d\n",step);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- step = 0;
|
|
|
- up_flag = 0;
|
|
|
- }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ //寻找上升沿
|
|
|
+ if (min_mag_index < max_mag_index && max_mag_front > min_mag_front + 100)
|
|
|
+ {
|
|
|
+ mag_cur_min = mag_cur_min > min_mag_front ? min_mag_front : mag_cur_min;
|
|
|
+ }
|
|
|
+ else if (min_mag_index > max_mag_index && max_mag_front > min_mag_front + 100)
|
|
|
+ {
|
|
|
+ mag_cur_min = 40000;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (mag_front[4] > mag_cur_min + 1000)
|
|
|
+ {
|
|
|
+ on_floor = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (last_floor_status == 1 && abs(acc_front_z[4] - acc_front_z[3]) < 105)
|
|
|
+ {
|
|
|
+ on_floor = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (on_floor)
|
|
|
+ {
|
|
|
+ press_up_wait_acc_time = 3; //拖延3个触地状态
|
|
|
+ }
|
|
|
+ else if (press_up_wait_acc_time > 0)
|
|
|
+ {
|
|
|
+ on_floor = 1;
|
|
|
+ }
|
|
|
|
|
|
- }
|
|
|
- else
|
|
|
+ if (press_up_wait_acc_time > 0)
|
|
|
{
|
|
|
- step = 0;
|
|
|
- counter2=0;
|
|
|
- if(counter1<300)
|
|
|
- {
|
|
|
- counter1++;
|
|
|
- }
|
|
|
- if((FrontTouchFlag==0)&&((abs(accZ_buf[0]-2000)>300)||(abs(accZ_buf[1]-2000)>300)||(abs(accZ_buf[2]-2000)>300))&&(counter1>=6))
|
|
|
- {
|
|
|
- up_flag=0;
|
|
|
- }
|
|
|
+ press_up_wait_acc_time--;
|
|
|
}
|
|
|
|
|
|
+
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- step = 0;
|
|
|
- LiftPoint = mag_buf[2];
|
|
|
- DropPoint = mag_buf[2];
|
|
|
+ time_count++;
|
|
|
}
|
|
|
|
|
|
+ uint8_t step = 0;
|
|
|
+
|
|
|
+ if(last_floor_status == 0 && on_floor == 1)
|
|
|
+ {
|
|
|
+ step = 1;
|
|
|
+ }
|
|
|
|
|
|
+ last_floor_status = on_floor;
|
|
|
|
|
|
return step;
|
|
|
}
|