• No results found

IMU.pdf

N/A
N/A
Protected

Academic year: 2021

Share "IMU.pdf"

Copied!
56
0
0

Loading.... (view fulltext now)

Full text

(1)

IM� & AH�� ����������

IM� & AH�� ����������

F�������

F�������

(2)

IM� & AH�� ����������

IM� & AH�� ����������

��� ��������� �� ����

��� ��������� �� ���� �������� �� �� ������� � ����� ����� ��

�������� �� �� ������� � ����� ����� ��

������ ���������� �� ����� IM��.

������ ���������� �� ����� IM��.

E�

E��� ������ ���

�� ������ ��� ���� �� ������ ��� �����, �� ��

���� �� ������ ��� �����, �� �� ��� �

��� �

 ���� ���

 ���� ���

 ���� 

 ���� 

 �������. ���� �������� ��� ������� �� ��� ��� ����������

 �������. ���� �������� ��� ������� �� ��� ��� ����������

���� ���� ����.

���� ���� ����.

(3)

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�

(4)

��������

��������

��� ���

�� ���� ����

� ����::

A������

A������ IDE

IDE

A������

A������ ���������

��������� ���

��� IM�

IM� �������

�������

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

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

  E����

  E����

PL��DAQ

PL��DAQ ������

������ ���

��� E����

E����

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

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

(5)

IM�

IM�

(6)

L��� ����� ���� � ���������

L��� ����� ���� � ���������

��

��

��

����

����

�����

�����

���

���

(7)

���� �� ��� ���� ��������� ����

���� �� ��� ���� ��������� ����

(8)

M��� ���� ���� ��������� �� �����

M��� ���� ���� ��������� �� �����

OK

OK

N��

N�� OK

OK

� �� ���������

� �� ���������

��

��

��

(9)

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); }

(10)

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); }

(11)

L��� ������ ��� ��� ���� �� E����.

L��� ������ ��� ��� ���� �� E����.

I� �� ��� ��������� ��� �� ������

I� �� ��� ��������� ��� �� ������

F���� ������� PL��DAQ ������ ��� E����

F���� ������� PL��DAQ ������ ��� E����

A�� ����� �������� ��

A�� ����� �������� �� �����

�����

() ��������

() ��������

������� ����� ����� ��

������� ����� ����� �� ����

����

() ��������, �� ���� ���

() ��������, �� ���� ���

Serial

Serial..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�� � �������� �����������, ���: ����://���������.����������.���/�������������������������������������

(12)

O��� PL� ������

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

�� E����, ������ ��� �����

������ ���� ��� �����

������ ���� ��� ����� �������

�������

D��� ������

D��� ������

�� ���� ����

�� ���� ����

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

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

�� ���� ���� ��

�� ���� ���� ��

���� ����

���� ����

(13)

N���: I ��� �������� ���� PL� ������.

N���: I ��� �������� ���� PL� ������.

�� �� PL� ������ ����� ��� ��� �����, �� �� ���� ��� ����, �� ���

�� �� PL� ������ ����� ��� ��� �����, �� �� ���� ��� ����, �� ���

�����: �����

�����: �����

�����

�����

 �� �����

 �� �����

 �������

 �������

�� ���� �BA

�� ���� �BA

���� ���� ���

���� ���� ��� ���� ���� ���� (�� ���� �� ����� �� ������) ���

���� ���� ���� (�� ���� �� ����� �� ������) ���

������� ���� ����.

������� ���� ����.

L��� ����

L��� ����

(14)

N�� ���� ��� ��� ���� ��� ���� �� ���� ���� ��

N�� ���� ��� ��� ���� ��� ���� �� ���� ���� ��

E����, �� ��� ����� ����������� ��� �������������

E����, �� ��� ����� ����������� ��� �������������

��� ������������� ������� ��� ������������, ������� ���� ��� ������ �� �� ���

��� ������������� ������� ��� ������������, ������� ���� ��� ������ �� �� ���

������� ��� ��� ������������� ��� �� ������.

������� ��� ��� ������������� ��� �� ������.

��� ���� ��� �� ��������� ��� ������ �� ��� ������������� ����� �� �� �� ��

