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@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?
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 | 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 |