CanSat2015aizu
/
compass_cal
a
Diff: main.cpp
- Revision:
- 0:64f7d228ad20
diff -r 000000000000 -r 64f7d228ad20 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Aug 07 10:29:15 2015 +0000 @@ -0,0 +1,81 @@ +#include "mbed.h" +#include "HMC5883L.h" + +#define STOP 0 //initial +#define CAL 1 //calibration +#define RUN 2 //run +//#define M_PI 3.141592 + +Serial pc(USBTX, USBRX); +HMC5883L compass(p9, p10); +Ticker interrupt; + +double heading0 = 0.0; +double heading1 = 0.0; +double heading2 = 0.0; +double heading3 = 0.0; +double headingLPF = 0.0; +double initHeading; +double tgtHeading; +double preHeading = 0.0; +unsigned char mode; + +int maxX, minX, maxY, minY; +int ofsX = 0; //calibration x +int ofsY = 0; //calibration y +int counter = 0; + +void intrpt() { + int16_t raw[3]; + + compass.getXYZ(raw); + double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2] + if(heading < 0)heading += 2*M_PI; + if(heading > 2*M_PI)heading -= 2*M_PI; + heading3 = heading2; + heading2 = heading1; + heading1 = heading0; + heading0 = heading; + headingLPF = (heading0 + heading1 + heading2 + heading3)/4; //low pass filter + switch(mode) { + case STOP: + if(counter == 50) { + initHeading = headingLPF; + mode = CAL; + maxX = raw[0]; + minX = raw[0]; + maxY = raw[2]; + minY = raw[2]; + counter = 0; + } + break; + case CAL: + if(raw[0] > maxX)maxX = raw[0]; + if(raw[0] < minX)minX = raw[0]; + if(raw[2] > maxY)maxY = raw[2]; + if(raw[2] < minY)minY = raw[2]; + if((counter > 50) + && (headingLPF > (initHeading-0.01)) + && (headingLPF < (initHeading+0.01))) { + ofsX = (maxX + minX)/2; + ofsY = (maxY + minY)/2; + mode = RUN; + counter = 0; + pc.printf("ofs=%d,%d\r\n",ofsX,ofsY); + } + break; + case RUN: + headingLPF = headingLPF * 180.0 / M_PI; + pc.printf("heading=%f\r\n",headingLPF); + break; + } + counter++; +} + +int main() { + mode = STOP; + compass.init(); + interrupt.attach(&intrpt, 0.04); + while(1) { + } +} \ No newline at end of file