Bo Carøe
HMC5882 from BlazeX
Dependents: Sensor_test Algoritmo_Fuzzy
HMC5883.cpp@0:a5e06bb74915, 2012-05-30 (annotated)
- Committer:
- caroe
- Date:
- Wed May 30 10:45:00 2012 +0000
- Revision:
- 0:a5e06bb74915
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| caroe | 0:a5e06bb74915 | 1 | #include "mbed.h" |
| caroe | 0:a5e06bb74915 | 2 | #include "HMC5883.h" |
| caroe | 0:a5e06bb74915 | 3 | |
| caroe | 0:a5e06bb74915 | 4 | #define I2CADR_W(ADR) (ADR<<1&0xFE) |
| caroe | 0:a5e06bb74915 | 5 | #define I2CADR_R(ADR) (ADR<<1|0x01) |
| caroe | 0:a5e06bb74915 | 6 | |
| caroe | 0:a5e06bb74915 | 7 | //Initialisieren |
| caroe | 0:a5e06bb74915 | 8 | HMC5883::HMC5883(I2C & I2CBus_, Timer & GlobalTime_) : |
| caroe | 0:a5e06bb74915 | 9 | I2CBus(I2CBus_), |
| caroe | 0:a5e06bb74915 | 10 | GlobalTime(GlobalTime_) |
| caroe | 0:a5e06bb74915 | 11 | |
| caroe | 0:a5e06bb74915 | 12 | {} |
| caroe | 0:a5e06bb74915 | 13 | |
| caroe | 0:a5e06bb74915 | 14 | void HMC5883::Init() |
| caroe | 0:a5e06bb74915 | 15 | { |
| caroe | 0:a5e06bb74915 | 16 | //Nullsetzen |
| caroe | 0:a5e06bb74915 | 17 | MeasurementError= 0; |
| caroe | 0:a5e06bb74915 | 18 | AutoCalibration= 0; |
| caroe | 0:a5e06bb74915 | 19 | #pragma unroll |
| caroe | 0:a5e06bb74915 | 20 | for(int i= 0; i < 3; i++) |
| caroe | 0:a5e06bb74915 | 21 | { |
| caroe | 0:a5e06bb74915 | 22 | RawMin[i]= -400; |
| caroe | 0:a5e06bb74915 | 23 | RawMax[i]= 400; |
| caroe | 0:a5e06bb74915 | 24 | Offset[i]= 0; |
| caroe | 0:a5e06bb74915 | 25 | Scale[i]= 1.0; |
| caroe | 0:a5e06bb74915 | 26 | RawMag[i]= 0; |
| caroe | 0:a5e06bb74915 | 27 | Mag[i]= 0; |
| caroe | 0:a5e06bb74915 | 28 | } |
| caroe | 0:a5e06bb74915 | 29 | |
| caroe | 0:a5e06bb74915 | 30 | |
| caroe | 0:a5e06bb74915 | 31 | //HMC5883 initialisieren |
| caroe | 0:a5e06bb74915 | 32 | char tx[4]; |
| caroe | 0:a5e06bb74915 | 33 | |
| caroe | 0:a5e06bb74915 | 34 | //1 Sample pro Messung |
| caroe | 0:a5e06bb74915 | 35 | //75Hz Output Rate |
| caroe | 0:a5e06bb74915 | 36 | //Range: +- 1.3 Gauss |
| caroe | 0:a5e06bb74915 | 37 | //Continuous-Measurement Mode |
| caroe | 0:a5e06bb74915 | 38 | tx[0]= 0x00; |
| caroe | 0:a5e06bb74915 | 39 | tx[1]= 0x78; //Configuration Register A |
| caroe | 0:a5e06bb74915 | 40 | tx[2]= 0x20; //Configuration Register B |
| caroe | 0:a5e06bb74915 | 41 | tx[3]= 0x00; //Mode Register |
| caroe | 0:a5e06bb74915 | 42 | I2CBus.write(I2CADR_W(HMC5883_ADRESS), tx, 4); |
| caroe | 0:a5e06bb74915 | 43 | |
| caroe | 0:a5e06bb74915 | 44 | |
| caroe | 0:a5e06bb74915 | 45 | Update(); |
| caroe | 0:a5e06bb74915 | 46 | } |
| caroe | 0:a5e06bb74915 | 47 | |
| caroe | 0:a5e06bb74915 | 48 | //Rohdaten lesen |
| caroe | 0:a5e06bb74915 | 49 | void HMC5883::ReadRawData() |
| caroe | 0:a5e06bb74915 | 50 | { |
| caroe | 0:a5e06bb74915 | 51 | //Drehrate aller Axen abholen |
| caroe | 0:a5e06bb74915 | 52 | char tx[1]; |
| caroe | 0:a5e06bb74915 | 53 | char rx[6]; |
| caroe | 0:a5e06bb74915 | 54 | |
| caroe | 0:a5e06bb74915 | 55 | tx[0]= 0x03; |
| caroe | 0:a5e06bb74915 | 56 | I2CBus.write(I2CADR_W(HMC5883_ADRESS), tx, 1); |
| caroe | 0:a5e06bb74915 | 57 | I2CBus.read (I2CADR_R(HMC5883_ADRESS), rx, 6); |
| caroe | 0:a5e06bb74915 | 58 | |
| caroe | 0:a5e06bb74915 | 59 | //Aus den einzelnen Bytes den 16-Bit-Wert zusammenbauen |
| caroe | 0:a5e06bb74915 | 60 | short r[3]; |
| caroe | 0:a5e06bb74915 | 61 | r[0]= rx[0]<<8|rx[1]; |
| caroe | 0:a5e06bb74915 | 62 | r[1]= rx[4]<<8|rx[5]; |
| caroe | 0:a5e06bb74915 | 63 | r[2]= rx[2]<<8|rx[3]; |
| caroe | 0:a5e06bb74915 | 64 | |
| caroe | 0:a5e06bb74915 | 65 | //Grober Messfehler? |
| caroe | 0:a5e06bb74915 | 66 | if(r[0] == -4096 |
| caroe | 0:a5e06bb74915 | 67 | || r[1] == -4096 |
| caroe | 0:a5e06bb74915 | 68 | || r[2] == -4096) |
| caroe | 0:a5e06bb74915 | 69 | MeasurementError= 1; |
| caroe | 0:a5e06bb74915 | 70 | else |
| caroe | 0:a5e06bb74915 | 71 | { |
| caroe | 0:a5e06bb74915 | 72 | MeasurementError= 0; |
| caroe | 0:a5e06bb74915 | 73 | RawMag[0]= r[0]; |
| caroe | 0:a5e06bb74915 | 74 | RawMag[1]= r[1]; |
| caroe | 0:a5e06bb74915 | 75 | RawMag[2]= r[2]; |
| caroe | 0:a5e06bb74915 | 76 | } |
| caroe | 0:a5e06bb74915 | 77 | } |
| caroe | 0:a5e06bb74915 | 78 | |
| caroe | 0:a5e06bb74915 | 79 | |
| caroe | 0:a5e06bb74915 | 80 | //Update-Methode |
| caroe | 0:a5e06bb74915 | 81 | void HMC5883::Update() |
| caroe | 0:a5e06bb74915 | 82 | { |
| caroe | 0:a5e06bb74915 | 83 | //Rohdaten lesen |
| caroe | 0:a5e06bb74915 | 84 | ReadRawData(); |
| caroe | 0:a5e06bb74915 | 85 | |
| caroe | 0:a5e06bb74915 | 86 | if(AutoCalibration) |
| caroe | 0:a5e06bb74915 | 87 | { |
| caroe | 0:a5e06bb74915 | 88 | #pragma unroll |
| caroe | 0:a5e06bb74915 | 89 | for(int i= 0; i < 3; i++) |
| caroe | 0:a5e06bb74915 | 90 | { |
| caroe | 0:a5e06bb74915 | 91 | //Neuer Min Max Wert? |
| caroe | 0:a5e06bb74915 | 92 | RawMin[i]= RawMin[i] < RawMag[i] ? RawMin[i] : RawMag[i]; |
| caroe | 0:a5e06bb74915 | 93 | RawMax[i]= RawMax[i] > RawMag[i] ? RawMax[i] : RawMag[i]; |
| caroe | 0:a5e06bb74915 | 94 | |
| caroe | 0:a5e06bb74915 | 95 | //Scale und Offset aus gesammelten Min Max Werten berechnen |
| caroe | 0:a5e06bb74915 | 96 | //Die neue Untere und obere Grenze bilden -1 und +1 |
| caroe | 0:a5e06bb74915 | 97 | Scale[i]= 2.0 / (float)(RawMax[i]-RawMin[i]); |
| caroe | 0:a5e06bb74915 | 98 | Offset[i]= 1.0 - (float)(RawMax[i]) * Scale[i]; |
| caroe | 0:a5e06bb74915 | 99 | } |
| caroe | 0:a5e06bb74915 | 100 | } |
| caroe | 0:a5e06bb74915 | 101 | |
| caroe | 0:a5e06bb74915 | 102 | //Feldstaerke berechnen |
| caroe | 0:a5e06bb74915 | 103 | Mag[0]= Scale[0] * (float)(RawMag[0]) + Offset[0]; |
| caroe | 0:a5e06bb74915 | 104 | Mag[1]= Scale[1] * (float)(RawMag[1]) + Offset[1]; |
| caroe | 0:a5e06bb74915 | 105 | Mag[2]= Scale[2] * (float)(RawMag[2]) + Offset[2]; |
| caroe | 0:a5e06bb74915 | 106 | } |
| caroe | 0:a5e06bb74915 | 107 | |
| caroe | 0:a5e06bb74915 | 108 | |
| caroe | 0:a5e06bb74915 | 109 | //Kalibrieren |
| caroe | 0:a5e06bb74915 | 110 | void HMC5883::Calibrate(const short * pRawMin, const short * pRawMax) |
| caroe | 0:a5e06bb74915 | 111 | { |
| caroe | 0:a5e06bb74915 | 112 | #pragma unroll |
| caroe | 0:a5e06bb74915 | 113 | for(int i= 0; i < 3; i++) |
| caroe | 0:a5e06bb74915 | 114 | { |
| caroe | 0:a5e06bb74915 | 115 | RawMin[i]= pRawMin[i]; |
| caroe | 0:a5e06bb74915 | 116 | RawMax[i]= pRawMax[i]; |
| caroe | 0:a5e06bb74915 | 117 | } |
| caroe | 0:a5e06bb74915 | 118 | char AutoCalibrationBak= AutoCalibration; |
| caroe | 0:a5e06bb74915 | 119 | AutoCalibration= 1; |
| caroe | 0:a5e06bb74915 | 120 | Update(); |
| caroe | 0:a5e06bb74915 | 121 | AutoCalibration= AutoCalibrationBak; |
| caroe | 0:a5e06bb74915 | 122 | } |
| caroe | 0:a5e06bb74915 | 123 | |
| caroe | 0:a5e06bb74915 | 124 | void HMC5883::Calibrate(int s) |
| caroe | 0:a5e06bb74915 | 125 | { |
| caroe | 0:a5e06bb74915 | 126 | char AutoCalibrationBak= AutoCalibration; |
| caroe | 0:a5e06bb74915 | 127 | AutoCalibration= 1; |
| caroe | 0:a5e06bb74915 | 128 | |
| caroe | 0:a5e06bb74915 | 129 | //Ende der Kalibrierung in ms Millisekunden berechnen |
| caroe | 0:a5e06bb74915 | 130 | int CalibEnd= GlobalTime.read_ms() + s*1000; |
| caroe | 0:a5e06bb74915 | 131 | |
| caroe | 0:a5e06bb74915 | 132 | while(GlobalTime.read_ms() < CalibEnd) |
| caroe | 0:a5e06bb74915 | 133 | { |
| caroe | 0:a5e06bb74915 | 134 | //Update erledigt alles |
| caroe | 0:a5e06bb74915 | 135 | Update(); |
| caroe | 0:a5e06bb74915 | 136 | } |
| caroe | 0:a5e06bb74915 | 137 | |
| caroe | 0:a5e06bb74915 | 138 | AutoCalibration= AutoCalibrationBak; |
| caroe | 0:a5e06bb74915 | 139 | } |