|
|
@@ -24,8 +24,6 @@ bool JumpHouse::unzuptValid(int* max_acc, int* min_acc)
|
|
|
{
|
|
|
if ((max_acc[0] - min_acc[0] > 2048 || max_acc[1] - min_acc[1] > 2048 || max_acc[2] - min_acc[2] > 2048) && min_acc[0] < 500)
|
|
|
{
|
|
|
- std::cout << "JumpHouse::unzuptValid, " << max_acc[0] << ", " << min_acc[0] << ", " << max_acc[1] << ", "
|
|
|
- << min_acc[1] << ", " << max_acc[2] << ", " << min_acc[2] << ", " <<endl;
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
@@ -133,10 +131,10 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
|
|
|
calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_heading, 0, LEFT_FOOT);
|
|
|
|
|
|
//通过加速度、以及vector已经缓存了空中的数据来定位判断命令所需的时间戳，这个时间戳是起延时判断的效果，延时是为了等合适的RSSI
|
|
|
- if (left_step_data_vector.size() > 9 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1)
|
|
|
+ if (left_step_data_vector.size() > 5 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1)
|
|
|
{
|
|
|
//if (unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left) && press_valid)
|
|
|
- if (unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
|
|
|
+ if (unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left) || left_front_press & 0x01)
|
|
|
{
|
|
|
left_zupt_extension_time = 20;
|
|
|
|
|
|
@@ -148,10 +146,10 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
|
|
|
|
|
|
}
|
|
|
|
|
|
- if (right_step_data_vector.size() > 9 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1)
|
|
|
+ if (right_step_data_vector.size() > 5 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1)
|
|
|
{
|
|
|
//if (unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right) && press_valid)
|
|
|
- if (unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
|
|
|
+ if (unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right) || right_front_press & 0x01)
|
|
|
{
|
|
|
right_zupt_extension_time = 20;
|
|
|
|
liang