【KW41Z】步行仪算法

dvd1478

【KW41Z】步行仪算法 [复制链接]

步行仪 一个关键算法是窗函数算法，进ACC进行动态阀值 。

1. void TimeWindow()
   
2. {
   
3.         if(ReReg==2)                //如果是新开始的第一步，直接在记步缓存中加1
   
4.         {
   
5.                 TempSteps++;
   
6.                 Interval=0;  //时间窗
   
7.                 ReReg=1;
   
8.                 InvalidSteps=0;
   
9.         }
   
10.         else                                //如果不是新开始的第一步
    
11.         {
    
12.                 if((Interval>=TIMEWINDOW_MIN)&&(Interval<=TIMEWINDOW_MAX))        //如果时间间隔在有效的时间窗内
    
13.                 {
    
14.                         InvalidSteps=0;
    
15.                         if(ReReg==1)                                        //如果还没有找到规律
    
16.                         {
    
17.                                 TempSteps++;                                //记步缓存加1
    
18.                                 if(TempSteps>=REGULATION)        //如果记步缓存达到所要求的规律数
    
19.                                 {
    
20.                                         ReReg=0;                                //已经找到规律
    
21.                                         STEPS=STEPS+TempSteps;        //更新显示
    
22.                                         TempSteps=0;
    
23.                                 }
    
24.                                 Interval=0;
    
25.                         }
    
26.                         else if(ReReg==0)                                //如果已经找到规律，直接更新显示
    
27.                         {
    
28.                                 STEPS++;
    
29.                                 TempSteps=0;
    
30.                                 Interval=0;
    
31.                         }
    
32.                 }
    
33.                 else if(Interval<TIMEWINDOW_MIN)        //如果时间间隔小于时间窗下限
    
34.                 {
    
35.                         if(ReReg==0)                                        //如果已经找到规律
    
36.                         {
    
37.                                 if(InvalidSteps<255)         InvalidSteps++;        //无效步缓存加1
    
38.                                 if(InvalidSteps>=INVALID)                                //如果无效步达到所要求的数值，则重新寻找规律
    
39.                                 {
    
40.                                         InvalidSteps=0;
    
41.                                         ReReg=1;
    
42.                                         TempSteps=1;
    
43.                                         Interval=0;
    
44.                                 }
    
45.                                 else                                                                        //否则，只丢弃这一次的记步，但是继续记步，不需要重新寻找规律
    
46.                                 {
    
47.                                         Interval=0;
    
48.                                 }
    
49.                         }
    
50.                         else if(ReReg==1)                                //如果还没有找到规律，则之前的寻找规律过程无效，重新寻找规律
    
51.                         {
    
52.                                 InvalidSteps=0;
    
53.                                 ReReg=1;
    
54.                                 TempSteps=1;
    
55.                                 Interval=0;
    
56.                         }
    
57.                 }
    
58.                 else if(Interval>TIMEWINDOW_MAX)        //如果时间间隔大于时间窗上限，记步已经间断，重新寻找规律
    
59.                 {
    
60.                         InvalidSteps=0;
    
61.                         ReReg=1;
    
62.                         TempSteps=1;
    
63.                         Interval=0;
    
64.                 }
    
65.         }
    
66. }

复制代码

二、计步算法

1. void step_counter()
   
