IM� & AH�� ����������
IM� & AH�� ����������
F�������
F�������
IM� & AH�� ����������
IM� & AH�� ����������
��� ��������� �� ����
��� ��������� �� ���� �������� �� �� ������� � ����� ����� ��
�������� �� �� ������� � ����� ����� ��
������ ���������� �� ����� IM��.
������ ���������� �� ����� IM��.
E�
E��� ������ ���
�� ������ ��� ���� �� ������ ��� �����, �� ��
���� �� ������ ��� �����, �� �� ��� �
��� �
���� ���
���� ���
����
����
�������. ���� �������� ��� ������� �� ��� ��� ����������
�������. ���� �������� ��� ������� �� ��� ��� ����������
���� ���� ����.
���� ���� ����.
H�������
H�������
�
��� ���
�� ���� ����
� ����::
• •C�������
C�������
• •A������
A������ ���
��� �3
�3
• •IM�
IM�
I ����
I ���� IM� M��IM��9 �3
IM� M��IM��9 �3 ���� P�����
���� P����� ����� ����: �����://���
����� ����: �����://���.������.���/�������/
.������.���/�������/2468
2468
F����� ��������� ��� ������� ��������, A������ ��� IM�
F����� ��������� ��� ������� ��������, A������ ��� IM�
��������
��������
�
��� ���
�� ���� ����
� ����::
•
•
A������
A������ IDE
IDE
•
•
A������
A������ ���������
��������� ���
��� IM�
IM� �������
�������
�����://���.������.���/�������/2468/���������
�����://���.������.���/�������/2468/���������
•
•
E����
E����
••
PL��DAQ
PL��DAQ ������
������ ���
��� E����
E����
����://���.��������.���/���������/�������
����://���.��������.���/���������/�������
IM�
IM�
L��� ����� ���� � ���������
L��� ����� ���� � ���������
��
��
��
����
����
�����
�����
���
���
���� �� ��� ���� ��������� ����
���� �� ��� ���� ��������� ����
M��� ���� ���� ��������� �� �����
M��� ���� ���� ��������� �� �����
OK
OK
N��
N�� OK
OK
� �� ���������
� �� ���������
��
��
��
N��, ���� ��� ���� ����
N��, ���� ��� ���� ����
#include <
#include <WireWire.h>.h> // load libraries// load libraries
#include < #include <L3GL3G.h>.h> #include < #include <LSM303LSM303.h>.h> L3G L3G gyro;gyro; LSM303 LSM303 compass;compass; float
floatA[4];A[4]; // declare variables A for accelerometer, G for gyro and M for // declare variables A for accelerometer, G for gyro and M for magnetometermagnetometer
float floatG[4];G[4]; float floatM[4];M[4]; void setup() void setup() { { Serial
Serial..beginbegin(9600);(9600); // start talking// start talking
Wire
Wire..beginbegin();(); gyro.
gyro.initinit();(); // start gyro// start gyro
gyro.
gyro.enableDefaultenableDefault();(); compass.
compass.initinit();(); // start accelerometer and magnetometer// start accelerometer and magnetometer
compass. compass.enableDefaultenableDefault();(); } } void loop() void loop() { { gyro.
gyro.readread();(); // get data from sensors// get data from sensors
compass. compass.readread();(); A[1] = compass.a.x;
A[1] = compass.a.x; // record data// record data
A[2] = compass.a.y; A[2] = compass.a.y; A[3] = compass.a.z; A[3] = compass.a.z; G[1] = gyro.g.x; G[1] = gyro.g.x; G[2] = gyro.g.y; G[2] = gyro.g.y; G[3] = gyro.g.z; G[3] = gyro.g.z; G[1] = gyro.g.x; G[1] = gyro.g.x; G[2] = gyro.g.y; G[2] = gyro.g.y; G[3] = gyro.g.z; G[3] = gyro.g.z; Serial
Serial..printprint(A[1]);(A[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(A[2]);(A[2]); SerialSerial..printprint(",");(","); SerialSerial..printprint(A[3]);(A[3]); SerialSerial..printprint(",");(","); // print all data// print all data
Serial
Serial..printprint(G[1]);(G[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(G[2]);(G[2]); SerialSerial..printprint(",");(","); SerialSerial..printprint(G[3]);(G[3]); SerialSerial..printprint(",");(","); Serial
Serial..printprint(M[1]);(M[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(M[2]);(M[2]); SerialSerial..printprint(",");(","); SerialSerial..printprint(M[3]);(M[3]); SerialSerial..printprint(",");(","); Serial
Serial..printlnprintln();(); delay
delay(20);(20); }
C���� ������ ��� ���� �� ��:
C���� ������ ��� ���� �� ��:
��� ���� ��������� �� ��� ������ ����.
��� ���� ��������� �� ��� ������ ����.
#include <
#include <WireWire.h>.h> // load libraries// load libraries
#include < #include <L3GL3G.h>.h> #include < #include <LSM303LSM303.h>.h> L3G L3G gyro;gyro; LSM303 LSM303 compass;compass; float
floatA[4];A[4]; // declare variables A for accelerometer, G for gyro and M for // declare variables A for accelerometer, G for gyro and M for magnetometermagnetometer
float floatG[4];G[4]; float floatM[4];M[4]; void setup() void setup() { { Serial
Serial..beginbegin(9600);(9600); // start talking// start talking
Wire
Wire..beginbegin();(); gyro.
gyro.initinit();(); // start gyro// start gyro
gyro.
gyro.enableDefaultenableDefault();(); compass.
compass.initinit();(); // start accelerometer and magnetometer// start accelerometer and magnetometer
compass. compass.enableDefaultenableDefault();(); } } void loop() void loop() { { gyro.
gyro.readread();(); // get data from sensors// get data from sensors
compass. compass.readread();(); A[1] = compass.a.x;
A[1] = compass.a.x; // place holder for data conversion// place holder for data conversion
A[2] = compass.a.y; A[2] = compass.a.y; A[3] = compass.a.z; A[3] = compass.a.z; G[1] = gyro.g.x; G[1] = gyro.g.x; G[2] = gyro.g.y; G[2] = gyro.g.y; G[3] = gyro.g.z; G[3] = gyro.g.z; G[1] = gyro.g.x; G[1] = gyro.g.x; G[2] = gyro.g.y; G[2] = gyro.g.y; G[3] = gyro.g.z; G[3] = gyro.g.z; Serial
Serial..printprint(A[1]);(A[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(A[2]);(A[2]); SerialSerial..printprint(",");(","); SerialSerial..printprint(A[3]);(A[3]); SerialSerial..printprint(",");(","); // print all data// print all data
Serial
Serial..printprint(G[1]);(G[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(G[2]);(G[2]); SerialSerial..printprint(",");(","); SerialSerial..printprint(G[3]);(G[3]); SerialSerial..printprint(",");(","); Serial
Serial..printprint(M[1]);(M[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(M[2]);(M[2]); SerialSerial..printprint(",");(","); SerialSerial..printprint(M[3]);(M[3]); SerialSerial..printprint(",");(","); Serial
Serial..printlnprintln();(); delay
delay(20);(20); }
L��� ������ ��� ��� ���� �� E����.
L��� ������ ��� ��� ���� �� E����.
I� �� ��� ��������� ��� �� ������
I� �� ��� ��������� ��� �� ������
F���� ������� PL��DAQ ������ ��� E����
F���� ������� PL��DAQ ������ ��� E����
A�� ����� �������� ��
A�� ����� �������� �� �����
�����
() ��������
() ��������
������� ����� ����� ��
������� ����� ����� �� ����
����
() ��������, �� ���� ���
() ��������, �� ���� ���
SerialSerial..printlnprintln(("CLEARDATA""CLEARDATA");); Serial
Serial..printlnprintln(("L"LABABELEL,t,timime,e,dtdt in in msms,a,accccee x,x,acaccece y,y,acaccece z,z,gygyroro x,x,gygyroro y,y,gygyroro z,z,mamagg x,x,mamagg y,y,mamagg z,z,rorollll,p,pititchch,y,yawaw""););
Serial
Serial..printprint(("DATA,TIME,""DATA,TIME,");); Serial
Serial..printprint(",");(","); SerialSerial..printprint(",");(","); Serial
Serial..printprint(A[1]);(A[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(A[2]);(A[2]); SerialSerial..printprint(",");(","); SerialSerial..printprint (A[3]);(A[3]); SerialSerial..printprint(",");(","); // print all data// print all data
Serial
Serial..printprint(G[1]);(G[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(G[2]);(G[2]); SerialSerial..printprint(",");(","); SerialSerial..printprint (G[3]);(G[3]); SerialSerial..printprint(",");(","); Serial
Serial..printprint(M[1]);(M[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(M[2]);(M[2]); SerialSerial..printprint(",");(","); SerialSerial..printprint (M[3]);(M[3]); SerialSerial..printprint(",");(","); Serial
Serial..printlnprintln();(); row++;
row++;
if (row > 500) {row=0;
if (row > 500) {row=0; SerialSerial..printlnprintln("ROW,SET,2"); }("ROW,SET,2"); }
A�� � ��� ��������
A�� � ��� ��������
int
int row = 0;row = 0;
F�� � �������� �����������, ���: ����://���������.����������.���/�������������������������������������
F�� � �������� �����������, ���: ����://���������.����������.���/�������������������������������������
O��� PL� ������
O��� PL� ������ ���� E����, ������ ��� �����
�� E����, ������ ��� �����
������ ���� ��� �����
������ ���� ��� ����� �������
�������
D��� ������
D��� ������
�� ���� ����
�� ���� ����
C����� � �����
C����� � �����
�� ���� ���� ��
�� ���� ���� ��
���� ����
���� ����
N���: I ��� �������� ���� PL� ������.
N���: I ��� �������� ���� PL� ������.
�� �� PL� ������ ����� ��� ��� �����, �� �� ���� ��� ����, �� ���
�� �� PL� ������ ����� ��� ��� �����, �� �� ���� ��� ����, �� ���
�����: �����
�����: �����
�����
�����
�� �����
�� �����
�������
�������
�� ���� �BA
�� ���� �BA
���� ���� ���
���� ���� ��� ���� ���� ���� (�� ���� �� ����� �� ������) ���
���� ���� ���� (�� ���� �� ����� �� ������) ���
������� ���� ����.
������� ���� ����.
L��� ����
L��� ����
N�� ���� ��� ��� ���� ��� ���� �� ���� ���� ��
N�� ���� ��� ��� ���� ��� ���� �� ���� ���� ��
E����, �� ��� ����� ����������� ��� �������������
E����, �� ��� ����� ����������� ��� �������������
��� ������������� ������� ��� ������������, ������� ���� ��� ������ �� �� ���
��� ������������� ������� ��� ������������, ������� ���� ��� ������ �� �� ���
������� ��� ��� ������������� ��� �� ������.
������� ��� ��� ������������� ��� �� ������.
��� ���� ��� �� ��������� ��� ������ �� ��� ������������� ����� �� �� �� ��
��� ���� ��� �� ��������� ��� ������ �� ��� ������������� ����� �� �� �� ��
����� ��� ������� ��� ������ ��
����� ��� ������� ��� ������ �� ��� ������ ����� ��
��� ������ ����� �� ��������
������������ ��� ��
���� ��� ��
�������, ��� ���� �� ��� ����� �� ������. �� ���� ����� ��� ������������� ����
�������, ��� ���� �� ��� ����� �� ������. �� ���� ����� ��� ������������� ����
�� ������� ������� ���� ����� ��� ������ �� �� ������� ������� �����. ���
�� ������� ������� ���� ����� ��� ������ �� �� ������� ������� �����. ���
���� ������� ��� 2 ������ �� ��� �������� ������.
���� ������� ��� 2 ������ �� ��� �������� ������.
���� ������ ��� ���������� ��� ��� ����� ����.
���� ������ ��� ���������� ��� ��� ����� ����.
� ����
� ����
� ����
� ����
� ��
� ��
� ��
� ��
� ����
� ����
� ��
� ��
� ����
� ����
����������
����������
F���� ����� �
F���� ����� �
���� ���� ��,
���� ���� ��,
N��� ����� �
N��� ����� �
���� ���� ��,
���� ���� ��,
F������ ����� �
F������ ����� �
���� ����
���� ����
��.
��.
���� ���� ��� ������� �� ���� ����� �������
���� ���� ��� ������� �� ���� ����� �������
N�� ���� ��� ��� ���� ��� ���� �� ���� ���� ��
N�� ���� ��� ��� ���� ��� ���� �� ���� ���� ��
E����, �� ��� ����� ����������� ��� �������������
E����, �� ��� ����� ����������� ��� �������������
I ����� ���
I ����� ��� �������
����������� ������� �� ����������� ������ ���
���� ������� �� ����������� ������ ��� �����
�����
() �������� ���� ����:
() �������� ���� ����:
N���: ������� ��� ��������������� ����� ������� �100 ��� 100 �� ���������, ���� ������ ���� ��
N���: ������� ��� ��������������� ����� ������� �100 ��� 100 �� ���������, ���� ������ ���� ��
����� ��� ������������� ������� �9.8 ��� 9.8
����� ��� ������������� ������� �9.8 ��� 9.8 ������� ���
������� ������� ��� ����
���� ��� ���� ������ �����������. I� ���
������ �����������. I� ���
��� ������������
��� �������������� ���� ���� ��
�� ���� ���� �� ���� �� ��������� ������ �� ���
���� �� ��������� ������ �� ��� ����� ������ ���� ��� ������
����� ������ ���� ��� ������
��������.
��������.
A ����
A ���� �������� ��� ����������� ��
�������� ��� ����������� �� ����� ����: ����://���
����� ����: ����://���.��������.���/
.��������.���/���������.����
���������.����
#define ACCEL_X_MIN ((#define ACCEL_X_MIN ((floatfloat) -16340)) -16340) // Add Min an Max values from calibration// Add Min an Max values from calibration
#define ACCEL_X_MAX ((
#define ACCEL_X_MAX ((floatfloat) 16975)) 16975) #define ACCEL_Y_MIN ((
#define ACCEL_Y_MIN ((floatfloat) -15830)) -15830) #define ACCEL_Y_MAX ((
#define ACCEL_Y_MAX ((floatfloat) 16380)) 16380) #define ACCEL_Z_MIN ((
#define ACCEL_Z_MIN ((floatfloat) -16570)) -16570) #define ACCEL_Z_MAX ((
#define ACCEL_Z_MAX ((floatfloat) 16910)) 16910) #define ACCEL_X_DIR ((
#define ACCEL_X_DIR ((intint) ) 1 1 )) // If up and down are reversed then the direction of the sensor is negative// If up and down are reversed then the direction of the sensor is negative
#define ACCEL_Y_DIR ((
#define ACCEL_Y_DIR ((intint) ) -1 -1 )) #define ACCEL_Z_DIR ((
#define ACCEL_Z_DIR ((intint) ) -1 -1 ))
#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f) // The Offset is the average of the Min and MAX values// The Offset is the average of the Min and MAX values
#define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f) #define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f) #define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f) #define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f) #define
#define ACCEL_XACCEL_X_SCALE _SCALE (100.0(100.0f / f / (ACCEL(ACCEL_X_MAX _X_MAX -- ACCEL_XACCEL_X_OFFSET_OFFSET) )) ) // Scale all accelerometers between -100 and 100// Scale all accelerometers between -100 and 100
#define
#define ACCEL_YACCEL_Y_SCALE _SCALE (100.0(100.0f / f / (ACCEL(ACCEL_Y_MAX _Y_MAX -- ACCEL_YACCEL_Y_OFFSET_OFFSET) )) ) #define
#define ACCEL_ZACCEL_Z_SCALE _SCALE (100.0(100.0f / f / (ACCEL(ACCEL_Z_MAX _Z_MAX -- ACCEL_ZACCEL_Z_OFFSET_OFFSET) )) )
N�� ���� ��� ��� ���� ��� ���� �� ���� ���� ��
N�� ���� ��� ��� ���� ��� ���� �� ���� ���� ��
E����, �� ��� ����� ����������� ��� �������������
E����, �� ��� ����� ����������� ��� �������������
N��
N��, �
, ������� ���
������ ��� �������������
������������� ���� ����
���� ����
���������� ������������� ����
���������� ������������� ����
� ����
� ����
������� ����
������� ����
A[1] = compass.a.x;A[1] = compass.a.x; // place holder for data conversion// place holder for data conversion
A[2] = compass.a.y; A[2] = compass.a.y; A[3] = compass.a.z; A[3] = compass.a.z;
I��� ����
I��� ����
A[1]A[1] = (co= (compass.ampass.a.x.x -- ACCEL_XACCEL_X_OFFSE_OFFSET) * T) * ACCEL_ACCEL_X_SCALE X_SCALE * AC* ACCEL_X_DCEL_X_DIR;IR; // accelerometer’ values are now between -100 and 100// accelerometer’ values are now between -100 and 100
A[2]
A[2] = (co= (compass.ampass.a.y.y -- ACCEL_YACCEL_Y_OFFSE_OFFSET) * T) * ACCEL_ACCEL_Y_SCALE Y_SCALE * AC* ACCEL_Y_DCEL_Y_DIR;IR; A[3]
A[3] = (co= (compass.ampass.a.z.z -- ACCEL_ZACCEL_Z_OFFSE_OFFSET) * T) * ACCEL_ACCEL_Z_SCALE Z_SCALE * AC* ACCEL_Z_DCEL_Z_DIR;IR;
F���� �������! A������������ ������ �� � �������� �� ����
F���� �������! A������������ ������ �� � �������� �� ����
A�� ������ ��� ����� ������� �100 ��� 100 ��� ����� ��� �������� ���� ������ ��
A�� ������ ��� ����� ������� �100 ��� 100 ��� ����� ��� �������� ���� ������ �� ���� ��� �������� ����
���� ��� �������� ����
������ �� ��
������ �� ��
� ��
� ��
� ��
� ��
� ����
� ����
� ��
� ��
� ����
� ����
N�� ����� ��������� ��� ������ ������
N�� ����� ��������� ��� ������ ������
G������ ��� ������ �� ��� ���� �� ����, ����� ��� ������ ����� (��� ������) ���
G������ ��� ������ �� ��� ���� �� ����, ����� ��� ������ ����� (��� ������) ���
���� �� ��� ������ ������ ��� � � ��� �.
���� �� ��� ������ ������ ��� � � ��� �.
N�� ���
N�� ������ ����
� ��������� ��� ��
����� ��� �����
���� �������
� ���������
��
L���� ����� ��� ��������� �� ��� ������, �� ������� 90� ��������, �������� 90�
L���� ����� ��� ��������� �� ��� ������, �� ������� 90� ��������, �������� 90�
�������� ��� ������ 90� ��������.
�������� ��� ������ 90� ��������.
��� �
��� �
���� �� �
���� �� �
��� �
��� �
���� �� �
���� �� �
����
����
��
��
�
�
��� �
��� �
� ��� � ���� ��� �� ��� ����� ���������
� ��� � ���� ��� �� ��� ����� ���������
�
�� ���
� ��� ����� �����������
����� ����������� ��� �
��� �����
����� ����
� ����
A�� ������ ��� ���������� �� ��� �
A�� ������ ��� ���������� �� ��� ���� �� �����������
��� �� �����������
define GYRO_OFFSET_X ((
define GYRO_OFFSET_X ((float
float
)
)
122.0
122.0 )
)
#define GYRO_OFFSET_Y ((
#define GYRO_OFFSET_Y ((float
float
) -496.0 )
) -496.0 )
#define GYRO_OFFSET_Z ((
#define GYRO_OFFSET_Z ((float
float
)
)
48.0
48.0 )
)
#define GYRO_X_DIR ((
#define GYRO_X_DIR ((int
int
)
)
1
1
)
)
#define GYRO_Y_DIR ((
#define GYRO_Y_DIR ((int
int
)
)
-1
-1
)
)
#define GYRO_Z_DIR ((
#define GYRO_Z_DIR ((int
int
)
)
-1
-1
)
)
����, ������� ���� �����
����, ������� ���� �����
G[1] = gyro.g.x;
G[1] = gyro.g.x;
G[2] = gyro.g.y;
G[2] = gyro.g.y;
G[3] = gyro.g.z;
G[3] = gyro.g.z;
B� ���� ���
B� ���� ���
G[1
G[1] = (gyr
] = (gyro.g.
o.g.x
x -
- GYR
GYRO_O
O_OFFS
FFSET_X
ET_X) * GYRO
) * GYRO_X_D
_X_DIR;
IR;
// This take into account the offset of the gyro and the direction
// This take into account the offset of the gyro and the direction
G[2
G[2] = (gyr
] = (gyro.g.
o.g.y
y -
- GYR
GYRO_O
O_OFFS
FFSET_Y
ET_Y) * GYRO
) * GYRO_Y_D
_Y_DIR;
IR;
// scale factor is still missing
// scale factor is still missing
G[3
N�� ��� ������ ���� �� 0 ���� ��� ���� �� ���
N�� ��� ������ ���� �� 0 ���� ��� ���� �� ���
������ ��� ��������� �������, ��� ���� ��� ��
������ ��� ��������� �������, ��� ���� ��� ��
�����
�����
L���� ��������� ��� ������ ����� ������.
L���� ��������� ��� ������ ����� ������.
�� ���� �� ���� ��� ��� ����� �� ��� ����
�� ���� �� ���� ��� ��� ����� �� ��� ����
��� ���� �������� � ���� �� ������, ��� ��� ������ ������, �� �� ���� ���� ����
��� ���� �������� � ���� �� ������, ��� ��� ������ ������, �� �� ���� ���� ����
���� �������
���� �������
F���� ������� ���� ���������
F���� ������� ���� ���������
floatfloat dt;dt; // time between gyro readings in milliseconds// time between gyro readings in milliseconds
float
float t t = = millis();millis(); // time = now in milliseconds// time = now in milliseconds
���� ����� ����� ������� ��� ���� ���� ���
���� ����� ����� ������� ��� ���� ���� ���
d dtt == millismillis(() ) -- tt;; t t == millismillis();();A�� ��� ���� ������� �������� ���� �����
A�� ��� ���� ������� �������� ���� �����
SerialSerial..printprint(("DATA,TIME,""DATA,TIME,");); Serial
Serial..printprint(dt);(dt); SerialSerial..printprint(",");(","); // dt// dt is addeis added here nod here nothing ething else chalse changenge
Serial
Serial..printprint(A[1]);(A[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(A[2]);(A[2]); SerialSerial..printprint(",");(","); SerialSerial..printprint (A[3]);(A[3]); SerialSerial..printprint(",");(","); Serial
Serial..printprint(G[1]);(G[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(G[2]);(G[2]); SerialSerial..printprint(",");(","); SerialSerial..printprint (G[3]);(G[3]); SerialSerial..printprint(",");(","); Serial
Serial..printprint(M[1]);(M[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(M[2]);(M[2]); SerialSerial..printprint(",");(","); SerialSerial..printprint (M[3]);(M[3]); SerialSerial..printprint(",");(","); Serial
N�� ��� ������ ���� �� 0 ���� ��� ���� �� ���
N�� ��� ������ ���� �� 0 ���� ��� ���� �� ���
������ ��� ���������� ��� �������.
������ ��� ���������� ��� �������.
N���, �� ���� �� ������� ��� ���� ���������
N���, �� ���� �� ������� ��� ���� ���������
����� �� ��������� ������.
����� �� ��������� ������.
I� ����� �� �� ���� �� ���� ��� ���� �� 90
I� ����� �� �� ���� �� ���� ��� ���� �� 90
������� ��� ������ ��� ����:
������� ��� ������ ��� ����:
A�� �� E���� � ������ ���� ��� ��. G��� � ��
A�� �� E���� � ������ ���� ��� ��. G��� � ��
G��� ���� ��� 90� �����
G��� ���� ��� 90� �����
G��� � ��
G��� � ��
A�� �� E���� � ������ ���� ��� ��.
A�� �� E���� � ������ ���� ��� ��. G��� � ��
G��� � ��
G��� ���� ��� 90� �����
G��� ���� ��� 90� �����
G��� � ��
G��� � ��
�� ��� �����
�� ��� ����� ��� ������
��� ������
��� ���: 10359600
��� ���: 10359600
���� ������ ���������� �
���� ������ ���������� �
90� �����, ��������� ��� ����
90� �����, ��������� ��� ����
����� ��
����� ��
90� / 10359600 = 0.000009
90� / 10359600 = 0.000009
��� ������ ������ ���� ����
��� ������ ������ ���� ����
��� ��� ����. I ������� ���� I
��� ��� ����. I ������� ���� I
��� ������� ��� ���� �����
��� ������� ��� ���� �����
������ ��� ���� ����� ��� ���,
������ ��� ���� ����� ��� ���,
�� I �� ����� ���� ��� ����
�� I �� ����� ���� ��� ����
����� ������ ��� ��� ����� ����.
����� ������ ��� ��� ����� ����.
F����
F���� �����
������ �����������
� �����������
A�� ������ ��� ���������� �� ��� �
A�� ������ ��� ���������� �� ��� ���� �� �����������
��� �� �����������
define GYRO_OFFSET_X ((
define GYRO_OFFSET_X ((floatfloat) ) 122.0 122.0 )) #define GYRO_OFFSET_Y ((
#define GYRO_OFFSET_Y ((floatfloat) ) -496.0 -496.0 )) #define GYRO_OFFSET_Z ((
#define GYRO_OFFSET_Z ((floatfloat) ) 48.0 48.0 )) #define GYRO_SCALE ((
#define GYRO_SCALE ((floatfloat) ) 0.0000090)0.0000090) #define GYRO_X_DIR ((
#define GYRO_X_DIR ((intint) ) 1 1 )) #define GYRO_Y_DIR ((
#define GYRO_Y_DIR ((intint) ) -1 -1 )) #define GYRO_Z_DIR ((
#define GYRO_Z_DIR ((intint) ) -1 -1 ))
����, ������� ���� �����
����, ������� ���� �����
G[1
G[1] = (gyr] = (gyro.g.o.g.xx -- GYRGYRO_OO_OFFSFFSET_XET_X) * GYRO) * GYRO_X_D_X_DIR;IR; // This take into account the offset of the gyro and the direction// This take into account the offset of the gyro and the direction
G[2
G[2] = (gyr] = (gyro.g.o.g.yy -- GYRGYRO_OO_OFFSFFSET_YET_Y) * GYRO) * GYRO_Y_D_Y_DIR;IR; G[3
G[3] = (gyr] = (gyro.g.o.g.zz -- GYRGYRO_OO_OFFSFFSET_ZET_Z) * GYRO) * GYRO_Z_D_Z_DIR;IR;
B� ���� ���
B� ���� ���
G[1]
G[1] = (g= (gyro.g.yro.g.xx -- GYRO_OGYRO_OFFSET_XFFSET_X) * ) * GYRO_SGYRO_SCALE CALE * GYR* GYRO_X_DIO_X_DIR;R; // now the gyr// now the gyro’s data is fullo’s data is fully calibraty calibrated [degreed [degrees / mses / ms / gyro unit]/ gyro unit]
G[2]
G[2] = (g= (gyro.g.yro.g.yy -- GYRO_OGYRO_OFFSET_YFFSET_Y) * ) * GYRO_SGYRO_SCALE CALE * GYR* GYRO_Y_DIO_Y_DIR;R; G[3]
G��� �� ��� ���������� !
G��� �� ��� ���������� !
����
C���������� �� ��� ������������ ��� �� �������
C���������� �� ��� ������������ ��� �� �������
A �
A ���� ���
��� ��������
����� ���
��� �� �
�� ����� ���
���� ����:
�:
�����://������.���/�������/������9��������/����/��������
�����://������.���/�������/������9��������/����/��������
I ���� �
I ���� � ������ ������
������ ����������� ����� ��� ��� ��� ������� ��
����� ����� ��� ��� ��� ������� �� ���
���
��������������
��������������� �������
� ����������� ���� ����
���� ���� ���� �������. ��� ������
�������. ��� ������ ���� ��
���� ��
���� �� �� ��� ���� �� ��� ��� ��� ��� ��� ������ ������� ���
���� �� �� ��� ���� �� ��� ��� ��� ��� ��� ������ ������� ���
���� ��
���� �� �������
��������� ��� ���
�� ��� ������������� �
���������� ������� �� ���
������ �� ��� ��������� ��
��������� ��
��� �
��� ������
������ �������� �����.
� �������� �����.
O�� �������� ��� �� �� ���� ���� ��� ���� �� ������ ��� ����
O�� �������� ��� �� �� ���� ���� ��� ���� �� ������ ��� ���� ��
��
�� �����. ���� �����, ��� I ������ �� ��� ��� ���� �� � ����� ��
�� �����. ���� �����, ��� I ������ �� ��� ��� ���� �� � ����� ��
����� �� ���� ���
����� �� ���� ���� I ��
� I �� �� ��� ����� �����. ����
�� ��� ����� �����. ���� ��� ���� ��
��� ���� �� E����
E����
(M����� ����� �� ���� ������).
(M����� ����� �� ���� ������).
L��� ���� �������������� ���
L��� ���� �������������� ���
��
��� ���
��� ���
��
I� ��� ���� �� ���� � ���� �� 3D �� �����, ��� ���� ���� ����
I� ��� ���� �� ���� � ���� �� 3D �� �����, ��� ���� ���� ����
���� ����:
���� ����: ����://���.����.��/E����3D������
����://���.����.��/E����3D�����������.���
�����.���
M����������� �����������
M����������� �����������
������������� �� ��� ���� �� 3D �� ���
������������� �� ��� ���� �� 3D �� ��� ���������
���������, ��� ������ �� ���� ����
, ��� ������ �� ���� ����
������� ������� �� ��������� (��� ��
������� ������� �� ��������� (��� �������, ������������
�����, ���������������
� ���� ��
���� ��
������ �� �� ���� ���������� ������ �������� �����). O��� ��� ���
������ �� �� ���� ���������� ������ �������� �����). O��� ��� ���
��� ���� ���� ����������
��� ���� ���� ���������� ��� ����������� �������
��� ����������� ���������� �� �����������
��� �� �����������
#define MAGN_X_MIN ((#define MAGN_X_MIN ((floatfloat) -1643)) -1643) #define MAGN_X_MAX ((
#define MAGN_X_MAX ((floatfloat) 3251)) 3251) #define MAGN_Y_MIN ((
#define MAGN_Y_MIN ((floatfloat) -2071)) -2071) #define MAGN_Y_MAX ((
#define MAGN_Y_MAX ((floatfloat) 2729)) 2729) #define MAGN_Z_MIN ((
#define MAGN_Z_MIN ((floatfloat) -1550)) -1550) #define MAGN_Z_MAX ((
#define MAGN_Z_MAX ((floatfloat) 3198)) 3198)
#define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f) #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f) #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f) #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f) #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f) #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f) #define MAGN
#define MAGN_X_SC_X_SCALE (100.0f / (MAGN_X_ALE (100.0f / (MAGN_X_MAX -MAX - MAGN_XMAGN_X_OFFSET_OFFSET)))) // again magnetometer is calibrated to be set between -100 and 100// again magnetometer is calibrated to be set between -100 and 100
#define MAGN
#define MAGN_Y_SC_Y_SCALE (100.0f / (MAGN_Y_ALE (100.0f / (MAGN_Y_MAX -MAX - MAGN_YMAGN_Y_OFFSET_OFFSET)))) #define MAGN
#define MAGN_Z_SC_Z_SCALE (100.0f / (MAGN_Z_ALE (100.0f / (MAGN_Z_MAX -MAX - MAGN_ZMAGN_Z_OFFSET_OFFSET))))
C����� ��� ������������ ������ ����
C����� ��� ������������ ������ ����
M[1] = (compass.m.x); M[1] = (compass.m.x); M[2] = (compass.m.y); M[2] = (compass.m.y); M[3] = (compass.m.z); M[3] = (compass.m.z);�
��
�
M[1]M[1] = (co= (compass.mmpass.m.x.x -- MAGN_X_MAGN_X_OFFSETOFFSET) * ) * MAGN_XMAGN_X_SCALE _SCALE * ACC* ACCEL_X_DEL_X_DIR;IR; // note the directions of the magnetometer are the same as the// note the directions of the magnetometer are the same as the
M[2]
M[2] = (co= (compass.mmpass.m.y.y -- MAGN_Y_MAGN_Y_OFFSETOFFSET) * ) * MAGN_YMAGN_Y_SCALE _SCALE * ACC* ACCEL_Y_DEL_Y_DIR;IR; //// acceaccelerleromeometer iter in my IMn my IMUU
M[3]
M����������� �� ��� ����������
M����������� �� ��� ����������
M����������� ������ ��� 360� ���
M����������� ������ ��� 360� ���
C C � � � � � � � � � � � � � � � � � � � � � � � � � �M����������� �� ��� ����������
M����������� �� ��� ����������
M����������� ������ ��� 360� �����
M����������� ������ ��� 360� �����
C C � � � � � � � � � � � � � � � � � � � � � � � � � �A�� 9 ������� � ��� ����������.
A�� 9 ������� � ��� ����������.
�
��
�
���� �� ��� ���� �� ��������� ���� ����� ��� ���.
���� �� ��� ���� �� ��������� ���� ����� ��� ���.
I �� ����� �� ��� � ������������� ������.
I �� ����� �� ��� � ������������� ������.
M��� ���� ����� �� ��� ��� �� ����� ��� �
M��� ���� ����� �� ��� ��� �� ����� ��� �
������������� ������ ����� ����:
������������� ������ ����� ����:
�����://�94��14129454��9��7�056�5�8�
�����://�94��14129454��9��7�056�5�8�89�9�17��0��.��������
89�9�17��0��.�����������.���/����/0B
���.���/����/0B0���L����
0���L�����6�2�3��F��D�N
�6�2�3��F��D�N���/������
���/������.���
.���
AH��
AH��
AH��
AH��
���� 1 C�������� ���� ����� ��� ����� ������������� ��� ������������
���� 1 C�������� ���� ����� ��� ����� ������������� ��� ������������
M��� �����������
M��� �����������
• •����
���� �� �������
�� ������� �� ��� ��1
�� ��� ��180, 180� �
80, 180� ����� ���
���� ��� ����� ��
����� ��
������� �� ���
������� �� ��� ��90, 90�
��90, 90� �����, ������
�����, ��������� ���
��� ��� ��� ���
��� ��� ���
���
���� �������� �� ��������� ���� ���� ��� ����� �� ��
���� �������� �� ��������� ���� ���� ��� ����� �� �� ��
��
����� �� ���� ���������
����� �� ���� ���������
��� ���������
��� ���������
� = ������������� � =
� = ������������� � = ���� � = ������������
���� � = ������������
� �����
� �����
((
,,
))
ℎ
ℎ
� ����
� ����
�
�
� ��
� ��
��
�
�
�����://���.������.���/����/��������/...?�������=0J434
�����://���.������.���/����/��������/...?�������=0J434
F���� ��������� ���� ����� ��� ���
F���� ��������� ���� ����� ��� ���
A�� 2 �����������
A�� 2 �����������
#define
#define TO_RAD(TO_RAD(x) (x * x) (x * 0.01740.01745329255329252)2) // *pi/180// *pi/180
#define
#define TO_DEG(TO_DEG(x) (x * x) (x * 57.29557.2957795137795131)1) // *180/pi// *180/pi
���� ����� ���� ���������� ��
���� ����� ���� ���������� �� ����
����
() �������� ���
() �������� ���
ro
rolll_l_AA = T= TO_O_DEDEG(G( atan2atan2(A[2], A[3]));(A[2], A[3]));
pit
pitch_Ach_A = = TO_DTO_DEG(EG( atanatan(A[1]/(A[1]/sqrtsqrt(A[2]*A[2]+A[3]*A[3])));(A[2]*A[2]+A[3]*A[3])));
X
Xhh = = MM[[11] ] ** coscos(TO_RAD(pitch)) + M[3] *(TO_RAD(pitch)) + M[3] * sinsin(TO_RAD(pitch));(TO_RAD(pitch)); Y
Yhh = = MM[[11] ] ** sinsin(TO_RAD(roll)) *(TO_RAD(roll)) * sinsin(TO_RAD(pitch)) + M[2] * cos(TO_RAD(pitch)) + M[2] * cos(TO(TO_RA_RAD(rD(rolloll)) -)) - M[3M[3] *] * sinsin(TO_RAD(roll)) *(TO_RAD(roll)) * coscos(TO_RAD(pitch));(TO_RAD(pitch)); y
���� ��
���� �� � ���� ��������� ��� ����� ��������� �������
� ���� ��������� ��� ����� ��������� �������
���� ��� IM� �� ���� (���� = ����� = 0)
���� ��� IM� �� ���� (���� = ����� = 0) ��� �������� ��
��� �������� ��
��� ����� ��������� (��� = 0)
��� ����� ��������� (��� = 0)
����� ��������� ���� ���� ������
����� ��������� ���� ���� ������� ���� ��� IM�
� ���� ��� IM� ����� ��� ��
����� ��� �� ��
�������
�����
���� ��� �������� �� ���
���� ��� �������� �� ��� ����� ���������:
����� ���������:
rorolll_l_AA = T= TO_O_DEDEG(G( atan2atan2(A[2(A[2], ], A[A[3]3])) -)) - rorollll_i_ininitt ;;
pit
pitch_Ach_A = = TO_DTO_DEG(EG( atanatan(A[1]/(A[1]/sqrtsqrt(A[(A[2]*2]*A[2A[2]+A[]+A[3]*3]*A[3A[3])) ])) ) -) - pitpitch_ch_initinit ;;
X
Xhh = = MM[[11] ] ** coscos(TO_RAD(pitch)) + M[3] *(TO_RAD(pitch)) + M[3] * sinsin(TO_RAD(pitch));(TO_RAD(pitch)); Y
Yhh = = MM[[11] ] ** sinsin(TO_RAD(roll)) *(TO_RAD(roll)) * sinsin(TO_RAD(pitch)) + M[2] * cos(TO_RAD(pitch)) + M[2] * cos(TO(TO_RA_RAD(rD(rolloll)) -)) - M[3M[3] *] * sinsin(TO_RAD(roll)) *(TO_RAD(roll)) * coscos(TO_RAD(pitch));(TO_RAD(pitch)); y
yaaww__MM = = TTOO__DDEEGG((atan2atan2(Yh,(Yh,Xh)Xh)) -) - HeaHeadinding ;g ;
// roll_init, pitch_init
B�� ��� ��������� ����� ������� ������� ��
B�� ��� ��������� ����� ������� ������� �� ���
���
��180, 180� �� ��90, 90�
��180, 180� �� ��90, 90� ����
������
��
�� I ������� � �������� �� ��� ��� ������ �� ��� ��
�� I ������� � �������� �� ��� ��� ������ �� ��� ����� ����
��� ������:
��:
floatfloat Correction (Correction (intint angle,angle, floatfloat aa)aa) // corec// corectt angles bangles between -9etween -90 to 90 or -180 t0 to 90 or -180 to 180o 180
{ {
if
if (aa(aa > > anganglele) ) { { aaaa -= -= ananglgle*e*2; 2; }} else if
else if (aa(aa < -< -anganglele) { a) { aaa += a+= angngle*le*2; }2; } else
else { { aaaa = = aaaa; ; }} return
return aa;aa; }
}
��� F���� ���� ����� ��� �������� ��������� ���:
��� F���� ���� ����� ��� �������� ��������� ���:
rol
roll_l_AA = Cor= Correrectctionion( 180 , T( 180 , TO_O_DEDEG(G( atan2atan2(A(A[2[2], A], A[3[3])])) -) - rorollll_i_ininitt ););
pitch_A
pitch_A = Corre= Correction( ction( 90 , TO90 , TO_DEG(_DEG( atanatan(A[1]/(A[1]/sqrtsqrt(A[(A[2]*2]*A[2A[2]+A[]+A[3]*3]*A[3]A[3])) ) )) ) -- pitpitch_ch_iniinitt ););
X
Xhh = = MM[[11] ] ** coscos(TO_RAD(pitch)) + M[3] *(TO_RAD(pitch)) + M[3] * sinsin(TO_RAD(pitch));(TO_RAD(pitch)); Y
Yhh = = MM[[11] ] ** sinsin(TO_RAD(roll)) *(TO_RAD(roll)) * sinsin(TO_RAD(pitch)) + M[2] * cos(TO_RAD(pitch)) + M[2] * cos(TO(TO_RA_RAD(rD(rolloll)) -)) - M[3M[3] *] * sinsin(TO_RAD(roll)) *(TO_RAD(roll)) * coscos(TO_RAD(pitch));(TO_RAD(pitch)); ya
A�� ��� ���� ���� E����
A�� ��� ���� ���� E����
J��� �� ����� ���� ���������� �� OK
J��� �� ����� ���� ���������� �� OK
������� ����� ����� �� ���� ���:
������� ����� ����� �� ���� ���:
SerialSerial..printprint(("DATA,TIME,""DATA,TIME,");); Serial
Serial..printprint(dt);(dt); SerialSerial..printprint(",");(","); Serial
Serial..printprint(A[1]);(A[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(A[2]); Serial(A[2]); Serial..printprint(",");(","); SerialSerial.print.print (A[3]);(A[3]); SerialSerial..printprint(",");(","); Serial
Serial..printprint(G[1]);(G[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(G[2]); Serial(G[2]); Serial..printprint(",");(","); SerialSerial.print.print (G[3]);(G[3]); SerialSerial..printprint(",");(","); Serial
Serial..printprint(M[1]);(M[1]); SerialSerial..printprint(",");(","); SerialSerial..printprint(M[2]); Serial(M[2]); Serial..printprint(",");(","); SerialSerial.print.print (M[3]);(M[3]); SerialSerial..printprint(",");(","); Serial
Serial..printprint(roll_A);(roll_A); SerialSerial..printprint(",");(","); // roll calculated using accelerometer’s data// roll calculated using accelerometer’s data
Serial
Serial..printprint(roll);(roll); SerialSerial..printprint(",");(","); // final roll, will be used in step 2, not used for now// final roll, will be used in step 2, not used for now
Serial
Serial..printprint(pitch_A);(pitch_A); SerialSerial..printprint(",");(","); // pitch calculated using accelerometer’s data// pitch calculated using accelerometer’s data
Serial
Serial..printprint(pitch);(pitch); SerialSerial..printprint(","); // final pi(","); // final pitch, will btch, will be used in ste used in step 2ep 2 , not used f, not used for nowor now
Serial
Serial..printprint(yaw_M);(yaw_M); SerialSerial..printprint(",");(","); // yaw calculated using magnetometer’s data// yaw calculated using magnetometer’s data
Serial
Serial..printprint(yaw);(yaw); SerialSerial..printprint(",");(","); // final ya// final yaw, will be uw, will be used in step 2sed in step 2 , not used f, not used for nowor now
Serial
A�� ��� ���� ���� E����
A�� ��� ���� ���� E����
AH��
AH��
���� 2 C�������� ���� ����� ��� ����� ����
���� 2 C�������� ���� ����� ��� ����� ����
����� ����
����� ����
I� IM� �� ���� �� �� ������
I� IM� �� ���� �� �� ������
ro rolll_l_GG = = rorolll l + + ddtt * * GG[[11] ] ;; pitpitchch_G_G = p= pititch ch + d+ dtt * G* G[2[2] ;] ; y yaaww__GG = = yyaaw w + + ddtt * * GG[[33] ] ;;
I� IM� �� ���
I� IM� �� ��� ���� �� �� ���
���� �� �� ��� ������, �� ���� ������� ��� ������� ����� ��
������, �� ���� ������� ��� ������� ����� ��
��� ���� �� E������ ������� ����� �����
��� ���� �� E������ ������� ����� �����
F���� ������ ���������:
F���� ������ ���������:
rorolll_l_GG = r= roolll + dl + dtt * (* (GG[[11]]++sinsin(TO_RAD(roll))*(TO_RAD(roll))*tantan(TO_RAD(pitch))*G[2]+(TO_RAD(pitch))*G[2]+coscos(TO_RAD(roll))*(TO_RAD(roll))*tantan(TO_RAD(pitch))*G[3]);(TO_RAD(pitch))*G[3]); pi
pitctch_h_GG = p= pititch ch + d+ dtt * (* (coscos(TO_RAD(roll))*G[2]-(TO_RAD(roll))*G[2]-sinsin(TO_RAD(roll))*G[3]);(TO_RAD(roll))*G[3]); y
yaaww__GG = = yyaaw w + + ddtt * * ((sinsin(TO_RAD(roll))/(TO_RAD(roll))/coscos(TO_RAD(pitch))*G[2]+(TO_RAD(pitch))*G[2]+coscos(TO_RAD(roll))/(TO_RAD(roll))/coscos(TO_RAD(pitch))*G[3] );(TO_RAD(pitch))*G[3] );
������:
AH��
AH��
����
C������������ ������
C������������ ������
������: ���
C������������ ������
C������������ ������
I� ���� ����� ��� ��� ��� �����, ���� ��
I� ���� ����� ��� ��� ��� �����, ���� �� �� ���� �� ����:
�� ���� �� ����:
ro
rolll l = = 0.0.002 2 * * rorolll_l_AA + + 0.0.98 98 * * rorollll_G_G ;; pit
pitch ch = = 0.00.02 2 * * pitpitch_Ach_A + + 0.90.98 8 * * pitpitch_ch_GG ;; y yaaw w = = 00..002 2 * * yyaaww__MM + + 00..998 8 * * yyaaww__GG ;;
B�� ���� ���� ���
B�� ���� ���� ��� ���� ���� ������ ��� �����
���� ���� ������ ��� �����
���� ������������ ������ 0
���� ������������ ������ 0
OK
OK
���� ������������ ������ 180
���� ������������ ������ 180
IM� �� �������
IM� �� �������
G������
G������
C������������ ������
C������������ ������
I� ���� ����� ��� ��� ��� ����� �� ���� �� ���� ���� ������� ����
I� ���� ����� ��� ��� ��� ����� �� ���� �� ���� ���� ������� ����
��� ���������� ���� �� ��� ��� ���� �� ��� ���� ���� �� ������
��� ���������� ���� �� ��� ��� ���� �� ��� ���� ���� �� ������
rollroll = = CompFilCompFilter( ter( 180, 180, roll_Aroll_A, , roll_Groll_G );); pitch
pitch = = CompFilCompFilter( ter( 90, 90, pitch_pitch_A, A, pitch_pitch_GG );); yaw
yaw = = CompFilCompFilter( ter( 180, 180, yaw_M, yaw_M, yaw_Gyaw_G ););
I ������� � �������� ���� ������� ��� ������ ������ �����������
I ������� � �������� ���� ������� ��� ������ ������ �����������
��� ����:
��� ����:
float
float CoCompmpFiFiltlterer ((intint angle,angle, floatfloat acce,acce, floatfloat gyro)gyro) {
{ if
if((absabs(a(accccee -- gygyroro) ) > > ananglgle) e) {{ if
if(gyro < -angle) gyro += angle*2;(gyro < -angle) gyro += angle*2; if
if(gyro (gyro > > angle) angle) gyro gyro -= -= angle*2;angle*2; }
} return
return 0.00.02 * ac2 * accece + 0.9+ 0.98 * gy8 * gyro;ro; } }