stepcounter.cpp

00001 #include "stepcounter.h"
00002 uint8_t step_cnt = 1;
00003 /*
00004 filter_avg_t acc_data;
00005 axis_info_t acc_sample;
00006 peak_value_t acc_peak;
00007 slid_reg_t acc_slid;
00008 */
00009 
00010 
00011 void filter_calculate(filter_avg_t *filter, axis_info_t *sample)
00012 {
00013     unsigned int i;
00014     short x_sum = 0, y_sum = 0, z_sum = 0;
00015     for (i = 0; i < FILTER_CNT; i++)
00016     {
00017         x_sum += filter->info[i].x;
00018         y_sum += filter->info[i].y;
00019         z_sum += filter->info[i].z;
00020     }
00021     sample->x = x_sum / FILTER_CNT;
00022     sample->y = y_sum / FILTER_CNT;
00023     sample->z = z_sum / FILTER_CNT;
00024 }
00025 
00026 void peak_value_init(peak_value_t *peak)
00027 {
00028     peak->newmax.x = -1000;
00029     peak->newmax.y = -1000;
00030     peak->newmax.z = -1000;
00031 
00032     peak->newmin.x = 1000;
00033     peak->newmin.y = 1000;
00034     peak->newmin.z = 1000;
00035 
00036     peak->oldmax.x = 0;
00037     peak->oldmax.y = 0;
00038     peak->oldmax.z = 0;
00039     peak->oldmin.x = 0;
00040     peak->oldmin.y = 0;
00041     peak->oldmin.z = 0;
00042 }
00043 
00044 void peak_update(peak_value_t *peak, axis_info_t *cur_sample)
00045 {
00046     static unsigned int sample_size = 0;
00047     sample_size++;
00048     if (sample_size > SAMPLE_SIZE)
00049     {
00050         /* 50 samples for an update*/
00051         sample_size = 1;
00052         peak->oldmax = peak->newmax;
00053         peak->oldmin = peak->newmin;
00054 
00055         peak->newmax.x = -1000;
00056         peak->newmax.y = -1000;
00057         peak->newmax.z = -1000;
00058 
00059         peak->newmin.x = 1000;
00060         peak->newmin.y = 1000;
00061         peak->newmin.z = 1000;
00062         // peak_value_init(peak);
00063     }
00064     peak->newmax.x = MAX(peak->newmax.x, cur_sample->x);
00065     peak->newmax.y = MAX(peak->newmax.y, cur_sample->y);
00066     peak->newmax.z = MAX(peak->newmax.z, cur_sample->z);
00067 
00068     peak->newmin.x = MIN(peak->newmin.x, cur_sample->x);
00069     peak->newmin.y = MIN(peak->newmin.y, cur_sample->y);
00070     peak->newmin.z = MIN(peak->newmin.z, cur_sample->z);
00071 }
00072 
00073 char slid_update(slid_reg_t *slid, axis_info_t *cur_sample)
00074 {
00075     char res = 0;
00076     if (ABS((cur_sample->x - slid->new_sample.x)) > DYNAMIC_PRECISION)
00077     {
00078         slid->old_sample.x = slid->new_sample.x;
00079         slid->new_sample.x = cur_sample->x;
00080         res = 1;
00081     }
00082     else
00083     {
00084         slid->old_sample.x = slid->new_sample.x;
00085     }
00086     if (ABS((cur_sample->y - slid->new_sample.y)) > DYNAMIC_PRECISION)
00087     {
00088         slid->old_sample.y = slid->new_sample.y;
00089         slid->new_sample.y = cur_sample->y;
00090         res = 1;
00091     }
00092     else
00093     {
00094         slid->old_sample.y = slid->new_sample.y;
00095     }
00096 
00097     if (ABS((cur_sample->z - slid->new_sample.z)) > DYNAMIC_PRECISION)
00098     {
00099         slid->old_sample.z = slid->new_sample.z;
00100         slid->new_sample.z = cur_sample->z;
00101         res = 1;
00102     }
00103     else
00104     {
00105         slid->old_sample.z = slid->new_sample.z;
00106     }
00107     return res;
00108 }
00109 
00110 /* the most active axis */
00111  char is_most_active(peak_value_t *peak)
00112 {
00113     char res = MOST_ACTIVE_NULL;
00114     short x_change = ABS((peak->newmax.x - peak->newmin.x));
00115     short y_change = ABS((peak->newmax.y - peak->newmin.y));
00116     short z_change = ABS((peak->newmax.z - peak->newmin.z));
00117 
00118     if (x_change > y_change && x_change > z_change && x_change >= ACTIVE_PRECISION)
00119     {
00120         res = MOST_ACTIVE_X;
00121     }
00122     else if (y_change > x_change && y_change > z_change && y_change >= ACTIVE_PRECISION)
00123     {
00124         res = MOST_ACTIVE_Y;
00125     }
00126     else if (z_change > x_change && z_change > y_change && z_change >= ACTIVE_PRECISION)
00127     {
00128         res = MOST_ACTIVE_Z;
00129     }
00130     return res;
00131 }
00132 
00133 void detect_step(peak_value_t *peak, slid_reg_t *slid, axis_info_t *cur_sample)
00134 {
00135     // static step_cnt = 0;
00136     //step_cnt++;
00137     char res = is_most_active(peak);
00138     switch (res)
00139     {
00140     case MOST_ACTIVE_NULL:
00141     {
00142         // fix
00143         break;
00144     }
00145     case MOST_ACTIVE_X:
00146     {
00147         short threshold_x = (peak->oldmax.x + peak->oldmin.x) / 2;
00148         // short shreshold_x =
00149         if (slid->old_sample.x > threshold_x && slid->new_sample.x < threshold_x)
00150         {
00151             step_cnt++;
00152         }
00153         break;
00154     }
00155     case MOST_ACTIVE_Y:
00156     {
00157         short threshold_y = (peak->oldmax.y + peak->oldmin.y) / 2;
00158 
00159         //  short threshold_y = 1050;
00160         if (slid->old_sample.y > threshold_y && slid->new_sample.y < threshold_y)
00161         {
00162             step_cnt++;
00163         }
00164         break;
00165     }
00166     case MOST_ACTIVE_Z:
00167     {
00168         short threshold_z = (peak->oldmax.z + peak->oldmin.z) / 2;
00169         if (slid->old_sample.z > threshold_z && slid->new_sample.z < threshold_z)
00170         {
00171             step_cnt++;
00172         }
00173         break;
00174     }
00175     default:
00176         break;
00177     }
00178 }
00179 
00180 uint8_t get_step(){
00181     return step_cnt;
00182 }