【翻译】利用加速度求解位置的算法——三轴传感器

                                      1 #include <hidef.h>
  2 #include "derivative.h"  3 #include "adc.h"  4 #include "buzzer.h"  5 #include "SCItx.h"  6#pragma DATA_SEG MY_ZEROPAGE
  7 unsigned char near Sample_X;
  8 unsigned char near Sample_Y;
  9 unsigned char near Sample_Z;
 10 unsigned char near Sensor_Data[8];
 11 unsigned char near countx,county ;
 12 signed int near accelerationx[2], accelerationy[2];
 13 signed long near velocityx[2], velocityy[2];
 14 signed long near positionX[2];
 15 signed long near positionY[2];
 16 signed long near positionZ[2];
 17 unsigned char near direction;
 18 unsigned long near sstatex,sstatey;
 19#pragma DATA_SEG DEFAULT
 20void init(void);
 21void Calibrate(void);
 22void data_transfer(void);
 23void concatenate_data(void);
 24void movement_end_check(void);
 25void position(void);
 26void main (void)
 27{
 28 init();
 29 get_threshold();
 30do 31 {
 32 position();
 33  }while(1);
 34}
 35/*******************************************************************************
 36 The purpose of the calibration routine is to obtain the value of the reference threshold.
 37 It consists on a 1024 samples average in no-movement condition.
 38********************************************************************************/ 39void Calibrate(void)
 40{
 41  unsigned int count1;
 42  count1 = 0;
 43do{
 44 ADC_GetAllAxis();
 45  sstatex = sstatex + Sample_X; // Accumulate Samples 46  sstatey = sstatey + Sample_Y;
 47  count1++;
 48  }while(count1!=0x0400); // 1024 times 49  sstatex=sstatex>>10; // division between 1024 50  sstatey=sstatey>>10;
 51}
 52/*****************************************************************************************/ 53/******************************************************************************************
 54This function obtains magnitude and direction
 55In this particular protocol direction and magnitude are sent in separate variables.
 56Management can be done in many other different ways.
 57*****************************************************************************************/ 58void data_transfer(void)
 59{
 60  signed long positionXbkp;
 61  signed long positionYbkp;
 62  unsigned int delay;
 63  unsigned char posx_seg[4], posy_seg[4];
 64if (positionX[1]>=0) { //This line compares the sign of the X direction data 65  direction= (direction | 0x10); //if its positive the most significant byte 66  posx_seg[0]= positionX[1] & 0x000000FF; // is set to 1 else it is set to 8 67  posx_seg[1]= (positionX[1]>>8) & 0x000000FF; // the data is also managed in the
 68// subsequent lines in order to 69  posx_seg[2]= (positionX[1]>>16) & 0x000000FF; // be sent. The 32 bit variable must be 70  posx_seg[3]= (positionX[1]>>24) & 0x000000FF; // split into 4 different 8 bit
 71// variables in order to be sent via
 72// the 8 bit SCI frame 73}
 74else {direction=(direction | 0x80);
 75  positionXbkp=positionX[1]-1;
 76  positionXbkp=positionXbkp^0xFFFFFFFF;
 77  posx_seg[0]= positionXbkp & 0x000000FF;
 78  posx_seg[1]= (positionXbkp>>8) & 0x000000FF;
 79  posx_seg[2]= (positionXbkp>>16) & 0x000000FF;
 80  posx_seg[3]= (positionXbkp>>24) & 0x000000FF;
 81 }
 82if (positionY[1]>=0) { // Same management than in the previous case 83  direction= (direction | 0x08); // but with the Y data. 84  posy_seg[0]= positionY[1] & 0x000000FF;
 85  posy_seg[1]= (positionY[1]>>8) & 0x000000FF;
 86  posy_seg[2]= (positionY[1]>>16) & 0x000000FF;
 87  posy_seg[3]= (positionY[1]>>24) & 0x000000FF;
 88 }
 89else {direction= (direction | 0x01);
 90  positionYbkp=positionY[1]-1;
 91  positionYbkp=positionYbkp^0xFFFFFFFF;
 92  posy_seg[0]= positionYbkp & 0x000000FF;
 93  posy_seg[1]= (positionYbkp>>8) & 0x000000FF;
 94  posy_seg[2]= (positionYbkp>>16) & 0x000000FF;
 95  posy_seg[3]= (positionYbkp>>24) & 0x000000FF;
 96 }
 97  delay = 0x0100;
 98  Sensor_Data[0] = 0x03;
 99  Sensor_Data[1] = direction;
100  Sensor_Data[2] = posx_seg[3];
101  Sensor_Data[3] = posy_seg[3];
102  Sensor_Data[4] = 0x01;
103  Sensor_Data[5] = 0x01;
104  Sensor_Data[6] = END_OF_FRAME;
105while (--delay);
106  SCITxMsg(Sensor_Data); // Data transferring function107while (SCIC2 & 0x08);
108}
109/*****************************************************************************************/110/******************************************************************************************
111This function returns data format to its original state. When obtaining the magnitude and
112direction of the position, an inverse two‘s complement is made. This function makes the two‘s
113complement in order to return the data to it original state.
114It is important to notice that the sensibility adjustment is greatly impacted here, the amount
115of "ones" inserted in the mask must be equivalent to the "ones" lost in the shifting made in
116the previous function upon the sensibility modification.
117 ******************************************************************************************/118void data_reintegration(void)
119{
120if (direction >=10)
121 {positionX[1]= positionX[1]|0xFFFFC000;} // 18 "ones" inserted. Same size as the
122//amount of shifts123  direction = direction & 0x01;
124if (direction ==1)
125  {positionY[1]= positionY[1]|0xFFFFC000;}
126}
127/******************************************************************************************
128This function allows movement end detection. If a certain number of acceleration samples are
129equal to zero we can assume movement has stopped. Accumulated Error generated in the velocity
130calculations is eliminated by resetting the velocity variables. This stops position increment
131and greatly eliminates position error.
132******************************************************************************************/133void movement_end_check(void)
134{
135if (accelerationx[1]==0) //we count the number of acceleration samples that equals cero136  { countx++;}
137else { countx =0;}
138if (countx>=25) //if this number exceeds 25, we can assume that velocity is cero139 {
140  velocityx[1]=0;
141  velocityx[0]=0;
142 }
143if (accelerationy[1]==0) //we do the same for the Y axis144  { county++;}
145else { county =0;}
146if (county>=25)
147 {
148  velocityy[1]=0;
149  velocityy[0]=0;
150 }
151}
152/*****************************************************************************************/153/******************************************************************************************
154This function transforms acceleration to a proportional position by integrating the
155acceleration data twice. It also adjusts sensibility by multiplying the "positionX" and
156"positionY" variables.
157This integration algorithm carries error, which is compensated in the "movenemt_end_check"
158subroutine. Faster sampling frequency implies less error but requires more memory. Keep in
159mind that the same process is applied to the X and Y axis.
160*****************************************************************************************/161void position(void)
162{
163 unsigned char count2 ;
164 count2=0;
165do{
166 ADC_GetAllAxis();
167  accelerationx[1]=accelerationx[1] + Sample_X; //filtering routine for noise attenuation168  accelerationy[1]=accelerationy[1] + Sample_Y; //64 samples are averaged. The resulting
169//average represents the acceleration of
170//an instant171  count2++;
172  }while (count2!=0x40); // 64 sums of the acceleration sample173  accelerationx[1]= accelerationx[1]>>6; // division by 64174  accelerationy[1]= accelerationy[1]>>6;
175  accelerationx[1] = accelerationx[1] - (int)sstatex; //eliminating zero reference
176//offset of the acceleration data177  accelerationy[1] = accelerationy[1] - (int)sstatey; // to obtain positive and negative
178//acceleration179if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied180  {accelerationx[1] = 0;} // to the X axis acceleration
181//variable182if ((accelerationy[1] <=3)&&(accelerationy[1] >= -3))
183  {accelerationy[1] = 0;}
184//first X integration:185 velocityx[1]= velocityx[0]+ accelerationx[0]+ ((accelerationx[1] -accelerationx[0])>>1);
186//second X integration:187 positionX[1]= positionX[0] + velocityx[0] + ((velocityx[1] - velocityx[0])>>1);
188//first Y integration:189 velocityy[1] = velocityy[0] + accelerationy[0] + ((accelerationy[1] -accelerationy[0])>>1);
190//second Y integration:191 positionY[1] = positionY[0] + velocityy[0] + ((velocityy[1] - velocityy[0])>>1);
192  accelerationx[0] = accelerationx[1]; //The current acceleration value must be sent
193//to the previous acceleration194  accelerationy[0] = accelerationy[1]; //variable in order to introduce the new
195//acceleration value.196  velocityx[0] = velocityx[1]; //Same done for the velocity variable197  velocityy[0] = velocityy[1];
198  positionX[1] = positionX[1]<<18; //The idea behind this shifting (multiplication)
199//is a sensibility adjustment.200  positionY[1] = positionY[1]<<18; //Some applications require adjustments to a
201//particular situation
202//i.e. mouse application203 data_transfer();
204  positionX[1] = positionX[1]>>18; //once the variables are sent them must return to205  positionY[1] = positionY[1]>>18; //their original state206 movement_end_check();
207  positionX[0] = positionX[1]; //actual position data must be sent to the208  positionY[0] = positionY[1]; //previous position209  direction = 0; // data variable to direction variable reset210}
211/*****************************************************************************************/