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

  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
 20 void init(void);
 21 void Calibrate(void);
 22 void data_transfer(void);
 23 void concatenate_data(void);
 24 void movement_end_check(void);
 25 void position(void);
 26 void main (void)
 27 {
 28  init();
 29  get_threshold();
 30  do
 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 ********************************************************************************/
 39 void Calibrate(void)
 40 {
 41  unsigned int count1;
 42  count1 = 0;
 43  do{
 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 /******************************************************************************************
 54 This function obtains magnitude and direction
 55 In this particular protocol direction and magnitude are sent in separate variables.
 56 Management can be done in many other different ways.
 57 *****************************************************************************************/
 58 void 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];
 64  if (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 }
 74  else {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  }
 82  if (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  }
 89  else {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;
105  while (--delay);
106  SCITxMsg(Sensor_Data); // Data transferring function
107  while (SCIC2 & 0x08);
108 }
109 /*****************************************************************************************/
110 /******************************************************************************************
111 This function returns data format to its original state. When obtaining the magnitude and
112 direction of the position, an inverse two‘s complement is made. This function makes the two‘s
113 complement in order to return the data to it original state.
114 It is important to notice that the sensibility adjustment is greatly impacted here, the amount
115 of "ones" inserted in the mask must be equivalent to the "ones" lost in the shifting made in
116 the previous function upon the sensibility modification.
117  ******************************************************************************************/
118 void data_reintegration(void)
119 {
120  if (direction >=10)
121 {positionX[1]= positionX[1]|0xFFFFC000;} // 18 "ones" inserted. Same size as the
122  //amount of shifts
123  direction = direction & 0x01;
124  if (direction ==1)
125  {positionY[1]= positionY[1]|0xFFFFC000;}
126 }
127 /******************************************************************************************
128 This function allows movement end detection. If a certain number of acceleration samples are
129 equal to zero we can assume movement has stopped. Accumulated Error generated in the velocity
130 calculations is eliminated by resetting the velocity variables. This stops position increment
131 and greatly eliminates position error.
132 ******************************************************************************************/
133 void movement_end_check(void)
134 {
135  if (accelerationx[1]==0) //we count the number of acceleration samples that equals cero
136  { countx++;}
137  else { countx =0;}
138  if (countx>=25) //if this number exceeds 25, we can assume that velocity is cero
139  {
140  velocityx[1]=0;
141  velocityx[0]=0;
142  }
143  if (accelerationy[1]==0) //we do the same for the Y axis
144  { county++;}
145  else { county =0;}
146  if (county>=25)
147  {
148  velocityy[1]=0;
149  velocityy[0]=0;
150  }
151 }
152 /*****************************************************************************************/
153 /******************************************************************************************
154 This function transforms acceleration to a proportional position by integrating the
155 acceleration data twice. It also adjusts sensibility by multiplying the "positionX" and
156 "positionY" variables.
157 This integration algorithm carries error, which is compensated in the "movenemt_end_check"
158 subroutine. Faster sampling frequency implies less error but requires more memory. Keep in
159 mind that the same process is applied to the X and Y axis.
160 *****************************************************************************************/
161 void position(void)
162 {
163 unsigned char count2 ;
164 count2=0;
165  do{
166  ADC_GetAllAxis();
167  accelerationx[1]=accelerationx[1] + Sample_X; //filtering routine for noise attenuation
168  accelerationy[1]=accelerationy[1] + Sample_Y; //64 samples are averaged. The resulting
169 //average represents the acceleration of
170  //an instant
171  count2++;
172  }while (count2!=0x40); // 64 sums of the acceleration sample
173  accelerationx[1]= accelerationx[1]>>6; // division by 64
174  accelerationy[1]= accelerationy[1]>>6;
175  accelerationx[1] = accelerationx[1] - (int)sstatex; //eliminating zero reference
176  //offset of the acceleration data
177  accelerationy[1] = accelerationy[1] - (int)sstatey; // to obtain positive and negative
178  //acceleration
179  if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied
180  {accelerationx[1] = 0;} // to the X axis acceleration
181  //variable
182  if ((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 acceleration
194  accelerationy[0] = accelerationy[1]; //variable in order to introduce the new
195 //acceleration value.
196  velocityx[0] = velocityx[1]; //Same done for the velocity variable
197  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 application
203  data_transfer();
204  positionX[1] = positionX[1]>>18; //once the variables are sent them must return to
205  positionY[1] = positionY[1]>>18; //their original state
206  movement_end_check();
207  positionX[0] = positionX[1]; //actual position data must be sent to the
208  positionY[0] = positionY[1]; //previous position
209  direction = 0; // data variable to direction variable reset
210 }
211 /*****************************************************************************************/