Group Beta Two
From TheUbberCannon
Conteúdo |
Alunos
- Guilherme Fernandes (fernandes@npd.ufsc.br)
- Murilo Vetter (mvetter@inf.ufsc.br)
- Rafael de Souza Coelho (socoelho@inf.ufsc.br)
Team Beta
The Team Beta is in charge of the Defense System, including the Radar and the Photonic Cannon. This team wins the game by destroying the Klingon Vulture (and, unfortunately killing Kirk and Spock).
Group B2
O objetivo da equipe eh escolher e implementar um algoritmo de localizacao que darah as coordenadas da nave klingon. Tambem deve-se realizar a comunicacao com os outros integrantes da equipe beta, para que se possa obter e fornecer os dados necessarios para que o algoritmo seja execurado de forma correta.
Schedule
Task Schedule for Group Beta 2
| # | Atividade | Descrição | Recurso | Status | Cronograma |
|---|---|---|---|---|---|
| 1
| Definição da API matemática | Reunião com o grupo Beta1 para escolha das funções matemáticas a serem usadas | Algoritmo de localização | OK | 1100-0000-0000-0000
|
| 2
| Localização | Estudo de Algorítmos de Localização | Internet, Dissertação Ricardo Righelin - LISHA | OK | 1111-0000-0000-0000
|
| 3
| EPOS | Estudo do Sistema Operacional EPOS | Manual EPOS | OK | 1111-0000-1000-0000
|
| 4
| Mica2 | Estudo do Mica2 | Mica2 | CH | 1111-0000-0000-0000
|
| 5
| Implementação | Implementação do algoritmo de localização | Sistema Operacional Linux, Compilador GCC, Computador, Manual de Referência Mica2 e GCC | OK | 1111-1111-1100-0000
|
| 6
| Integração | Integração com outras equipes | Mica2, o aplicativo desenvolvido, outras equipes | OK | 0001-1111-1111-1100
|
| 7
| Apresentação | OK | 0000-0001-0000-0000
| ||
| 8
| Jogo | OK | 0000-0000-0000-0011
| ||
| 9
| PowerPC | Estudo do PowerPC | PowerPC | OK | 1111-0000-0000-0000
|
Legenda
- EA - Em andamento
- OK - Terminado com sucesso
- NOK - Problemas (Bloqueado)
- AT - Atrasado
- CH - Mudança de Plano, Tarefa cancelada
- TI - Tarefa incluida
Definições
Problema de Localização
O problema de localização é muitas vezes resolvido utilizando a intensidade de sinal (RSSI) que os rádios (sensores) produzem. Estes dados devem ser traduzidos para a escala já conhecida que utiliza medidas padrões (no caso metros) para se obter a equivalência de cada dado. Um bom sistema de localização se vale de 2 fatores importantes:
1. Equivalência de RSSIXCoordenadas: Deve se ter confiabilidade entre as medidas de RSSIs reais geradas para o padrão de coordenadas adotado.
2. Localização: Dados valores equivalentes entre si o sistema adotado para localização foi o trilateração (vale-se de 3 coordenadas) e o multilateração (neste caso vale-se de 4 coordenadas, porém pode ter mais que 4 coordenadas).
Relatório
Começamos o projeto realizando o cronograma e definindo as tarefas de cada integrante do grupo. A idéia inicial foi pesquisar nos sites de repositório de artigos técnicas de localização que pudessem ser aplicadas ao nosso problema. Após diversos artigos lidos, concluímos que a melhor maneira (mais viável, até em função do tempo) seria a de implementação da localização através de multilateração e com uma possível correção de heurística seguindo a tese de mestrado do Ricardo Reghelin.
Este modelo foi implementado para a arquitetura IA32 e testado em cima de dados obtidos em campo por uma equipe do LISHA. Os dados estavam dispostos em um grid de 10mx10m, com um rádio a cada quadrante de 5mx5m. O erro médio da localização estimada com relação à localização real nestes testes foi de aproximadente 3 metros, sendo o mínimo de 1.84 e o máximo de 4.78. Maiores informações sobre os testes realizados podem ser encontrados na apresentação de progresso apresentada durante a disciplina.
Vendo a dimensão dos erros, e apesar de saber que erros de 2m eram o esperado para estimação por RSSI, tentamos encontrar outras formas de utilizar heurística e inteligência artificial no algoritmo. Isto se tornava possível por termos transferido o código de localização para a arquitetura PowerPC onde rodaria o sistema de integração dos módulos do grupo Beta, ao invés de rodar nos micas como previsto inicialmente. Mas depois de alguma pesquisa, e considerando que estavamos nos aproximando do final da disciplina, vimos que seria muito complicado desenvolver um algoritmo com inteligência artificial e treiná-lo de forma a que os valores pudessem ser utilizáveis em um novo teste de campo. Isto se tornou algo acertado, já que os testes obtidos em campo pela equipe do lisha (RSSIs de 30 a 180) diferiam enormemente dos valores obtidos na hora do jogo (variações de 1200 a números de 4 bytes).
A integração com o PowerPC e os demais módulos não foi tão fácil como se previa. Estava ocorrendo algum erro para a compilação do código utilizando ponto flutuante e não conseguimos contorná-lo. Foi necessária a re-implementação do código todo em ponto fixo, perdendo ainda mais precisão nas estimativas. Nesta etapa dividimos o código em dois a fim de ser testado se o código simples sem heurística acaba se aproximando mais dos testes reais do que o código com heurística por causa da imprecisão dos cálculos e da aleatoriedade dos RSSIs. Infelizmente também não foi possível testar ambos os códigos, já que houveram outros problemas de integração entre os outros módulos do grupo Beta. O algoritmo escolhido para o jogo foi o algoritmo localizacao.cc.2, que realiza a estimativa utilizando a heurística simples baseada na tese de mestrado do Ricardo Reghelin.
Implementação
A primeira implementação que se teve deste código se utilizou de números com ponto flutuante. Devido a problemas na arquitetura utilizada para o projeto (PowerPC) em trabalhar com ponto flutuantes, teve-se que migrar todo o código-fonte já implementado para utilização de ponto fixo (long/int). Utilizou-se 1 ou 2 casas para a representação da parte de fração dos números, perdendo um pouco na precisão. A utilização de ponto flutuante se demonstrou satisfatória para o problema.
O código em mica_app.cc é o código de uma aplicação de teste que utiliza o Epos para testar a Aplicação. Para testar esta aplicação é necessário uma aplicação que emule uma ambiente virtual, tal qual VMVare.
Algoritmos de Localização
Implementou-se 2 algoritmos de localização com o intuíto de comparação e validação.
Algoritmo Baseado em Histórico
Este algoritmo se vale apenas de valores atuais e de um histórico para aproximar a localização
Algoritmo Baseado em Fator de Correção
Este algoritmo se vale valores atuais, histórico e um fator de correção que calculado e corrigido a cada passada do algoritmo para aproximar a localização
Código Fonte
trilateracao.h
#ifndef __trilateracao_h
#define __trilateracao_h
#include <system/config.h>
__BEGIN_SYS
class Trilateracao {
private:
long A[4], AT[4], B[4], preB[4];
long x1, y1, x2, y2, x3, y3;
void matrizA();
void preMatrizB();
void transposta();
long* multiplicaAtA(long *m);
long* multiplicaXAt(long *X);
long* multiplicaXB(long *X);
long* inversa(long *A);
public:
Trilateracao(){};
~Trilateracao(){};
void aproxime(long*);
void beacons(long x1, long y1, long x2, long y2, long x3, long y3);
void distancias(long d1, long d2, long d3);
void preCalculo();
};
__END_SYS
#endif
trilateracao.cc
#include "trilateracao.h"
__BEGIN_SYS
void Trilateracao::beacons(long x1, long y1, long x2, long y2, long x3, long y3) {
this->x1 = x1;
this->y1 = y1;
this->x2 = x2;
this->y2 = y2;
this->x3 = x3;
this->y3 = y3;
};
void Trilateracao::distancias(long d1, long d2, long d3) {
this->B[0] = (this->preB[0]*100) + (d3 * d3) - (d1 * d1);
this->B[1] = (this->preB[1]*100) + (d3 * d3) - (d2 * d2);
};
void Trilateracao::matrizA() {
this->A[0] = (2 * (this->x1 - this->x3))/10;
this->A[1] = (2 * (this->y1 - this->y3))/10;
this->A[2] = (2 * (this->x2 - this->x3))/10;
this->A[3] = (2 * (this->y2 - this->y3))/10;
};
void Trilateracao::preMatrizB() {
this->preB[0] = (this->x1 * this->x1) - (this->x3 * this->x3) + (this->y1 * this->y1) - (this->y3 * this->y3);
this->preB[1] = (this->x2 * this->x2) - (this->x3 * this->x3) + (this->y2 * this->y2) - (this->y3 * this->y3);
};
void Trilateracao::transposta() {
this->AT[0] = this->A[0];
this->AT[1] = this->A[2];
this->AT[2] = this->A[1];
this->AT[3] = this->A[3];
};
long* Trilateracao::multiplicaAtA(long* retorno) {
retorno[0] = (this->AT[0] * this->A[0]) + (this->AT[1] * this->A[2]);
retorno[1] = (this->AT[0] * this->A[1]) + (this->AT[1] * this->A[3]);
retorno[2] = (this->AT[2] * this->A[0]) + (this->AT[3] * this->A[2]);
retorno[3] = (this->AT[2] * this->A[1]) + (this->AT[3] * this->A[3]);
return retorno;
};
long* Trilateracao::multiplicaXAt(long *X) {
long retorno[4];
retorno[0] = ((X[0]/100) * this->AT[0]) + ((X[1]/100) * this->AT[2]);
retorno[1] = ((X[0]/100) * this->AT[1]) + ((X[1]/100) * this->AT[3]);
retorno[2] = ((X[2]/100) * this->AT[0]) + ((X[3]/100) * this->AT[2]);
retorno[3] = ((X[2]/100) * this->AT[1]) + ((X[3]/100) * this->AT[3]);
X[0] = retorno[0];
X[1] = retorno[1];
X[2] = retorno[2];
X[3] = retorno[3];
return X;
};
long* Trilateracao::multiplicaXB(long *X) {
long retorno[2];
retorno[0] = ((X[0]/100) * (this->B[0]/100)) + ((X[1]/100) * (this->B[1]/100));
retorno[1] = ((X[2]/100) * (this->B[0]/100)) + ((X[3]/100) * (this->B[1]/100));
X[0] = retorno[0];
X[1] = retorno[1];
return X;
};
long* Trilateracao::inversa(long *A) {
long retorno[4];
long det = 1000000000 / ((A[0] * A[3]) - (A[1] * A[2]));
retorno[0] = A[3] * det;
retorno[1] = -1 * A[1] * det;
retorno[2] = -1 * A[2] * det;
retorno[3] = A[0] * det;
A[0] = retorno[0];
A[1] = retorno[1];
A[2] = retorno[2];
A[3] = retorno[3];
return A;
};
void Trilateracao::aproxime(long* m) {
multiplicaXB(multiplicaXAt(inversa(multiplicaAtA(m))));
};
void Trilateracao::preCalculo() {
matrizA();
preMatrizB();
transposta();
};
__END_SYS
multilateracao.h
#ifndef __multilateracao_h
#define __multilateracao_h
#include <system/config.h>
__BEGIN_SYS
class Multilateracao {
private:
long A[6], AT[6], B[3], preB[3];
long x1, y1, x2, y2, x3, y3, x4, y4;
void matrizA();
void preMatrizB();
void transposta();
long* multiplicaAtA(long *m);
long* multiplicaXAt(long *X);
long* multiplicaXB(long *X);
long* inversa(long *A);
public:
Multilateracao(){};
~Multilateracao(){};
void aproxime(long*);
void beacons(long x1, long y1, long x2, long y2, long x3, long y3, long x4, long y4);
void distancias(long d1, long d2, long d3, long d4);
void preCalculo();
};
__END_SYS
#endif
multilateracao.cc
#include <multilateracao.h>
__BEGIN_SYS
void Multilateracao::beacons(long x1, long y1, long x2, long y2, long x3, long y3, long x4, long y4) {
this->x1 = x1;
this->y1 = y1;
this->x2 = x2;
this->y2 = y2;
this->x3 = x3;
this->y3 = y3;
this->x4 = x4;
this->y4 = y4;
};
void Multilateracao::distancias(long d1, long d2, long d3, long d4) {
this->B[0] = (this->preB[0]*100) + (d4 * d4) - (d1 * d1);
this->B[1] = (this->preB[1]*100) + (d4 * d4) - (d2 * d2);
this->B[2] = (this->preB[2]*100) + (d4 * d4) - (d3 * d3);
};
void Multilateracao::matrizA() {
this->A[0] = (2 * (this->x1 - this->x4))/10;
this->A[1] = (2 * (this->y1 - this->y4))/10;
this->A[2] = (2 * (this->x2 - this->x4))/10;
this->A[3] = (2 * (this->y2 - this->y4))/10;
this->A[4] = (2 * (this->x3 - this->x4))/10;
this->A[5] = (2 * (this->y3 - this->y4))/10;
};
void Multilateracao::preMatrizB() {
this->preB[0] = (this->x1 * this->x1) - (this->x4 * this->x4) + (this->y1 * this->y1) - (this->y4 * this->y4);
this->preB[1] = (this->x2 * this->x2) - (this->x4 * this->x4) + (this->y2 * this->y2) - (this->y4 * this->y4);
this->preB[2] = (this->x3 * this->x3) - (this->x4 * this->x4) + (this->y3 * this->y3) - (this->y4 * this->y4);
};
void Multilateracao::transposta() {
this->AT[0] = this->A[0];
this->AT[1] = this->A[2];
this->AT[2] = this->A[4];
this->AT[3] = this->A[1];
this->AT[4] = this->A[3];
this->AT[5] = this->A[5];
};
long* Multilateracao::multiplicaAtA(long* retorno) {
retorno[0] = (this->AT[0] * this->A[0]) + (this->AT[1] * this->A[2]) + (this->AT[2] * this->A[4]);
retorno[1] = (this->AT[0] * this->A[1]) + (this->AT[1] * this->A[3]) + (this->AT[2] * this->A[5]);
retorno[2] = (this->AT[3] * this->A[0]) + (this->AT[4] * this->A[2]) + (this->AT[5] * this->A[4]);
retorno[3] = (this->AT[3] * this->A[1]) + (this->AT[4] * this->A[3]) + (this->AT[5] * this->A[5]);
return retorno;
};
long* Multilateracao::multiplicaXAt(long *X) {
long retorno[6];
retorno[0] = ((X[0]/100) * this->AT[0]) + ((X[1]/100) * this->AT[3]);
retorno[1] = ((X[0]/100) * this->AT[1]) + ((X[1]/100) * this->AT[4]);
retorno[2] = ((X[0]/100) * this->AT[2]) + ((X[1]/100) * this->AT[5]);
retorno[3] = ((X[2]/100) * this->AT[0]) + ((X[3]/100) * this->AT[3]);
retorno[4] = ((X[2]/100) * this->AT[1]) + ((X[3]/100) * this->AT[4]);
retorno[5] = ((X[2]/100) * this->AT[2]) + ((X[3]/100) * this->AT[5]);
X[0] = retorno[0];
X[1] = retorno[1];
X[2] = retorno[2];
X[3] = retorno[3];
X[4] = retorno[4];
X[5] = retorno[5];
return X;
};
long* Multilateracao::multiplicaXB(long *X) {
long retorno[2];
retorno[0] = ((X[0]/100) * (this->B[0]/100)) + ((X[1]/100) * (this->B[1]/100)) + ((X[2]/100) * (this->B[2]/100));
retorno[1] = ((X[3]/100) * (this->B[0]/100)) + ((X[4]/100) * (this->B[1]/100)) + ((X[5]/100) * (this->B[2]/100));
X[0] = retorno[0];
X[1] = retorno[1];
return X;
};
long* Multilateracao::inversa(long *A) {
long retorno[4];
long det = 1000000000 / ((A[0] * A[3]) - (A[1] * A[2]));
retorno[0] = A[3] * det;
retorno[1] = -1 * A[1] * det;
retorno[2] = -1 * A[2] * det;
retorno[3] = A[0] * det;
A[0] = retorno[0];
A[1] = retorno[1];
A[2] = retorno[2];
A[3] = retorno[3];
return A;
};
void Multilateracao::aproxime(long* m) {
multiplicaXB(multiplicaXAt(inversa(multiplicaAtA(m))));
};
void Multilateracao::preCalculo() {
matrizA();
preMatrizB();
transposta();
};
__END_SYS
localizacao.h
#ifndef __localizacao_h
#define __localizacao_h
#include <system/config.h>
#include <tipos.h>
__BEGIN_SYS
class Localizacao
{
private:
long d[3]; // distancia fixa entre beacons
long coefs[4][4][4]; // coeficientes de correcao
unsigned short rssis[4];
unsigned short ctr[4]; // controle de chegada
int dcs; // decisao se usa trilat/multi
long rssisinv[4], dnave[4];
int count[4];
bool full[4]; // array de historico completo
int lastcount[4];
int rcvd[4];
int beacon;
static const long pos[4][2];
void reset();
Target trilat(bool);
Target multi(bool);
public:
Localizacao():beacon(0){
d[0] = 204;
d[1] = 315;
d[2] = 376;
rcvd[0] = -1;
rcvd[1] = -1;
rcvd[2] = -1;
rcvd[3] = -1;
}
~Localizacao(){}
Target update(PDU pdu);
};
__END_SYS
#endif
localizacao.cc.1
#include <localizacao.h>
#include <trilateracao.h>
#include <multilateracao.h>
#include <utility/ostream.h>
__USING_SYS
const long Localizacao::pos[4][2] = { {(long)0,(long)0}, {(long)315,(long)0}, {(long)315,(long)204}, {(long)0,(long)204} };
OStream cout;
Target Localizacao::update(PDU pdu){
int i;
for(int j=0; j < 4; j++){
if(pdu.i[j] == 0) return Target();
if(pdu.i[j] == 0xFFFF) i = j;
}
Target result;
if(ctr[i] == 1){
if(rssis[i] == pdu.ialvo){
return Target();
}else if(dcs == 3){
result = trilat(true);
}
reset();
}
for(int k=0; k < 4; ++k){
if(k == i)
continue;
int f = i-k;
if (f < 0) f *= -1;
if ((k == 2 && i == 1) || (k == 1 && i == 2) || f == 3) f = 0;
long asoma = (long) ((d[f]*10000)/pdu.i[k]);
coefs[i][count[i]][k] = (long)asoma;
}
if(pdu.ialvo == 0) return Target();
rcvd[beacon++] = i;
rssis[i] = pdu.ialvo;
ctr[i] = 1;
dcs = 0;
for (int a=0;a<4;a++)
if (ctr[a] == 1)
dcs++;
if (dcs == 3){
return this->trilat(false);
}
else if (dcs == 4){
return this->multi(true);
}
if(result.fire)
return result;
return Target();
}
void Localizacao::reset(){
rcvd[0] = -1;
rcvd[1] = -1;
rcvd[2] = -1;
rcvd[3] = -1;
beacon = 0;
for (int a=0;a<4;a++){
ctr[a] = 0;
if(count[a] == 3){
full[a] = true;
count[a] = 0;
}
else
count[a]++;
}
}
Target Localizacao::trilat(bool _f){
long prop[3];
long menor = 0xFFFF;
int ind;
for (int j=0;j<4;j++)
if (ctr[j]==0) ind = j;
for(int k=0;k<4;k++){
if (k==ind) continue;
int lim;
if (!full[k]){
lim = count[k];
}else{
lim = 3;
}
long soma = 0;
for(int m=0;m<=lim;m++){
soma += (coefs[k][m][0] + coefs[k][m][1] + coefs[k][m][2] + coefs[k][m][3] - coefs[k][m][ind]) / 2;
}
prop[k] = soma/(lim+1);
dnave[k] = (long) (prop[k] * rssis[k])/1000;
}
long coord[4];
Trilateracao t;
switch(ind) {
case 0:
t.beacons(pos[1][0],pos[1][1],pos[2][0],pos[2][1],pos[3][0],pos[3][1]);
t.preCalculo();
t.distancias(dnave[1], dnave[2], dnave[3]);
break;
case 1:
t.beacons(pos[0][0],pos[0][1],pos[2][0],pos[2][1],pos[3][0],pos[3][1]);
t.preCalculo();
t.distancias(dnave[0], dnave[2], dnave[3]);
break;
case 2:
t.beacons(pos[0][0],pos[0][1],pos[1][0],pos[1][1],pos[3][0],pos[3][1]);
t.preCalculo();
t.distancias(dnave[0], dnave[1], dnave[3]);
break;
case 3:
t.beacons(pos[0][0],pos[0][1],pos[1][0],pos[1][1],pos[2][0],pos[2][1]);
t.preCalculo();
t.distancias(dnave[0], dnave[1], dnave[2]);
break;
}
t.aproxime(coord);
if(coord[0] < 0) coord[0] = 0;
else if(coord[0] > 170000000) coord[0] = 170000000;
if(coord[1] < 0) coord[1] = 0;
else if(coord[1] > 280000000) coord[1] = 280000000;
return Target((long) coord[0] / 34000000, (long) coord[1] / 35000000, _f);
}
Target Localizacao::multi(bool _f){
long prop[4];
long menor = 0xFFFF;
for(int k=0;k<4;k++){
long soma = 0;
int lim = 0;
if (!full[k]){
lim = count[k];
}else{
lim = 3;
}
for(int m=0;m<=lim;m++){
soma += (coefs[k][m][0] + coefs[k][m][1] + coefs[k][m][2] + coefs[k][m][3]) / 3;
}
prop[k] = soma/(lim+1);
dnave[k] = (long) (prop[k] * rssis[k])/1000;
}
long coord[4];
Multilateracao t;
t.beacons(pos[0][0],pos[0][1],pos[1][0],pos[1][1],pos[2][0],pos[2][1],pos[3][0],pos[3][1]);
t.preCalculo();
t.distancias(dnave[0], dnave[1], dnave[2], dnave[3]);
t.aproxime(coord);
this->reset();
if(coord[0] < 0) coord[0] = 0;
else if(coord[0] > 170000000) coord[0] = 170000000;
if(coord[1] < 0) coord[1] = 0;
else if(coord[1] > 280000000) coord[1] = 280000000;
return Target((long) coord[0] / 34000000, (long) coord[1] / 35000000, _f);
}
localizacao.cc.2
#include <localizacao.h>
#include <trilateracao.h>
#include <multilateracao.h>
#include <utility/ostream.h>
__USING_SYS
const long Localizacao::pos[4][2] = { {(long)0,(long)0}, {(long)315,(long)0}, {(long)315,(long)204}, {(long)0,(long)204} };
Target Localizacao::update(PDU pdu){
int i;
for(int j=0; j < 4; j++){
if(pdu.i[j] == 0) return Target();
if(pdu.i[j] == 0xFFFF) i = j;
}
Target result;
if(ctr[i] == 1){
if(rssis[i] == pdu.ialvo){
return Target();
}else if(beacon == 3){
result = trilat(true);
}
reset();
}
for(int k=0; k < 4; ++k){
if(k == i)
continue;
int f = i-k;
if(i < k) f = f * -1;
if ((k == 2 && i == 1) || (k == 1 && i == 2) || f == 3) f = 0;
if(!full[i]){
long asoma = (long)(d[f]*1000000)/pdu.i[k]; // alpha atual 2.7
for(int j=0; j < count[i]; ++j){
asoma += coefs[i][j][k];
}
coefs[i][count[i]][k] = (long)asoma/(count[i] + 1); // coefs sempre 2.7
} else {
coefs[i][count[i]][k] = (long)((d[f]*1000000) / pdu.i[k] +
coefs[i][0][k] +
coefs[i][1][k] +
coefs[i][2][k] +
coefs[i][3][k])/5;
}
}
if(pdu.ialvo == 0) return Target();
rcvd[beacon++] = i;
rssis[i] = pdu.ialvo;
rssisinv[i] = (long)100000/pdu.ialvo; // rssisinv 0.5
ctr[i] = 1;
int dcs = 0; // decisao se usa trilat/multi
for (int a=0;a<4;a++)
if (ctr[a] == 1)
dcs++;
if (dcs == 3){
return this->trilat(false);
}
else if (dcs == 4){
return this->multi(true);
}
if(result.fire)
return result;
return Target();
}
void Localizacao::reset(){
rcvd[0] = -1;
rcvd[1] = -1;
rcvd[2] = -1;
rcvd[3] = -1;
beacon = 0;
for (int a=0;a<4;a++){
ctr[a] = 0;
if(count[a] == 3){
full[a] = true;
count[a] = 0;
}
else
count[a]++;
}
}
Target Localizacao::trilat(bool _f){
long prop[3];
for (int k = 0; k < 3; k++){ // calculo da proporcao
prop[rcvd[k]] = ((rssisinv[rcvd[k]]*10000) / (rssisinv[rcvd[0]] + rssisinv[rcvd[1]] + rssisinv[rcvd[2]]))/10; // prop 0.3
}
for(int k=0;k<3;k++){ // calculo das distancias entre a nave e os beacons // dnave 5.2
dnave[rcvd[k]] = (long)((( (coefs[rcvd[k]][count[k]][rcvd[0]]/1000) * prop[rcvd[0]] +
(coefs[rcvd[k]][count[k]][rcvd[1]]/1000) * prop[rcvd[1]] +
(coefs[rcvd[k]][count[k]][rcvd[2]]/1000) * prop[rcvd[2]]) /
(prop[rcvd[0]] + prop[rcvd[1]] + prop[rcvd[2]] - prop[rcvd[k]])) * rssis[rcvd[k]])/100;
if(dnave[rcvd[k]] > d[2]*10) dnave[rcvd[k]] = d[2]*10;
}
Trilateracao t;
long coord[4];
t.beacons(pos[rcvd[0]][0],
pos[rcvd[0]][1],
pos[rcvd[1]][0],
pos[rcvd[1]][1],
pos[rcvd[2]][0],
pos[rcvd[2]][1]);
t.preCalculo();
t.distancias(dnave[0], dnave[1], dnave[2]);
t.aproxime(coord);
if(coord[0] < 0) coord[0] = 0;
else if(coord[0] > 170000000) coord[0] = 170000000;
if(coord[1] < 0) coord[1] = 0;
else if(coord[1] > 280000000) coord[1] = 280000000;
return Target((long) coord[0] / 34000000, (long) coord[1] / 35000000, _f);
}
Target Localizacao::multi(bool _f){
long prop[4];
for (int k = 0; k < 4; ++k){ // calculo da proporcao
prop[k] = ((rssisinv[k]*10000)/(rssisinv[0]+rssisinv[1]+rssisinv[2]+rssisinv[3]))/10;
}
for(int k=0; k < 4; ++k){ // calculo das distancias entre a nave e os beacons
dnave[k] = (long)((( (coefs[k][count[k]][0]/1000) * prop[0] +
(coefs[k][count[k]][1]/1000) * prop[1] +
(coefs[k][count[k]][2]/1000) * prop[2] +
(coefs[k][count[k]][3]/1000) * prop[3])/
(prop[0] + prop[1] + prop[2] + prop[3] - prop[k])) * rssis[k])/100;
if(dnave[rcvd[k]] > d[2]*10) dnave[rcvd[k]] = d[2]*10;
}
Multilateracao t;
long coord[4];
t.beacons(pos[rcvd[0]][0],
pos[rcvd[0]][1],
pos[rcvd[1]][0],
pos[rcvd[1]][1],
pos[rcvd[2]][0],
pos[rcvd[2]][1],
pos[rcvd[3]][0],
pos[rcvd[3]][1]);
t.preCalculo();
t.distancias(dnave[0], dnave[1], dnave[2], dnave[3]);
t.aproxime(coord);
this->reset();
if(coord[0] < 0) coord[0] = 0;
else if(coord[0] > 170000000) coord[0] = 170000000;
if(coord[1] < 0) coord[1] = 0;
else if(coord[1] > 280000000) coord[1] = 280000000;
return Target((long) coord[0] / 34000000, (long) coord[1] / 35000000, true);
}
tipos.h
#ifndef __tipos_h
#define __tipos_h
#include <system/config.h>
#include <utility/queue.h>
__BEGIN_SYS
struct PDU {
unsigned short i[4];
unsigned short ialvo;
Queue<PDU>::Element _link;
PDU():_link(this){}
PDU(short b0, short b1, short b2, short b3, short t):_link(this), ialvo(t){
i[0] = b0;
i[1] = b1;
i[2] = b2;
i[3] = b3;
}
};
struct Target {
short x, y;
bool fire;
Target(short _x, short _y): x(_x), y(_y), fire(false){}
Target(short _x, short _y, bool _a): x(_x), y(_y), fire(_a){}
Target(long _x = 0, long _y = 0, bool _a = false): fire(_a){
x = static_cast<short>(_x);
y = static_cast<short>(_y);
}
};
__END_SYS
#endif
mica_app.cc
#include <traits.h>
#include <display.h>
#include <machine.h>
#include <alarm.h>
#include <thread.h>
#include <semaphore.h>
#include <utility/queue.h>
#include <cannon.h>
#include <tipos.h>
#include <localizacao.h>
#include <trilateracao.h>
#include <multilateracao.h>
__USING_SYS
extern OStream cout;
class AutoTargetSystem{
Localizacao loc;
public:
AutoTargetSystem(){}
Target update(PDU p){
return loc.update(p);
}
};
typedef AutoTargetSystem TargetSystem;
typedef Queue<PDU, true> PDUQueue;
Semaphore* q_ready;
bool hit;
int receive_mica(PDUQueue* pdus){
UART uart(9600,8,0,1, 1);
while(!hit){
char c;
short pdu[5];
bool received = false;
while(!received){
cout << "esperando uart\n";
c = uart.get();
switch(c){
case 'p':
cout << "enviando o\n";
uart.put('o');
for (int i = 0; i < 5; ++i){
c = uart.get();
pdu[i] = c << 7;
c = uart.get();
pdu[i] |= c;
}
cout << "package ok?\n";
c = uart.get();
if (c =='e'){
cout << "package ok\n";
uart.put('o');
received = true;
} else {
cout << "package nok\n";
uart.put('n');
}
break;
default:
cout << "strange!!\n";
break;
}
}
if(c == 'e'){
cout << "enqueueing(?)\n";
PDU* p = new PDU(pdu[0], pdu[1], pdu[2], pdu[3], pdu[4]);
pdus->insert(&p->_link);
q_ready->v();
}
}
return 0;
};
PDU* get_next_pdu(PDUQueue* pdus){about:
return pdus->remove()->object();
}
int main() {
TargetSystem tsystem;
PDUQueue pdus;
short _pdus[13][4][5] = {
{ {0xFFFF, 1574, 1880, 1026, 706}, {1574, 0xFFFF, 1026, 1882, 1183}, {1881, 1025, 0xFFFF, 1575, 1195}, {1026, 1883, 1575, 0xFFFF, 725} },
{ {0xFFFF, 1573, 1879, 1025, 705}, {1573, 0xFFFF, 1025, 1881, 1182}, {1880, 1024, 0xFFFF, 1574, 1194}, {1025, 1882, 1574, 0xFFFF, 724} },
{ {0xFFFF, 1575, 1880, 1027, 707}, {1574, 0xFFFF, 1027, 1883, 1184}, {1882, 1025, 0xFFFF, 1574, 1194}, {1028, 1882, 1574, 0xFFFF, 724} },
{ {0xFFFF, 1573, 1881, 1025, 705}, {1573, 0xFFFF, 1025, 1883, 1182}, {1880, 1024, 0xFFFF, 1574, 1196}, {1027, 1882, 1574, 0xFFFF, 726} },
{ {0xFFFF, 1573, 1880, 1025, 700}, {1570, 0xFFFF, 1023, 1885, 1182}, {1880, 1020, 0xFFFF, 1575, 1196}, {1025, 1882, 1572, 0xFFFF, 728} },
{ {0xFFFF, 1570, 1879, 1025, 705}, {1573, 0xFFFF, 1025, 1881, 1182}, {1880, 1024, 0xFFFF, 1574, 1194}, {1025, 1882, 1573, 0xFFFF, 724} },
{ {0xFFFF, 1576, 1877, 1025, 705}, {1571, 0xFFFF, 1022, 1881, 1182}, {1884, 1024, 0xFFFF, 1574, 1194}, {1025, 1888, 1574, 0xFFFF, 724} },
{ {0xFFFF, 1571, 1879, 1025, 705}, {1573, 0xFFFF, 1025, 1881, 1182}, {1880, 1024, 0xFFFF, 1574, 1194}, {1025, 1884, 1574, 0xFFFF, 724} },
{ {0xFFFF, 1577, 1878, 1025, 703}, {1575, 0xFFFF, 1022, 1883, 1182}, {1884, 1025, 0xFFFF, 1574, 1194}, {1025, 1885, 1574, 0xFFFF, 724} },
{ {0xFFFF, 1570, 1879, 1025, 705}, {1573, 0xFFFF, 1025, 1881, 1182}, {1880, 1024, 0xFFFF, 1574, 1194}, {1025, 1883, 1574, 0xFFFF, 724} },
{ {0xFFFF, 1571, 1879, 1025, 703}, {1573, 0xFFFF, 1025, 1883, 1182}, {1882, 1025, 0xFFFF, 1574, 1194}, {1025, 1882, 1574, 0xFFFF, 724} },
{ {0xFFFF, 1572, 1880, 1025, 705}, {1577, 0xFFFF, 1022, 1881, 1182}, {1880, 1024, 0xFFFF, 1574, 1194}, {1025, 1882, 1574, 0xFFFF, 724} },
{ {0xFFFF, 1580, 1879, 1020, 705}, {1573, 0xFFFF, 1025, 1881, 1182}, {1881, 1025, 0xFFFF, 1574, 1194}, {1025, 1882, 1574, 0xFFFF, 724} } };
for(int i = 0;i < 13; i++){
for (int j = 0; j < 4; j++){
PDU* p = new PDU(_pdus[i][j][0], _pdus[i][j][1], _pdus[i][j][2], _pdus[i][j][3], _pdus[i][j][4]);
pdus.insert(&p->_link);
}
}
hit = false;
int attempt = 0;
cout << "Cannon Controler\n";
cout << "green to begin\n";
while(!hit) {
PDU* p = get_next_pdu (&pdus); // Block !
Target t = tsystem.update(*p);
delete p;
cout << "\nnon-fire: " << t.x << " , " << t.y << "\n";
if ( t.fire ) {
cout << "fire: " << t.x << " , " << t.y << "\n";
}
}
long coord[4];
Trilateracao t;
t.beacons((long)0,(long)205,(long)315,(long)0,(long)315,(long)205);
t.preCalculo();
t.distancias((long)310, (long)1587, (long)932);
t.aproxime(coord);
cout << "\nCOORD 1 = " << coord[0] << ", " << coord[1];
t.beacons((long)0,(long)0,(long)0,(long)205,(long)315,(long)205);
t.preCalculo();
t.distancias((long)1414, (long)1450, (long)2393);
t.aproxime(coord);
cout << "\nCOORD 1 = " << coord[0] << ", " << coord[1];
t.beacons((long)0,(long)205,(long)315,(long)205,(long)315,(long)0);
t.preCalculo();
t.distancias((long)1450, (long)2393, (long)2371);
t.aproxime(coord);
cout << "\nCOORD 2 = " << coord[0] << ", " << coord[1];
t.beacons((long)315,(long)205,(long)315,(long)0,(long)0,(long)0);
t.preCalculo();
t.distancias((long)2393, (long)2371, (long)1414);
t.aproxime(coord);
cout << "\nCOORD 3 = " << coord[0] << ", " << coord[1];
t.beacons((long)315,(long)0,(long)0,(long)0,(long)0,(long)205);
t.preCalculo();
t.distancias((long)2371, (long)1414, (long)1450);
t.aproxime(coord);
cout << "\nCOORD 4 = " << coord[0] << ", " << coord[1];
Multilateracao m;
m.beacons((long)0,(long)0,(long)0,(long)205,(long)315,(long)205,(long)315,(long)0);
m.preCalculo();
m.distancias((long)1414, (long)1450, (long)2393, (long)2371);
m.aproxime(coord);
cout << "\nCOORD M = " << coord[0] << ", " << coord[1];
t.beacons((long)0,(long)0,(long)315,(long)0,(long)315,(long)205);
t.preCalculo();
t.distancias((long)1412, (long)2364, (long)2388);
t.aproxime(coord);
cout << "\nCOORD 4 = " << coord[0] << ", " << coord[1];
return 0;
}
Programa teste que gera Sinais para Teste
public class Teste {
static double hor = 30.6;
static double ver = 21;
static double dia = 37.1;
static double mlt = 4;
/**
* @param args
*/
public static void main(String[] args) {
String pdu[][]=new String[4][5];
double r[]=point(Double.valueOf(args[0]),Double.valueOf(args[1]));
for (int i=0;i<4;i++){
for (int j=0;j<4;j++)
if (i==j) pdu[i][j]="FFFF";
else if ((i+j)==3) pdu[i][j]=toHex(ver*mlt);
else if (Math.abs(i-j)==1) pdu[i][j]=toHex(hor*mlt);
else if (Math.abs(i-j)==2) pdu[i][j]=toHex(dia*mlt);
pdu[i][4]=toHex(r[i]*mlt);
}
for (int i=0;i<4;i++){
System.out.print(70);
for (int j=0;j<5;j++) System.out.print(pdu[i][j]);
System.out.println(65);
}
}
public static double[] point(double x, double y) {
double retorno[] = new double[4];
retorno[0] = Math.sqrt(x*x + y*y);
retorno[1] = Math.sqrt((hor-x)*(hor-x) + y*y);
retorno[2] = Math.sqrt((hor-x)*(hor-x) + (ver-y)*(ver-y));
retorno[3] = Math.sqrt(x*x + (ver-y)*(ver-y));
return retorno;
}
public static String toHex(double valor){
String hex = Integer.toHexString((int)(valor*mlt)).toUpperCase();
int l = 4 - hex.length();
String distancia = "";
for (int j=0;j<l;j++)
distancia = distancia + "0";
return distancia + hex;
}
}
