IMU.pdf

June 3, 2018 | Author: Jessica Avila | Category: Accelerometer, Physics, Physics & Mathematics, Technology, Mathematics

Share Embed Donate

Report this link

Short Description

Download IMU.pdf...

Description

IM� & AH�� F��

IM� & AH�� IM��. E�� E�� , �� . ��   ��. �� .

H�� : �� : •

C��

•

A�� 3

•

IM� I �� IM� M��IM��9 �3 �� P�� : ��://��.��.��/��/ ��://��.��.��/��/2468 2468

F�� , A�� IM�

�� : �� : •

A�� IDE

•

A�� IM� �� ://��.��.��/��/2468/��

•

•

E�� PL��DAQ �� E�� ://��.��.��/��/��

IM�

L��

�

��

� ��

��

M��

OK

N�� OK � ��

� �

�

N��, ��

#include .h> #include .h> #include .h>

// load libraries

L3G gyro; LSM303 compass; float A[4]; float G[4]; float M[4];

// declare variables A for accelerometer, G for gyro and M for magnetometer

void setup() { Serial. Serial .begin begin(9600); (9600); // start talking Wire. Wire .begin begin(); (); gyro.init gyro.init(); (); // start gyro gyro.enableDefault gyro.enableDefault(); (); compass.init compass.init(); (); // start accelerometer and magnetometer compass.enableDefault compass.enableDefault(); (); } void loop() { gyro.read gyro.read(); (); compass.read compass.read(); (); A[1] A[2] A[3] G[1] G[2] G[3] G[1] G[2] G[3]

= = = = = = = = =

compass.a.x; compass.a.y; compass.a.z; gyro.g.x; gyro.g.y; gyro.g.z; gyro.g.x; gyro.g.y; gyro.g.z;

// get data from sensors

// record data

Serial.print Serial. print(A[1]); (A[1]); Serial. Serial.print print(","); (","); Serial. Serial.print print(A[2]); (A[2]); Serial. Serial.print print(","); (","); Serial. Serial. print (A[3]); Serial. Serial.print print(","); (","); // print all data Serial. Serial .print print(G[1]); (G[1]); Serial Serial. .print(","); print(","); Serial Serial. .print(G[2]); print(G[2]); Serial Serial. .print print(","); (","); Serial Serial. . print (G[3]); Serial Serial. .print(","); print(","); Serial. Serial .print print(M[1]); (M[1]); Serial Serial. .print print(","); (","); Serial Serial. .print print(M[2]); (M[2]); Serial Serial. .print print(","); (","); Serial Serial. . print (M[3]); Serial Serial. .print print(","); (","); Serial. Serial .println println(); (); delay(20); delay (20); }

C�� : �� .

#include .h> #include .h> #include .h>

// load libraries

L3G gyro; LSM303 compass; float A[4]; float G[4]; float M[4];

// declare variables A for accelerometer, G for gyro and M for magnetometer

void setup() { Serial. Serial .begin begin(9600); (9600); // start talking Wire. Wire .begin begin(); (); gyro.init gyro.init(); (); // start gyro gyro.enableDefault gyro.enableDefault(); (); compass.init compass.init(); (); // start accelerometer and magnetometer compass.enableDefault compass.enableDefault(); (); } void loop() { gyro.read gyro.read(); (); compass.read compass.read(); (); A[1] A[2] A[3] G[1] G[2] G[3] G[1] G[2] G[3]

= = = = = = = = =

compass.a.x; compass.a.y; compass.a.z; gyro.g.x; gyro.g.y; gyro.g.z; gyro.g.x; gyro.g.y; gyro.g.z;

// get data from sensors

// place holder for data conversion

Serial.print Serial. print(A[1]); (A[1]); Serial. Serial.print print(","); (","); Serial. Serial.print print(A[2]); (A[2]); Serial. Serial.print print(","); (","); Serial. Serial. print (A[3]); Serial. Serial.print print(","); (","); // print all data Serial. Serial .print print(G[1]); (G[1]); Serial Serial. .print(","); print(","); Serial Serial. .print(G[2]); print(G[2]); Serial Serial. .print print(","); (","); Serial Serial. . print (G[3]); Serial Serial. .print(","); print(","); Serial. Serial .print print(M[1]); (M[1]); Serial Serial. .print print(","); (","); Serial Serial. .print print(M[2]); (M[2]); Serial Serial. .print print(","); (","); Serial Serial. . print (M[3]); Serial Serial. .print print(","); (","); Serial. Serial .println println(); (); delay(20); delay (20); }

L�� E��. I� ��

F�� PL��DAQ �� E�� A�� int row = 0;

A�� () () �� Serial.println Serial. println( ("CLEARDATA" "CLEARDATA"); ); Serial. Serial .println println( ("L "LAB ABEL EL,t ,tim ime, e,dt dt in ms ms,a ,acc cce e x, x,ac acce ce y, y,ac acce ce z, z,gy gyro ro x, x,gy gyro ro y, y,gy gyro ro z, z,ma mag g x, x,ma mag g y, y,ma mag g z, z,ro roll ll,p ,pit itch ch,y ,yaw aw" " );

�� () () ��, �� Serial.print Serial. print( ("DATA,TIME," "DATA,TIME,"); ); Serial. Serial .print print(","); (","); Serial Serial. .print print(","); (","); Serial. Serial .print print(A[1]); (A[1]); Serial Serial. .print print(","); (","); Serial. Serial.print print(A[2]); (A[2]); Serial. Serial.print print(","); (","); Serial. Serial.print (A[3]); Serial Serial. .print print(","); (","); // print all data Serial. Serial .print print(G[1]); (G[1]); Serial Serial. .print print(","); (","); Serial Serial. .print print(G[2]); (G[2]); Serial Serial. .print print(","); (","); Serial Serial. .print (G[3]); Serial. Serial.print print(","); (","); Serial. Serial .print print(M[1]); (M[1]); Serial Serial. .print print(","); (","); Serial Serial. .print print(M[2]); (M[2]); Serial Serial. .print print(","); (","); Serial Serial. .print (M[3]); Serial Serial. .print print(","); (","); Serial. Serial .println println(); (); row++; if (row > 500) {row=0; Serial Serial. .println println("ROW,SET,2"); ("ROW,SET,2"); }

F�� , ��: ��://��.��.��/��

O�� PL� �� E��, ��

D��

C��

N��: I �� PL� ��. �� PL� �� , �� , �� : �� BA �� (�� ) �� .

L��

N�� E��, �� , �� . �� , �� . �� . �� 2 �� . �� .

N�� E��, ��

� ��

� ��

� ��

� ��

� ��

� ��

� ��

�� 

F�� ,



N�� ,



F�� .

��

N�� E��, �� I �� () () �� : #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define

ACCEL_X_MIN ((float (( float) ) -16340) // Add Min an Max values from calibration ACCEL_X_MAX ((float (( float) ) 16975) ACCEL_Y_MIN ((float (( float) ) -15830) ACCEL_Y_MAX ((float (( float) ) 16380) ACCEL_Z_MIN ((float (( float) ) -16570) ACCEL_Z_MAX ((float (( float) ) 16910) ACCEL_X_DIR ((int (( int) ) 1 ) // If up and down are reversed then the direction of the sensor is negative ACCEL_Y_DIR ((int (( int) ) -1 ) ACCEL_Z_DIR ((int (( int) ) -1 ) ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f) // The Offset is the average of the Min and MAX values ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f) ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f) ACCEL_X_SCALE ACCEL_X_SCALE (100.0f (100.0f / (ACCEL_X_MAX (ACCEL_X_MAX - ACCEL_X_OFFSET ACCEL_X_OFFSET) ) ) // Scale all accelerometers between -100 and 100 ACCEL_Y_SCALE ACCEL_Y_SCALE (100.0f (100.0f / (ACCEL_Y_MAX (ACCEL_Y_MAX - ACCEL_Y_OFFSET ACCEL_Y_OFFSET) ) ) ACCEL_Z_SCALE ACCEL_Z_SCALE (100.0f (100.0f / (ACCEL_Z_MAX (ACCEL_Z_MAX - ACCEL_Z_OFFSET ACCEL_Z_OFFSET) ) )

N��: �� 100 �� 100 �� , �� 9.8 �� 9.8 �� . I� �� . A �� : ��://��.��.��/ ��://��.��.��/��.�� .��

N��, N��, �� A[1] = compass.a.x; A[2] = compass.a.y; A[3] = compass.a.z;

// place holder for data conversion

I�� A[1] = (compass.a (compass.a.x .x - ACCEL_X_OFFSE ACCEL_X_OFFSET) T) * ACCEL_X_SCALE ACCEL_X_SCALE * ACCEL_X_D ACCEL_X_DIR; IR; // accelerometer’ values are now between -100 and 100 A[2] = (compass.a (compass.a.y .y - ACCEL_Y_OFFSE ACCEL_Y_OFFSET) T) * ACCEL_Y_SCALE ACCEL_Y_SCALE * ACCEL_Y_D ACCEL_Y_DIR; IR; A[3] = (compass.a (compass.a.z .z - ACCEL_Z_OFFSE ACCEL_Z_OFFSET) T) * ACCEL_Z_SCALE ACCEL_Z_SCALE * ACCEL_Z_D ACCEL_Z_DIR; IR;

F�� ! A��

� ��

� ��

� ��

� ��

� ��

� ��

A�� 100 �� 100 ��

N�� G�� , �� (�� ) �� .

N�� L�� , �� 90� ��, �� 90� �� 90� ��.

��

��

��

��

��

� ��

�� A�� define GYRO_OFFSET_X ((float (( float) ) 122.0 ) #define GYRO_OFFSET_Y ((float (( float) ) -496.0 ) #define GYRO_OFFSET_Z ((float (( float) ) 48.0 ) #define GYRO_X_DIR ((int (( int) ) #define GYRO_Y_DIR (( ((int int) ) #define GYRO_Z_DIR ((int (( int) )

1 -1 -1

) ) )

��, �� G[1] = gyro.g.x; G[2] = gyro.g.y; G[3] = gyro.g.z;

B� �� G[1] = (gyro.g. G[1] (gyro.g.x x - GYR GYRO_O O_OFFS FFSET_X ET_X) ) * GYRO_X_D GYRO_X_DIR; IR; G[2] G[2 ] = (gyro.g. (gyro.g.y y - GYRO_O GYRO_OFFS FFSET_Y ET_Y) ) * GYRO_Y_D GYRO_Y_DIR; IR; G[3] G[3 ] = (gyro.g. (gyro.g.z z - GYR GYRO_O O_OFFS FFSET_Z ET_Z) ) * GYRO_Z_D GYRO_Z_DIR; IR;

// This take into account the offset of the gyro and the direction // scale factor is still missing

N�� 0 �� , �� L�� .

�� , �� , �� F�� float dt;

// time between gyro readings in milliseconds

float t

// time = now in milliseconds

= millis();

�� dt = millis millis( () - t; t

= millis millis(); ();

A�� Serial.print Serial. print( ("DATA,TIME," "DATA,TIME,"); ); Serial. Serial .print print(dt); (dt); Serial. Serial .print print(","); (","); // dt is added added here nothing nothing else else change change Serial. Serial .print print(A[1]); (A[1]); Serial Serial. .print print(","); (","); Serial. Serial.print print(A[2]); (A[2]); Serial Serial. .print print(","); (","); Serial Serial. .print (A[3]); Serial Serial. .print print(","); (","); Serial. Serial .print print(G[1]); (G[1]); Serial Serial. .print print(","); (","); Serial Serial. .print print(G[2]); (G[2]); Serial Serial. .print print(","); (","); Serial Serial. .print (G[3]); Serial. Serial.print print(","); (","); Serial. Serial .print print(M[1]); (M[1]); Serial Serial. .print print(","); (","); Serial Serial. .print print(M[2]); (M[2]); Serial Serial. .print print(","); (","); Serial Serial. .print (M[3]); Serial Serial. .print(","); print(","); Serial. Serial .println println(); ();

N�� 0 �� . N��, �� . I� �� 90 �� :

A�� E�� . G��

G�� 90� ��

G��

A�� E�� . G��

�� : 10359600

G�� 90� ��

�� 90� ��, �� 90� / 10359600 = 0.000009

G��

�� . I �� I �� , �� I �� .

F�� A�� define GYRO_OFFSET_X ((float (( float) ) 122.0 ) #define GYRO_OFFSET_Y ((float (( float) ) -496.0 ) #define GYRO_OFFSET_Z ((float (( float) ) 48.0 ) #define GYRO_SCALE ((float (( float) ) 0.0000090) #define GYRO_X_DIR ((int (( int) ) 1 ) #define GYRO_Y_DIR ((int (( int) ) -1 ) #define GYRO_Z_DIR ((int (( int) ) -1 )

��, �� G[1] G[1] = (gyro.g. (gyro.g.x x - GYRO_O GYRO_OFFS FFSET_X ET_X) ) * GYRO_X_D GYRO_X_DIR; IR; G[2] G[2] = (gyro.g. (gyro.g.y y - GYRO_O GYRO_OFFS FFSET_Y ET_Y) ) * GYRO_Y_D GYRO_Y_DIR; IR; G[3] G[3] = (gyro.g. (gyro.g.z z - GYRO_O GYRO_OFFS FFSET_Z ET_Z) ) * GYRO_Z_D GYRO_Z_DIR; IR;

// This take into account the offset of the gyro and the direction

B� �� G[1] = (gyro.g. (gyro.g.x x - GYRO_OFFSET_X GYRO_OFFSET_X) ) * GYRO_SCALE GYRO_SCALE * GYR GYRO_X_DI O_X_DIR; R; G[2] = (gyro.g. (gyro.g.y y - GYRO_OFFSET_Y GYRO_OFFSET_Y) ) * GYRO_SCALE GYRO_SCALE * GYR GYRO_Y_DI O_Y_DIR; R; G[3] = (gyro.g. (gyro.g.z z - GYRO_OFFSET_Z GYRO_OFFSET_Z) ) * GYRO_SCALE GYRO_SCALE * GYR GYRO_Z_DI O_Z_DIR; R;

// now the gyro’s gyro’s data is fully fully calibrated calibrated [degrees [degrees / ms / gyro unit]

G�� !

��

��

��

C�� A �� : ��: ��://��.��/��/��9��/��/�� I �� . �� . O�� . �� , �� I �� I �� . �� E�� (M�� ).

L��

I� �� 3D �� , �� : ��://��.��.��/E��3D�� ://��.��.��/E��3D��.�� .��

M�� 3D �� , ��, �� (�� , ��, �� ). O�� #define #define #define #define #define #define #define #define #define #define #define #define

MAGN_X_MIN ((float (( float) ) -1643) MAGN_X_MAX ((float (( float) ) 3251) MAGN_Y_MIN ((float (( float) ) -2071) MAGN_Y_MAX ((float (( float) ) 2729) MAGN_Z_MIN ((float (( float) ) -1550) MAGN_Z_MAX ((float (( float) ) 3198) MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f) MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f) MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f) MAGN_X_SC MAGN_X_SCALE ALE (100.0f / (MAGN_X_MAX (MAGN_X_MAX - MAGN_X_OFFSET MAGN_X_OFFSET)) )) // again magnetometer is calibrated to be set between -100 and 100 MAGN_Y_SC MAGN_Y_SCALE ALE (100.0f / (MAGN_Y_MAX (MAGN_Y_MAX - MAGN_Y_OFFSET MAGN_Y_OFFSET)) )) MAGN_Z_SC MAGN_Z_SCALE ALE (100.0f / (MAGN_Z_MAX (MAGN_Z_MAX - MAGN_Z_OFFSET MAGN_Z_OFFSET)) ))

C�� M[1] = (compass.m.x); M[2] = (compass.m.y); M[3] = (compass.m.z);

�� M[1] = (compass.m (compass.m.x .x - MAGN_X_OFFSET MAGN_X_OFFSET) ) * MAGN_X_SCALE MAGN_X_SCALE * ACCEL_X_D ACCEL_X_DIR; IR; // note the directions of the magnetometer are the same as the M[2] = (compass.m (compass.m.y .y - MAGN_Y_OFFSET MAGN_Y_OFFSET) ) * MAGN_Y_SCALE MAGN_Y_SCALE * ACCEL_Y_D ACCEL_Y_DIR; IR; // acceler accelerome ometer ter in in my IMU IMU M[3] = (compass.m (compass.m.z .z - MAGN_Z_OFFSET MAGN_Z_OFFSET) ) * MAGN_Z_SCALE MAGN_Z_SCALE * ACCEL_Z_D ACCEL_Z_DIR; IR;

M��

   �    �    �    �     �    �    �    �    �     �    �     �    �    C

M�� 360� ��

M��

   �    �    �    �     �    �    �    �    �     �    �     �    �    C

M�� 360� ��

A�� 9 �� . �� . I �� . M�� : ��://�94��14129454��9��7�056�5�8�89�9�17��0��.�� ://�94��14129454��9��7�056�5�8� 89�9�17��0��.��.��/��/0B ��.��/��/0B0��L�� 0��L��6�2�3��F��D�N �6�2�3��F��D�N��/�� /��.�� .��

AH��

AH�� 1 C��

M�� •

�� 180, ��180, 180� �� 90, 90� ��, ��

�� = �� = �� = ��

 � ��( ,  )

ℎ � ��

  � 

 � �� �  �

��://��.��.��/��/��/...?��=0J434

F�� A�� 2 �� #define TO_RAD(x) TO_RAD(x) (x * 0.0174532925 0.01745329252) 2)

// *pi/180

#define TO_DEG(x) TO_DEG(x) (x * 57.295779513 57.2957795131) 1)

// *180/pi

�� () () �� rol roll_A l_A

= TO_ TO_DE DEG( G( atan2 atan2(A[2], (A[2], A[3]));

pitch_A pitch_A = TO_DEG( TO_DEG( atan atan(A[1]/ (A[1]/sqrt sqrt(A[2]*A[2]+A[3]*A[3]))); (A[2]*A[2]+A[3]*A[3]))); Xh

= M[1] * cos cos(TO_RAD(pitch)) (TO_RAD(pitch)) + M[3] * sin sin(TO_RAD(pitch)); (TO_RAD(pitch));

Yh

= M[1] * sin sin(TO_RAD(roll)) (TO_RAD(roll)) * sin sin(TO_RAD(pitch)) (TO_RAD(pitch)) + M[2] * cos cos(TO (TO_RA _RAD(r D(roll oll)) )) - M[3] M[3] * sin sin(TO_RAD(roll)) (TO_RAD(roll)) * cos cos(TO_RAD(pitch)); (TO_RAD(pitch));

yaw_M

= TO_DEG(atan2 atan2(Yh,Xh)); (Yh,Xh));

�� IM� �� (�� = �� = 0) �� (�� = 0) �� IM� �� : rol roll_A l_A

= TO_ TO_DE DEG( G( atan2 atan2(A (A[2 [2], ], A[3] A[3])) )) - roll roll_i _ini nit t ;

pitch_A pitch_A = TO_DEG( TO_DEG( atan atan(A[1]/ (A[1]/sqrt sqrt(A[ (A[2]* 2]*A[2 A[2]+A[ ]+A[3]* 3]*A[3 A[3])) ])) ) - pitch_ pitch_init init ; Xh

= M[1] * cos cos(TO_RAD(pitch)) (TO_RAD(pitch)) + M[3] * sin sin(TO_RAD(pitch)); (TO_RAD(pitch));

Yh

= M[1] * sin sin(TO_RAD(roll)) (TO_RAD(roll)) * sin sin(TO_RAD(pitch)) (TO_RAD(pitch)) + M[2] * cos cos(TO (TO_RA _RAD(r D(roll oll)) )) - M[3] M[3] * sin sin(TO_RAD(roll)) (TO_RAD(roll)) * cos cos(TO_RAD(pitch)); (TO_RAD(pitch));

yaw_M

= TO_DEG(atan2 atan2(Yh, (Yh,Xh) Xh)) ) - Headin Heading g ;

// roll_init, pitch_init and Heading are the original roll pitch pitch and yaw when the code started

B�� 180, 180� �� 90, 90� �� I �� : ��: float Correction (int ( int angle, float aa)

// corect corect angles between between -90 -90 to 90 or -180 to to 180

{ if

(aa >

angle angle) ) { aa -= angl angle* e*2; 2; }

else if (aa < -ang -angle le) ) { aa aa += ang angle* le*2; 2; } else

{ aa = aa; aa;

}

return aa; }

�� F�� : roll_ roll_A A

= Cor Corre rect ction ion( ( 180 , TO_ TO_DE DEG( G( atan2 atan2(A (A[2 [2], ], A[3 A[3]) ])) ) - roll roll_i _ini nit t );

pitch_A = Correction( Correction( 90 , TO_DEG( TO_DEG( atan atan(A[1]/ (A[1]/sqrt sqrt(A[ (A[2]* 2]*A[2 A[2]+A[ ]+A[3]* 3]*A[3] A[3])) )) ) - pitch_ pitch_ini init t ); Xh

= M[1] * cos cos(TO_RAD(pitch)) (TO_RAD(pitch)) + M[3] * sin sin(TO_RAD(pitch)); (TO_RAD(pitch));

Yh

= M[1] * sin sin(TO_RAD(roll)) (TO_RAD(roll)) * sin sin(TO_RAD(pitch)) (TO_RAD(pitch)) + M[2] * cos cos(TO (TO_RA _RAD(r D(roll oll)) )) - M[3] M[3] * sin sin(TO_RAD(roll)) (TO_RAD(roll)) * cos cos(TO_RAD(pitch)); (TO_RAD(pitch));

yaw_ yaw_M M

= Cor Corre rect ctio ion( n( 180 180 , TO_D TO_DEG EG( ( atan2 atan2(Yh (Yh,Xh ,Xh)) )) - Heading Heading ); );

A�� E�� J�� OK �� : Serial. Serial .print print( ("DATA,TIME," "DATA,TIME,"); ); Serial. Serial .print print(dt); (dt);

Serial. Serial .print print(","); (",");

Serial. Serial .print print(A[1]); (A[1]);

Serial. Serial .print print(","); (","); Serial Serial. .print print(A[2]); (A[2]); Serial Serial. .print print(","); (","); Serial Serial. .print (A[3]); Serial Serial. .print print(","); (",");

Serial. Serial .print print(G[1]); (G[1]);

Serial. Serial .print print(","); (","); Serial Serial. .print print(G[2]); (G[2]); Serial Serial. .print print(","); (","); Serial Serial. .print (G[3]); Serial Serial. .print print(","); (",");

Serial. Serial .print print(M[1]); (M[1]);

Serial. Serial .print print(","); (","); Serial Serial. .print print(M[2]); (M[2]); Serial Serial. .print print(","); (","); Serial Serial. .print (M[3]); Serial Serial. .print print(","); (",");

Serial. Serial .print print(roll_A); (roll_A);

Serial. Serial .print print(","); (","); // roll calculated using accelerometer’s data

Serial. Serial .print print(roll); (roll);

Serial. Serial .print print(","); (","); // final roll, will be used in step 2, not used for now

Serial. Serial .print print(pitch_A); (pitch_A); Serial Serial. .print print(","); (","); // pitch calculated using accelerometer’s data Serial. Serial .print print(pitch); (pitch);

Serial. Serial .print print(","); (","); // final pitch, pitch, will be be used in step step 2 , not used for for now

Serial. Serial .print print(yaw_M); (yaw_M);

Serial. Serial .print print(","); (","); // yaw calculated using magnetometer’s data

Serial. Serial .print print(yaw); (yaw);

Serial. Serial .print print(","); (","); // final yaw, yaw, will be used used in step 2 , not used for for now

Serial. Serial .println println(); ();

A�� E��

AH�� 2 C��

�� I� IM� �� rol roll_G l_G

= rol roll +

dt * G[1] ;

pitch pitch_G _G = pit pitch ch + dt dt * G[2 G[2] ] ; yaw_G

= yaw +

dt * G[3] ;

I� IM� �� , �� E��

��: ��://��.��.��/��/�� ://��.��.��/��/��

F�� : rol roll_G l_G

= ro roll + dt dt * (G (G[1]+ sin sin(TO_RAD(roll))* (TO_RAD(roll))*tan tan(TO_RAD(pitch))*G[2]+ (TO_RAD(pitch))*G[2]+cos cos(TO_RAD(roll))* (TO_RAD(roll))*tan tan(TO_RAD(pitch))*G[3]); (TO_RAD(pitch))*G[3]);

pitc pitch_ h_G G = pit pitch ch + dt dt * (cos (cos(TO_RAD(roll))*G[2](TO_RAD(roll))*G[2]-sin sin(TO_RAD(roll))*G[3]); (TO_RAD(roll))*G[3]); yaw_G

= yaw + dt * (sin sin(TO_RAD(roll))/ (TO_RAD(roll))/cos cos(TO_RAD(pitch))*G[2]+ (TO_RAD(pitch))*G[2]+cos cos(TO_RAD(roll))/ (TO_RAD(roll))/cos cos(TO_RAD(pitch))*G[3] (TO_RAD(pitch))*G[3] );

AH�� 3 C�� 1 �� 2

C��

��: ��://�94��14129454��9��7�056 �� ://�94��14129454��9��7�056 �5�8�89�9�17��0��.��.��/�� 5�8�89�9�17��0��.��.��/��/0B0��L��6�2�3��F��D�N��/��.�� /0B0��L��6�2�3��F��D�N��/��.��

C�� I� �� , �� : rol roll

= 0.0 0.02 * rol roll_A l_A

pitch pitch

= 0.02 0.02 * pitch_A pitch_A + 0.98 0.98 * pitch_ pitch_G G ;

+ 0.98 0.98 * roll roll_G _G

yaw

= 0.02 * yaw_M

+ 0.98 * yaw_G

; ;

B��

OK

G��

�� 0

�� 180 IM� ��

C�� I� �� roll

= CompFilter( CompFilter( 180, roll_A, roll_A, roll_G );

pitch

= CompFilter( CompFilter( 90, pitch_A, pitch_A, pitch_G pitch_G );

yaw

= CompFilter( CompFilter( 180, yaw_M, yaw_G );

I �� : float Comp CompFi Filt lter er ( int angle, float acce, float gyro) { if( if (abs abs(a (acc cce e - gyro gyro) ) > angl angle) e)

{

if(gyro if (gyro < -angle) gyro += angle*2; if(gyro if (gyro >

angle) gyro -= angle*2;

} return 0.02 0.02 * acce acce + 0.98 0.98 * gyro; gyro; }

OK

OK

�� 0

�� 180 IM� ��

C�� CompFilter �� roll

= atan2(0.02*sin atan2(0.02*sin(roll_A) (roll_A) +0.98*sin +0.98*sin(roll_G) (roll_G)

pitch

= atan2(0.02*sin atan2(0.02*sin(pitch_A)+0.98* (pitch_A)+0.98*sin sin(pitch_G) (pitch_G) , 0.02*cos 0.02*cos(pitch_A)+0.98* (pitch_A)+0.98*cos cos(pitch_G)); (pitch_G));

yaw

= atan2(0.02*sin atan2(0.02*sin(yaw_M) (yaw_M)

+0.98*sin +0.98*sin(yaw_G) (yaw_G)

, 0.02*cos 0.02*cos(roll_A) (roll_A) +0.98*cos +0.98*cos(roll_G)) (roll_G)) ; , 0.02*cos 0.02*cos(yaw_M) (yaw_M)

+0.98*cos +0.98*cos(yaw_G)) (yaw_G))

;

�� , �� CompFilter �� .

M�� : ��://��.�� ://��.��.��/��/M�� .��/��/M��

IM� �� AH��

D�� E��

D�� P�� L�� E�� , � ��

F��

�� AH�� , �� IM� �� (�) = � �� , �� < 20�

�� , �� A��, �� E�� , �� . ��

E�� D�� void loop loop() () { // GET DATA --------------------------------------------------------------------------------------------------------------------------------------compass.read compass.read(); (); gyro.read gyro.read(); (); dt = millis millis()-t; ()-t; t = millis millis(); (); // CALIBRATE DATA --------------------------------------------------------------------------------------------------------------------------A[2] = (compas (compass.a.y s.a.y - ACCEL_Y_O ACCEL_Y_OFFSET FFSET) ) * ACCEL_Y_S ACCEL_Y_SCALE CALE * ACCEL_Y ACCEL_Y_DIR; _DIR; // Gather only necessary data A[3] = (compas (compass.a.z s.a.z - ACCEL_Z_O ACCEL_Z_OFFSET FFSET) ) * ACCEL_Z_S ACCEL_Z_SCALE CALE * ACCEL_Z ACCEL_Z_DIR; _DIR; G[1] G[1] = (gyr (gyro. o.g. g.x x - GYRO GYRO_O _OFF FFSE SET_ T_X) X) * GYRO GYRO_S _SCA CALE LE * GYRO GYRO_X _X_D _DIR IR ; // CALCULATE ROLL ONLY ----------------------------------------------------------------------------------------------------------------roll roll_A _A = TO_ TO_DE DEG( G(A[ A[2] 2]/A /A[3 [3]) ]) - roll roll_i _ini nit; t; // no trigonometry here, accurate for angles bellow 30 degrees roll roll_ _G = roll roll + dt * G[1] G[1]; ; // no body frame to Euler conversion here roll = 0.04 * roll_A roll_A + 0.96 * roll_G; roll_G; // simple complementary filter here // PID -----------------------------------------------------------------------------------------------------------------------------------------------erro error r = Setp Setpoi oint nt - roll roll; ; integral = integral integral + error * dt; derivativ derivative e = (error (error - previous_ previous_error error) ) / dt; previo previous_ us_err error or = error error; ; Output = Kp*error + Ki*integral + Kd*derivative; // MOTORS ------------------------------------------------------------------------------------------------------------------------------------------motorGo(0, Output); // function that turn on the motor’s to a specific speed motorGo(1, Output); }

��

GL HF

IMU.pdf

Short Description

Description

Comments

We need your help!