Jingtian Dang / Mbed 2 deprecated 4180project

ECE4180

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 
Project;

Who changed what in which revision?

UserRevisionLine numberNew 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 }