Bussola::Bussola(){
pointer=0;
pointerSize = TAMANHO_FILA;
filaCheia=0;
Serial.println("Starting the I2C interface.");
Wire.begin(); // Start the I2C interface.
Serial.println("Constructing new HMC5883L");
compass = HMC5883L(); // Construct a new HMC5883 compass.
int error=0;
Serial.println("Setting scale to +/- 1.3 Ga");
error = compass.SetScale(1.3); // Set the scale of the compass.
if(error != 0) // If there is an error, print it out.
Serial.println(compass.GetErrorText(error));
Serial.println("Setting measurement mode to continous.");
error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
if(error != 0) // If there is an error, print it out.
Serial.println(compass.GetErrorText(error));
delay(10);
//Preenche a fila
for(int i=0; i<=TAMANHO_FILA; i++){
calcHeading();
delay(1);
}
}
// Returns the angular difference in the horizontal plane between
// a default vector an north in degrees.
float getHeading()
{
// Default vector of x axis
float from[3] = { 1, 0, 0 };
// Get latest compass readings for calculations
accRead(&acc);
magRead(&mag);
return calcHeading(from, &acc, &mag);
}
/**Retorna o valor mediano apos a ordenacao**/
float Bussola::getHeading(){
calcHeading();
if(filaCheia>=TAMANHO_FILA){
int x, y;
float aux;
for( x = 0; x < pointerSize; x++ ){
for( y = x + 1; y < pointerSize; y++ ){ // sempre 1 elemento à frente
// se o (x > (x+1)) então o x passa pra frente (ordem crescente)
if ( filacirc[x] > filacirc[y] ){
aux = filacirc[x];
filacirc[x] = filacirc[y];
filacirc[y] = aux;
}
}
}
return filacirc[(int)(TAMANHO_FILA/2)];
}else{
return 0;
}
}