CanSat2015aizu / Mbed 2 deprecated compass_cal

a

Dependencies:   HMC5883L mbed

main.cpp@0:64f7d228ad20, 2015-08-07 (annotated)

Committer:
s1210160
Date:
Fri Aug 07 10:29:15 2015 +0000
Revision:
0:64f7d228ad20
compass

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1210160 0:64f7d228ad20 1 #include "mbed.h"
s1210160 0:64f7d228ad20 2 #include "HMC5883L.h"
s1210160 0:64f7d228ad20 3
s1210160 0:64f7d228ad20 4 #define STOP 0 //initial
s1210160 0:64f7d228ad20 5 #define CAL 1 //calibration
s1210160 0:64f7d228ad20 6 #define RUN 2 //run
s1210160 0:64f7d228ad20 7 //#define M_PI 3.141592
s1210160 0:64f7d228ad20 8
s1210160 0:64f7d228ad20 9 Serial pc(USBTX, USBRX);
s1210160 0:64f7d228ad20 10 HMC5883L compass(p9, p10);
s1210160 0:64f7d228ad20 11 Ticker interrupt;
s1210160 0:64f7d228ad20 12
s1210160 0:64f7d228ad20 13 double heading0 = 0.0;
s1210160 0:64f7d228ad20 14 double heading1 = 0.0;
s1210160 0:64f7d228ad20 15 double heading2 = 0.0;
s1210160 0:64f7d228ad20 16 double heading3 = 0.0;
s1210160 0:64f7d228ad20 17 double headingLPF = 0.0;
s1210160 0:64f7d228ad20 18 double initHeading;
s1210160 0:64f7d228ad20 19 double tgtHeading;
s1210160 0:64f7d228ad20 20 double preHeading = 0.0;
s1210160 0:64f7d228ad20 21 unsigned char mode;
s1210160 0:64f7d228ad20 22
s1210160 0:64f7d228ad20 23 int maxX, minX, maxY, minY;
s1210160 0:64f7d228ad20 24 int ofsX = 0; //calibration x
s1210160 0:64f7d228ad20 25 int ofsY = 0; //calibration y
s1210160 0:64f7d228ad20 26 int counter = 0;
s1210160 0:64f7d228ad20 27
s1210160 0:64f7d228ad20 28 void intrpt() {
s1210160 0:64f7d228ad20 29 int16_t raw[3];
s1210160 0:64f7d228ad20 30
s1210160 0:64f7d228ad20 31 compass.getXYZ(raw);
s1210160 0:64f7d228ad20 32 double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2]
s1210160 0:64f7d228ad20 33 if(heading < 0)heading += 2*M_PI;
s1210160 0:64f7d228ad20 34 if(heading > 2*M_PI)heading -= 2*M_PI;
s1210160 0:64f7d228ad20 35 heading3 = heading2;
s1210160 0:64f7d228ad20 36 heading2 = heading1;
s1210160 0:64f7d228ad20 37 heading1 = heading0;
s1210160 0:64f7d228ad20 38 heading0 = heading;
s1210160 0:64f7d228ad20 39 headingLPF = (heading0 + heading1 + heading2 + heading3)/4; //low pass filter
s1210160 0:64f7d228ad20 40 switch(mode) {
s1210160 0:64f7d228ad20 41 case STOP:
s1210160 0:64f7d228ad20 42 if(counter == 50) {
s1210160 0:64f7d228ad20 43 initHeading = headingLPF;
s1210160 0:64f7d228ad20 44 mode = CAL;
s1210160 0:64f7d228ad20 45 maxX = raw[0];
s1210160 0:64f7d228ad20 46 minX = raw[0];
s1210160 0:64f7d228ad20 47 maxY = raw[2];
s1210160 0:64f7d228ad20 48 minY = raw[2];
s1210160 0:64f7d228ad20 49 counter = 0;
s1210160 0:64f7d228ad20 50 }
s1210160 0:64f7d228ad20 51 break;
s1210160 0:64f7d228ad20 52 case CAL:
s1210160 0:64f7d228ad20 53 if(raw[0] > maxX)maxX = raw[0];
s1210160 0:64f7d228ad20 54 if(raw[0] < minX)minX = raw[0];
s1210160 0:64f7d228ad20 55 if(raw[2] > maxY)maxY = raw[2];
s1210160 0:64f7d228ad20 56 if(raw[2] < minY)minY = raw[2];
s1210160 0:64f7d228ad20 57 if((counter > 50)
s1210160 0:64f7d228ad20 58 && (headingLPF > (initHeading-0.01))
s1210160 0:64f7d228ad20 59 && (headingLPF < (initHeading+0.01))) {
s1210160 0:64f7d228ad20 60 ofsX = (maxX + minX)/2;
s1210160 0:64f7d228ad20 61 ofsY = (maxY + minY)/2;
s1210160 0:64f7d228ad20 62 mode = RUN;
s1210160 0:64f7d228ad20 63 counter = 0;
s1210160 0:64f7d228ad20 64 pc.printf("ofs=%d,%d\r\n",ofsX,ofsY);
s1210160 0:64f7d228ad20 65 }
s1210160 0:64f7d228ad20 66 break;
s1210160 0:64f7d228ad20 67 case RUN:
s1210160 0:64f7d228ad20 68 headingLPF = headingLPF * 180.0 / M_PI;
s1210160 0:64f7d228ad20 69 pc.printf("heading=%f\r\n",headingLPF);
s1210160 0:64f7d228ad20 70 break;
s1210160 0:64f7d228ad20 71 }
s1210160 0:64f7d228ad20 72 counter++;
s1210160 0:64f7d228ad20 73 }
s1210160 0:64f7d228ad20 74
s1210160 0:64f7d228ad20 75 int main() {
s1210160 0:64f7d228ad20 76 mode = STOP;
s1210160 0:64f7d228ad20 77 compass.init();
s1210160 0:64f7d228ad20 78 interrupt.attach(&intrpt, 0.04);
s1210160 0:64f7d228ad20 79 while(1) {
s1210160 0:64f7d228ad20 80 }
s1210160 0:64f7d228ad20 81 }