Jingtian Dang / Mbed 2 deprecated 4180project

ECE4180

Dependencies:   mbed PulseSensor mbed-rtos LSM9DS1_Library_cal

stepcounter.cpp@7:88d71c228407, 2021-12-01 (annotated)

Committer:
yutation
Date:
Wed Dec 01 00:20:32 2021 +0000
Revision:
7:88d71c228407
Child:
8:2d43385e7784
4180;

Who changed what in which revision?

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