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.
Sensors/Comp/HMC5883.cpp@11:9bf69bc6df45, 2012-10-18 (annotated)
- Committer:
- maetugr
- Date:
- Thu Oct 18 20:04:16 2012 +0000
- Revision:
- 11:9bf69bc6df45
- Child:
- 12:67a06c9b69d5
Kompass immernoch nicht gut, vor Kalibrierungsberechnung
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 11:9bf69bc6df45 | 1 | #include "mbed.h" |
maetugr | 11:9bf69bc6df45 | 2 | #include "HMC5883.h" |
maetugr | 11:9bf69bc6df45 | 3 | |
maetugr | 11:9bf69bc6df45 | 4 | HMC5883::HMC5883(PinName sda, PinName scl) : i2c(sda, scl) |
maetugr | 11:9bf69bc6df45 | 5 | { |
maetugr | 11:9bf69bc6df45 | 6 | // initialize HMC5883 for scaling |
maetugr | 11:9bf69bc6df45 | 7 | writeReg(HMC5883_CONF_REG_A, 0x19); // 8 samples, 75Hz output, test mode for scaling! |
maetugr | 11:9bf69bc6df45 | 8 | writeReg(HMC5883_CONF_REG_B, 0x20); // Gain for +- 1.3 gauss (earth compass ~0.6 gauss) |
maetugr | 11:9bf69bc6df45 | 9 | writeReg(HMC5883_MODE_REG, 0x00); // continuous measurement-mode |
maetugr | 11:9bf69bc6df45 | 10 | |
maetugr | 11:9bf69bc6df45 | 11 | // Scaling |
maetugr | 11:9bf69bc6df45 | 12 | for(int j = 0; j < 3; j++) // set all scales to 1 first so the measurement for scaling is not already scaled |
maetugr | 11:9bf69bc6df45 | 13 | scale[j] = 1; |
maetugr | 11:9bf69bc6df45 | 14 | |
maetugr | 11:9bf69bc6df45 | 15 | int data50[3] = {0,0,0}; // to save the 50 measurements |
maetugr | 11:9bf69bc6df45 | 16 | for(int i = 0; i < 50; i++) // measure 50 times the testmode value to get an average |
maetugr | 11:9bf69bc6df45 | 17 | { |
maetugr | 11:9bf69bc6df45 | 18 | read(); |
maetugr | 11:9bf69bc6df45 | 19 | for(int j = 0; j < 3; j++) |
maetugr | 11:9bf69bc6df45 | 20 | data50[j] += data[j]; |
maetugr | 11:9bf69bc6df45 | 21 | } |
maetugr | 11:9bf69bc6df45 | 22 | 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 |
maetugr | 11:9bf69bc6df45 | 23 | scale[1] = (1.16 * 1090)/(data50[1]/50.0); |
maetugr | 11:9bf69bc6df45 | 24 | scale[2] = (1.08 * 1090)/(data50[2]/50.0); |
maetugr | 11:9bf69bc6df45 | 25 | |
maetugr | 11:9bf69bc6df45 | 26 | // set normal mode |
maetugr | 11:9bf69bc6df45 | 27 | writeReg(HMC5883_CONF_REG_A, 0x78); // 8 samples, 75Hz output, normal mode |
maetugr | 11:9bf69bc6df45 | 28 | } |
maetugr | 11:9bf69bc6df45 | 29 | |
maetugr | 11:9bf69bc6df45 | 30 | void HMC5883::read() |
maetugr | 11:9bf69bc6df45 | 31 | { |
maetugr | 11:9bf69bc6df45 | 32 | char buffer[6]; |
maetugr | 11:9bf69bc6df45 | 33 | int dataint[3]; |
maetugr | 11:9bf69bc6df45 | 34 | |
maetugr | 11:9bf69bc6df45 | 35 | readMultiReg(HMC5883_DATA_OUT_X_MSB, buffer, 6); |
maetugr | 11:9bf69bc6df45 | 36 | |
maetugr | 11:9bf69bc6df45 | 37 | // join MSB and LSB of X, Z and Y (yes, order is so stupid, see datasheet) |
maetugr | 11:9bf69bc6df45 | 38 | dataint[0] = (short) (buffer[0] << 8 | buffer[1]); |
maetugr | 11:9bf69bc6df45 | 39 | dataint[1] = (short) (buffer[4] << 8 | buffer[5]); |
maetugr | 11:9bf69bc6df45 | 40 | dataint[2] = (short) (buffer[2] << 8 | buffer[3]); |
maetugr | 11:9bf69bc6df45 | 41 | |
maetugr | 11:9bf69bc6df45 | 42 | for(int j = 0; j < 3; j++) { |
maetugr | 11:9bf69bc6df45 | 43 | Min[j]= Min[j] < dataint[j] ? Min[j] : dataint[j]; |
maetugr | 11:9bf69bc6df45 | 44 | Max[j]= Max[j] > dataint[j] ? Max[j] : dataint[j]; |
maetugr | 11:9bf69bc6df45 | 45 | data[j] = dataint[j]/1.090; //* scale[j]; |
maetugr | 11:9bf69bc6df45 | 46 | } |
maetugr | 11:9bf69bc6df45 | 47 | |
maetugr | 11:9bf69bc6df45 | 48 | heading = 57.295779513082320876798154814105*atan2(data[1], data[0]); |
maetugr | 11:9bf69bc6df45 | 49 | } |
maetugr | 11:9bf69bc6df45 | 50 | |
maetugr | 11:9bf69bc6df45 | 51 | void HMC5883::writeReg(char address, char data){ |
maetugr | 11:9bf69bc6df45 | 52 | char tx[2]; |
maetugr | 11:9bf69bc6df45 | 53 | tx[0] = address; |
maetugr | 11:9bf69bc6df45 | 54 | tx[1] = data; |
maetugr | 11:9bf69bc6df45 | 55 | i2c.write(I2CADR_W(HMC5883_ADDRESS), tx, 2); |
maetugr | 11:9bf69bc6df45 | 56 | } |
maetugr | 11:9bf69bc6df45 | 57 | |
maetugr | 11:9bf69bc6df45 | 58 | void HMC5883::readMultiReg(char address, char* output, int size) { |
maetugr | 11:9bf69bc6df45 | 59 | i2c.write(I2CADR_W(HMC5883_ADDRESS), &address, 1); //tell it where to read from |
maetugr | 11:9bf69bc6df45 | 60 | i2c.read(I2CADR_R(HMC5883_ADDRESS) , output, size); //tell it where to store the data read |
maetugr | 11:9bf69bc6df45 | 61 | } |
maetugr | 11:9bf69bc6df45 | 62 | /* |
maetugr | 11:9bf69bc6df45 | 63 | void HMC5883::Calibrate(int s) |
maetugr | 11:9bf69bc6df45 | 64 | { |
maetugr | 11:9bf69bc6df45 | 65 | |
maetugr | 11:9bf69bc6df45 | 66 | //Ende der Kalibrierung in ms Millisekunden berechnen |
maetugr | 11:9bf69bc6df45 | 67 | int CalibEnd= GlobalTime.read_ms() + s*1000; |
maetugr | 11:9bf69bc6df45 | 68 | |
maetugr | 11:9bf69bc6df45 | 69 | while(GlobalTime.read_ms() < CalibEnd) |
maetugr | 11:9bf69bc6df45 | 70 | { |
maetugr | 11:9bf69bc6df45 | 71 | //Update erledigt alles |
maetugr | 11:9bf69bc6df45 | 72 | Update(); |
maetugr | 11:9bf69bc6df45 | 73 | } |
maetugr | 11:9bf69bc6df45 | 74 | |
maetugr | 11:9bf69bc6df45 | 75 | AutoCalibration= AutoCalibrationBak; |
maetugr | 11:9bf69bc6df45 | 76 | }*/ |
maetugr | 11:9bf69bc6df45 | 77 | |
maetugr | 11:9bf69bc6df45 | 78 | // Winkel berechnen |
maetugr | 11:9bf69bc6df45 | 79 | //--------------------------------------------------------------------------------------------------------------------------------------- |
maetugr | 11:9bf69bc6df45 | 80 | float HMC5883::getAngle(float x, float y) |
maetugr | 11:9bf69bc6df45 | 81 | { |
maetugr | 11:9bf69bc6df45 | 82 | #define Rad2Deg 57.295779513082320876798154814105 |
maetugr | 11:9bf69bc6df45 | 83 | #define PI 3.1415926535897932384626433832795 |
maetugr | 11:9bf69bc6df45 | 84 | |
maetugr | 11:9bf69bc6df45 | 85 | float Heading; |
maetugr | 11:9bf69bc6df45 | 86 | float DecAngle; |
maetugr | 11:9bf69bc6df45 | 87 | |
maetugr | 11:9bf69bc6df45 | 88 | DecAngle = 1.367 / Rad2Deg; //Missweisung = Winkel zwischen geographischer und magnetischer Nordrichtung |
maetugr | 11:9bf69bc6df45 | 89 | //Bern ca. 1.367 Grad Ost |
maetugr | 11:9bf69bc6df45 | 90 | //http://www.swisstopo.admin.ch/internet/swisstopo/de/home/apps/calc/declination.html |
maetugr | 11:9bf69bc6df45 | 91 | Heading = atan2((float)y,(float)x); |
maetugr | 11:9bf69bc6df45 | 92 | |
maetugr | 11:9bf69bc6df45 | 93 | Heading += DecAngle; //bei Ost-Deklination += DecAngle, bei West-Deklination -= DecAngle |
maetugr | 11:9bf69bc6df45 | 94 | |
maetugr | 11:9bf69bc6df45 | 95 | if(Heading < 0) |
maetugr | 11:9bf69bc6df45 | 96 | Heading += 2*PI; //korrigieren bei negativem Vorzeichen |
maetugr | 11:9bf69bc6df45 | 97 | |
maetugr | 11:9bf69bc6df45 | 98 | if(Heading > 2*PI) |
maetugr | 11:9bf69bc6df45 | 99 | Heading -= 2*PI; //auf 2Pi begrenzen |
maetugr | 11:9bf69bc6df45 | 100 | |
maetugr | 11:9bf69bc6df45 | 101 | return (Heading * 180/PI); //Radianten in Grad konvertieren |
maetugr | 11:9bf69bc6df45 | 102 | } |
maetugr | 11:9bf69bc6df45 | 103 | |
maetugr | 11:9bf69bc6df45 | 104 |