ECE 4180 Final Project
Dependencies: mbed PulseSensor mbed-rtos LSM9DS1_Library_cal
stepcounter.cpp@8:2d43385e7784, 2021-12-02 (annotated)
- Committer:
- yutation
- Date:
- Thu Dec 02 00:58:37 2021 +0000
- Revision:
- 8:2d43385e7784
- Parent:
- 7:88d71c228407
- Child:
- 9:dcbd546412ea
Project;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yutation | 7:88d71c228407 | 1 | #include "stepcounter.h" |
yutation | 8:2d43385e7784 | 2 | uint8_t step_cnt = 1; |
yutation | 8:2d43385e7784 | 3 | /* |
yutation | 8:2d43385e7784 | 4 | filter_avg_t acc_data; |
yutation | 8:2d43385e7784 | 5 | axis_info_t acc_sample; |
yutation | 8:2d43385e7784 | 6 | peak_value_t acc_peak; |
yutation | 8:2d43385e7784 | 7 | slid_reg_t acc_slid; |
yutation | 8:2d43385e7784 | 8 | */ |
yutation | 7:88d71c228407 | 9 | |
yutation | 7:88d71c228407 | 10 | |
yutation | 7:88d71c228407 | 11 | void filter_calculate(filter_avg_t *filter, axis_info_t *sample) |
yutation | 7:88d71c228407 | 12 | { |
yutation | 7:88d71c228407 | 13 | unsigned int i; |
yutation | 7:88d71c228407 | 14 | short x_sum = 0, y_sum = 0, z_sum = 0; |
yutation | 7:88d71c228407 | 15 | for (i = 0; i < FILTER_CNT; i++) |
yutation | 7:88d71c228407 | 16 | { |
yutation | 7:88d71c228407 | 17 | x_sum += filter->info[i].x; |
yutation | 7:88d71c228407 | 18 | y_sum += filter->info[i].y; |
yutation | 7:88d71c228407 | 19 | z_sum += filter->info[i].z; |
yutation | 7:88d71c228407 | 20 | } |
yutation | 7:88d71c228407 | 21 | sample->x = x_sum / FILTER_CNT; |
yutation | 7:88d71c228407 | 22 | sample->y = y_sum / FILTER_CNT; |
yutation | 7:88d71c228407 | 23 | sample->z = z_sum / FILTER_CNT; |
yutation | 7:88d71c228407 | 24 | } |
yutation | 7:88d71c228407 | 25 | |
yutation | 7:88d71c228407 | 26 | void peak_value_init(peak_value_t *peak) |
yutation | 7:88d71c228407 | 27 | { |
yutation | 7:88d71c228407 | 28 | peak->newmax.x = -1000; |
yutation | 7:88d71c228407 | 29 | peak->newmax.y = -1000; |
yutation | 7:88d71c228407 | 30 | peak->newmax.z = -1000; |
yutation | 7:88d71c228407 | 31 | |
yutation | 7:88d71c228407 | 32 | peak->newmin.x = 1000; |
yutation | 7:88d71c228407 | 33 | peak->newmin.y = 1000; |
yutation | 7:88d71c228407 | 34 | peak->newmin.z = 1000; |
yutation | 7:88d71c228407 | 35 | |
yutation | 7:88d71c228407 | 36 | peak->oldmax.x = 0; |
yutation | 7:88d71c228407 | 37 | peak->oldmax.y = 0; |
yutation | 7:88d71c228407 | 38 | peak->oldmax.z = 0; |
yutation | 7:88d71c228407 | 39 | peak->oldmin.x = 0; |
yutation | 7:88d71c228407 | 40 | peak->oldmin.y = 0; |
yutation | 7:88d71c228407 | 41 | peak->oldmin.z = 0; |
yutation | 7:88d71c228407 | 42 | } |
yutation | 7:88d71c228407 | 43 | |
yutation | 7:88d71c228407 | 44 | void peak_update(peak_value_t *peak, axis_info_t *cur_sample) |
yutation | 7:88d71c228407 | 45 | { |
yutation | 7:88d71c228407 | 46 | static unsigned int sample_size = 0; |
yutation | 7:88d71c228407 | 47 | sample_size++; |
yutation | 7:88d71c228407 | 48 | if (sample_size > SAMPLE_SIZE) |
yutation | 7:88d71c228407 | 49 | { |
yutation | 7:88d71c228407 | 50 | /* 50 samples for an update*/ |
yutation | 7:88d71c228407 | 51 | sample_size = 1; |
yutation | 7:88d71c228407 | 52 | peak->oldmax = peak->newmax; |
yutation | 7:88d71c228407 | 53 | peak->oldmin = peak->newmin; |
yutation | 7:88d71c228407 | 54 | |
yutation | 7:88d71c228407 | 55 | peak->newmax.x = -1000; |
yutation | 7:88d71c228407 | 56 | peak->newmax.y = -1000; |
yutation | 7:88d71c228407 | 57 | peak->newmax.z = -1000; |
yutation | 7:88d71c228407 | 58 | |
yutation | 7:88d71c228407 | 59 | peak->newmin.x = 1000; |
yutation | 7:88d71c228407 | 60 | peak->newmin.y = 1000; |
yutation | 7:88d71c228407 | 61 | peak->newmin.z = 1000; |
yutation | 7:88d71c228407 | 62 | // peak_value_init(peak); |
yutation | 7:88d71c228407 | 63 | } |
yutation | 7:88d71c228407 | 64 | peak->newmax.x = MAX(peak->newmax.x, cur_sample->x); |
yutation | 7:88d71c228407 | 65 | peak->newmax.y = MAX(peak->newmax.y, cur_sample->y); |
yutation | 7:88d71c228407 | 66 | peak->newmax.z = MAX(peak->newmax.z, cur_sample->z); |
yutation | 7:88d71c228407 | 67 | |
yutation | 7:88d71c228407 | 68 | peak->newmin.x = MIN(peak->newmin.x, cur_sample->x); |
yutation | 7:88d71c228407 | 69 | peak->newmin.y = MIN(peak->newmin.y, cur_sample->y); |
yutation | 7:88d71c228407 | 70 | peak->newmin.z = MIN(peak->newmin.z, cur_sample->z); |
yutation | 7:88d71c228407 | 71 | } |
yutation | 7:88d71c228407 | 72 | |
yutation | 7:88d71c228407 | 73 | char slid_update(slid_reg_t *slid, axis_info_t *cur_sample) |
yutation | 7:88d71c228407 | 74 | { |
yutation | 7:88d71c228407 | 75 | char res = 0; |
yutation | 7:88d71c228407 | 76 | if (ABS((cur_sample->x - slid->new_sample.x)) > DYNAMIC_PRECISION) |
yutation | 7:88d71c228407 | 77 | { |
yutation | 7:88d71c228407 | 78 | slid->old_sample.x = slid->new_sample.x; |
yutation | 7:88d71c228407 | 79 | slid->new_sample.x = cur_sample->x; |
yutation | 7:88d71c228407 | 80 | res = 1; |
yutation | 7:88d71c228407 | 81 | } |
yutation | 7:88d71c228407 | 82 | else |
yutation | 7:88d71c228407 | 83 | { |
yutation | 7:88d71c228407 | 84 | slid->old_sample.x = slid->new_sample.x; |
yutation | 7:88d71c228407 | 85 | } |
yutation | 7:88d71c228407 | 86 | if (ABS((cur_sample->y - slid->new_sample.y)) > DYNAMIC_PRECISION) |
yutation | 7:88d71c228407 | 87 | { |
yutation | 7:88d71c228407 | 88 | slid->old_sample.y = slid->new_sample.y; |
yutation | 7:88d71c228407 | 89 | slid->new_sample.y = cur_sample->y; |
yutation | 7:88d71c228407 | 90 | res = 1; |
yutation | 7:88d71c228407 | 91 | } |
yutation | 7:88d71c228407 | 92 | else |
yutation | 7:88d71c228407 | 93 | { |
yutation | 7:88d71c228407 | 94 | slid->old_sample.y = slid->new_sample.y; |
yutation | 7:88d71c228407 | 95 | } |
yutation | 7:88d71c228407 | 96 | |
yutation | 7:88d71c228407 | 97 | if (ABS((cur_sample->z - slid->new_sample.z)) > DYNAMIC_PRECISION) |
yutation | 7:88d71c228407 | 98 | { |
yutation | 7:88d71c228407 | 99 | slid->old_sample.z = slid->new_sample.z; |
yutation | 7:88d71c228407 | 100 | slid->new_sample.z = cur_sample->z; |
yutation | 7:88d71c228407 | 101 | res = 1; |
yutation | 7:88d71c228407 | 102 | } |
yutation | 7:88d71c228407 | 103 | else |
yutation | 7:88d71c228407 | 104 | { |
yutation | 7:88d71c228407 | 105 | slid->old_sample.z = slid->new_sample.z; |
yutation | 7:88d71c228407 | 106 | } |
yutation | 7:88d71c228407 | 107 | return res; |
yutation | 7:88d71c228407 | 108 | } |
yutation | 7:88d71c228407 | 109 | |
yutation | 7:88d71c228407 | 110 | /* the most active axis */ |
yutation | 7:88d71c228407 | 111 | char is_most_active(peak_value_t *peak) |
yutation | 7:88d71c228407 | 112 | { |
yutation | 8:2d43385e7784 | 113 | char res = MOST_NULL; |
yutation | 7:88d71c228407 | 114 | short x_change = ABS((peak->newmax.x - peak->newmin.x)); |
yutation | 7:88d71c228407 | 115 | short y_change = ABS((peak->newmax.y - peak->newmin.y)); |
yutation | 7:88d71c228407 | 116 | short z_change = ABS((peak->newmax.z - peak->newmin.z)); |
yutation | 7:88d71c228407 | 117 | |
yutation | 7:88d71c228407 | 118 | if (x_change > y_change && x_change > z_change && x_change >= ACTIVE_PRECISION) |
yutation | 7:88d71c228407 | 119 | { |
yutation | 7:88d71c228407 | 120 | res = MOST_ACTIVE_X; |
yutation | 7:88d71c228407 | 121 | } |
yutation | 7:88d71c228407 | 122 | else if (y_change > x_change && y_change > z_change && y_change >= ACTIVE_PRECISION) |
yutation | 7:88d71c228407 | 123 | { |
yutation | 7:88d71c228407 | 124 | res = MOST_ACTIVE_Y; |
yutation | 7:88d71c228407 | 125 | } |
yutation | 7:88d71c228407 | 126 | else if (z_change > x_change && z_change > y_change && z_change >= ACTIVE_PRECISION) |
yutation | 7:88d71c228407 | 127 | { |
yutation | 7:88d71c228407 | 128 | res = MOST_ACTIVE_Z; |
yutation | 7:88d71c228407 | 129 | } |
yutation | 7:88d71c228407 | 130 | return res; |
yutation | 7:88d71c228407 | 131 | } |
yutation | 7:88d71c228407 | 132 | |
yutation | 7:88d71c228407 | 133 | void detect_step(peak_value_t *peak, slid_reg_t *slid, axis_info_t *cur_sample) |
yutation | 7:88d71c228407 | 134 | { |
yutation | 7:88d71c228407 | 135 | // static step_cnt = 0; |
yutation | 8:2d43385e7784 | 136 | //step_cnt++; |
yutation | 7:88d71c228407 | 137 | char res = is_most_active(peak); |
yutation | 7:88d71c228407 | 138 | switch (res) |
yutation | 7:88d71c228407 | 139 | { |
yutation | 7:88d71c228407 | 140 | case MOST_ACTIVE_NULL: |
yutation | 7:88d71c228407 | 141 | { |
yutation | 7:88d71c228407 | 142 | // fix |
yutation | 7:88d71c228407 | 143 | break; |
yutation | 7:88d71c228407 | 144 | } |
yutation | 7:88d71c228407 | 145 | case MOST_ACTIVE_X: |
yutation | 7:88d71c228407 | 146 | { |
yutation | 7:88d71c228407 | 147 | short threshold_x = (peak->oldmax.x + peak->oldmin.x) / 2; |
yutation | 7:88d71c228407 | 148 | // short shreshold_x = |
yutation | 7:88d71c228407 | 149 | if (slid->old_sample.x > threshold_x && slid->new_sample.x < threshold_x) |
yutation | 7:88d71c228407 | 150 | { |
yutation | 7:88d71c228407 | 151 | step_cnt++; |
yutation | 7:88d71c228407 | 152 | } |
yutation | 7:88d71c228407 | 153 | break; |
yutation | 7:88d71c228407 | 154 | } |
yutation | 7:88d71c228407 | 155 | case MOST_ACTIVE_Y: |
yutation | 7:88d71c228407 | 156 | { |
yutation | 7:88d71c228407 | 157 | short threshold_y = (peak->oldmax.y + peak->oldmin.y) / 2; |
yutation | 7:88d71c228407 | 158 | |
yutation | 7:88d71c228407 | 159 | // short threshold_y = 1050; |
yutation | 7:88d71c228407 | 160 | if (slid->old_sample.y > threshold_y && slid->new_sample.y < threshold_y) |
yutation | 7:88d71c228407 | 161 | { |
yutation | 7:88d71c228407 | 162 | step_cnt++; |
yutation | 7:88d71c228407 | 163 | } |
yutation | 7:88d71c228407 | 164 | break; |
yutation | 7:88d71c228407 | 165 | } |
yutation | 7:88d71c228407 | 166 | case MOST_ACTIVE_Z: |
yutation | 7:88d71c228407 | 167 | { |
yutation | 7:88d71c228407 | 168 | short threshold_z = (peak->oldmax.z + peak->oldmin.z) / 2; |
yutation | 7:88d71c228407 | 169 | if (slid->old_sample.z > threshold_z && slid->new_sample.z < threshold_z) |
yutation | 7:88d71c228407 | 170 | { |
yutation | 7:88d71c228407 | 171 | step_cnt++; |
yutation | 7:88d71c228407 | 172 | } |
yutation | 7:88d71c228407 | 173 | break; |
yutation | 7:88d71c228407 | 174 | } |
yutation | 7:88d71c228407 | 175 | default: |
yutation | 7:88d71c228407 | 176 | break; |
yutation | 7:88d71c228407 | 177 | } |
yutation | 8:2d43385e7784 | 178 | } |
yutation | 8:2d43385e7784 | 179 | |
yutation | 8:2d43385e7784 | 180 | uint8_t get_step(){ |
yutation | 8:2d43385e7784 | 181 | return step_cnt; |
yutation | 7:88d71c228407 | 182 | } |