NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.
Diff: Sensors/Comp/HMC5883.cpp
- Revision:
- 12:67a06c9b69d5
- Parent:
- 11:9bf69bc6df45
- Child:
- 13:4737ee9ebfee
diff -r 9bf69bc6df45 -r 67a06c9b69d5 Sensors/Comp/HMC5883.cpp --- a/Sensors/Comp/HMC5883.cpp Thu Oct 18 20:04:16 2012 +0000 +++ b/Sensors/Comp/HMC5883.cpp Sat Oct 20 17:28:28 2012 +0000 @@ -1,14 +1,23 @@ #include "mbed.h" #include "HMC5883.h" -HMC5883::HMC5883(PinName sda, PinName scl) : i2c(sda, scl) -{ +HMC5883::HMC5883(PinName sda, PinName scl) : local("local"), i2c(sda, scl) +{ + // load calibration values + FILE *fp = fopen("/local/compass.txt", "r"); + for(int i = 0; i < 3; i++) + fscanf(fp, "%f", &scale[i]); + for(int i = 0; i < 3; i++) + fscanf(fp, "%f", &offset[i]); + fclose(fp); + // initialize HMC5883 for scaling writeReg(HMC5883_CONF_REG_A, 0x19); // 8 samples, 75Hz output, test mode for scaling! writeReg(HMC5883_CONF_REG_B, 0x20); // Gain for +- 1.3 gauss (earth compass ~0.6 gauss) writeReg(HMC5883_MODE_REG, 0x00); // continuous measurement-mode - // Scaling + /* + // Scaling with testmode (not important, just from data sheet) for(int j = 0; j < 3; j++) // set all scales to 1 first so the measurement for scaling is not already scaled scale[j] = 1; @@ -22,6 +31,7 @@ scale[0] = (1.16 * 1090)/(data50[0]/50.0); // value that it should be with selftest of 1.1 Gauss * 1090 LSB/Gauss / the value it is scale[1] = (1.16 * 1090)/(data50[1]/50.0); scale[2] = (1.08 * 1090)/(data50[2]/50.0); + */ // set normal mode writeReg(HMC5883_CONF_REG_A, 0x78); // 8 samples, 75Hz output, normal mode @@ -29,23 +39,49 @@ void HMC5883::read() { + readraw(); + for(int i = 0; i < 3; i++) + data[i] = scale[i] * (float)(raw[i]) + offset[i]; +} + +void HMC5883::calibrate(int s) +{ + Timer calibrate_timer; + calibrate_timer.start(); + + while(calibrate_timer.read() < s) + { + readraw(); + for(int i = 0; i < 3; i++) { + Min[i]= Min[i] < raw[i] ? Min[i] : raw[i]; + Max[i]= Max[i] > raw[i] ? Max[i] : raw[i]; + + //Scale und Offset aus gesammelten Min Max Werten berechnen + //Die neue Untere und obere Grenze bilden -1 und +1 + scale[i]= 2000 / (float)(Max[i]-Min[i]); + offset[i]= 1000 - (float)(Max[i]) * scale[i]; + } + } + + // save values + FILE *fp = fopen("/local/compass.txt", "w"); + for(int i = 0; i < 3; i++) + fprintf(fp, "%f\r\n", scale[i]); + for(int i = 0; i < 3; i++) + fprintf(fp, "%f\r\n", offset[i]); + fclose(fp); +} + +void HMC5883::readraw() +{ char buffer[6]; - int dataint[3]; - readMultiReg(HMC5883_DATA_OUT_X_MSB, buffer, 6); + readMultiReg(HMC5883_DATA_OUT_X_MSB, buffer, 6); // read the raw data from I2C // join MSB and LSB of X, Z and Y (yes, order is so stupid, see datasheet) - dataint[0] = (short) (buffer[0] << 8 | buffer[1]); - dataint[1] = (short) (buffer[4] << 8 | buffer[5]); - dataint[2] = (short) (buffer[2] << 8 | buffer[3]); - - for(int j = 0; j < 3; j++) { - Min[j]= Min[j] < dataint[j] ? Min[j] : dataint[j]; - Max[j]= Max[j] > dataint[j] ? Max[j] : dataint[j]; - data[j] = dataint[j]/1.090; //* scale[j]; - } - - heading = 57.295779513082320876798154814105*atan2(data[1], data[0]); + raw[0] = (short) (buffer[0] << 8 | buffer[1]); + raw[1] = (short) (buffer[4] << 8 | buffer[5]); + raw[2] = (short) (buffer[2] << 8 | buffer[3]); } void HMC5883::writeReg(char address, char data){ @@ -59,46 +95,27 @@ i2c.write(I2CADR_W(HMC5883_ADDRESS), &address, 1); //tell it where to read from i2c.read(I2CADR_R(HMC5883_ADDRESS) , output, size); //tell it where to store the data read } -/* -void HMC5883::Calibrate(int s) + +float HMC5883::get_angle() { - - //Ende der Kalibrierung in ms Millisekunden berechnen - int CalibEnd= GlobalTime.read_ms() + s*1000; - - while(GlobalTime.read_ms() < CalibEnd) - { - //Update erledigt alles - Update(); - } - - AutoCalibration= AutoCalibrationBak; -}*/ - -// Winkel berechnen -//--------------------------------------------------------------------------------------------------------------------------------------- -float HMC5883::getAngle(float x, float y) - { #define Rad2Deg 57.295779513082320876798154814105 - #define PI 3.1415926535897932384626433832795 float Heading; - float DecAngle; + + Heading = Rad2Deg * atan2(data[0],data[1]); - DecAngle = 1.367 / Rad2Deg; //Missweisung = Winkel zwischen geographischer und magnetischer Nordrichtung + Heading += 1.367; //bei Ost-Deklination += DecAngle, bei West-Deklination -= DecAngle + //Missweisung = Winkel zwischen geographischer und magnetischer Nordrichtung //Bern ca. 1.367 Grad Ost //http://www.swisstopo.admin.ch/internet/swisstopo/de/home/apps/calc/declination.html - Heading = atan2((float)y,(float)x); - - Heading += DecAngle; //bei Ost-Deklination += DecAngle, bei West-Deklination -= DecAngle if(Heading < 0) - Heading += 2*PI; //korrigieren bei negativem Vorzeichen + Heading += 360; // minimum 0 degree - if(Heading > 2*PI) - Heading -= 2*PI; //auf 2Pi begrenzen + if(Heading > 360) + Heading -= 360; // maximum 360 degree - return (Heading * 180/PI); //Radianten in Grad konvertieren - } + return Heading; +}