Keith G / Mbed 2 deprecated compass_test

Initial import

Dependencies:   HMC5883L mbed

main.cpp@0:b3e9ce4cc500, 2016-03-08 (annotated)

Committer:
Condo2k4
Date:
Tue Mar 08 13:13:09 2016 +0000
Revision:
0:b3e9ce4cc500
Child:
2:3a288d8c816b
Initial import

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Condo2k4 0:b3e9ce4cc500 1 #include "mbed.h"
Condo2k4 0:b3e9ce4cc500 2
Condo2k4 0:b3e9ce4cc500 3 //Test for correctness when in a room that does produce it's own magnetic field...
Condo2k4 0:b3e9ce4cc500 4 //Magnetic Declination for Canterbury = WEST 0deg 16min
Condo2k4 0:b3e9ce4cc500 5 #define DECLINATION_ANGLE ((-16.0*M_PI)/(60.0*180.0))
Condo2k4 0:b3e9ce4cc500 6 #include "HMC5883L.h"
Condo2k4 0:b3e9ce4cc500 7
Condo2k4 0:b3e9ce4cc500 8 Serial usb(USBTX,USBRX);
Condo2k4 0:b3e9ce4cc500 9
Condo2k4 0:b3e9ce4cc500 10 HMC5883L compass(I2C_SDA, I2C_SCL);
Condo2k4 0:b3e9ce4cc500 11 #define SMOOTHING 0.75
Condo2k4 0:b3e9ce4cc500 12
Condo2k4 0:b3e9ce4cc500 13 double smoothedHeading() {
Condo2k4 0:b3e9ce4cc500 14 static int historic = compass.getHeadingXYDeg();
Condo2k4 0:b3e9ce4cc500 15
Condo2k4 0:b3e9ce4cc500 16 double h = compass.getHeadingXYDeg();
Condo2k4 0:b3e9ce4cc500 17
Condo2k4 0:b3e9ce4cc500 18 if(h==historic) return h;
Condo2k4 0:b3e9ce4cc500 19
Condo2k4 0:b3e9ce4cc500 20 if( (h<180.0)==(historic<180.0) ) {
Condo2k4 0:b3e9ce4cc500 21
Condo2k4 0:b3e9ce4cc500 22 historic = historic*SMOOTHING + h*(1.0-SMOOTHING);
Condo2k4 0:b3e9ce4cc500 23
Condo2k4 0:b3e9ce4cc500 24 } else {
Condo2k4 0:b3e9ce4cc500 25 if(h<180) {
Condo2k4 0:b3e9ce4cc500 26
Condo2k4 0:b3e9ce4cc500 27 historic = historic*SMOOTHING + (h+360.0)*(1.0-SMOOTHING);
Condo2k4 0:b3e9ce4cc500 28
Condo2k4 0:b3e9ce4cc500 29 } else {
Condo2k4 0:b3e9ce4cc500 30
Condo2k4 0:b3e9ce4cc500 31 historic = (historic+360.0)*SMOOTHING + h*(1.0-SMOOTHING);
Condo2k4 0:b3e9ce4cc500 32
Condo2k4 0:b3e9ce4cc500 33 }
Condo2k4 0:b3e9ce4cc500 34 if(historic>=360.0) {
Condo2k4 0:b3e9ce4cc500 35 historic-=360.0;
Condo2k4 0:b3e9ce4cc500 36 }
Condo2k4 0:b3e9ce4cc500 37 }
Condo2k4 0:b3e9ce4cc500 38
Condo2k4 0:b3e9ce4cc500 39 return historic;
Condo2k4 0:b3e9ce4cc500 40
Condo2k4 0:b3e9ce4cc500 41 }
Condo2k4 0:b3e9ce4cc500 42
Condo2k4 0:b3e9ce4cc500 43 int main()
Condo2k4 0:b3e9ce4cc500 44 {
Condo2k4 0:b3e9ce4cc500 45 compass.init();
Condo2k4 0:b3e9ce4cc500 46 for(;;) {
Condo2k4 0:b3e9ce4cc500 47
Condo2k4 0:b3e9ce4cc500 48 wait_ms(500);
Condo2k4 0:b3e9ce4cc500 49
Condo2k4 0:b3e9ce4cc500 50 double h = smoothedHeading();
Condo2k4 0:b3e9ce4cc500 51 usb.printf("%.2f\r\n",h);
Condo2k4 0:b3e9ce4cc500 52 }
Condo2k4 0:b3e9ce4cc500 53 }