Matthias Grob / Mbed 2 deprecated FlyBed1

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.

Dependencies:   mbed MODI2C

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?

UserRevisionLine numberNew 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