123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664 |
- #include "detect_step_by_acc.h"
- static int ACC_WINDOW_SIZE=20;
- static int16_t acc_y_buff[20];
- static int16_t acc_z_buff[20];
- static int16_t acc_x_buff[20];
- static int32_t acc_xyz_buff[20];
- static int acc_y_status = 1;
- static int acc_z_status = 1;
- static int acc_x_status = 1;
- static int last_acc_x_status;
- static int last_acc_y_status;
- static int last_acc_z_status;
- static int x_raised;
- static int y_raised;
- static int z_raised;
- static int x_hollow;
- static int y_hollow;
- static int z_hollow;
- static int zero_x_wait;
- static int zero_y_wait;
- static int zero_z_wait;
- static int last_time_stamp = 0;
- static int16_t max_z_unzero = 2048;
- static int16_t min_z_unzero = 2048;
- void detect_raised_hollow(int16_t *acc, int length, int16_t max_val_thresh, int16_t min_val_thresh, int16_t distance, int *raised, int *hollow)
- {
-
-
- int start_index = ACC_WINDOW_SIZE - length;
-
- if(start_index < 0)
- {
- return;
- }
-
- int16_t max_val = acc[ACC_WINDOW_SIZE - length / 2];
- int16_t min_val = acc[ACC_WINDOW_SIZE - length / 2];
-
- for(int i = ACC_WINDOW_SIZE - length / 2; i < ACC_WINDOW_SIZE; i++)
- {
- if(max_val < acc[i])
- {
- max_val = acc[i];
- }
-
- if(min_val > acc[i])
- {
- min_val = acc[i];
- }
- }
-
- if(max_val - min_val < 410)
- {
- *raised = 0;
- *hollow = 0;
-
- return;
- }
-
- for(int i = 0; i < ACC_WINDOW_SIZE - length / 2; i++)
- {
- if(max_val < acc[i])
- {
- max_val = acc[i];
- }
-
- if(min_val > acc[i])
- {
- min_val = acc[i];
- }
- }
-
-
-
- if(max_val - acc[0] > distance && max_val - acc[ACC_WINDOW_SIZE-1] > distance && *raised == 0 && max_val > max_val_thresh)
- {
- *raised = 1;
- }
-
- if(acc[0] - min_val > distance && acc[ACC_WINDOW_SIZE - 1] - min_val > distance && *hollow == 0 && min_val < min_val_thresh)
- {
- *hollow = 1;
- }
- }
- int detect_turn_line(int16_t *acc, int length, int tag)
- {
- int start_index = ACC_WINDOW_SIZE - length;
-
- int16_t min_val = acc[start_index];
-
- int min_index = start_index;
-
- int16_t max_val = acc[start_index];
-
- int max_index = start_index;
-
- for(int i = 8; i < ACC_WINDOW_SIZE; i ++)
- {
- if(min_val > acc[i])
- {
- min_val = acc[i];
- min_index = i;
- }
-
- if(max_val < acc[i])
- {
- max_val = acc[i];
- max_index = i;
- }
-
- }
-
- int16_t min_val_5 = acc[ACC_WINDOW_SIZE - 5];
- int16_t max_val_5 = acc[ACC_WINDOW_SIZE - 5];
-
- for(int i = ACC_WINDOW_SIZE - 5; i < ACC_WINDOW_SIZE; i++ )
- {
- if(min_val_5 > acc[i])
- {
- min_val_5 = acc[i];
- }
-
- if(max_val_5 < acc[i])
- {
- max_val_5 = acc[i];
- }
- }
-
- if(max_val - min_val > 1024 && max_val_5 - min_val_5 < 410)
- {
- if(tag == 1)
- {
- if(max_index < min_index)
- {
- return 1;
- }
- else
- {
- return 0;
- }
- }
- return 1;
- }
-
- return 0;
- }
- int station_signal(int16_t *acc, int length)
- {
- int start_index = ACC_WINDOW_SIZE - length;
-
- int16_t max_val = acc[start_index];
- int16_t min_val = acc[start_index];
-
-
- for(int i = start_index; i < ACC_WINDOW_SIZE; i++)
- {
- if(max_val < acc[i])
- {
- max_val = acc[i];
- }
-
- if(min_val > acc[i])
- {
- min_val = acc[i];
- }
- }
-
- if(max_val - min_val < 307)
- {
-
- return 1;
- }
-
- return 0;
- }
- int severe_turn_point_z(int16_t *acc, int length)
- {
- int16_t max_val = acc[0];
- int16_t min_val = acc[0];
-
- int16_t min_temp = acc[0];
-
- int16_t max_distance = max_val - min_val;
-
- for(int i = 1 ; i < ACC_WINDOW_SIZE; i++)
- {
- int16_t distance_tmp = acc[i-1] - acc[i];
-
- if(i > 1 && distance_tmp < (acc[i-2] - acc[i]))
- distance_tmp = (acc[i-2] - acc[i]);
-
- if(i > 2 && distance_tmp < (acc[i-3] - acc[i]))
- distance_tmp = (acc[i-3] - acc[i]);
-
- if(i > 3 && distance_tmp < (acc[i-4] - acc[i]))
- distance_tmp = (acc[i-4] - acc[i]);
-
- if(i > 4 && distance_tmp < (acc[i-5] - acc[i]))
- distance_tmp = (acc[i-5] - acc[i]);
-
- if(max_distance < distance_tmp)
- {
- max_distance = distance_tmp;
- }
-
- if(min_temp > acc[i])
- {
- min_temp = acc[i];
- }
- }
-
- max_val = acc[ACC_WINDOW_SIZE-5];
- min_val = acc[ACC_WINDOW_SIZE-5];
-
- for(int i = ACC_WINDOW_SIZE - 5; i< ACC_WINDOW_SIZE; i++)
- {
- if(max_val < acc[i])
- {
- max_val = acc[i];
- }
-
- if(min_val > acc[i])
- {
- min_val = acc[i];
- }
- }
-
- int16_t min_distance = max_val - min_val;
-
- if(max_distance > 3072 && min_distance < 614 && min_temp > 1024)
- {
- return 1;
- }
-
- return 0;
-
- }
- int severe_turn_point(int16_t *acc, int length)
- {
- int16_t max_val = acc[0];
- int16_t min_val = acc[0];
-
- int16_t max_distance = max_val - min_val;
-
- for(int i = 1 ; i < ACC_WINDOW_SIZE; i++)
- {
- int16_t distance_tmp = acc[i] - acc[i-1];
-
- if(i > 1 && distance_tmp < fabsf(acc[i] - acc[i-2]))
- distance_tmp = fabsf(acc[i] - acc[i-2]);
-
- if(i > 2 && distance_tmp < fabsf(acc[i] - acc[i-3]))
- distance_tmp = fabsf(acc[i] - acc[i-3]);
-
- if(i > 3 && distance_tmp < fabsf(acc[i] - acc[i-4]))
- distance_tmp = fabsf(acc[i] - acc[i-4]);
-
- if(i > 4 && distance_tmp < fabsf(acc[i] - acc[i-5]))
- distance_tmp = fabsf(acc[i] - acc[i-5]);
-
- if(max_distance < distance_tmp)
- {
- max_distance = distance_tmp;
- }
- }
-
- max_val = acc[ACC_WINDOW_SIZE-5];
- min_val = acc[ACC_WINDOW_SIZE-5];
-
- for(int i = ACC_WINDOW_SIZE - 5; i< ACC_WINDOW_SIZE; i++)
- {
- if(max_val < acc[i])
- {
- max_val = acc[i];
- }
-
- if(min_val > acc[i])
- {
- min_val = acc[i];
- }
- }
-
- int16_t min_distance = max_val - min_val;
-
- if(max_distance > 2662 && min_distance < 615)
- {
- return 1;
- }
-
- return 0;
-
- }
- int32_t var_acc(int16_t *acc, int length)
- {
- if(length < 10)
- {
- return 0;
- }
-
- int32_t mean_x = 0;
-
- int32_t sum_x = 0;
-
- for(int i = length - 10; i < length; i++)
- {
- sum_x += acc[i];
- }
-
- mean_x = sum_x / 10;
-
- sum_x = 0;
-
- for(int i = length - 10; i < length; i++)
- {
- sum_x += ((acc[i]- mean_x) * (acc[i]- mean_x));
- }
-
-
- return sum_x / 10 ;
- }
- int16_t min_window(int16_t *acc, int length)
- {
- int start_index = ACC_WINDOW_SIZE - length;
-
- int16_t min_val = acc[start_index];
-
- for(int i = 0; i < ACC_WINDOW_SIZE; i++)
- {
- if(acc[i] < min_val)
- {
- min_val = acc[i];
- }
- }
-
- return min_val;
- }
- int16_t max_window(int16_t *acc, int length)
- {
- int start_index = ACC_WINDOW_SIZE - length;
-
- int16_t max_val = acc[start_index];
-
- for(int i = 0; i < ACC_WINDOW_SIZE; i++)
- {
- if(acc[i] > max_val)
- {
- max_val = acc[i];
- }
- }
-
- return max_val;
- }
- int station_window_xyz(int32_t *acc)
- {
- int32_t max_val = acc[0];
- int32_t min_val = acc[0];
-
- for(int i = 0; i < ACC_WINDOW_SIZE; i++)
- {
- if(acc[i] > max_val)
- {
- max_val = acc[i];
- }
-
- if(acc[i] < min_val)
- {
- min_val = acc[i];
- }
- }
-
- if(max_val - min_val < 419430)
- {
- return 1;
- }
-
- return 0;
-
- }
- void detect_step_by_acc(int16_t *acc)
- {
-
-
- static uint32_t time_stamp;
- memcpy(acc_x_buff, acc_x_buff + 1, (ACC_WINDOW_SIZE - 1) * sizeof(int16_t));
-
- acc_x_buff[(ACC_WINDOW_SIZE - 1)] = acc[0];
-
- memcpy(acc_y_buff, acc_y_buff + 1, (ACC_WINDOW_SIZE - 1) * sizeof(int16_t));
-
- acc_y_buff[(ACC_WINDOW_SIZE - 1)] = acc[1];
-
- memcpy(acc_z_buff, acc_z_buff + 1, (ACC_WINDOW_SIZE - 1) * sizeof(int16_t));
-
- acc_z_buff[(ACC_WINDOW_SIZE - 1)] = acc[2];
-
- memcpy(acc_xyz_buff, acc_xyz_buff + 1, (ACC_WINDOW_SIZE - 1) * sizeof(int32_t));
-
- acc_xyz_buff[(ACC_WINDOW_SIZE - 1)] = acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2];
-
-
-
-
-
- detect_raised_hollow(acc_x_buff, ACC_WINDOW_SIZE, 1434, -1434, 1024, &x_raised, &x_hollow);
-
- detect_raised_hollow(acc_y_buff, ACC_WINDOW_SIZE, 1434, -1434, 1024, &y_raised, &y_hollow);
-
- detect_raised_hollow(acc_z_buff, ACC_WINDOW_SIZE, 2458, 1638, 820, &z_raised, &z_hollow);
- if(x_raised == 1 && x_hollow == 1 && zero_x_wait == 0)
- {
- zero_x_wait = 30;
- }
-
- if(y_raised == 1 && y_hollow == 1 && zero_y_wait == 0)
- {
- zero_y_wait = 30;
- }
-
- if(z_raised == 1 && z_hollow == 1 && zero_z_wait == 0)
- {
- zero_z_wait = 30;
- }
- acc_x_status = 0;
- acc_y_status = 0;
- acc_z_status = 0;
-
- if((zero_x_wait > 0 || acc_x_status == 1) && detect_turn_line(acc_x_buff, 10, 0))
- {
- acc_x_status = 1;
- }
-
- if((zero_y_wait > 0 || acc_y_status == 1) && detect_turn_line(acc_y_buff, 10, 0))
- {
- acc_y_status = 1;
- }
-
- if((zero_z_wait > 0 || acc_z_status == 1) && detect_turn_line(acc_z_buff, 10, 1))
- {
- acc_z_status = 1;
- }
-
-
-
- if(severe_turn_point(acc_x_buff, ACC_WINDOW_SIZE))
- {
- acc_x_status = 1;
- }
-
- if(severe_turn_point(acc_y_buff, ACC_WINDOW_SIZE))
- {
- acc_y_status = 1;
- }
-
- if(severe_turn_point_z(acc_z_buff, ACC_WINDOW_SIZE))
- {
- acc_z_status = 1;
- }
-
-
- int special_zupt = 0;
-
- if(var_acc(acc_z_buff, ACC_WINDOW_SIZE) > 2097152 && max_window(acc_z_buff, 10) > 2048 && acc[2] > 1434)
- {
- acc_x_status = 1;
- acc_y_status = 1;
- acc_z_status = 3;
-
- special_zupt = 1;
- }
-
-
- if(last_acc_x_status && station_signal(acc_x_buff, 5))
- {
- acc_x_status = 2;
-
- }
-
- if(last_acc_y_status && station_signal(acc_y_buff, 5))
- {
- acc_y_status = 2;
- }
- if(last_acc_z_status && station_signal(acc_z_buff, 5))
- {
- acc_z_status = 2;
- }
-
- if((acc_x_status || acc_y_status) && abs(acc[2] - 2048) < 410 && station_signal(acc_z_buff, 10))
- {
- acc_z_status = 2;
- }
-
- if(acc_z_status == 0 && last_acc_z_status == 3 && max_window(acc_z_buff, 10) - acc[2] > 2048 && acc[2] > 1434 && acc[2] < 3072)
- {
- acc_z_status = 3;
- }
-
-
- if(time_stamp - last_time_stamp < 20)
- {
-
- if(station_window_xyz(acc_xyz_buff))
- {
- acc_x_status = 1;
- acc_y_status = 1;
- acc_z_status = 1;
- }
- }
-
- if(special_zupt == 0 && (acc[2] < 1638 || abs(acc[0]) > 1024 || abs(acc[1]) > 1024))
- {
- acc_x_status = 0;
- acc_y_status = 0;
- acc_z_status = 0;
- }
-
-
-
- if(zero_x_wait > 0)
- {
- zero_x_wait --;
- if(zero_x_wait == 0)
- {
- x_raised = 0;
- x_hollow = 0;
- }
- }
- if(zero_y_wait > 0)
- {
- zero_y_wait --;
-
- if(zero_y_wait == 0)
- {
- y_raised = 0;
- y_hollow = 0;
- }
- }
-
- if(zero_z_wait > 0)
- {
- zero_z_wait --;
-
- if(zero_z_wait == 0)
- {
- z_raised = 0;
- z_hollow = 0;
- }
- }
- last_acc_x_status = acc_x_status;
- last_acc_y_status = acc_y_status;
- last_acc_z_status = acc_z_status;
-
- if(time_stamp - last_time_stamp < 15)
- {
- if(acc[2] > max_z_unzero)
- {
- max_z_unzero = acc[2];
- }
-
- if(acc[2] < min_z_unzero)
- {
- min_z_unzero = acc[2];
- }
- }
- else
- {
- max_z_unzero = 2048;
- min_z_unzero = 2048;
- }
-
-
-
- if( (acc_z_status > 0 || (acc_x_status > 0 && acc_y_status > 0)) )
- {
-
- if(time_stamp - last_time_stamp > 15)
- {
-
- mFlash.mStep.stepCur[0]++;
- }
- else if(min_z_unzero < 0 && max_z_unzero > 4096 && time_stamp - last_time_stamp > 5)
- {
- mFlash.mStep.stepCur[0]++;
-
- max_z_unzero = 2048;
- min_z_unzero = 2048;
-
- }
- last_time_stamp = time_stamp;
- }
-
-
-
- time_stamp ++;
-
- }
|