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

HMC5883/HMC5883.cpp@5:818c0668fd2d, 2012-10-13 (annotated)

Committer:
maetugr
Date:
Sat Oct 13 08:59:03 2012 +0000
Revision:
5:818c0668fd2d
Child:
9:4e0c3936c756
?bergang von LCD auf PC-Terminal

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 5:818c0668fd2d 1 #include "mbed.h"
maetugr 5:818c0668fd2d 2 #include "HMC5883.h"
maetugr 5:818c0668fd2d 3
maetugr 5:818c0668fd2d 4 #define I2CADR_W(ADR) (ADR<<1&0xFE)
maetugr 5:818c0668fd2d 5 #define I2CADR_R(ADR) (ADR<<1|0x01)
maetugr 5:818c0668fd2d 6
maetugr 5:818c0668fd2d 7 //Initialisieren
maetugr 5:818c0668fd2d 8 HMC5883::HMC5883(PinName sda, PinName scl, Timer & GlobalTime_) : i2c_(sda, scl), GlobalTime(GlobalTime_)
maetugr 5:818c0668fd2d 9 {
maetugr 5:818c0668fd2d 10 Init();
maetugr 5:818c0668fd2d 11 }
maetugr 5:818c0668fd2d 12
maetugr 5:818c0668fd2d 13 void HMC5883::Init()
maetugr 5:818c0668fd2d 14 {
maetugr 5:818c0668fd2d 15 //Nullsetzen
maetugr 5:818c0668fd2d 16 MeasurementError= 0;
maetugr 5:818c0668fd2d 17 AutoCalibration= 0;
maetugr 5:818c0668fd2d 18 #pragma unroll
maetugr 5:818c0668fd2d 19 for(int i= 0; i < 3; i++)
maetugr 5:818c0668fd2d 20 {
maetugr 5:818c0668fd2d 21 RawMin[i]= -400;
maetugr 5:818c0668fd2d 22 RawMax[i]= 400;
maetugr 5:818c0668fd2d 23 Offset[i]= 0;
maetugr 5:818c0668fd2d 24 Scale[i]= 1.0;
maetugr 5:818c0668fd2d 25 RawMag[i]= 0;
maetugr 5:818c0668fd2d 26 Mag[i]= 0;
maetugr 5:818c0668fd2d 27 }
maetugr 5:818c0668fd2d 28
maetugr 5:818c0668fd2d 29
maetugr 5:818c0668fd2d 30 //HMC5883 initialisieren
maetugr 5:818c0668fd2d 31 char tx[4];
maetugr 5:818c0668fd2d 32
maetugr 5:818c0668fd2d 33 //1 Sample pro Messung
maetugr 5:818c0668fd2d 34 //75Hz Output Rate
maetugr 5:818c0668fd2d 35 //Range: +- 1.3 Gauss
maetugr 5:818c0668fd2d 36 //Continuous-Measurement Mode
maetugr 5:818c0668fd2d 37 tx[0]= 0x00;
maetugr 5:818c0668fd2d 38 tx[1]= 0x78; //Configuration Register A
maetugr 5:818c0668fd2d 39 tx[2]= 0x20; //Configuration Register B
maetugr 5:818c0668fd2d 40 tx[3]= 0x00; //Mode Register
maetugr 5:818c0668fd2d 41 i2c_.write(I2CADR_W(HMC5883_ADRESS), tx, 4);
maetugr 5:818c0668fd2d 42
maetugr 5:818c0668fd2d 43
maetugr 5:818c0668fd2d 44 Update();
maetugr 5:818c0668fd2d 45 }
maetugr 5:818c0668fd2d 46
maetugr 5:818c0668fd2d 47 //Rohdaten lesen
maetugr 5:818c0668fd2d 48 void HMC5883::ReadRawData()
maetugr 5:818c0668fd2d 49 {
maetugr 5:818c0668fd2d 50 //Drehrate aller Axen abholen
maetugr 5:818c0668fd2d 51 char tx[1];
maetugr 5:818c0668fd2d 52 char rx[6];
maetugr 5:818c0668fd2d 53
maetugr 5:818c0668fd2d 54 tx[0]= 0x03;
maetugr 5:818c0668fd2d 55 i2c_.write(I2CADR_W(HMC5883_ADRESS), tx, 1);
maetugr 5:818c0668fd2d 56 i2c_.read (I2CADR_R(HMC5883_ADRESS), rx, 6);
maetugr 5:818c0668fd2d 57
maetugr 5:818c0668fd2d 58 //Aus den einzelnen Bytes den 16-Bit-Wert zusammenbauen
maetugr 5:818c0668fd2d 59 short r[3];
maetugr 5:818c0668fd2d 60 r[0]= rx[0]<<8|rx[1];
maetugr 5:818c0668fd2d 61 r[1]= rx[4]<<8|rx[5];
maetugr 5:818c0668fd2d 62 r[2]= rx[2]<<8|rx[3];
maetugr 5:818c0668fd2d 63
maetugr 5:818c0668fd2d 64 //Grober Messfehler?
maetugr 5:818c0668fd2d 65 if(r[0] == -4096
maetugr 5:818c0668fd2d 66 || r[1] == -4096
maetugr 5:818c0668fd2d 67 || r[2] == -4096)
maetugr 5:818c0668fd2d 68 MeasurementError= 1;
maetugr 5:818c0668fd2d 69 else
maetugr 5:818c0668fd2d 70 {
maetugr 5:818c0668fd2d 71 MeasurementError= 0;
maetugr 5:818c0668fd2d 72 RawMag[0]= r[0];
maetugr 5:818c0668fd2d 73 RawMag[1]= r[1];
maetugr 5:818c0668fd2d 74 RawMag[2]= r[2];
maetugr 5:818c0668fd2d 75 }
maetugr 5:818c0668fd2d 76 }
maetugr 5:818c0668fd2d 77
maetugr 5:818c0668fd2d 78
maetugr 5:818c0668fd2d 79 //Update-Methode
maetugr 5:818c0668fd2d 80 void HMC5883::Update()
maetugr 5:818c0668fd2d 81 {
maetugr 5:818c0668fd2d 82 //Rohdaten lesen
maetugr 5:818c0668fd2d 83 ReadRawData();
maetugr 5:818c0668fd2d 84
maetugr 5:818c0668fd2d 85 if(AutoCalibration)
maetugr 5:818c0668fd2d 86 {
maetugr 5:818c0668fd2d 87 #pragma unroll
maetugr 5:818c0668fd2d 88 for(int i= 0; i < 3; i++)
maetugr 5:818c0668fd2d 89 {
maetugr 5:818c0668fd2d 90 //Neuer Min Max Wert?
maetugr 5:818c0668fd2d 91 RawMin[i]= RawMin[i] < RawMag[i] ? RawMin[i] : RawMag[i];
maetugr 5:818c0668fd2d 92 RawMax[i]= RawMax[i] > RawMag[i] ? RawMax[i] : RawMag[i];
maetugr 5:818c0668fd2d 93
maetugr 5:818c0668fd2d 94 //Scale und Offset aus gesammelten Min Max Werten berechnen
maetugr 5:818c0668fd2d 95 //Die neue Untere und obere Grenze bilden -1 und +1
maetugr 5:818c0668fd2d 96 Scale[i]= 2.0 / (float)(RawMax[i]-RawMin[i]);
maetugr 5:818c0668fd2d 97 Offset[i]= 1.0 - (float)(RawMax[i]) * Scale[i];
maetugr 5:818c0668fd2d 98 }
maetugr 5:818c0668fd2d 99 }
maetugr 5:818c0668fd2d 100
maetugr 5:818c0668fd2d 101 //Feldstaerke berechnen
maetugr 5:818c0668fd2d 102 Mag[0]= Scale[0] * (float)(RawMag[0]) + Offset[0];
maetugr 5:818c0668fd2d 103 Mag[1]= Scale[1] * (float)(RawMag[1]) + Offset[1];
maetugr 5:818c0668fd2d 104 Mag[2]= Scale[2] * (float)(RawMag[2]) + Offset[2];
maetugr 5:818c0668fd2d 105 }
maetugr 5:818c0668fd2d 106
maetugr 5:818c0668fd2d 107
maetugr 5:818c0668fd2d 108 //Kalibrieren
maetugr 5:818c0668fd2d 109 void HMC5883::Calibrate(const short * pRawMin, const short * pRawMax)
maetugr 5:818c0668fd2d 110 {
maetugr 5:818c0668fd2d 111 #pragma unroll
maetugr 5:818c0668fd2d 112 for(int i= 0; i < 3; i++)
maetugr 5:818c0668fd2d 113 {
maetugr 5:818c0668fd2d 114 RawMin[i]= pRawMin[i];
maetugr 5:818c0668fd2d 115 RawMax[i]= pRawMax[i];
maetugr 5:818c0668fd2d 116 }
maetugr 5:818c0668fd2d 117 char AutoCalibrationBak= AutoCalibration;
maetugr 5:818c0668fd2d 118 AutoCalibration= 1;
maetugr 5:818c0668fd2d 119 Update();
maetugr 5:818c0668fd2d 120 AutoCalibration= AutoCalibrationBak;
maetugr 5:818c0668fd2d 121 }
maetugr 5:818c0668fd2d 122
maetugr 5:818c0668fd2d 123 void HMC5883::Calibrate(int s)
maetugr 5:818c0668fd2d 124 {
maetugr 5:818c0668fd2d 125 char AutoCalibrationBak= AutoCalibration;
maetugr 5:818c0668fd2d 126 AutoCalibration= 1;
maetugr 5:818c0668fd2d 127
maetugr 5:818c0668fd2d 128 //Ende der Kalibrierung in ms Millisekunden berechnen
maetugr 5:818c0668fd2d 129 int CalibEnd= GlobalTime.read_ms() + s*1000;
maetugr 5:818c0668fd2d 130
maetugr 5:818c0668fd2d 131 while(GlobalTime.read_ms() < CalibEnd)
maetugr 5:818c0668fd2d 132 {
maetugr 5:818c0668fd2d 133 //Update erledigt alles
maetugr 5:818c0668fd2d 134 Update();
maetugr 5:818c0668fd2d 135 }
maetugr 5:818c0668fd2d 136
maetugr 5:818c0668fd2d 137 AutoCalibration= AutoCalibrationBak;
maetugr 5:818c0668fd2d 138 }
maetugr 5:818c0668fd2d 139
maetugr 5:818c0668fd2d 140 // Winkel berechnen
maetugr 5:818c0668fd2d 141 //---------------------------------------------------------------------------------------------------------------------------------------
maetugr 5:818c0668fd2d 142 float HMC5883::getAngle(float x, float y)
maetugr 5:818c0668fd2d 143 {
maetugr 5:818c0668fd2d 144 #define Rad2Deg 57.295779513082320876798154814105
maetugr 5:818c0668fd2d 145 #define PI 3.1415926535897932384626433832795
maetugr 5:818c0668fd2d 146
maetugr 5:818c0668fd2d 147 float Heading;
maetugr 5:818c0668fd2d 148 float DecAngle;
maetugr 5:818c0668fd2d 149
maetugr 5:818c0668fd2d 150 DecAngle = 1.367 / Rad2Deg; //Missweisung = Winkel zwischen geographischer und magnetischer Nordrichtung
maetugr 5:818c0668fd2d 151 //Bern ca. 1.367 Grad Ost
maetugr 5:818c0668fd2d 152 //http://www.swisstopo.admin.ch/internet/swisstopo/de/home/apps/calc/declination.html
maetugr 5:818c0668fd2d 153 Heading = atan2((float)y,(float)x);
maetugr 5:818c0668fd2d 154
maetugr 5:818c0668fd2d 155 Heading += DecAngle; //bei Ost-Deklination += DecAngle, bei West-Deklination -= DecAngle
maetugr 5:818c0668fd2d 156
maetugr 5:818c0668fd2d 157 if(Heading < 0)
maetugr 5:818c0668fd2d 158 Heading += 2*PI; //korrigieren bei negativem Vorzeichen
maetugr 5:818c0668fd2d 159
maetugr 5:818c0668fd2d 160 if(Heading > 2*PI)
maetugr 5:818c0668fd2d 161 Heading -= 2*PI; //auf 2Pi begrenzen
maetugr 5:818c0668fd2d 162
maetugr 5:818c0668fd2d 163 return (Heading * 180/PI); //Radianten in Grad konvertieren
maetugr 5:818c0668fd2d 164 }
maetugr 5:818c0668fd2d 165
maetugr 5:818c0668fd2d 166