|
@@ -44,25 +44,25 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
|
|
|
int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
|
|
|
int jump, int down, int rssi)
|
|
|
{
|
|
|
- //锟斤拷锟斤拷一锟斤拷锟节的匡拷锟斤拷锟斤拷锟斤拷
|
|
|
+ //缓存一步内的空中数据
|
|
|
setData(right_shoes_data_vector, time_stamp, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
|
|
|
right_att[0] * 0.0001f, right_att[1] * 0.0001f, right_att[2] * 0.0001f, right_zupt, rssi);
|
|
|
|
|
|
setData(left_shoes_data_vector, time_stamp, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
|
|
|
left_att[0] * 0.0001f, left_att[1] * 0.0001f, left_att[2] * 0.0001f, left_zupt, rssi);
|
|
|
|
|
|
- //锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟捷o拷锟斤拷锟斤拷锟饺�拷锟轿伙拷锟�
|
|
|
+ //根据新来的数据,计算的全局位置
|
|
|
calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, right_heading, 0, RIGHT_FOOT);
|
|
|
|
|
|
calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_heading, 0, LEFT_FOOT);
|
|
|
|
|
|
- //锟斤拷锟斤拷锟脚革拷锟斤拷氐锟斤拷锟斤拷锟�
|
|
|
+ //补充后脚跟落地的问题
|
|
|
|
|
|
left_back_press_vector.push_back(left_back_press);
|
|
|
|
|
|
if(left_back_press_vector.size()> 10)
|
|
|
{
|
|
|
- //锟斤拷示锟叫达拷锟斤拷锟斤拷锟斤拷时锟津,达拷锟斤拷锟斤拷锟�, 锟角诧拷锟斤拷为锟劫达拷锟斤拷
|
|
|
+ //表示有触地命令时候,触发检测, 是不是为假触地
|
|
|
left_back_press_vector.pop_front();
|
|
|
auto max_point = std::max_element(left_back_press_vector.begin(), left_back_press_vector.end());
|
|
|
auto min_point = std::min_element(left_back_press_vector.begin(), left_back_press_vector.end());
|
|
@@ -99,7 +99,7 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
|
|
|
|
|
|
if (right_back_press_vector.size() > 10)
|
|
|
{
|
|
|
- //锟斤拷示锟叫达拷锟斤拷锟斤拷锟斤拷时锟津,达拷锟斤拷锟斤拷锟�, 锟角诧拷锟斤拷为锟劫达拷锟斤拷
|
|
|
+ //表示有触地命令时候,触发检测, 是不是为假触地
|
|
|
right_back_press_vector.pop_front();
|
|
|
auto max_point = std::max_element(right_back_press_vector.begin(), right_back_press_vector.end());
|
|
|
auto min_point = std::min_element(right_back_press_vector.begin(), right_back_press_vector.end());
|
|
@@ -181,7 +181,7 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- //通锟斤拷锟斤拷锟劫度★拷锟皆硷拷vector锟窖撅拷锟斤拷锟斤拷锟剿匡拷锟叫碉拷锟斤拷锟斤拷锟斤拷锟斤拷位锟叫讹拷锟斤拷锟斤拷锟斤拷锟斤拷锟绞憋拷锟斤拷锟斤拷锟斤拷锟绞憋拷锟斤拷锟斤拷锟斤拷锟斤拷时锟叫断碉拷效锟斤拷锟斤拷锟斤拷时锟斤拷为锟剿等猴拷锟绞碉拷RSSI
|
|
|
+ //通过加速度、以及vector已经缓存了空中的数据来定位判断命令所需的时间戳,这个时间戳是起延时判断的效果,延时是为了等合适的RSSI
|
|
|
if (left_step_data_vector.size() > 4 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1
|
|
|
&& (left_back_press_detect_time == 0 || left_back_press_detect_time == 50))
|
|
|
{
|
|
@@ -214,7 +214,7 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
|
|
|
|
|
|
}
|
|
|
|
|
|
- /*锟斤拷蛹锟斤拷俣裙锟斤拷锟斤拷锟�
|
|
|
+ /*添加加速度过滤器
|
|
|
for (int i = 1; i < 3; i++)
|
|
|
{
|
|
|
for (int k = 0; k < 3; k++)
|
|
@@ -271,11 +271,11 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
|
|
|
|
|
|
count++;
|
|
|
|
|
|
- //锟斤拷锟斤拷锟斤拷锟斤拷
|
|
|
+ //重置命令
|
|
|
memset(result, -1, 4 * sizeof(int));
|
|
|
|
|
|
- //锟叫讹拷双锟脚达拷锟截碉拷时锟斤拷, rssi < 23 锟斤拷锟斤拷锟轿伙拷锟斤拷锟饺灰裁伙拷旆�拷锟斤拷锟�
|
|
|
- //锟斤拷锟斤拷锟斤拷rssi锟斤拷锟接筹拷锟叫癸拷锟矫碉拷
|
|
|
+ //判断双脚触地的时候, rssi < 23 必须归位,不然也没办法处理
|
|
|
+ //看起来rssi的延迟有够烂的
|
|
|
|
|
|
if (left_init == 1 && time_stamp - left_cmd_wait_time >= 9 && time_stamp - left_cmd_wait_time < 20 && left_press_valid > 0)
|
|
|
{
|