��� ���� ��� �� ��������� ��� ������ �� ��� ������������� ����� �� �� �� ��

����� ��� ������� ��� ������ ��

����� ��� ������� ��� ������ �� ��� ������ ����� ��

��� ������ ����� �� ��������

������������ ��� ��

���� ��� ��

�������, ��� ���� �� ��� ����� �� ������. �� ���� ����� ��� ������������� ����

�������, ��� ���� �� ��� ����� �� ������. �� ���� ����� ��� ������������� ����

�� ������� ������� ���� ����� ��� ������ �� �� ������� ������� �����. ���

�� ������� ������� ���� ����� ��� ������ �� �� ������� ������� �����. ���

���� ������� ��� 2 ������ �� ��� �������� ������.

���� ������� ��� 2 ������ �� ��� �������� ������.

���� ������ ��� ���������� ��� ��� ����� ����.

���� ������ ��� ���������� ��� ��� ����� ����.

(15)

� ����

� ����

� ����

� ����

� ��

� ��

� ��

� ��

� ����

� ����

� ��

� ��

� ����

� ����

����������

����������

 

F���� ����� �

F���� ����� �

���� ���� ��,

���� ���� ��,

 

N��� ����� �

N��� ����� �

���� ���� ��,

���� ���� ��,

 

F������ ����� �

F������ ����� �

���� ����

���� ����

��.

��.

���� ���� ��� ������� �� ���� ����� �������

���� ���� ��� ������� �� ���� ����� �������

N�� ���� ��� ��� ���� ��� ���� �� ���� ���� ��

N�� ���� ��� ��� ���� ��� ���� �� ���� ���� ��

E����, �� ��� ����� ����������� ��� �������������

E����, �� ��� ����� ����������� ��� �������������

(16)

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����, �� ��� ����� ����������� ��� �������������

(17)

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 ��� ����� ��� �������� ���� ������ �� ���� ��� �������� ����

���� ��� �������� ����

������ �� ��

������ �� ��

� ��

� ��

� ��

� ��

� ����

� ����

� ��

� ��

� ����

� ����

(18)

N�� ����� ��������� ��� ������ ������

N�� ����� ��������� ��� ������ ������

G������ ��� ������ �� ��� ���� �� ����, ����� ��� ������ ����� (��� ������) ���

G������ ��� ������ �� ��� ���� �� ����, ����� ��� ������ ����� (��� ������) ���

���� �� ��� ������ ������ ��� � � ��� �.

���� �� ��� ������ ������ ��� � � ��� �.

(19)

N�� ���

N�� ������ ����

� ��������� ��� ��

����� ��� �����

���� �������

� ���������

��

L���� ����� ��� ��������� �� ��� ������, �� ������� 90� ��������, �������� 90�

L���� ����� ��� ��������� �� ��� ������, �� ������� 90� ��������, �������� 90�

�������� ��� ������ 90� ��������.

�������� ��� ������ 90� ��������.

��� �

��� �

���� �� �

���� �� �

��� �

��� �

���� �� �

���� �� �

����

����

��

��

��� �

��� �

� ��� � ���� ��� �� ��� ����� ���������

� ��� � ���� ��� �� ��� ����� ���������

(20)

�� ���

� ��� ����� �����������

����� ����������� ��� �

��� �����

����� ����

� ����

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

(21)

N�� ��� ������ ���� �� 0 ���� ��� ���� �� ���

N�� ��� ������ ���� �� 0 ���� ��� ���� �� ���

������ ��� ��������� �������, ��� ���� ��� ��

������ ��� ��������� �������, ��� ���� ��� ��

�����

�����

L���� ��������� ��� ������ ����� ������.

L���� ��������� ��� ������ ����� ������.

(22)

�� ���� �� ���� ��� ��� ����� �� ��� ����

�� ���� �� ���� ��� ��� ����� �� ��� ����

��� ���� �������� � ���� �� ������, ��� ��� ������ ������, �� �� ���� ���� ����

��� ���� �������� � ���� �� ������, ��� ��� ������ ������, �� �� ���� ���� ����

���� �������

���� �������

F���� ������� ���� ���������

