Rolf Rahm / Mbed 2 deprecated m3pi_Kompass

Using LSM303DLM Kompass-Module and Library for m3pi-Robot.

Dependencies:   LSM303DLM m3pi mbed

main.cpp@0:ccf3df1312aa, 2014-11-25 (annotated)

Committer:
rahm
Date:
Tue Nov 25 13:52:55 2014 +0000
Revision:
0:ccf3df1312aa
Digital Kompass with LSM303DLM and m3pi-Robot.

;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rahm 0:ccf3df1312aa 1 #include "mbed.h"
rahm 0:ccf3df1312aa 2 #include <math.h>
rahm 0:ccf3df1312aa 3 #include "LSM303DLM.h"
rahm 0:ccf3df1312aa 4 #include "m3pi.h"
rahm 0:ccf3df1312aa 5
rahm 0:ccf3df1312aa 6 #define YOFF 100.0 // Y-Offset
rahm 0:ccf3df1312aa 7 #define XOFF 2.0 // X-Offset
rahm 0:ccf3df1312aa 8 #define WOFF 12.0 // Winkel-Offset
rahm 0:ccf3df1312aa 9
rahm 0:ccf3df1312aa 10 //Konstruktoren
rahm 0:ccf3df1312aa 11 //Serial pc(USBTX,USBRX); // Für Testausgabe auf PC
rahm 0:ccf3df1312aa 12 LSM303DLM compass(p28, p27);
rahm 0:ccf3df1312aa 13 m3pi robo;
rahm 0:ccf3df1312aa 14
rahm 0:ccf3df1312aa 15 // Funktionsprototyp calcHeading
rahm 0:ccf3df1312aa 16 double calcHeading(int *mag);
rahm 0:ccf3df1312aa 17
rahm 0:ccf3df1312aa 18 // Funktion main
rahm 0:ccf3df1312aa 19 int main()
rahm 0:ccf3df1312aa 20 {
rahm 0:ccf3df1312aa 21 double hdg;
rahm 0:ccf3df1312aa 22 int mag[3];
rahm 0:ccf3df1312aa 23
rahm 0:ccf3df1312aa 24 //pc.printf("LSM303DLH Test \n\r");
rahm 0:ccf3df1312aa 25 robo.locate(0,0);
rahm 0:ccf3df1312aa 26 robo.print("LSM303DL",8);
rahm 0:ccf3df1312aa 27 while(1)
rahm 0:ccf3df1312aa 28 {
rahm 0:ccf3df1312aa 29 compass.readMag(mag);
rahm 0:ccf3df1312aa 30 hdg = calcHeading(mag);
rahm 0:ccf3df1312aa 31
rahm 0:ccf3df1312aa 32 //pc.printf(" %.1f ", hdg);
rahm 0:ccf3df1312aa 33 robo.locate(0,1);
rahm 0:ccf3df1312aa 34 robo.printf(" %.1f\xdf ", hdg); // \xdf = °-Symbol im Display-Zeichensatz
rahm 0:ccf3df1312aa 35 wait(0.4);
rahm 0:ccf3df1312aa 36 }
rahm 0:ccf3df1312aa 37 }
rahm 0:ccf3df1312aa 38
rahm 0:ccf3df1312aa 39 double calcHeading(int *mag)
rahm 0:ccf3df1312aa 40 {
rahm 0:ccf3df1312aa 41 double x,y;
rahm 0:ccf3df1312aa 42 double hdg;
rahm 0:ccf3df1312aa 43
rahm 0:ccf3df1312aa 44 x = mag[0] + XOFF; // X-Achsen Ausgleich
rahm 0:ccf3df1312aa 45 y = mag[1] + YOFF; // Y-Achsen Ausgleich
rahm 0:ccf3df1312aa 46
rahm 0:ccf3df1312aa 47 hdg = atan(y/x);
rahm 0:ccf3df1312aa 48 hdg *= 57.2958; // Umrechnung von Bogen- nach Winkelmass
rahm 0:ccf3df1312aa 49 if (x > 0)
rahm 0:ccf3df1312aa 50 {
rahm 0:ccf3df1312aa 51 if(y>0) hdg = hdg; // Korrektur 1. Quadrant
rahm 0:ccf3df1312aa 52 else hdg = 360.0 + hdg; // Korrektur 4. Quadrant
rahm 0:ccf3df1312aa 53 }
rahm 0:ccf3df1312aa 54 else
rahm 0:ccf3df1312aa 55 {
rahm 0:ccf3df1312aa 56 if(y>0) hdg = 180.0 + hdg; // Korrektur 2. Quadrant
rahm 0:ccf3df1312aa 57 else hdg = 180.0 + hdg; // Korrektur 3. Quadrant
rahm 0:ccf3df1312aa 58 }
rahm 0:ccf3df1312aa 59
rahm 0:ccf3df1312aa 60 hdg -= WOFF; // Korrektur Winkel (Vorsicht: Bei Winkeln < WOFF wird hdg negativ!)
rahm 0:ccf3df1312aa 61 if (hdg <0) hdg = 360 + hdg; // Winkel soll nicht negativ werden! (Bsp.: -5° => 355°)
rahm 0:ccf3df1312aa 62 return (hdg);
rahm 0:ccf3df1312aa 63 }