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@15:753c5d6a63b3, 2012-10-29 (annotated)
- Committer:
- maetugr
- Date:
- Mon Oct 29 16:43:10 2012 +0000
- Revision:
- 15:753c5d6a63b3
- Parent:
- 14:cf260677ecde
- Child:
- 17:e096e85f6c36
I2C/Interrupt-Problem gel?st!! erste Versuche an gespannter schnur, falsche PID werte mitten im umschreiben auf I2C_Sensor mutterklasse
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 | 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 | 15:753c5d6a63b3 | 97 | i2c.write(I2CADR_W(HMC5883_ADDRESS), &address, 1, true); //tell it where to read from |
maetugr | 15:753c5d6a63b3 | 98 | i2c.read(I2CADR_R(HMC5883_ADDRESS) , output, size, true); //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 |