Test BMA180 and ITG3200 sensors.

Dependencies:   mbed BMA180

Fork of Sensor_test by Bo Carøe

main.cpp

Committer:
caroe
Date:
2012-05-30
Revision:
0:f4e9301d548b
Child:
1:855d13c5051c

File content as of revision 0:f4e9301d548b:

#include "mbed.h"
#include "HMC5883.h"
#include "BMA180.h"
#include "ITG3200.h"
Serial pc(USBTX, USBRX);

DigitalOut l1(LED1);
I2C I2CBus(p28, p27);
Timer GlobalTime;

#define PI             3.1415926535897932384626433832795
HMC5883 Mag(I2CBus, GlobalTime);
BMA180 Acc(I2CBus, GlobalTime);
ITG3200 Gyro(I2CBus, GlobalTime);

int HMC5883_getAngle(float x, float y)
{
float heading = atan2((float)y,(float)x); 
// Your mrad result / 1000.00 (to turn it into radians).
  float declinationAngle = 21.18  / 1000.0;
  // If you have an EAST declination, use += declinationAngle, if you have a WEST declination, use -= declinationAngle
  heading += declinationAngle;  
if(heading < 0)  heading += 2*PI;// Correct for when signs are reversed.
if(heading > 2*PI)   heading -= 2*PI; // Check for wrap due to addition of declination.
return(heading * 180/PI); // Convert radians to degrees for readability.
}


int main() 
{
    pc.baud(115200);
  
    I2CBus.frequency(400000);
    GlobalTime.start();
//***
    Acc.Init();
    wait_ms(500);
      //0.5 Sekunden kalibrieren
    //1 g sind ca -2870 auf der 3. Achse
    short Raw1g[3]= {0, 0, 0}; // -2870
    Acc.Calibrate(500, Raw1g);
//***   
    Gyro.Init();
    wait_ms(500);
    
    //0.5 Sekunden kalibrieren
    Gyro.Calibrate(500);
//***  
    
    Mag.Init();
    #if 1
        Mag.AutoCalibration= 1;                     //In Echtzeit kalibrieren
    #else
        short MagRawMin[3]= {-400, -400, -400};     //Gespeicherte Werte
        short MagRawMax[3]= {400, 400, 400};
        Mag.Calibrate(MagRawMin, MagRawMax);
    #endif
    
    while(1)
    {
        Mag.Update();
        
        l1= Mag.MeasurementError;
        Acc.Update();
        Gyro.Update();
        
        pc.printf("Gyro:%.3f %.3f %.3f   ",
            (Gyro.Rate[0]),
            (Gyro.Rate[1]),
            (Gyro.Rate[2]));     
        
        
        pc.printf("BMA180:%.3f %.3f %.3f  ",
            (Acc.Acc[0]),
            (Acc.Acc[1]),
            (Acc.Acc[2]));
        
     

       // pc.printf("Min: %i %i %i Max: %i %i %i Scale: %.3f %.3f %.3f Offset: %.3i %.3i %.3i Mag: %.3f %.3f %.3f\r\n",
        /*    Mag.RawMin[0], Mag.RawMin[1], Mag.RawMin[2],
            Mag.RawMax[0], Mag.RawMax[1], Mag.RawMax[2],
            Mag.Scale[0] , Mag.Scale[1] , Mag.Scale[2],
            Mag.Offset[0], Mag.Offset[1], Mag.Offset[2],
            Mag.Mag[0]   , Mag.Mag[1]   , Mag.Mag[2]);*/
         printf("grader = %i \n",HMC5883_getAngle(Mag.Mag[0],Mag.Mag[1]));
        
        wait_ms(20);
    }
}