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@14:cf260677ecde, 2012-10-27 (annotated)

Committer:
maetugr
Date:
Sat Oct 27 10:53:43 2012 +0000
Revision:
14:cf260677ecde
Parent:
13:4737ee9ebfee
Child:
15:753c5d6a63b3
I2C Workaround gefunden!!! erster Test geklappt, vor Umschreibung

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 14:cf260677ecde 4 HMC5883::HMC5883(PinName sda, PinName scl) : i2c(sda, scl), local("local")
maetugr 12:67a06c9b69d5 5 {
maetugr 14:cf260677ecde 6 i2c.frequency(400000); // TODO: zu testen!!
maetugr 13:4737ee9ebfee 7
maetugr 12:67a06c9b69d5 8 // load calibration values
maetugr 12:67a06c9b69d5 9 FILE *fp = fopen("/local/compass.txt", "r");
maetugr 12:67a06c9b69d5 10 for(int i = 0; i < 3; i++)
maetugr 12:67a06c9b69d5 11 fscanf(fp, "%f", &scale[i]);
maetugr 12:67a06c9b69d5 12 for(int i = 0; i < 3; i++)
maetugr 12:67a06c9b69d5 13 fscanf(fp, "%f", &offset[i]);
maetugr 12:67a06c9b69d5 14 fclose(fp);
maetugr 12:67a06c9b69d5 15
maetugr 11:9bf69bc6df45 16 // initialize HMC5883 for scaling
maetugr 11:9bf69bc6df45 17 writeReg(HMC5883_CONF_REG_A, 0x19); // 8 samples, 75Hz output, test mode for scaling!
maetugr 11:9bf69bc6df45 18 writeReg(HMC5883_CONF_REG_B, 0x20); // Gain for +- 1.3 gauss (earth compass ~0.6 gauss)
maetugr 11:9bf69bc6df45 19 writeReg(HMC5883_MODE_REG, 0x00); // continuous measurement-mode
maetugr 11:9bf69bc6df45 20
maetugr 14:cf260677ecde 21 /* (not important, just from data sheet)
maetugr 14:cf260677ecde 22 // Scaling with testmode
maetugr 11:9bf69bc6df45 23 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 24 scale[j] = 1;
maetugr 11:9bf69bc6df45 25
maetugr 11:9bf69bc6df45 26 int data50[3] = {0,0,0}; // to save the 50 measurements
maetugr 11:9bf69bc6df45 27 for(int i = 0; i < 50; i++) // measure 50 times the testmode value to get an average
maetugr 11:9bf69bc6df45 28 {
maetugr 11:9bf69bc6df45 29 read();
maetugr 11:9bf69bc6df45 30 for(int j = 0; j < 3; j++)
maetugr 11:9bf69bc6df45 31 data50[j] += data[j];
maetugr 11:9bf69bc6df45 32 }
maetugr 11:9bf69bc6df45 33 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 34 scale[1] = (1.16 * 1090)/(data50[1]/50.0);
maetugr 11:9bf69bc6df45 35 scale[2] = (1.08 * 1090)/(data50[2]/50.0);
maetugr 12:67a06c9b69d5 36 */
maetugr 11:9bf69bc6df45 37
maetugr 11:9bf69bc6df45 38 // set normal mode
maetugr 11:9bf69bc6df45 39 writeReg(HMC5883_CONF_REG_A, 0x78); // 8 samples, 75Hz output, normal mode
maetugr 11:9bf69bc6df45 40 }
maetugr 11:9bf69bc6df45 41
maetugr 11:9bf69bc6df45 42 void HMC5883::read()
maetugr 11:9bf69bc6df45 43 {
maetugr 12:67a06c9b69d5 44 readraw();
maetugr 12:67a06c9b69d5 45 for(int i = 0; i < 3; i++)
maetugr 12:67a06c9b69d5 46 data[i] = scale[i] * (float)(raw[i]) + offset[i];
maetugr 12:67a06c9b69d5 47 }
maetugr 12:67a06c9b69d5 48
maetugr 12:67a06c9b69d5 49 void HMC5883::calibrate(int s)
maetugr 12:67a06c9b69d5 50 {
maetugr 12:67a06c9b69d5 51 Timer calibrate_timer;
maetugr 12:67a06c9b69d5 52 calibrate_timer.start();
maetugr 12:67a06c9b69d5 53
maetugr 12:67a06c9b69d5 54 while(calibrate_timer.read() < s)
maetugr 12:67a06c9b69d5 55 {
maetugr 12:67a06c9b69d5 56 readraw();
maetugr 12:67a06c9b69d5 57 for(int i = 0; i < 3; i++) {
maetugr 12:67a06c9b69d5 58 Min[i]= Min[i] < raw[i] ? Min[i] : raw[i];
maetugr 12:67a06c9b69d5 59 Max[i]= Max[i] > raw[i] ? Max[i] : raw[i];
maetugr 12:67a06c9b69d5 60
maetugr 12:67a06c9b69d5 61 //Scale und Offset aus gesammelten Min Max Werten berechnen
maetugr 12:67a06c9b69d5 62 //Die neue Untere und obere Grenze bilden -1 und +1
maetugr 12:67a06c9b69d5 63 scale[i]= 2000 / (float)(Max[i]-Min[i]);
maetugr 12:67a06c9b69d5 64 offset[i]= 1000 - (float)(Max[i]) * scale[i];
maetugr 12:67a06c9b69d5 65 }
maetugr 12:67a06c9b69d5 66 }
maetugr 12:67a06c9b69d5 67
maetugr 12:67a06c9b69d5 68 // save values
maetugr 12:67a06c9b69d5 69 FILE *fp = fopen("/local/compass.txt", "w");
maetugr 12:67a06c9b69d5 70 for(int i = 0; i < 3; i++)
maetugr 12:67a06c9b69d5 71 fprintf(fp, "%f\r\n", scale[i]);
maetugr 12:67a06c9b69d5 72 for(int i = 0; i < 3; i++)
maetugr 12:67a06c9b69d5 73 fprintf(fp, "%f\r\n", offset[i]);
maetugr 12:67a06c9b69d5 74 fclose(fp);
maetugr 12:67a06c9b69d5 75 }
maetugr 12:67a06c9b69d5 76
maetugr 12:67a06c9b69d5 77 void HMC5883::readraw()
maetugr 12:67a06c9b69d5 78 {
maetugr 11:9bf69bc6df45 79 char buffer[6];
maetugr 11:9bf69bc6df45 80
maetugr 12:67a06c9b69d5 81 readMultiReg(HMC5883_DATA_OUT_X_MSB, buffer, 6); // read the raw data from I2C
maetugr 11:9bf69bc6df45 82
maetugr 11:9bf69bc6df45 83 // join MSB and LSB of X, Z and Y (yes, order is so stupid, see datasheet)
maetugr 12:67a06c9b69d5 84 raw[0] = (short) (buffer[0] << 8 | buffer[1]);
maetugr 12:67a06c9b69d5 85 raw[1] = (short) (buffer[4] << 8 | buffer[5]);
maetugr 12:67a06c9b69d5 86 raw[2] = (short) (buffer[2] << 8 | buffer[3]);
maetugr 11:9bf69bc6df45 87 }
maetugr 11:9bf69bc6df45 88
maetugr 11:9bf69bc6df45 89 void HMC5883::writeReg(char address, char data){
maetugr 11:9bf69bc6df45 90 char tx[2];
maetugr 11:9bf69bc6df45 91 tx[0] = address;
maetugr 11:9bf69bc6df45 92 tx[1] = data;
maetugr 11:9bf69bc6df45 93 i2c.write(I2CADR_W(HMC5883_ADDRESS), tx, 2);
maetugr 11:9bf69bc6df45 94 }
maetugr 11:9bf69bc6df45 95
maetugr 11:9bf69bc6df45 96 void HMC5883::readMultiReg(char address, char* output, int size) {
maetugr 11:9bf69bc6df45 97 i2c.write(I2CADR_W(HMC5883_ADDRESS), &address, 1); //tell it where to read from
maetugr 11:9bf69bc6df45 98 i2c.read(I2CADR_R(HMC5883_ADDRESS) , output, size); //tell it where to store the data read
maetugr 11:9bf69bc6df45 99 }
maetugr 12:67a06c9b69d5 100
maetugr 12:67a06c9b69d5 101 float HMC5883::get_angle()
maetugr 11:9bf69bc6df45 102 {
maetugr 11:9bf69bc6df45 103 #define Rad2Deg 57.295779513082320876798154814105
maetugr 11:9bf69bc6df45 104
maetugr 11:9bf69bc6df45 105 float Heading;
maetugr 12:67a06c9b69d5 106
maetugr 12:67a06c9b69d5 107 Heading = Rad2Deg * atan2(data[0],data[1]);
maetugr 12:67a06c9b69d5 108 Heading += 1.367; //bei Ost-Deklination += DecAngle, bei West-Deklination -= DecAngle
maetugr 12:67a06c9b69d5 109 //Missweisung = Winkel zwischen geographischer und magnetischer Nordrichtung
maetugr 11:9bf69bc6df45 110 //Bern ca. 1.367 Grad Ost
maetugr 11:9bf69bc6df45 111 //http://www.swisstopo.admin.ch/internet/swisstopo/de/home/apps/calc/declination.html
maetugr 11:9bf69bc6df45 112 if(Heading < 0)
maetugr 12:67a06c9b69d5 113 Heading += 360; // minimum 0 degree
maetugr 11:9bf69bc6df45 114
maetugr 12:67a06c9b69d5 115 if(Heading > 360)
maetugr 12:67a06c9b69d5 116 Heading -= 360; // maximum 360 degree
maetugr 14:cf260677ecde 117
maetugr 12:67a06c9b69d5 118 return Heading;
maetugr 12:67a06c9b69d5 119 }
maetugr 11:9bf69bc6df45 120
maetugr 11:9bf69bc6df45 121