F���� ������� ���� ���������

float

float 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�� ��� ���� ������� �������� ���� �����

Serial

Serial..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

(23)

N�� ��� ������ ���� �� 0 ���� ��� ���� �� ���

N�� ��� ������ ���� �� 0 ���� ��� ���� �� ���

������ ��� ���������� ��� �������.

������ ��� ���������� ��� �������.

N���, �� ���� �� ������� ��� ���� ���������

N���, �� ���� �� ������� ��� ���� ���������

����� �� ��������� ������.

����� �� ��������� ������.

I� ����� �� �� ���� �� ���� ��� ���� �� 90

I� ����� �� �� ���� �� ���� ��� ���� �� 90

������� ��� ������ ��� ����:

������� ��� ������ ��� ����:

(24)

A�� �� E���� � ������ ���� ��� ��. G��� � ��

A�� �� E���� � ������ ���� ��� ��. G��� � ��

G��� ���� ��� 90� �����

G��� ���� ��� 90� �����

G��� � ��

G��� � ��

(25)

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 �� ����� ���� ��� ����

����� ������ ��� ��� ����� ����.

����� ������ ��� ��� ����� ����.

(26)

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]

(27)

G��� �� ��� ���������� !

G��� �� ��� ���������� !

����

(28)

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

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

A �

A ���� ���

��� ��������

����� ���

��� �� �

�� ����� ���

���� ����:

�:

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

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

I ���� �

I ���� � ������ ������

������ ����������� ����� ��� ��� ��� ������� ��

����� ����� ��� ��� ��� ������� �� ���

���

��������������

��������������� �������

� ����������� ���� ����

���� ���� ���� �������. ��� ������

�������. ��� ������ ���� ��

���� ��

���� �� �� ��� ���� �� ��� ��� ��� ��� ��� ������ ������� ���

���� �� �� ��� ���� �� ��� ��� ��� ��� ��� ������ ������� ���

���� ��

���� �� �������

��������� ��� ���

�� ��� ������������� �

���������� ������� �� ���

������ �� ��� ��������� ��

��������� ��

��� �

��� ������

������ �������� �����.

� �������� �����.

O�� �������� ��� �� �� ���� ���� ��� ���� �� ������ ��� ����

O�� �������� ��� �� �� ���� ���� ��� ���� �� ������ ��� ���� ��

��

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

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

����� �� ���� ���

����� �� ���� ���� I ��

� I �� �� ��� ����� �����. ����

�� ��� ����� �����. ���� ��� ���� ��

��� ���� �� E����

E����

(M����� ����� �� ���� ������).

(M����� ����� �� ���� ������).

(29)

L��� ���� �������������� ���

L��� ���� �������������� ���

��

 ��� ���

 ��� ���

��

I� ��� ���� �� ���� � ���� �� 3D �� �����, ��� ���� ���� ����

I� ��� ���� �� ���� � ���� �� 3D �� �����, ��� ���� ���� ����

���� ����:

���� ����: ����://���.����.��/E����3D������

����://���.����.��/E����3D�����������.���

�����.���

(30)

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]

(31)

M����������� �� ��� ����������

M����������� �� ��� ����������

M����������� ������ ��� 360� ���

M����������� ������ ��� 360� ���

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

(32)

M����������� �� ��� ����������

M����������� �� ��� ����������

M����������� ������ ��� 360� �����

M����������� ������ ��� 360� �����

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

(33)

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���/������

���/������.��� 

.��� 

(34)

AH��

AH��

(35)

AH��

AH��

���� 1 C�������� ���� ����� ��� ����� ������������� ��� ������������

���� 1 C�������� ���� ����� ��� ����� ������������� ��� ������������

(36)

M��� �����������

M��� �����������

• •

����

���� �� �������

�� ������� �� ��� ��1

�� ��� ��180, 180� �

80, 180� ����� ���

���� ��� ����� ��

����� ��

������� �� ���

������� �� ��� ��90, 90�

��90, 90� �����, ������

�����, ��������� ���

��� ��� ��� ���

��� ��� ���

���

���� �������� �� ��������� ���� ���� ��� ����� �� ��

