void initMagneto(void){
int i;
printf("Magnetometer Initializing..........\n");
if(first_boot){
pinMode(MAG_LED, OUTPUT);
if( (mag.fd = wiringPiI2CSetup(HMC5883L_I2C)) < 0 )
printf ("I2C connection failed: %s\n", strerror(errno));
if( (wiringPiI2CWriteReg8(mag.fd, HMC5883L_REG_A, HIGH_FREQ8)) < 0 )
printf("Write to HMC5883L_REG_A failed: %s/n", strerror(errno));
if( (wiringPiI2CWriteReg8(mag.fd, HMC5883L_REG_B, SENSOR_GAIN)) < 0 )
printf("Write to HMC5883L_REG_B failed: %s/n", strerror(errno));
if( (wiringPiI2CWriteReg8(mag.fd, HMC5883L_MODE_REG, CONTINUOUS)) < 0)
printf ("Write to HMC5883L_MODE_REG failed: %s/n", strerror(errno));
first_boot=0;
}
mag_starting_up = 1;
mag.reading = 0;
mag.average = 0;
mag.deviation = 0;
mag.magnitude = 0;
mag.oldest_sample = 0;
for(i = 0 ; i < MAX_SAMPLES ; i++){
readMagneto();
}
updateMagneto();
printf("Magnetometer Initialized...........\n");
mag_starting_up = 0;
}
/*
* send mag. values to uart 3
*/
void sendMagnetoData(void) {
readMagneto();
short b;
unsigned char mag[20];
mag[19] = '\0';
b = usprintf((char*) mag, "%i,%i,%i\n", Mx, My, Mz);
#ifndef wlan
nrf24l01p_send(mag);
#else
for (int i = 0; i < b; i++) {
ROM_UARTCharPut(UART3_BASE, mag[i]);
}
#endif
}
int fieldDisruptionDetected(void){
readMagneto();
delay(1);
if( ( mag.low < mag.reading && mag.reading < mag.high ) ){
digitalWrite(MAG_LED, 0);
return 0;
}
// else if(( (mag.low - DEVIATION_SENSITIVITY*mag.deviation ) > mag.reading || mag.reading > (mag.high + DEVIATION_SENSITIVITY*mag.deviation ) )){
// initMagneto();
// }
else{
digitalWrite(MAG_LED, 1);
return 1;
}
}
/*
* read and calculate heading with basic tilt compensation
* @returns float containing heading value -> -180 to +180
*
*/
float getHeading(void) {
readMagneto();
readAccel();
double pitch, roll;
if ((Az > 9500 && Az < 10050) || (Az < -9500 && Az > -10050)) {
pitch = roll = 0;
} else {
pitch = getPitch();
roll = getRoll();
}
int Xh = Mx * cos(pitch) + Mz * sin(pitch); // Virtual X and Virtual Y (corrected)
int Yh = Mx * sin(pitch) * sin(roll) + My * cos(roll)
- Mz * sin(roll) * cos(pitch);
float heading = (atan2f(Yh, Xh) * 180 / M_PI);
if (heading < 0) {
heading = 360 + heading;
}
return heading;
}
