123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589 |
- #include "SingleFootAction.h"
- void SingleFootAction::run(uint32_t _time, int x, int y, int z,int zupt, int rssi, float pitch, float *acc)
- {
- //std::cout << "pitch : " << pitch <<endl;
- /*
- int enter_signal_result = -1;
- int direction_signal_result = -1;
- can_listen_enter_signal(acc, pitch);
- int enter_signal_tmp = enter_signal_listen(acc, pitch);
- if (last_enter_signal != enter_signal_tmp && enter_signal_tmp > 0 && enter_signal_cooling == 0)
- {
- enter_signal = enter_signal_tmp;
- enter_signal_result = ENTER_KEY;
-
- enter_signal_cooling = 20;
- std::cout << "输出垫脚信号 : " << enter_signal << endl;
- }
- if (enter_signal_tmp > 0)
- {
- enter_signal_cooling = 20;
- }
- last_enter_signal = enter_signal_tmp;
-
- can_listen_direction_signal(rssi);
-
- direction_signal = direction_signal_listen(last_x, last_y, zupt, rssi);
- if (direction_signal > 0 && can_report_motion == 1)
- {
- can_report_motion = 0;
- if (pitch < 0.78f)
- {
- direction_signal_result = direction_signal;
- }
- std::cout << "direction_signal : " << direction_signal << endl;
- }
- */
- direction_signal_listen_new(_time, last_x, last_y, zupt, rssi, pitch);
- int zupt_trigger = 0;
- if (last_zupt == 0 && zupt == 1)
- {
- zupt_trigger = 1;
- }
- enter_signal_listen_new(_time, acc, pitch, zupt_trigger);
- last_zupt = zupt;
- last_x = x; last_y = y; last_z = z;
- /*
- if (enter_signal_cooling > 0)
- {
- enter_signal_cooling--;
- }
- if (enter_signal_result != -1 )
- {
- if (footActionState.FootState == ENTER_KEY)
- {
- footActionState.StateCount ++;
- footActionState.Triggering_time = _time;
- }
- else
- {
- footActionState.FootState = ENTER_KEY;
- footActionState.StateCount = 1;
- footActionState.Triggering_time = _time;
- }
- }
- if (zupt == 1)
- {
- footActionState.Triggering_time = _time;
- }
- */
- }
- void SingleFootAction::reset_interation_state()
- {
- footActionState.FootState = 0;
- footActionState.StateCount = 0;
- footActionState.Triggering_time = 0;
- }
- float cal_acc_var(float* acc, int length)
- {
- //计算均值
- float mean_val = 0;
- for (int i = 0; i < length; i++)
- {
- mean_val += acc[i];
- }
-
- mean_val /= length;
- //计算总体方差
- float var_val = 0;
- for (int i = 0; i < length; i++)
- {
- var_val += ((acc[i] - mean_val) * (acc[i] - mean_val));
- }
- return var_val / length;
- }
- int acc_is_valid(float* acc_window, int length)
- {
- //要求窗口窗口长度大于10
- //先判断最后五个元素是平稳的,阈值设置为0.06
- float station_max = acc_window[length - 1];
- float station_min = acc_window[length - 1];
- float window_max = acc_window[length - 1];
- float window_min = acc_window[length - 1];
-
- int window_max_index = length - 1;
- int window_min_index = length - 1;
- for (int i = length - 1; i >= 0; i--)
- {
- if (acc_window[i] > window_max)
- {
- window_max = acc_window[i];
- window_max_index = i;
- }
- if (acc_window[i] < window_min)
- {
- window_min = acc_window[i];
- window_min_index = i;
- }
- }
- for(int i = length - 1; i >= length - 3; i--)
- {
- if (acc_window[i] > station_max)
- {
- station_max = acc_window[i];
- }
- if (acc_window[i] < station_min)
- {
- station_min = acc_window[i];
- }
- }
- if (window_max - window_min < 0.2f)
- {
- return 1;
- }
- return 0;
- }
- int SingleFootAction::enter_signal_listen_new(uint32_t _time, float* acc, float pitch, int zupt_trigger)
- {
- memcpy(acc_x_buff, acc_x_buff + 1, 9 * sizeof(float));
- acc_x_buff[9] = acc[0];
- //计算加速度的总体方差(n)
- float acc_var = cal_acc_var(acc_x_buff, 10);
- acc_x_max = acc_x_max > acc[0] ? acc_x_max : acc[0];
- acc_x_min = acc_x_min < acc[0] ? acc_x_min : acc[0];
- if(acc_is_valid(acc_x_buff, 10))
- {
- station_count ++;
- }
- else
- {
- station_count = 0;
- }
-
- //x轴速度剧烈抖动,而且俯仰角大于45度,则证明是踮脚
- if (station_count > 20 && pitch > 0.6f && acc_x_max - acc_x_min > 1.0f)
- {
- //can_report_enter_signal = 0;
- //std::cout << "抖动状态下判断脚掂起来" << endl;
- acc_x_max = acc[0];
- acc_x_min = acc[0];
- if (footActionState.FootState != ENTER_KEY)
- {
- std::cout << "抖动状态下判断脚掂起来" << endl;
- footActionState.FootState = ENTER_KEY;
- footActionState.StateCount = 1;
- }
- else
- {
- if (_time - footActionState.Triggering_time > 10)
- {
- //std::cout << "抖动状态下判断脚掂起来" << endl;
- footActionState.StateCount ++ ;
- std::cout << "test" << endl;
- }
- }
- footActionState.Triggering_time = _time;
- }
-
- return -1;
- }
- int SingleFootAction::enter_signal_listen(float *acc, float pitch)
- {
- memcpy(acc_x_buff, acc_x_buff + 1, 9 * sizeof(float));
- acc_x_buff[9] = acc[0];
- //计算加速度的总体方差(n)
- float acc_var = cal_acc_var(acc_x_buff, 10);
-
- //x轴速度剧烈抖动,而且俯仰角大于45度,则证明是踮脚
- if (acc_var > 1.0f && pitch > 0.78f)
- {
- can_report_enter_signal = 0;
- //std::cout << "抖动状态下判断脚掂起来" << endl;
- return 1;
- }
- //如果抖动检测不到,转入平稳状态, 判断俯仰角持续两秒
- float acc_norm = sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
- //std::cout << "acc_norm : " << acc[0] << ", " << acc[1] << ", " << acc[2] << endl;
- if (acc_norm < 1.1f && acc_norm > 0.9f && pitch > 0.78f)
- {
- big_pitch_count++;
- }
- else
- {
- big_pitch_count = 0;
- }
- if (big_pitch_count > 90 && can_report_enter_signal)
- {
- can_report_enter_signal = 0;
- big_pitch_count = 0;
- std::cout << "平稳状态下判断脚掂起来" << endl;
- return 1;
- }
- return -1;
- }
- int SingleFootAction::can_listen_enter_signal(float* acc, float pitch)
- {
- if (sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]) > 1.5f || fabsf(pitch) < 0.2f)
- {
- can_report_enter_signal = 1;
- }
- return 0;
- }
- FootActionState SingleFootAction::get_interation_state()
- {
- return footActionState;
- }
- int SingleFootAction::direction_signal_listen_new(uint32_t _time, int x, int y, int zupt, int rssi, float pitch)
- {
- float x_offset = x;
- float y_offset = y;
- if (pitch > 0.5f)
- {
- return -1;
- }
- if (last_zupt == 0 && zupt == 1)
- {
- if (_time - footActionState.Triggering_time > 10)
- {
- if (left_or_right == LEFT_FOOT)
- {
- footActionState.FootState = BACK_LEFT;
- }
- else
- {
- footActionState.FootState = BACK_RIGHT;
- }
- footActionState.StateCount++;
- }
- }
- if (zupt)
- {
- footActionState.Triggering_time = _time;
- }
- return -1;
- }
- /*
- int SingleFootAction::direction_signal_listen_new(uint32_t _time, int x, int y, int zupt, int rssi, float pitch)
- {
- float x_offset = x;
- float y_offset = y;
- if (pitch > 0.78f)
- {
- return -1;
- }
- if ( last_zupt == 0 && zupt == 1)
- {
- if (_time - footActionState.FootState < 10)
- {
- return -1;
- }
- //触发判断大概的位置, 短距离通过RSSI判断
- if (rssi < 30)
- {
- //先判断当前状态是什么状态,再判断次数
- std::cout << "rssi 小于阈值" << endl;
- if (footActionState.FootState == 0 || (footActionState.FootState != BACK_LEFT && footActionState.FootState != BACK_RIGHT))
- {
- if (left_or_right == LEFT_FOOT)
- {
- footActionState.FootState = BACK_LEFT;
- }
- else
- {
- footActionState.FootState = BACK_RIGHT;
- }
- footActionState.StateCount = 1;
- footActionState.Triggering_time = _time;
- }
- else
- {
-
- footActionState.StateCount ++;
- footActionState.Triggering_time = _time;
- }
-
- }
- //长距离移动判断, 通过IMU算出来的距离来判断
- else if (sqrt(x_offset * x_offset + y_offset * y_offset) > 10.f)
- {
- if (left_or_right == LEFT_FOOT)
- {
- x_offset -= 5;
- }
- else
- {
- x_offset += 5;
- }
- float angle = atan2(y_offset, x_offset);
- std::cout << "距离 大于阈值" << endl;
- if (left_or_right == LEFT_FOOT && angle > PI / 2.0 && angle < PI)
- {
- if (footActionState.FootState == FRONT_LEFT)
- {
- footActionState.StateCount++;
- footActionState.Triggering_time = _time;
- }
- else
- {
- footActionState.FootState = FRONT_LEFT;
- footActionState.StateCount = 1;
- footActionState.Triggering_time = _time;
- }
- }
- else if (left_or_right == RIGHT_FOOT && angle < PI / 2.0 && angle >0.0f)
- {
- if (footActionState.FootState == FRONT_RIGHT)
- {
- footActionState.StateCount++;
- footActionState.Triggering_time = _time;
- }
- else
- {
- footActionState.FootState = FRONT_RIGHT;
- footActionState.StateCount = 1;
- footActionState.Triggering_time = _time;
- }
- }
- }
- else
- {
- std::cout << "距离、rssi 不符合阈值" << endl;
- //rssi > 35 && length < 0.15
- footActionState.StateCount++;
- footActionState.Triggering_time = _time;
- }
- }
- return -1;
- }
- */
- int SingleFootAction::direction_signal_listen(int x, int y, int zupt, int rssi)
- {
- float x_offset = x;
- float y_offset = y;
- if (left_or_right == LEFT_FOOT)
- {
- x_offset -= 5;
- }
- else
- {
- y_offset += 5;
- }
- //当触地、双脚靠拢的时候,判断归位,触发位置的判断
- if (zupt == 1 && rssi < 35)
- {
- has_reset_signal = 1;
- }
- if (has_reset_signal == 1 && last_zupt == 0 && zupt == 1)
- {
- //触发判断大概的位置
- if (rssi < 30 && on_floor_count == 0)
- {
- //短距离通过RSSI判断
- if (left_or_right == LEFT_FOOT)
- {
- cmd_stage = BACK_LEFT;
- }
- else
- {
- cmd_stage = BACK_RIGHT;
- }
- time_s = clock();
- on_floor_count = 1;
- return -1;
-
- }
- //长距离移动判断, 通过IMU算出来的距离来判断
- else if (sqrt(x_offset * x_offset + y_offset * y_offset) > 15.f)
- {
- float angle = atan2(y_offset, x_offset);
- if (left_or_right == LEFT_FOOT && angle > PI / 2.0 && angle < PI)
- {
- cmd_stage = FRONT_LEFT;
- on_floor_count = 1;
- time_s = clock();
- return -1;
- }
- else if (left_or_right == RIGHT_FOOT && angle < PI / 2.0 && angle >0.0f)
- {
- cmd_stage = FRONT_RIGHT;
- on_floor_count = 1;
- time_s = clock();
- return -1;
- }
- }
-
- /*
- //第一次触地,触发命令类别的判断
- if (on_floor_count == 0)
- {
- on_floor_count ++;
-
- //记录触发的时间,用以确认的时候,判断是不是在有效时间里面
- if (cmd_stage == FRONT_LEFT || cmd_stage == FRONT_RIGHT || cmd_stage == BACK_LEFT || cmd_stage == BACK_RIGHT)
- {
- time_s = clock();
- }
- return -1;
- }
- */
- else if (on_floor_count == 1)
- {
- //已经确认了位置
- //先判断是否在有效时间内
- int result = -1;
- double time_interval = (double)(clock() - time_s) / CLOCKS_PER_SEC;
-
- if (time_interval > 1.5)
- {
- std::cout << "debug : 两步之间超过限制 -- " << endl;
- }
- if (time_interval < 0.06)
- {
- std::cout << "抹去0.06s的噪声" << endl;
-
- return result;
- }
- if (time_interval < 1.5)
- {
- if ((cmd_stage == FRONT_LEFT || cmd_stage == FRONT_RIGHT) && sqrt(x_offset * x_offset + y_offset * y_offset) < 15.f)
- {
- result = cmd_stage;
- }
- else if ((cmd_stage == BACK_LEFT || cmd_stage == BACK_RIGHT) && rssi < 35)
- {
- result = cmd_stage;
- }
- else
- {
- std::cout << "出现其他问题了" << std::endl;
- }
- }
- on_floor_count = 0;
- has_reset_signal = 0;
- cmd_stage = 0;
- return result;
- }
- }
- return -1;
- }
- void SingleFootAction::can_listen_direction_signal(int rssi)
- {
- if (rssi < 30)
- {
- can_report_motion = 1;
- }
- }
|