���� �������� �� ��������� ���� ���� ��� ����� �� �� ��

��

����� �� ���� ���������

����� �� ���� ���������

(37)

��� ���������

��� ���������

� = ������������� � =

� = ������������� � = ���� � = ������������

���� � = ������������





 � �����

 � �����

((

,,

 

 

))

ℎ

ℎ

 � ����

 � ����

� 

� 





 � ��

 � ��

�� 

 �

 �

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

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

(38)

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

(39)

���� ��

���� �� � ���� ��������� ��� ����� ��������� �������

� ���� ��������� ��� ����� ��������� �������

���� ��� IM� �� ���� (���� = ����� = 0)

���� ��� IM� �� ���� (���� = ����� = 0) ��� �������� ��

��� �������� ��

��� ����� ��������� (��� = 0)

��� ����� ��������� (��� = 0)

����� ��������� ���� ���� ������

����� ��������� ���� ���� ������� ���� ��� IM�

� ���� ��� IM� ����� ��� ��

����� ��� �� ��

�������

�����

���� ��� �������� �� ���

���� ��� �������� �� ��� ����� ���������:

����� ���������:

ro

rolll_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

(40)

B�� ��� ��������� ����� ������� ������� ��

B�� ��� ��������� ����� ������� ������� �� ���

���

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

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

������

��

�� I ������� � �������� �� ��� ��� ������ �� ��� ��

�� I ������� � �������� �� ��� ��� ������ �� ��� ����� ����

��� ������:

��:

float

float 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

(41)

A�� ��� ���� ���� E����

A�� ��� ���� ���� E����

J��� �� ����� ���� ���������� �� OK

J��� �� ����� ���� ���������� �� OK

������� ����� ����� �� ���� ���:

������� ����� ����� �� ���� ���:

Serial

Serial..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

(42)

A�� ��� ���� ���� E����

A�� ��� ���� ���� E����

(43)

AH��

AH��

���� 2 C�������� ���� ����� ��� ����� ����

���� 2 C�������� ���� ����� ��� ����� ����

(44)

����� ����

����� ����

I� IM� �� ���� �� �� ������

I� IM� �� ���� �� �� ������

ro rolll_l_GG = = rorolll l + + ddtt * * GG[[11] ] ;; pit

pitchch_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���� ������ ���������:

ro

rolll_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] );

������:

(45)

AH��

AH��

����

(46)

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

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

������: ���

(47)

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������

(48)

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

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

I� ���� ����� ��� ��� ��� ����� �� ���� �� ���� ���� ������� ����

I� ���� ����� ��� ��� ��� ����� �� ���� �� ���� ���� ������� ����

��� ���������� ���� �� ��� ��� ���� �� ��� ���� ���� �� ������

��� ���������� ���� �� ��� ��� ���� �� ��� ���� ���� �� ������

roll

roll = = 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; } }

���� ������������ ������ 0

���� ������������ ������ 0

OK

OK

���� ������������ ������ 180

���� ������������ ������ 180

IM� �� �������

IM� �� �������

OK

OK

References

Related documents

Interviews were transcribed according to the agile methods used: three distance factors (temporal, geo- graphical, sociocultural) and three develop- ment

It inclu des all those instances in which the relevant population is split into two subpopulations, eligibles and non-eligibles, provided that (i) the eligibility status

The AFL-CIO hosted regional futures forums with young workers across the country this spring, followed by a Next Up summit of more than 400 workers younger than 35—the

During the second round, this difference became more evident: Yanukovych, the pro-Russian candidate (winning with a difference slightly lower than 900,000 votes),

Globalisation and the cross-border hyper mobility of capital have increased the vulnerability of open economies to financial crises caused by the sudden reversal of

This report describes work completed over the past year on our project, entitled “Unmanned Aircraft Systems (UAS) Research: The AG Channel, Robust Waveforms, and Aeronautical Network

Initial results indicate that the hands-free teleoperation system permits a higher command rate than traditional interfaces, and is currently limited not by

2 National Institute of Health Research Biomedical Research Centre for Mental Health, Maudsley Hospital and Institute of Psychiatry, Psychology and Neuroscience, King ’ s