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:
- 11:9bf69bc6df45
- Child:
- 12:67a06c9b69d5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/Comp/HMC5883.cpp Thu Oct 18 20:04:16 2012 +0000 @@ -0,0 +1,104 @@ +#include "mbed.h" +#include "HMC5883.h" + +HMC5883::HMC5883(PinName sda, PinName scl) : i2c(sda, scl) +{ + // 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 + 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; + + int data50[3] = {0,0,0}; // to save the 50 measurements + for(int i = 0; i < 50; i++) // measure 50 times the testmode value to get an average + { + read(); + for(int j = 0; j < 3; j++) + data50[j] += data[j]; + } + 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 +} + +void HMC5883::read() +{ + char buffer[6]; + int dataint[3]; + + readMultiReg(HMC5883_DATA_OUT_X_MSB, buffer, 6); + + // 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]); +} + +void HMC5883::writeReg(char address, char data){ + char tx[2]; + tx[0] = address; + tx[1] = data; + i2c.write(I2CADR_W(HMC5883_ADDRESS), tx, 2); +} + +void HMC5883::readMultiReg(char address, char* output, int size) { + 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) +{ + + //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; + + DecAngle = 1.367 / Rad2Deg; //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 + + if(Heading > 2*PI) + Heading -= 2*PI; //auf 2Pi begrenzen + + return (Heading * 180/PI); //Radianten in Grad konvertieren + } + +