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@12:67a06c9b69d5, 2012-10-20 (annotated)

Committer:
maetugr
Date:
Sat Oct 20 17:28:28 2012 +0000
Revision:
12:67a06c9b69d5
Parent:
11:9bf69bc6df45
Child:
13:4737ee9ebfee
mit unterbrochenem RC signal, kompass fertig mit speicherung!

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