2. {        //采样周期50Hz
   
3.         unsigned char jtemp = 0;
   
4. 
   
5. //                PORTD=0xFF;
   
6. 
   
7.         //----------------------------------------------ADC采样----------------------//
   
8.         for(jtemp=X_CHANNEL; jtemp<=Z_CHANNEL; jtemp++)
   
9.         {
   
10.                 #if 0
    
11.                 adc_read(jtemp); //加速度的采样值
    
12. 
    
13.                 //Step_Result[jtemp*2+5]=Adresult>>8;
    
14.                 //Step_Result[jtemp*2+6]=Adresult & 0xFF;
    
15.                
    
16.                 _array3[jtemp]=_array2[jtemp];//滑动平均
    
17.                 _array2[jtemp]=_array1[jtemp];
    
18.                 _array1[jtemp]=_array0[jtemp];
    
19. 
    
20.                 _array0[jtemp]=StepAcc[jtemp];
    
21.                 _adresult[jtemp]=_array0[jtemp]+_array1[jtemp]+_array2[jtemp]+_array3[jtemp];
    
22.                 _adresult[jtemp]=_adresult[jtemp]>>2;//求平均值
    
23.                 #endif
    
24.                
    
25.                 _adresult[jtemp]=StepAcc[jtemp];
    
26.                
    
27.                 if (_adresult[jtemp]>_max[jtemp])
    
28.                 {
    
29.                         _max[jtemp]=_adresult[jtemp]; //
    
30.                 }
    
31.                 if (_adresult[jtemp]<_min[jtemp])
    
32.                 {
    
33.                         _min[jtemp]=_adresult[jtemp];
    
34.                 }
    
35.         }
    
36. 
    
37.         sampling_counter=sampling_counter+1;
    
38. 
    
39.         //----------------------------------计算动态门限和动态精度-----------------------//
    
40.         if (sampling_counter==50*2)  //60
    
41.         {
    
42.                 sampling_counter=0;
    
43. 
    
44.                 for(jtemp=X_CHANNEL; jtemp<=Z_CHANNEL; jtemp++)
    
45.                 {
    
46.                         _vpp[jtemp]=_max[jtemp]-_min[jtemp];//动态阈值
    
47.                         _dc[jtemp]=_min[jtemp]+(_vpp[jtemp]>>1); //中心值
    
48.                         _max[jtemp]=0;
    
49.                         _min[jtemp]=1023;
    
50.                         _bad_flag[jtemp]=0;
    
51.                         if (_vpp[jtemp]>=160)  //动态精度
    
52.                         {
    
53.                                 _precision[jtemp]=_vpp[jtemp]/32; //8
    
54.                         }
    
55.                         else if ((_vpp[jtemp]>=50)&& (_vpp[jtemp]<160))
    
56.                         {
    
57.                                 _precision[jtemp]=5;
    
58.                         }
    
59.                         else if ((_vpp[jtemp]>=15) && (_vpp[jtemp]<50))
    
60.                         {
    
61.                                 _precision[jtemp]=3;
    
62.                         }
    
63. 
    
64.                         else
    
65.                         {
    
66.                                 _precision[jtemp]=2;
    
67.                                 _bad_flag[jtemp]=1;
    
68.                         }
    
69. 
    
70.                 }
    
71.         }
    
72. 
    
73.         //--------------------------线性移位寄存器--------------------------------------
    
74. 
    
75.         for(jtemp=X_CHANNEL; jtemp<=Z_CHANNEL; jtemp++)
    
76.         {  //移位寄存器组可以消除高频噪声，从而保证结果更加精确
    
77.                 _old_fixed[jtemp]=_new_fixed[jtemp];
    
78.                
    
79.                 //如果加速度变化大于预定义精度，则最新的采样结果sample_result移入sample_new寄存器，否则sample_new寄存器保持不变
    
80.                 if (_adresult[jtemp]>=_new_fixed[jtemp])
    
81.                 {
    
82.                         if((_adresult[jtemp]-_new_fixed[jtemp])>=_precision[jtemp])
    
83.                         {
    
84.                                 _new_fixed[jtemp]=_adresult[jtemp];
    
85.                         }
    
86.                 }
    
87.                 if (_adresult[jtemp]<_new_fixed[jtemp])
    
88.                 {
    
89.                         if((_new_fixed[jtemp]-_adresult[jtemp])>=_precision[jtemp])
    
90.                         {
    
91.                                 _new_fixed[jtemp]=_adresult[jtemp];
    
92.                         }
    
93.                 }
    
94.         }
    
95. 
    
96.         //------------------------- 动态门限判决 ----------------------------------
    
97.         if(Interval < 0xFF)        //时间间隔数加1，但不能产生溢出  //调用周期为100Hz
    
98.         {
    
99.                 Interval++;
    
100.         }
     
101.         //步伐迈出的条件定义为：当加速度曲线跨过动态阈值下方时，加速度曲线的斜率为负值(sample_new <sample_old)
     
102.         if ((_vpp[X_CHANNEL]>=_vpp[Y_CHANNEL])&&(_vpp[X_CHANNEL]>=_vpp[Z_CHANNEL]))
     
103.         {//X轴上敏感
     
104.                 if ((_old_fixed[X_CHANNEL]>=_dc[X_CHANNEL])&&(_new_fixed[X_CHANNEL]<_dc[X_CHANNEL])&&(_bad_flag[X_CHANNEL]==0))
     
105.                 {
     
106.                         TimeWindow();
     
107.                         //STEPS=STEPS+1;
     
108.                 }
     
109.         }
     
110.         else if ((_vpp[Y_CHANNEL]>=_vpp[X_CHANNEL])&&(_vpp[Y_CHANNEL]>=_vpp[Z_CHANNEL]))
     
111.         {//Y轴上敏感
     
112.                 if ((_old_fixed[Y_CHANNEL]>=_dc[Y_CHANNEL])&&(_new_fixed[Y_CHANNEL]<_dc[Y_CHANNEL])&&(_bad_flag[Y_CHANNEL]==0))
     
113.                 {
     
114.                         TimeWindow();
     
115.                         //STEPS=STEPS+1;
     
116.                 }
     
117.         }
     
118.         else if ((_vpp[Z_CHANNEL]>=_vpp[Y_CHANNEL])&&(_vpp[Z_CHANNEL]>=_vpp[X_CHANNEL]))
     
119.         {//Z轴上敏感
     
120.                 if ((_old_fixed[Z_CHANNEL]>=_dc[Z_CHANNEL])&&(_new_fixed[Z_CHANNEL]<_dc[Z_CHANNEL])&&(_bad_flag[Z_CHANNEL]==0))
     
121.                 {
     
122.                         TimeWindow();
     
123.                         //STEPS=STEPS+1;
     
124.                 }
     
125.         }
     
126. 
     
127.         //---------------------------------- 保存结果-----------------------------------------
     
128.         //Save_Result();
     
129. }

复制代码

荣丰宇

我也在pudn看到了这个~

dvd1478

荣丰宇 发表于 2017-7-10 13:21
我也在pudn看到了这个~

嗯，源于ADI的

mars4zhu

我记得ADI公开的一个应用笔记？？？？