Escolar Documentos
Profissional Documentos
Cultura Documentos
Informtica
2009
Agradecimentos /
acknowledgements
palavras-chave
resumo
keywords
abstract
This work aims to develop an IMU (Inertial Measurement Unit), making use of
low cost MEMS (Micro-machined Electro-Mechanical Systems)
accelerometers and gyroscopes . This emerging technology opens new doors
in the world of inertial navigation systems due to its mechanical simplicity. This
allows transfer IMU measures to a processing/storage unit, hence its
integration into more complex navigation systems that make use of multiple
units of measure: IMU, GPS - Global Positioning System, odometer,
magnetometers.
Contedo
1
Introduo
Enquadramento
2.1
. . . . . . . . . . . . . . . . . . . . .
2.2
Giroscpio e Acelermetro
. . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2.1
Giroscpio MEMS
. . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2.2
Acelermetro MEMS . . . . . . . . . . . . . . . . . . . . . . . . . .
10
Concepo do IMU
13
3.1
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.2
Projecto do esquemtico
3.3
Projecto do PCB
3.4
Produto nal
3.5
14
. . . . . . . . . . . . . . . . . . . . . . . . . . . .
15
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
20
. . . . . . . . . . . . . . . . . . . . . . . . .
21
23
4.1
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
24
4.2
Clculo da Orientao
4.1.1
Teoria
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
24
4.1.2
Prtica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
26
Clculo da Posio
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
27
4.2.1
Teoria
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
27
4.2.2
Prtica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
28
Resultados
29
Concluses
32
Apndice A
33
Apndice B
38
Lista de Figuras
1
. . . . . . . . . . . . . . . .
M y M x = Ky C y
. . . . . . . . . . . . . . .
11
11
. . . . . . . . . . . . . .
13
14
. . . . . . . . . . . . . . . . . . . . . . . . . . . .
16
representam
vectores dos eixos de sensibilidade dos acelermetros e os giroscpios, respectivamente, orientados em trs direces ortogonais
(Xl , Yl , Zl ).
. . . .
17
10
. . . . . . . . . . . .
17
11
12
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
18
. . . . . . . . . .
19
13
. . . . . . . . . . . . . . . . . . . . .
20
14
Fotograa do IMU. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
21
15
. . . . . . . . . . . . . . .
22
16
(adaptado de [2]) . . . . . . . . . . .
23
17
. . . . . . . . . . . . . . . . . . . . . . . .
24
strapdown
19
Random walk que advm da dupla integrao das aceleraes que provm
do acelermetro Ax.
20
29
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
30
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
30
21
31
22
31
. . . . . .
Lista de Tabelas
1
15
29
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
29
Introduo
Enquadramento
sensores inerciais foram sendo desenvolvidos, levando existncia de duas classes distintas
de sensores - os de elevada preciso (high-grade) e os de menor preciso (low-grade). Naturalmente, os primeiros so extremamente mais caros que os segundos, e so de natureza
conceptual muito diferente.
Com
o aparecimento da tecnologia GPS (Global Positioning System), o posicionamento absoluto de preciso tornou-se muito acessvel para muitas aplicaes.
No entanto, quando
dead-reckoning
frequentemente utilizados sensores inerciais de mdia e elevada preciso (ver, por exemplo,
[4]-[7]). Muitos destes sistemas integrados foram aplicaes directas de sistemas integrados desenvolvidos para aeronaves, onde as exigncias de preciso so muito elevadas. No
entanto, para sistemas terrestres, este rigor posicional muitas vezes menos exigente, o
que possibilita a utilizao de equipamento mais barato (mas menos preciso). No caso em
concreto dos automveis, e dada a disponibilidade dos sinais GPS, so pouco frequentes as
situaes onde o posicionamento nal depende apenas dos sensores inerciais durante um
largo perodo de tempo. Mais realista considerar-se que a informao dos sensores inerciais servem para suavizar os erros associados s posies GPS ou para calcular, durante
curtos intervalos de tempo, a posio da viatura quando os sinais GPS so bloqueados.
Assim, torna-se interessante testar o comportamento de um sistema de navegao inte-
grado que considere apenas sensores de muito baixo custo (sensores inerciais MEMS),
averiguando-se, em situaes reais, quais os nveis de preciso e robustez que se atingem
com este sistema. Neste sentido, o sistema inercial desenvolvido neste trabalho tem como
objectivo ser inserido num sistema de navegao para uma viatura, cujo desenvolvimento
dos restantes elementos considerado nos trabalhos da dissertao de Mestrado Integrado
em Engenharia Electrnica e de Telecomunicaes, da Universidade de Aveiro, intitulada
"Desenvolvimento e anlise de desempenho de um sistema de navegao integrado [8].
strapdown ).
(gimbals) e de trs motores que permitem plataforma total liberdade de orientao Figura 1. Quando a plataforma sujeita a um deslocamento angular, os giroscpios quanticam esse movimento fazendo com que os motores contrariem esse deslocamento. Desta
forma garante-se que integrando as medidas dos acelermetros se obtm deslocamentos
lineares relativos ao referencial global.
Nos princpios da dcada de setenta do sculo passado, a industria da navegao inercial
strapdown ).
Este tipo de sistemas usa sensores que, ao contrrio das plataformas giro-estabilizadas,
esto xados ao corpo cuja dinmica se pretende descrever, portanto as medidas que
daqui so retiradas dizem respeito ao referencial local do corpo. Para que as aceleraes
provenientes dos trs acelermetros sejam integradas correctamente estas medidas tero
de ser previamente projectadas no referencial global, e isso consegue-se custa de trs
giroscpios dispostos ortogonalmente que permitem uma contnua estimativa da orientao
do referencial local relativamente ao referencial global.
articulados (
estes sistemas mais veis. Por outro lado, esta xao possibilita que se construam os
sistemas em dimenses mais reduzidas e a custo mais baixo. Para alm destes benefcios,
estes sistemas trazem o inconveniente de serem computacionalmente mais exigentes.
2.2.1
Giroscpio MEMS
sofre o efeito
de uma fora
FC = 2 M ( v)
(1)
O modelo mais simples consiste numa massa de prova que levada a vibrar ao longo
de um eixo X que quando sofre uma rotao em torno de Z induz, segundo o efeito de
coriolis, uma vibrao no eixo Y, perpendicular a X e Z - gura 3.
Estes sensores so constitudos por um nmero reduzido de componentes mecnicos, e
por conseguinte so relativamente baratos de construir.
As principais vantagens deste tipo de giroscpios relativamente a outros, nomeadamente os mecnicos, so:
tamanho/peso reduzido
estrutura slida
no requer manuteno
Erro Sistemtico: possvel medir o erro sistemtico do sensor integrando as medidas sucessivamente quando no se lhe imposto qualquer rotao e se verica
uma diferena entre o valor esperado e o valor real calculado em
determinado o seu valor
,
/h.
Uma vez
extradas do sensor. Caso esta compensao no se verique, os deslocamentos angulares calculados sero afectados de um deslocamento proporcional a
,
que cresce
(t) = t
(2)
Rudo Branco: As principais causas de rudo branco nos giroscpios MEMS so perturbaes termo-mecnicas que geram variaes de frequncia muito superior taxa
de amostragem. Este rudo caracterizado por um conjunto de variveis aleatrias
de mdia nula em que cada uma destas variveis est igualmente distribuda e caracterizada por uma varincia nita
2.
random walk
(t) =
t t
(3)
Para uma explicao um pouco mais detalhada deve referir-se a [1] e [2].
Fliker Noise ):
iker noise
2.2.2
Acelermetro MEMS
Os acelermetros tm por objectivo medir aceleraes com alta preciso numa direco
muito bem denida.
10
ou popularmente
M y M x = Ky C y
g.
acelermetros MEMS consiste numa massa suspensa por um brao de natureza idntica
ao de uma mola e de um amortecedor em srie.
F = ma
(4)
H ainda outra categoria de acelermetros MEMS cujo princpio o de medir as variaes da frequncia de uma onda acstica de superfcie presente num brao exvel com
uma massa suspensa - gura 5.
Estes dispositivos usufruem das vantagens da tecnologia MEMS e por isso partilham
11
Erro Sistemtico: Quando o acelermetro est sujeito a condies tais, que o valor do
sinal seja previsvel, nomeadamente quando est em repouso, possvel determinar
o erro sistemtico
s (t) =
t2
2
(5)
Rudo Branco: Tal como os giroscpios MEMS os acelermetros sofrem de perturbaes termo-mecnicas geradoras de desvios no sinal. Neste caso, devido dupla
integrao do sinal, o rudo branco gera
random walk
r
s t3/2
12
t
3
(6)
Concepo do IMU
seis amplicadores concebidos para maximizar a excurso de sinal de cada um dos sinais
enviados pelos sensores. Dado que o micro-controlador usado contm apenas um conversor
analgico-digital (ADC), com um tempo de converso obviamente no nulo, seria impossvel guardar quaisquer seis medidas que dissessem respeito ao mesmo instante de tempo.
Visto isto, implementou-se um andar de sincronizao com o objectivo de memorizar,
durante um determinado tempo, as medidas, provenientes do andar de amplicao, permitindo assim ADC converter os seis sinais relativos ao mesmo instante de tempo. Por
outro lado, foi implementado um sensor de temperatura com o intuito de ser feita uma
anlise dos erros com origem nas variaes da temperatura. Depois de toda a informao
ser processada, as medidas so enviadas, j em formato digital, para o sistema de armazenamento de dados do sistema de navegao integrado, ou para um computador (para
possibilitar vrios testes), que far a integrao de todos os dados.
13
gs
exceder em demasia as especicaes exigidas pela aplicao para que haja um mximo
aproveitamento da excurso do sinal. Neste caso, para um implementao onde se consideram condies de circulao normais para uma viatura, espera-se que uma variao at
mV
e em
%/ C , respectivamente.
No caso do giroscpio, a mesma ideia aplica-se para o alcance, mas desta vez as unidades
vm expressas em
/s.
70 /s.
Assim sendo, foram escolhidos o acelermetro ADXL103 (Analog Devices) e o giroscpio MEV-50A-R (Murata), por apresentarem um custo reduzido e por corresponderem s
exigncias acima mencionadas - gura .
1 C -
por no
sensores, tais como a PTC ou o termopar, este apresenta uma razo preo/qualidade
melhor.
Na tabela 1 esto descritas as caractersticas acima mencionadas para cada sensor.
14
Sensor
Preo
Alcance
Rudo
Sens. Temp.
Tamanho
ADXL103
16 euro
14 euro
1, 7g
70 /s
6mV rms
7mV rms
0, 3%/ C
4%/ C
552
10 10 3
MEV-50A-R
input
oset
dos
[3, 3](m/s )
- gura 10.
hardware faa
software antes
de proceder integrao. Estes ganhos foram pensados por forma a maximizar a gama de
utilizao do IMU. Este foi pensado para um carro que se desloca dentro de uma cidade,
com variaes de dinmica adequadas s condies normais de circulao.
Este circuito dever ser implementado trs vezes ( excepo do sensor de temperatura)
e cada um deles ser disposto de forma ortogonal relativamente aos outros, tal como se
pode ver na gura 9.
Depois de acondicionados, os sinais so transferidos para o andar de sincronizao
15
16
Figura 9:
representam
(Xl , Yl , Zl ).
17
18
Colocando o acelermetro e o
giroscpio o mais prximos de um dos cantos da PCB (gura 13) torna possvel montar
as trs PCBs com base na congurao da gura 9.
A PCB foi concebida de forma a se poder, posteriormente, editar algumas ligaes
com
19
Desta maneira
20
ADC converter os dados, do mesmo instante, de cada sensor, sequencialmente - gura 15.
Todo o procedimento funcionar com base num algortmo temporal com a qualidade
de se poder sincronizar a partir de um sinal externo, oriundo de um GPS (1PPS - 1 pulso
por segundo).
21
srie RS232.
22
strapdown
tem
essencialmente quatro fases como se pode ver na gura 16. Na primeira determina-se a
orientao do dispositivo fazendo uso dos sinais captados dos giroscpios, na segunda fase
usa-se a orientao previamente calculada para projectar as medidas vindas dos acelermetros no referencial global. De seguida faz-se a compensao das medidas para eliminar
a inuncia da gravidade e nalmente aplica-se a dupla integrao por forma a obter a
posio relativamente s condies iniciais estabelecidas.
strapdown
(adaptado de [2])
Nesta seco a letra 'l' ser usada em ndice de vectores que se reram ao referencial
local. O mesmo se aplica letra 'g', mas neste caso os vectores referir-se-o ao referencial
global, tal como indica a gura 17.
23
Teoria
Para representar a orientao do IMU sero usados os cossenos directores, isto , a atitude
do referencial local ser descrita pela matriz de projeco
respondem aos versores directores dos eixos do referencial
para projectar um dado vector
vg = C v l
(7)
Sendo
C (t)
24
pode-
se exprimir a variao de
(8)
C (t + t)
C (t), com a
A (t)
C (t + t) = C (t) A (t)
(9)
em que
A (t) = I +
(10)
=
0
0
de notar que
pequenos.
A (t) = I +
(11)
muito
1 Um
dado vector v quando multiplicado por uma matriz rotao altera a sua direco mantendo a
norma. As trs matrizes responsveis por rodar um vector no espao 3-D so as seguintes:
1
0
0
Rx = 0 cos () sin ()
0 sin () cos ()
cos () 0 sin ()
0
1
0
Ry =
sin () 0 cos ()
cos () sin () 0
Rz = sin () cos () 0
0
0
1
Uma rotao custa de ngulos positivos signica rodar em torno do eixo das abcissas aplicando a regrada-mo-direita com o polegar a apontar no sentido positivo do eixo. Desta forma, qualquer rotao no
espao pode ser representada pela multiplicao das trs matrizes previamente descritas
vR = Rx Ry Rz v
sendo vR o vector v aps a rotao. No entanto, a ordem pela qual as matrizes so multiplicadas deve ser
respeitada, isto , rotaes no espao tridimensional no so comutativas. Por outro lado, para ngulos de
rotao muito pequenos o resultado de sin () , sin () e sin () aproxima-se de , e respectivamente
e os cossenos destes ngulos tendero para 1. Feitas as substituies, as multiplicao entre as matrizes
25
= lim C (t + t) C (t)
C
t0
t
C (t) A (t) C (t)
= lim
t0
t
C (t) (I + ) C (t)
= lim
t0
t
= C (t) lim
t0 t
(12)
(13)
(14)
(15)
= (t)
t0 t
(16)
0
lz (t) ly (t)
(t) = lz (t)
0
lx (t)
ly (t) lz (t)
0
(17)
lim
onde
(18)
(t) dt
(19)
0
em que
C (0)
4.1.2
Prtica
1
R = Rx Ry Rz
26
Para o perodo
(2.13) a seguinte
t+t
C (t + t) = C (t) exp
(t) dt
(20)
onde
t+t
(t) dt = B
(21)
t
com
0
lz t ly t
B = lz t
0
lx t
ly t lx t
0
(22)
B2 B3 B4
+
+
+ ...
C (t + t) = C (t) I + B +
2!
3!
4!
2
2
2 2
B
B
=
C (t) I + B +
+ ...
2!
3!
4!
1
2 4
2 4
= C (t) I + 1
+
... B +
+
. . . B2
3!
5!
2!
4!
6!
1 cos () 2
sin ()
B+
B
= C (t) I +
2
em que
l = (lx , ly , lz )T . A equao
projeco C todas as iteraes.
= |l t|
actualizar a matriz
(23)
(24)
(25)
(26)
Teoria
se-
jam correctamente integradas, estas devem ser projectadas no referencial global da seguinte
maneira:
27
(27)
Depois, com as medidas j referenciadas aos eixos globais, aplica-se a dupla integrao e
faz-se a compensao da acelerao da gravidade
vg (t) = vg (0) +
(ag (t) gg ) dt
(28)
vg (t) dt
(29)
pg (t) = pg (0) +
0
onde
vg (0)
pg (0)
gg
4.2.2
Prtica
t,
vg (t + t) = vg (t) + t (ag (t + t) gg )
(30)
pg (t + t) = pg (t) + t vg (t + t)
(31)
28
Mdia
Eixo de sensibilidade
Giroscpios ( /s)
2
Acelermetros (m/s )
6.225 10
4.930 104
7.948 10
6.790 104
4.904 104
9.069 104
Giroscpios ( /s)
2
Acelermetros (m/s )
Tabela 3:
1.424 10
3.169 106
1.900 10
2.642 105
2.438 105
1.006 104
repouso.
Resultados
Aqui mostram-se alguns resultados aps a implementao do IMU: efectuou-se uma anlise
estatstica com os sensores em repouso e outra quando sujeitos a variaes de temperatura.
Nas seguintes tabelas (2 e 3) encontram-se os resultados destas medies.
, tambm, possvel vericar a importncia de um sensor de temperatura de maneira
a poder fazer-se uma compensao dos desvios acumulados por variaes da temperatura.
Figura 18: Valor medido do acelermeto Az (horizontal) com o IMU em repouso, temperatura ambiente (20C).
29
Figura 19: Random walk que advm da dupla integrao das aceleraes que provm do
acelermetro Ax.
30
Para fazer a anlise da inuncia da temperatura nos sinais dos sensores, introduziu-
31
Concluses
Hoje em dia, a tecnologia MEMS ainda no permite criar sistemas inerciais que mantenham um erro de posio inferior a 1 metro, ao m de 1 minuto, quando em repouso. Foi
visto que este erro gerado, mesmo quando o IMU est parado, deve-se ao desvio que a
integrao do rudo dos sensores inerciais proporciona. Durante este projecto registou-se,
tambm, que outra forte componente do erro se deve imperfeita ortogonalidade dos sensores entre si, consequncia de toda a electrnica envolvida ter sido montada manualmente.
Por outro lado, apesar de um IMU no ser capaz de determinar a posio garantindo um
erro constante, este pode ser uma boa soluo quando complementado com outros sistemas
de posicionamento, tais como o GPS. Deste modo, o desvio gerado pelo IMU corrige-se
periodicamente custa das medies absolutas do GPS e as zonas mortas compreendidas
entre sucessivas actualizaes do GPS passam a ser conhecidas com a contribuio do
IMU. Esta cooperao extremamente interessante, pois permite complementar um GPS,
reduzindo signicativamente o erro, tornando-o comparvel a GPSs de maior qualidade e
custo. Este IMU, pode ser uma mais valia em experincias futuras que envolvam outros
dispositivos de navegao e outros algoritmos.
32
Apndice A
____________
_PROTOCOL_
#
#
header
measures
time
|
| chksum |
| L s B y t e |CHKSUM|
#
#
#
#
#!C : / P r o g r a m a s / P e r l / b i n / p e r l . e x e
use
strict ;
use
Win32 : : S e r i a l P o r t ;
use
Math : : M a t r i x ;
use
Time : : H i R e s
use
POSIX qw ( c e i l
qw ( g e t t i m e o f d a y
tv_interval ) ;
floor );
constants
use
constant
PI
=> 4
use
constant
X_OFFSET_LIN
=>
use
constant
Y_OFFSET_LIN
=>
constant
Z_OFFSET_LIN
=>
128;
use
constant
X_OFFSET_ANG
=>
128;
use
constant
Y_OFFSET_ANG
=>
129;
use
constant
Z_OFFSET_ANG
=>
128;
my
1);
128;
use
atan2 ( 1 ,
128;
variables
$out ;
my
$port ;
my
@matched ;
my
$var ;
my
$checksum ;
my
$chk_aux ;
my
$ a=c h r ( 0 x e a ) ;
my
$b=c h r ( 0 xbb ) ;
my
$ c=c h r ( 0 x 0 0 ) ;
my
$d=c h r ( 0 x0b ) ;
my
$t_start ;
my
$t0 ;
my
$t1 ;
my
@filter ;
my
$count_filter
0;
my $B
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ,
[0 ,0 ,0] ,
[0 ,0 ,0]);
my $C
= new
Math : : M a t r i x ( [ 1 , 0 , 0 ] ,
[0 ,1 ,0] ,
[0 ,0 ,1]);
my
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
$ang_velocity
my
$lin_acceleration
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$lin_accel_proj
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$lin_vel_proj
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$lin_pos_proj
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$temperature ;
my
$time ;
# My way
variables
my
$pos_1
= new
my
$pos
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$vel_1
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$vel
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
# Raw
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
Measures
my
$raw_lin_acc
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$r aw_ an g_ ve l = new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$raw_temperature ;
my
$raw_time ;
#
my
Calibration
Variables
@offset_ang_sum
my
@offset_lin_sum
(0 ,0 ,0);
(0 ,0 ,0);
33
my
@offset_lin_calibrated
my
@offset_ang_calibrated
###################################################################
# MAIN
# START TIME COUNTING
$t_start
time ;
# COMMUNICATION
$port
= new
( rs232 )
Win32 : : S e r i a l P o r t
( "COM3 " ) ;
$ p o r t >b a u d r a t e ( 1 1 5 2 0 0 ) ;
$ p o r t >d a t a b i t s ( 8 ) ;
$ p o r t > s t o p b i t s ( 1 ) ;
$ p o r t >p a r i t y ( " n o n e " ) ;
while
(1){
$out
$ p o r t >i n p u t ;
i f ( $out
ne
' '){
# INPUT MEASURES # # # # # # # # # # # # # # # #
$var
$var . $out ;
i f ( $ v a r =~/( $ a $ b $ c $ d ) ( . { 1 } ) ( . { 1 } ) ( . { 1 } ) ( . { 1 } ) ( . { 1 } ) ( . { 1 } )
(.{1})(.{1})(.{1})(.{1})(.{1})(.{1})
(.{1})(.{1})(.{1})(.{1})(.{1})(.{1})(.
*)/ o)
$ c h e c k s u m =(0 x e a +0xbb+0x0b+o r d ( $ 2 )+ o r d ( $ 3 )+ o r d ( $ 4 )+
o r d ( $ 5 )+ o r d ( $ 6 )+ o r d ( $ 7 )+ o r d ( $ 8 )+ o r d ( $ 9 )+
o r d ( $ 1 0 )+ o r d ( $ 1 1 )+ o r d ( $ 1 2 )+ o r d ( $ 1 3 )+ o r d ( $ 1 4 )+
o r d ( $ 1 5 )+ o r d ( $ 1 6 )+ o r d ( $ 1 7 )+ o r d ( $ 1 8 ) ) & ( 0 xFF ) ;
$chk_aux =
#
$19 ;
CHECKSUM TRUE ?
i f ( $ c h e c k s u m ==
print
$var
$20 ;
@matched =
#
o r d ( $chk_aux ) ) {
( $1 ,
$2 , $3 , $4 , $5 , $6 , $7 , $8 , $9 , $ 1 0 , $ 1 1 , $ 1 2 , $ 1 3 ,
s p r i n t f ("% x " ,
o r d ( $matched [ 1 1 ] )
<<
$14 ,
$19 ) ;
8);
# TREAT MEASURES # # # # # # # # # # # # # # #
( $raw_ang_vel ,
$raw_lin_acc ,
( $ang_velocity ,
#
#
#
#
#
#
$raw_temperature ,
$lin_acceleration ,
$raw_time )
$temperature ,
$time )
s p r i n t f ( "TIME :
%i " , $ t i m e ) ;
>p r i n t ( " l i n _ a c e
$lin_acceleration
$temperature ;
$time ;
values
:");
=pod
foreach
( @matched ) {
i s o 2 h e x ( $_ ) ;
}
print
"\ n " ;
=c u t
PROCESS INPUT # # # # # # # # # # # # # # # # # #
=pod
=
0;
0;
$ang_velocity
>[0][0]
>[0][1]
>[0][2]
0;
$ang_velocity
>[0][1]
+=
$ang_velocity
$ang_velocity
#=c u t
#
=pod
$lin_acceleration
0.06;
>[0][0]
=
0;
$lin_acceleration
>[0][1]
>[0][2]
0;
$lin_acceleration
>[0][0]
+=
$lin_acceleration
0;
=c u t
#
if (
#
= GetRawMeasures ( \ @matched ) ;
G e t M e a s u r e s ( \ @matched ) ;
( time
print
$t_start )
" DONE
Calculate
!";
1;
> $ARGV [ 0 ] ) {
#AFTER warm
up
time
Mtx B
$B = CalcMtxB ( $ a n g _ v e l o c i t y ) ;
Calculate
Mtx C
34
$C = CalcMtxC ( $ a n g _ v e l o c i t y ,
#
$lin_acceleration
Project
$C ) ;
Acceleration
$lin_accel_proj
#
$B ,
ProjAccel ( $lin_acceleration ,
$ l i n _ a c c e l _ p r o j >p r i n t ( " l
$C ) ;
p:");
=pod
###############################################################
# # # # #
# MY WAY
#
Integrates
$vel_1
#
Acceleration
= MyIntAcc ( $ l i n _ a c c e l _ p r o j ,
Integrates
$pos_1 =
$vel ) ;
Velocity
MyIntVel ( $ l i n _ a c c e l _ p r o j ,
$vel
$pos
= $pos_1 ;
$vel ,
$pos ) ;
$vel_1 ;
# # # # # # # # # # #
=c u t
###############################################################
# # # # # # #
# ARTICLE WAY:
#
Euler
Integrates
method
Acceleration
$lin_vel_proj
$ l i n _ v e l _ p r o j >p r i n t ( " v e l
Integrates
$lin_accel_proj ) ;
proj ");
Velocity
$ l i n _ p o s _ p r o j=I n t e g V e l o c i t y ( $ l i n _ v e l _ p r o j , $ l i n _ p o s _ p r o j ) ;
###############################################################
}
else {
$raw_ang_vel
$raw_lin_acc
$raw_lin_acc
$raw_lin_acc
$offset_ang_sum [ 0 ]
+= $raw_ang_vel
>[0][0]/10.0;
$offset_ang_sum [ 1 ]
+= $raw_ang_vel
>[0][1]/10.0;
$offset_ang_sum [ 2 ]
+=
$offset_lin_sum [ 0 ]
+=
$offset_lin_sum [ 1 ]
+=
$offset_lin_sum [ 2 ]
+=
if
(++ $ c o u n t _ f i l t e r
>
>[0][2]/10.0;
>[0][0]/10.0;
>[0][1]/10.0;
>[0][2]/10.0;
9){
" @ f i l t e r \n " ;
$offset_ang_sum [ 0 ] ,
$offset_ang_sum [ 1 ] ,
$offset_ang_sum [ 2 ] ,
$offset_lin_sum [ 0 ] ,
$offset_lin_sum [ 1 ] ,
$offset_lin_sum [ 2 ] ) ;
@offset_ang_calibrated
@offset_lin_calibrated
( $offset_ang_sum [ 0 ] ,
( $offset_lin_sum [ 0 ] ,
@offset_ang_sum
(0 ,
0,
0);
@offset_lin_sum
(0 ,
0,
0);
$count_filter
0;
}
# # # # # # # # # # #
# $ p o s >p r i n t ( "My
pos :
$time
$ l i n _ v e l _ p r o j >p r i n t ( " v e l :
");
$time
");
# $B>p r i n t
( "B : \ n " ) ;
$t1 )
" elapsed :
$t0
$t1 ;
gettimeofday ;
",
( $t1
$t0 ) ,
"\ n " ;
=c u t
if
( ( time
$ t _ s t a r t ) >= $ARGV [ 1 ] ) {
exit ;
}
}
else {
print
$offset_ang_sum [ 1 ] ,
$offset_lin_sum [ 1 ] ,
35
$offset_ang_sum [ 2 ] ) ;
$offset_lin_sum [ 2 ] ) ;
print
#
sprintf
sprintf
("%2 x \ n "
("%2 x \ n "
sprintf
, $checksum ) ;
, $checksum )
("%d \ n "
,"
, $chk_aux ) ,
",
o r d ( $chk_aux ) ;
"\ n " ;
}
}
}
}
#p r i n t
$var ;
###################################################################
# FUNCTIONS
##################################
##
Integrates
sub
Acceleration
MyIntAcc
(MY WAY)
my
$l_a
= $_ [ 0 ] ;
my
$l_v
= $_ [ 1 ] ;
my
$grav
= new
Math : : M a t r i x ( [ 0 . 0 ,
1.0 ,
0.0]);
return
}
##################################
##
Integrates
sub
Acceleration
MyIntVel
(MY WAY)
my
$l_a
= $_ [ 0 ] ;
my
$l_v
= $_ [ 1 ] ;
my
$l_p
= $_ [ 2 ] ;
my
$grav
= new
Math : : M a t r i x ( [ 0 . 0 ,
1.0 ,
0.0]);
( ( ( ( $l _ a >s u b t r a c t ( $ g r a v ))> m u l t i p l y _ s c a l a r ( 0 . 0 1 0
return
}
##################################
##
Integrates
sub
Acceleration
IntegAcceleration
my
$l_v_p
= $_ [ 0 ] ;
my
$l_a_p
= $_ [ 1 ] ;
my
$grav
= new
Math : : M a t r i x ( [ 0 . 0 ,
$l_a_p>p r i n t ( " l i n
ace
prj :
9.8 ,
0.0]);
");
ace
proj :
");
# Vg ( t+ t )
return
= Vg ( t )
+ t
* ( Ag ( t+ t )
( ( $l_a_p>add ( $ g r a v )
Gg )
);
}
##################################
##
Integrates
sub
Velocity
IntegVelocity
my
$l_v_p
my
$l_p_p
return
= $_ [ 0 ] ;
= $_ [ 1 ] ;
( $l_v_p>m u l t i p l y _ s c a l a r ( 0 . 0 1 0 ) ) > add ( $l_p_p )
);
}
##################################
## Get RAW M e a s u r e s
sub
GetRawMeasures
from
@matched
No
units
convertions
are
done !
my @m = @{$_ [ 0 ] } ;
my $a_v = new
my
$l_a
my
= new
$temp =
my
$ti
<<
<<
8
o r d ( $m [ 2 ] ) ,
o r d ( $m [ 8 ] ) ,
( o r d ( $m [ 1 1 ] )
( o r d ( $m [ 5 ] )
<<
o r d ( $m [ 1 2 ] ) ,
<<
o r d ( $m [ 6 ] ) ,
8)
o r d ( $m [ 1 7 ] ) ;
( o r d ( $m [ 3 ] )
( o r d ( $m [ 9 ] )
<<
<<
)
o r d ( $m [ 1 3 ] ) ;
return
Math : : M a t r i x ( [ ( o r d ( $m [ 7 ] )
Math : : M a t r i x [ ( o r d ( $m [ 1 ] )
( o r d ( $m [ 1 4 ] )
( $a_v ,
$ l_ a ,
<<
24)
$temp ,
( o r d ( $m [ 1 5 ] )
<<
16)
( o r d ( $m [ 1 6 ] )
<<
$ti );
}
##################################
## Get
sub
Measures
GetMeasures
from
@matched
my @m = @{$_ [ 0 ] } ;
# my
@offset_ang_calibrated
= @{$_ [ 1 ] } ;
# my
@offset_lin_calibrated
= @{$_ [ 2 ] } ;
my $a_v =
new
Math : : M a t r i x ( [ ( ( ( o r d ( $m [ 7 ] )
( ( ( o r d ( $m [ 1 1 ] )
o r d ( $m [ 1 2 ] ) )
# my
new
<<
<<
$offset_ang_calibrated
[0])
*100/( $offset_ang_calibrated
[1])
*100/( $offset_ang_calibrated
[1]) ,
[2])
*100/( $offset_ang_calibrated
[2])]);
Math : : M a t r i x ( [
( o r d ( $m [ 3 ] )
o r d ( $m [ 8 ] ) )
$offset_ang_calibrated
$l_a
)+
$offset_ang_calibrated
( ( ( o r d ( $m [ 3 ] )
o r d ( $m [ 4 ] ) )
<<
( o r d ( $m [ 1 ] )
$offset_lin_calibrated [0])
$offset_lin_calibrated [1])
*(2.5/8.3)/(
*(2.5/8.3)/(
$offset_lin_calibrated [0]) ,
$offset_lin_calibrated [1]) ,
36
[0]) ,
o r d ( $m
( o r d ( $m [ 5 ] )
my
new
$l_a
$offset_lin_calibrated [2])
<<
( ( ( o r d ( $m [ 9 ] )
<<
$temp =
my
$ti
$offset_lin_calibrated [ 2 ] ) ] ) ;
8 ) + o r d ( $m [ 2 ] ) ) $ o f f s e t _ l i n _ c a l i b r a t e d [ 0 ] ) * ( 2 . 5 / 5 ) / ( $ o f f s e t _ l i n _ c a l i b r a t e d
$offset_lin_calibrated [1])*(2.5/5)/( $offset_lin_calibrated [1]) ,
o r d ( $m [ 1 0 ] ) ) $ o f f s e t _ l i n _ c a l i b r a t e d [ 2 ] ) * ( 2 . 5 / 5 ) / ( $ o f f s e t _ l i n _ c a l i b r a t e d [ 2 ] ) ] ) ;
Math : : M a t r i x ( [ ( ( ( o r d ( $m [ 1 ] )
( ( ( o r d ( $m [ 5 ] )
my
*(2.5/8.3)/(
return
<<
[0]) ,
o r d ( $m [ 6 ] ) )
o r d ( $m [ 1 3 ] ) ;
( o r d ( $m [ 1 4 ] )
<<
24)
( o r d ( $m [ 1 5 ] )
s p r i n t f ("% x " ,
( o r d ( $m [ 1 1 ] )
<<
8))
s p r i n t f ("% x " ,
( o r d ( $m [ 1 0 ] )
<<
8)
( $a_v ,
$ l_ a ,
$temp ,
+
+
<<
16)
( o r d ( $m [ 1 6 ] )
<<
8)
o r d ( $m [ 1 7 ] ) ;
( o r d ( $m [ 1 0 ] ) ) ;
( o r d ( $m [ 1 1 ] ) )
);
$ti );
}
##################################
## Mtx B
sub
CalcMtxB
my $B_aux = new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ,
[0 ,0 ,0] ,
[0 ,0 ,0]);
my $a_v = $_ [ 0 ] ;
#
line
$B_aux > [ 0 ] [ 0 ]
$B_aux > [ 0 ] [ 1 ]
$B_aux > [ 0 ] [ 2 ]
line
$B_aux > [ 1 ] [ 0 ]
$B_aux > [ 1 ] [ 1 ]
$B_aux > [ 1 ] [ 2 ]
#l i n e
0;
$a_v > [ 0 ] [ 2 ] * 0 . 0 1 0 ;
$a_v > [ 0 ] [ 1 ] * 0 . 0 1 0 ;
$a_v > [ 0 ] [ 2 ]
*0.010;
0;
$a_v
>[0][0]
*0.010;
$B_aux > [ 2 ] [ 0 ]
$B_aux > [ 2 ] [ 1 ]
$B_aux > [ 2 ] [ 2 ]
return
$a_v > [ 0 ] [ 1 ] * 0 . 0 1 0 ;
$a_v > [ 0 ] [ 0 ] * 0 . 0 1 0 ;
0;
$B_aux ;
} ##################################
## Mtx C
sub
CalcMtxC
my $a_v
= $_ [ 0 ] ;
my
$b
= $_ [ 1 ] ;
my
$c
= $_ [ 2 ] ;
my
$c_next
my
$identity
= new
my
$sigma
my
$sigma_rad
i f ( $sigma
= new
!=
Math : : M a t r i x ( [ 0 , 0 , 0 ] , [ 0 , 0 , 0 ] , [ 0 , 0 , 0 ] ) ;
Math : : M a t r i x ( [ 1 , 0 , 0 ] , [ 0 , 1 , 0 ] , [ 0 , 0 , 1 ] ) ;
( $a_v>m u l t i p l y _ s c a l a r ( 0 . 0 1 0 ) ) > a b s o l u t e ;
$sigma
* PI / 1 8 0 ;
0){
>add ( $b>m u l t i p l y _ s c a l a r ( s i n ( $ s i g m a _ r a d ) / $ s i g m a _ r a d ) ) ;
( $b>m u l t i p l y ( $b))> m u l t i p l y _ s c a l a r ((1 c o s ( $ s i g m a _ r a d ) ) / ( $ s i g m a _ r a d * $ s i g m a _ r a d ) )
$c_next
$identity
$c_next
$ c _ n e x t >add (
$c_next
$ c >m u l t i p l y ( $ c _ n e x t ) ;
}
else {
$identity
>add ( $b )
)>add (
$c_next
$c_next
$ c >m u l t i p l y ( $ c _ n e x t ) ;
( $b>m u l t i p l y ( $b))> m u l t i p l y _ s c a l a r ( 0 . 5 )
}
return
$c_next ;
}
##################################
## Ag ( t )
sub
my
$l_a
my
$c
{
= $_ [ 0 ] ;
= $_ [ 1 ] ;
# Hardware
$ l_ a
compensation
>[0][1]
#c o n v e r t
$l_a
* Ab ( t )
= C( t )
ProjAccel
+=
g' s
to
canceling
1;
m/ s ^2
= $ l _ a >m u l t i p l y _ s c a l a r ( 9 . 8 ) ;
#$ l _ a >p r i n t ( " l
return
a :");
}
##################################
##
sub
string
to
hex
iso2hex ($)
my
for
$hex
(my
my
=
$i
' ';
=
$ordno
$hex
.=
0; $i
=
<
ord
l e n g t h ( $_ [ 0 ] ) ; $ i ++)
s u b s t r ( $_ [ 0 ] ,
s p r i n t f ("[%02 x ] " ,
$i ,
1);
$ordno ) ;
37
);
);
);
$hex
=~
s/
$ //;;
$hex ;
}
Apndice B
MAIN . C
#i n c l u d e
//# i n c l u d e
<h t c . h>
//
DEFINES
//
GLOBAL VARIABLES
// i n t
rec
0;
// u n s i g n e d
char
ola
// u n s i g n e d
char
a;
0;
// u n s i g n e d
char
aux ;
// u n s i g n e d
char
buf ;
// u n s i g n e d
char
ack
// u n s i g n e d
char
ack2
0;
// u n s i g n e d
char
conv_result
// u n s i g n e d
long
out ;
// u n s i g n e d
long
time ;
// u n s i g n e d
char
str [10];
0;
= 0 x00 ;
unsigned
char
adc_buffer_full
unsigned
char
c h a n n e l =0;
unsigned
char
str [10];
unsigned
char
measures [ 6 ] [ 2 ] ;
// u n s i g n e d
unsigned
long
char
1;
measures_long [ 6 ] ;
measures_char [ 7 ] ;
// u n s i g n e d
long
// u n s i g n e d
long
filtered_measures [ 6 ] ;
// u n s i g n e d
char
window =
// u n s i g n e d
char
unsigned
char
w i n d o w _ m e a s u r e s [ 6 ] [ WINDOW_SIZE ] ;
0;
0;
delta_10ms
0;
// u n s i g n e d
long
measures_average [ 6 ] ;
// u n s i g n e d
long
n =
0;
unsigned
long
time
unsigned
long
time_aux =
unsigned
char
unsigned
char
time_byte [ 4 ] ;
// u n s i g n e d
unsigned
char
long
char
nono
// eeprom
0;
0;
0;
aux ;
ack
1;
= 0 x35 ;
unsigned
char
var1 [ 2 5 6 ] ;
/////////////////////////////////
////
MAIN/ / / / / / / / / / / / / / / / / / / / / / / /
/////////////////////////////////
void
main ( v o i d )
{
/
GIE =
1;
PEIE =
SSPIE =
1;
1;
*/
//
Configurations
disableInterrupts ();
initUsart ();
confTIMER0 ( ) ;
//
confTIMER1 ( ) ;
//
InitI2cMaster ( ) ;
//
InitI2cSlave ();
enableInterrupts ( ) ;
ENABLE_TIMER0_INT
//
//////////////
TIMER . h
38
time
= 0 x7778797A ;
while (1){
//
unsigned
long
to
bytes
( i
*8);
f o r ( i = 0 ; i < 4 ; i ++){
time_aux =
t i m e >>
time_byte [ i ]
( unsigned
c h a r ) time_aux ;
putch ( time_byte [ i ] ) ;
}
putch ( ' \ n ' ) ;
*/
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//
I2C
//
/////////
while (1)
//
MAIN WHILE
{
/
//
SSPCON1bits . SSPEN =
0;
SSPCON1bits . SSPEN =
1;
( temperature )
DelayMs ( 1 ) ;
INTCONbits . GIE =
1;
P I E 1 b i t s . SSPIE =
1;
INTCON2bits . RBPU =
0;
T R I S B b i t s . TRISB1 =
0;
T R I S B b i t s . TRISB2 =
0;
T R I S B b i t s . TRISB3 =
0;
PORTBbits . RB1 =
0;
PORTBbits . RB2 =
0;
PORTBbits . RB3 =
0;
/ / PORTB PULLUPS ON
DelayMs ( 5 ) ;
InitI2cMaster ( ) ;
while (1){
StartI2c ( ) ;
//
//
RepStartI2c ( ) ;
//
//
Begin
//
transmission
Begin
//
Slave
w h i l e ( SSPCON2bits . ACKSTAT ) ;
putch ( 'A ' ) ;
Address
//
Check
for
Acknoledge
from
slave
0: sent ;
1 : not
tab
WaitIdleI2c ( ) ;
//
Wait
SSPCON2bits . RCEN= 1 ;
//
( repeat )
DelayUs ( 2 5 ) ;
W r i t e I 2 c ( 0 x91 ) ;
//
transmission
nl
//
here
while
enables
Lines
arent
idle
reception
DelayMs ( 2 ) ;
DelayUs ( 1 0 0 ) ;
//
str );
putstr ( str ) ;
nl
//
//
Get
the
byte
the
string
received
and
convert
it
to
string
SSPBUF = 0 x 0 0 ;
WaitIdleI2c ( ) ;
//
//
Wait
SSPCON2bits .ACKDT =
0;
SSPCON2bits .ACKEN =
1;
here
while
Lines
arent
idle
DelayMs ( 2 ) ;
RepStartI2c ( ) ;
//
//
l t o a ( SSPBUF ,
str );
//
putstr ( str ) ;
nl
Begin
//
//
transmission
Get
the
byte
the
string
( repeat )
received
and
39
convert
it
to
string
sent
SSPCON2bits . RCEN= 1 ;
//
//
l t o a ( SSPBUF ,
str );
//
putstr ( str ) ;
nl
enables
//
//
//
DelayMs ( 2 ) ;
//
SSPCON2bits . RCEN= 1 ;
reception
Get
the
byte
the
string
//
enables
received
and
convert
it
to
string
reception
DelayMs ( 1 0 0 ) ;
StopI2c ( ) ;
}
*/
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//
while ( 1 ) ;
//
i f ( adc_buffer_full ){
//
Save
//
ADC
value
measures_char [ channel ]
= ADRESH ;
measures [ channel ] [ 0 ]
= ADRESH ;
measures [ channel ] [ 1 ]
= ADRESL ;
/ / ADC b e c o m e s
Show
me
i f ( channel
channel
//
the
==
bits
YES
then
conversion
empty
adc_buffer_full
//
0;
next
channel
5){
0;
SaveCurrentTemperature ( ) ;
i f ( delta_10ms ) {
// ShowMeasures ( ) ;
nl
SendMeasures ( ) ;
delta_10ms
0;
// e l s e {
//
n++;
//}
}
e l s e { c h a n n e l ++;}
i f ( channel
==
0){
//
start
if
SAMPLE/HOLD
SampleAndHold ( 1 0 ) ;
}
//
Start
conversion
ADC( c h a n n e l ) ;
}
}
}
//
//
(SPENDING TIME
!)
///////////////////////////////////////
////
INT
//////////////////////////////
///////////////////////////////////////
void
e n a b l e I n t e r r u p t s ( void )
{ INTCONbits . GIE = 1 ; }
void
d i s a b l e I n t e r r u p t s ( void )
{ INTCONbits . GIE = 0 ; }
//
//
//
High
#pragma
priority
code
interrupt
vector
InterruptVectorHigh
= 0 x08
void
InterruptVectorHigh
( void )
40
_asm
goto
InterruptHandlerHigh
/ / jump
to
interrupt
routine
_endasm
}
//
//
High
priority
interrupt
#pragma
code
#pragma
interrupt
routine
InterruptHandlerHigh
void
InterruptHandlerHigh
()
{
INTCONbits . GIE =
0;
//
TIMER
if
( INTCONbits . TMR0IF ) {
delta_10ms
// c h e c k
for
TMR0
overflow
1;
t i m e ++;
TMR0L=TIMER0_VALUE_LOW ;
TMR0L=TIMER0_VALUE_HIGH ;
INTCONbits . TMR0IF =
0;
// c l e a r
interrupt
flag
/ / ADC
i f ( P I R 1 b i t s . ADIF ) {
//
measures [ channel ] [ 0 ]
= ADRESH ;
//
measures [ channel ] [ 1 ]
= ADRESL ;
adc_buffer_full
P I R 1 b i t s . ADIF =
1;
0;
}
/
//
I2C
i f ( P I R 1 b i t s . SSPIF ) {
//
b t o a ( SSPSTAT ,
//
putstr ( str ) ;
str );
putch ( ' \ n ' ) ;
P I R 1 b i t s . SSPIF =
0;
*/
INTCONbits . GIE =
1;
}
//
void
ltoa ( long
int
value ,
char
* dest )
{
long
int
char
r e s t o =0 ,
i f ( value
val
i f ( val
v a l=v a l u e ;
<
i =0 ,
j =0 ,
aux = 0 ;
0)
==
0){
val ;
d e s t [ 0 ] = 0 x30 ;
dest [1]=0;
return ;
}
f o r ( i =0;
val
>
0;
i ++)
{
resto
val
10;
val
val
10;
dest [ i ]
resto
i f ( value
<
0)
+ 0 x30 ;
{
d e s t [ i ++] =
'
';
}
dest [ i
]=0;
f o r ( j =0;
j <i ;
j ++,
{
aux =
dest [ j ] ;
dest [ j ]
dest [ i ]
= aux ;
dest [ i ] ;
41
}
}
void
inttoa ( int
value ,
char
* dest )
{
int
v a l=v a l u e ;
char
r e s t o =0 ,
i =0 ,
j =0 ,
aux = 0 ;
i f ( ! value )
{
dest [0]= '0 ';
dest [1]= '\0 ';
return
}
i f ( value
val
<
f o r ( i =0;
0)
val ;
val
>
0;
i ++)
{
resto
val
10;
val
val
10;
dest [ i ]
resto
i f ( value
<
0)
+ 0 x30 ;
{
d e s t [ i ++] =
'
';
}
dest [ i
]=0;
f o r ( j =0;
j <i ;
j ++,
{
aux =
dest [ j ] ;
dest [ j ]
dest [ i ]
= aux ;
dest [ i ] ;
}
}
/
void
Sample ( v o i d ) {
TRISD = 0 x 0 0 ;
PORTD = 0 x 0 0 ;
}
void
Hold ( v o i d ) {
TRISD = 0 x 0 0 ;
PORTD = 0xFF ;
}
*/
void
SampleAndHold ( u n s i g n e d
unsigned
int
int
sample_time ) {
i =0;
TRISD = 0 x 0 0 ;
PORTD = 0 x 0 0 ;
w h i l e ( sample_time >
i ) { i ++;}
TRISD = 0 x 0 0 ;
PORTD = 0xFF ;
}
void
SendMeasures ( v o i d ){
char
//
check_sum =
header
0;
bytes
p u t c h ( 0 xEA ) ;
//
Our
p u t c h ( 0 xBB ) ;
//
IMU
stuff
putch ( 0 x00 ) ;
//
Message
type
p u t c h ( 0 x0B ) ;
//
Payload
length
*
//
data
bytes
//
bits
conversion
putch ( measures_char [ 0 ] ) ;
putch ( measures_char [ 1 ] ) ;
putch ( measures_char [ 2 ] ) ;
putch ( measures_char [ 3 ] ) ;
putch ( measures_char [ 4 ] ) ;
putch ( measures_char [ 5 ] ) ;
putch ( measures_char [ 6 ] ) ;
*/
//
10
bits
conversions
putch ( measures [ 0 ] [ 0 ] ) ;
putch ( measures [ 0 ] [ 1 ] ) ;
42
putch ( measures [ 1 ] [ 0 ] ) ;
putch ( measures [ 1 ] [ 1 ] ) ;
putch ( measures [ 2 ] [ 0 ] ) ;
putch ( measures [ 2 ] [ 1 ] ) ;
putch ( measures [ 3 ] [ 0 ] ) ;
putch ( measures [ 3 ] [ 1 ] ) ;
putch ( measures [ 4 ] [ 0 ] ) ;
putch ( measures [ 4 ] [ 1 ] ) ;
putch ( measures [ 5 ] [ 0 ] ) ;
putch ( measures [ 5 ] [ 1 ] ) ;
//
temperature
dummy
putch ( 0 x00 ) ;
//
unsigned
long
to
bytes
( i
*8);
f o r ( i = 0 ; i < 4 ; i ++){
time_aux =
t i m e >>
t i m e _ b y t e [3 i ]
( unsigned
c h a r ) time_aux ;
}
//
time
bytes
( big
endien )
putch ( time_byte [ 0 ] ) ;
putch ( time_byte [ 1 ] ) ;
putch ( time_byte [ 2 ] ) ;
putch ( time_byte [ 3 ] ) ;
check_sum =
0xEA +
0xBB +
0 x0B +
/
bits
conversion
measures_char [ 0 ] +
measures_char [ 1 ] +
measures_char [ 2 ] +
measures_char [ 3 ] +
measures_char [ 4 ] +
measures_char [ 5 ] +
measures_char [ 6 ] +
*/
//
10
bits
conversion
measures [ 0 ] [ 0 ] +
measures [ 0 ] [ 1 ] +
measures [ 1 ] [ 0 ] +
measures [ 1 ] [ 1 ] +
measures [ 2 ] [ 0 ] +
measures [ 2 ] [ 1 ] +
measures [ 3 ] [ 0 ] +
measures [ 3 ] [ 1 ] +
measures [ 4 ] [ 0 ] +
measures [ 4 ] [ 1 ] +
measures [ 5 ] [ 0 ] +
measures [ 5 ] [ 1 ] +
time_byte [ 0 ] +
time_byte [ 1 ] +
time_byte [ 2 ] +
time_byte [ 3 ] ;
// c h e c k
sum
byte
43
p u t c h ( check_sum ) ;
}
/
unsigned
long
DoubleByteToLong ( u n s i g n e d
unsigned
long
unsigned
long
high_aux ;
unsigned
long
low_aux ;
low_aux =
( unsigned
( unsigned
high_aux
h i g h _ a u x <<
high_aux
return
high ,
unsigned
l o n g ) low ;
high_aux
aux =
char
aux = 0 ;
long ) high ;
8;
low_aux ;
aux ;
*/
void
ShowMeasures ( v o i d ) {
char
for
i ;
( i = 0 ; i < 6 ; i ++){
l t o a ( measures_char [ i ] ,
putstr ( str ) ;
str );
tab
}
}
/
void
ShowMeasuresAverage ( v o i d ) {
char
for
i ;
( i = 0 ; i < 6 ; i ++){
l t o a ( measures_average [ i ] ,
putstr ( str ) ;
str );
tab
}
}
*/
/*
void
FilterMeasures ( void ){
measures_long [ 0 ] ;
w i n d o w _ m e a s u r e s [ 1 ] [ window ]
measures_long [ 1 ] ;
w i n d o w _ m e a s u r e s [ 2 ] [ window ]
measures_long [ 2 ] ;
w i n d o w _ m e a s u r e s [ 3 ] [ window ]
measures_long [ 3 ] ;
w i n d o w _ m e a s u r e s [ 4 ] [ window ]
measures_long [ 4 ] ;
w i n d o w _ m e a s u r e s [ 5 ] [ window ]
measures_long [ 5 ] ;
filtered_measures [ 0 ]
+= w i n d o w _ m e a s u r e s [ 0 ] [ window ] ;
filtered_measures [ 0 ]
+= w i n d o w _ m e a s u r e s [ 1 ] [ window ] ;
filtered_measures [ 0 ]
+= w i n d o w _ m e a s u r e s [ 2 ] [ window ] ;
filtered_measures [ 0 ]
+= w i n d o w _ m e a s u r e s [ 3 ] [ window ] ;
filtered_measures [ 0 ]
+= w i n d o w _ m e a s u r e s [ 4 ] [ window ] ;
filtered_measures [ 0 ]
+= w i n d o w _ m e a s u r e s [ 5 ] [ window ] ;
window++;
}
else {
filtered_measures [ 0 ]
+=
measures_long [ 0 ] ;
filtered_measures [ 0 ]
window_measures [ 0 ] [ j ] ;
filtered_measures [ 1 ]
+= w i n d o w _ m e a s u r e s [ 1 ] [ j ] ;
filtered_measures [ 2 ]
+= w i n d o w _ m e a s u r e s [ 2 ] [ j ] ;
filtered_measures [ 3 ]
+= w i n d o w _ m e a s u r e s [ 3 ] [ j ] ;
filtered_measures [ 4 ]
+= w i n d o w _ m e a s u r e s [ 4 ] [ j ] ;
filtered_measures [ 5 ]
+= w i n d o w _ m e a s u r e s [ 5 ] [ j ] ;
window_measures [ 0 ] [ j ]
measures_long [ 0 ] ;
window_measures [ 1 ] [ j ]
measures_long [ 1 ] ;
window_measures [ 2 ] [ j ]
measures_long [ 2 ] ;
window_measures [ 3 ] [ j ]
measures_long [ 3 ] ;
window_measures [ 4 ] [ j ]
measures_long [ 4 ] ;
window_measures [ 5 ] [ j ]
measures_long [ 5 ] ;
j ++;
if ( j
>
9){ j
0;}
44
char
low ) {
*/
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/ / ADC. C
#i n c l u d e
void
"ADC. h "
ADC( u n s i g n e d
unsigned
i
//
char
char
channel ){
i ;
0;
RIGHT_JUSTIFIED
/ / PORT CONFIGURATION CONTROL
ALL_ANALOGIC
//
FREQ_OSC_32
//
For
XTAL =
2 0MHz
/ / CHANNEL
i f ( channel
==
0){
CHANNEL_0 }
//
pin2
( RA0 )
else
i f ( channel
==
1){
CHANNEL_1 }
//
pin3
( RA1 )
else
i f ( channel
==
2){
CHANNEL_2 }
//
pin4
( RA2 )
else
i f ( channel
==
3){
CHANNEL_3 }
//
pin5
( RA3 )
else
i f ( channel
==
4){
CHANNEL_4 }
//
pin7
( RA5 )
else
i f ( channel
==
5){
CHANNEL_5 }
//
pin8
( RE0 )
else
i f ( channel
==
6){
CHANNEL_6 }
//
pin9
( RE1 )
else
i f ( channel
==
7){
CHANNEL_7 }
//
p i n 1 0 ( RE2 )
INTERRUPTS
INTERRUPTS_ON
//
13 us
INSTRUCTION_64
//
CONVERSION SWITCH
. . .
Start
conversion
CONVERSION_ON
//
. . .
interrupt
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
I2C . C
#i n c l u d e
//
INIT MASTER
void
I n i t I 2 c M a s t e r ( void ){
T R I S C b i t s . TRISC3
1;
T R I S C b i t s . TRISC4
1;
SSPCON1
= 0 x38 ;
SSPCON2 = 0 x 0 0 ;
//
SSPSTAT = 0 x 8 0 ;
SSPSTATbits . SMP =
1;
SSPSTATbits . CKE =
1;
SSPADD
//
SSPADD
49;
//
= 0 x0C ;
2 0M/ ( 4
//
* SSPADD+1)
P I R 1 b i t s . PSPIF
0;
//
Parallel
P I R 2 b i t s . BCLIF
0;
//
Bus
P I E 1 b i t s . SSPIE
INTCONbits . PEIE
1 0 0K <=> SSPADD =
49
4 0 0 kHz
1;
=
//
1;
Slave
collision
Synchronous
//
Port
Serial
Peripheral
RESERVED on
Detection
Port
Interrupt
PIC16F873 / 8 7 6 ;
MUST BE CLEARED
cleared
Interrupt
Enable
Bit
Bit
enabled
enabled
}
//
INIT
void
SLAVE
I n i t I 2 c S l a v e ( void ){
T R I S C b i t s . TRISC3
1;
T R I S C b i t s . TRISC4
1;
SSPCON1
= 0 x36 ;
SSPCON2 = 0 x 0 0 ;
SSPSTAT = 0 x 8 0 ;
SSPADD
2;
P I R 1 b i t s . PSPIF
0;
//
Parallel
P I R 2 b i t s . BCLIF
0;
//
Bus
Slave
collision
Port
RESERVED on
Detection
cleared
45
PIC16F873 / 8 7 6 ;
MUST BE CLEARED
SSPCON2bits . GCEN
P I E 1 b i t s . SSPIE
=
=
INTCONbits . PEIE
1;
1;
=
//
//
1;
General
Call
Synchronous
//
Enabled
Serial
Peripheral
Port
Interrupt
Interrupt
Enable
Bit
}
//
START
void
S t a r t I 2 c ( void ){
WaitIdleI2c ( ) ;
SSPCON2bits . SEN= 1 ;
//
automatically
cleared
by
hardware
}
//
REPEATED START
void
RepStartI2c ( void ){
WaitIdleI2c ( ) ;
SSPCON2bits . RSEN= 1 ;
//
automatically
cleared
by
hardware
}
//
STOP
void
StopI2c ( void ){
WaitIdleI2c ( ) ;
SSPCON2bits . PEN= 1 ;
//
automatically
cleared
by
hardware
}
/ / READ SLAVE
unsigned
char
unsigned
ReadSlaveI2c ( void ){
char
i2cReadData ;
WaitIdleI2c ( ) ;
//
RCEN= 1 ;
//
S h o w R e g i s t e r ( SSPBUF ) ;
//
ShowI2cRegisters ( ) ;
//
WaitIdleI2c ( ) ;
//
delay (1000);
i2cReadData
//
= SSPBUF ;
WaitIdleI2c ( ) ;
/ / ACKEN =
/
nl
1;
//
send
acknowledge
*
if
ack
{
ACKDT= 0 ;
}
else
{
ACKDT= 1 ;
}
ACKEN= 1 ;
//
send
acknowledge
sequence
*/
return (
i2cReadData
);
}
/ / READ MASTER
unsigned
char
unsigned
ReadMasterI2c ( void ){
char
i2cReadData ;
WaitIdleI2c ( ) ;
SSPCON2bits . RCEN= 1 ;
//
S h o w R e g i s t e r ( SSPBUF ) ;
//
ShowI2cRegisters ( ) ;
//
WaitIdleI2c ( ) ;
nl
delay (1000);
i2cReadData
= SSPBUF ;
WaitIdleI2c ( ) ;
SSPCON2bits .ACKEN =
/
1;
//
send
acknowledge
*
if
ack
{
ACKDT= 0 ;
}
else
{
ACKDT= 1 ;
}
ACKEN= 1 ;
//
send
acknowledge
sequence
*/
return (
i2cReadData
);
}
//
WRITE
unsigned
char
WriteI2c ( unsigned
char
i2cWriteData ){
46
Bit
enabled
enabled
WaitIdleI2c ( ) ;
SSPBUF =
i2cWriteData ;
//
delay ( 3 5 ) ;
//
//
return
ACKSTAT
return
Delay
must
);
be
//
higher
function
than
31
returns
to
allow
'1 '
if
ACKNOWLEDGE
transmission
reception
is
acknowledged
0;
}
//
void
WaitIdleI2c (){
while
((
SSPSTATbits .R_W )
{};
//
}
void
ShowI2cRegisters ( void ){
unsigned
char
string [ 9 ] ;
string );
putstr ( string ) ;
tab
string );
putstr ( string ) ;
tab
string );
putstr ( string ) ;
}
void
ShowRegister ( unsigned
unsigned
char
btoa ( reg ,
char
reg ){
bits [ 9 ] ;
bits );
putstr ( bits ) ;
}
void
btoa ( unsigned
char
val ,
unsigned
str [0]
( val
& 0 x 8 0 ) >>
7)
+ 0 x30 ;
str [1]
( val
& 0 x 4 0 ) >>
6)
+ 0 x30 ;
str [2]
( val
& 0 x 2 0 ) >>
5)
+ 0 x30 ;
str [3]
( val
& 0 x 1 0 ) >>
4)
+ 0 x30 ;
str [4]
( val
& 0 x 0 8 ) >>
3)
+ 0 x30 ;
str [5]
( val
& 0 x 0 4 ) >>
2)
+ 0 x30 ;
str [6]
( val
& 0 x 0 2 ) >>
1)
+ 0 x30 ;
str [7]
( val
str [8]
0;
& 0 x01 )
char
* s t r ){
+ 0 x30 ;
}
//
DELAY ALPHA
void
delay ( unsigned
unsigned
for
int
int
count ){
i ;
( i = 0 ; i <c o u n t ;++ i ) { }
}
//
void
I 2 c I n t e r r u p t S e r v i c e R o u t i n e ( void ){
static
if
char
( state
s t a t e =0;
==
0){
s t a t e =1;
W r i t e I 2 c ( 0 x02 ) ;
}
if
(( state
==
1)
&&
(ACKSTAT ==
0)){
s t a t e =2;
W r i t e I 2 c ( 0 x05 ) ;
}
i f (( state
==
2)
&&
(ACKSTAT ==
0)){
s t a t e =0;
StopI2c ( ) ;
}
}
*/
//
void
I 2 c I n t e r r u p t S e r v i c e R o u t i n e ( void ){
static
char
unsigned
i f (( state
s t a t e =0;
char
==
buffy ;
0)){
state
buffy
= SSPBUF ;
1;
ShowRegister ( buffy ) ;
nl
47
wait
for
idle
and
not
writing
else {
putch ( ' F ' ) ;
}
i f ( state
==
1){
state
0;
buffy
= SSPBUF ;
ShowRegister ( buffy ) ;
nl
}
else {
putch ( ' F ' ) ;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
DELAY . C
*
* Delay f u n c t i o n s
* See delay . h f o r d e t a i l s
*
* Make s u r e t h i s c o d e i s c o m p i l e d
*/
#i n c l u d e
with
full
optimization ! ! !
" d e l a y . h"
void
DelayMs ( u n s i g n e d
char
cnt )
{
#i f
do
DelayUs ( 9 9 6 ) ;
w h i l e ( c n t ) ;
#e n d i f
#i f
XTAL_FREQ >
unsigned
do
do
char
2MHZ
i ;
4;
DelayUs ( 2 5 0 ) ;
w h i l e ( i ) ;
}
}
w h i l e ( c n t ) ;
#e n d i f
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
USART . C
*
* USART MODULE
*
* f i l e s : usart . c ,
*
*/
#i n c l u d e
void
usart . h
" u s a r t . h"
initUsart ()
{
T R I S C b i t s . TRISC7 =
1;
//
RX
( input )
T R I S C b i t s . TRISC6 =
0;
//
TX
( output )
SPBRG =
10;
/ / baud
TXSTAbits . SYNC =
21 >57600
0;
RCSTAbits . SPEN =
1;
TXSTAbits .BRGH =
1;
RCSTAbits . CREN =
1;
INTCONbits . PEIE =
0;
P I E 1 b i t s . RCIE =
0;
P I E 1 b i t s . TXIE =
0;
TXSTAbits . TXEN =
10 >115200;
//
Periferal
interrupt
enable
bit
1;
}
void
pu ts tr ( char
* str )
{
while ((
putch (
* s t r ) > 0)
* ( s t r ++)
);
48
( inturrupes
p/
usart )
}
void
putch ( unsigned
char
byte )
{
w h i l e ( ! P I R 1 b i t s . TXIF )
continue ;
TXREG =
byte ;
}
unsigned
char
getch ()
{
w h i l e ( ! P I R 1 b i t s . RCIF )
continue ;
return
RCREG;
}
void
SerString_rom ( s t a t i c
char
rom
* str )
//
send
string
via
RS232
{
while ((
putch (
* s t r ) > 0)
* ( s t r ++)
);
}
void
putch_uchar
unsigned
char
value
{
unsigned
char
if
char
i ;
numero [ 1 0 ] ;
( value
==
0)
( value )
// putch
'0 '
numero [ i ++] =
value
( value
'0 '
value
10)
);
( v a l u e %10);
10;
}
while ( i
p u t c h ( numero [ i ] ) ;
}
void
putch_ushortlong (
unsigned
short
long
value )
{
unsigned
char
if
char
i ;
numero [ 1 0 ] ;
( value
==
0)
i =0;
while
( value )
numero [ i ++] =
value
value
'0 '
/
( v a l u e %10);
10;
}
while ( i
p u t c h ( numero [ i ] ) ;
}
unsigned
char
getch_number ( )
{
// i n s i r a
um
unsigned
char
numero
v a l u e =0 ,
tmp = 0 ;
getch ( ) ;
while
( tmp
!=
13)
{
if
if
( tmp ==
( tmp >=
8)
'0 ')
&&
value
( tmp
value
value
*10
value /10;
');
putch ( '
');
putch ( '
');
49
tmp =
getch ( ) ;
}
putch ( ' \ r ' ) ;
putch ( ' : ' ) ;
putch_uchar ( v a l u e ) ;
putch ( '
');
putch ( '
');
putch ( '
return
');
value ;
}
void
putch_ulong (
unsigned
long
value
{
unsigned
char
if
char
i ;
numero [ 1 0 ] ;
( value
==
0)
i =0;
while
( value )
numero [ i ++] =
value
value
'0 '
/
( v a l u e %10);
10;
}
while ( i
p u t c h ( numero [ i ] ) ;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
TIMER0 . C
*
* TIMER0
*
* files :
*
*/
MODULE
TIMER0 . c ,
TIMER0 . h
#i f n d e f
__TIMER0_C__
#d e f i n e
__TIMER0_C__
#i n c l u d e
"TIMER0 . h "
//
Global
//
Functions
void
Variables
confTIMER0 ( v o i d )
{
//
1;
T0CONbits . T08BIT =
switch
//
0;
PRESCALER :
//
8 bits
1= n o t
1;
assigned ;
16 b i t s
0= a s s i g n e d
= 0
(TIMER0_PRESCALER)
{
case
2:
T0CONbits . T0PS2 = 0 ;
T0CONbits . T0PS1 = 0 ;
T0CONbits . T0PS0 = 0 ;
break ;
case
4:
T0CONbits . T0PS2 = 0 ;
T0CONbits . T0PS1 = 0 ;
T0CONbits . T0PS0 = 1 ;
break ;
case
8:
T0CONbits . T0PS2 = 0 ;
T0CONbits . T0PS1 = 1 ;
T0CONbits . T0PS0 = 0 ;
break ;
case
16:
T0CONbits . T0PS2 = 0 ;
T0CONbits . T0PS1 = 1 ;
T0CONbits . T0PS0 = 1 ;
break ;
case
32:
T0CONbits . T0PS2 = 1 ;
T0CONbits . T0PS1 = 0 ;
T0CONbits . T0PS0 = 0 ;
break ;
case
64:
T0CONbits . T0PS2 = 1 ;
T0CONbits . T0PS1 = 0 ;
T0CONbits . T0PS0 = 1 ;
break ;
case
128:
T0CONbits . T0PS2 = 1 ;
T0CONbits . T0PS1 = 1 ;
T0CONbits . T0PS0 = 0 ;
break ;
case
256:
T0CONbits . T0PS2 = 1 ;
T0CONbits . T0PS1 = 1 ;
T0CONbits . T0PS0 = 1 ;
break ;
T0CONbits . T0PS2 = 1 ;
T0CONbits . T0PS1 = 1 ;
T0CONbits . T0PS0 = 1 ;
break ;
default :
}
TMR0L=TIMER0_VALUE_LOW ;
//230;
//
100
TMR0H=TIMER0_VALUE_HIGH ;
//
INTCONbits . T0IE = 0 ; / /
TIMER0
interrupt
enabled
}
#e n d i f
50
Referncias
[1] Telmo Reis Cunha. High Precision Navigation Integrating Satellite Information - GPS
- and Inertial System Data. Tese de Doutoramento, Faculdade de Engenharia da Universidade do Porto, 2002
[2] Oliver J. Woodman. An Introduction to inertial navigation. University of Cambridge,
2007
[3] A. D. King. Inertial Navigation - Forty Years of Evolution. 1998
[4] Chen, W. and P.A. Cross (1990) Integration of GPS and Inertial System for Precise Surveying Applications. Department of Surveying, University of Newcastle upon
Tyne, pp. 375 - 395.
[5] Nayak, R.A. (2000) Reliable and Continuous Urban Navigation Using Multiple GPS
Antennas and a Low Cost IMU. Master Thesis, Department of Geomatics Engineering,
University of Calgary, Alberta.
[6] Shin, E.H. (2001) Accuracy Improvement of Low Cost INS/GPS for Land Applications. Master Thesis, Department of Geomatics Engineering, University of Calgary,
Alberta.
[7] Schwarz, K.P., H.E. Mantell, N. EI-Sheimy, R. Li, M. A. Chapman and D. Cosandier
1993: VISAT - A Mobile Highway Survey System of High Accuracy. Proceedings of
the VNIS'93, pp.476-481. Stevens
[8] Magno Morgado, G., "Desenvolvimento e anlise de desempenho de um sistema de
navegao integrado", dissertao de Mestrado Integrado em Engenharia Electrnica
e Telecomunicaes, Universidade de Aveiro, Nov. 2008.
51