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

1 #include

2 #include "derivative.h"

3 #include "adc.h"

4 #include "buzzer.h"

5 #include "SCItx.h"

6 #pragma DATA_SEG MY_ZEROPAGE

7 unsigned charnear Sample_X;8 unsigned charnear Sample_Y;9 unsigned charnear Sample_Z;10 unsigned char near Sensor_Data[8];11 unsigned charnear 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 charnear direction;18 unsigned longnear 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 intcount1;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 direction55 In this particular protocol direction and magnitude are sent in separate variables.56 Management can be done in many other different ways.57 ***************************