local compass for tugboat project
Fork of HMC5883L by
Diff: HMC5883L.cpp
- Revision:
- 1:8a1357c351c6
- Parent:
- 0:8b84d61cee94
- Child:
- 2:8eb755577f83
diff -r 8b84d61cee94 -r 8a1357c351c6 HMC5883L.cpp --- a/HMC5883L.cpp Wed Oct 31 04:35:08 2012 +0000 +++ b/HMC5883L.cpp Wed Oct 31 05:06:34 2012 +0000 @@ -127,4 +127,21 @@ for(int i = 0; i < 3; i++) // fill the output variables output[i] = int16_t(((unsigned char)data[i*2] << 8) | (unsigned char)data[i*2+1]); +} + +double HMC5883L::getHeadingXY() +{ + int raw_data[3]; + getXYZ(raw_data); + double heading = atan2(raw_data[1], raw_data[0]); // heading = arctan(Y/X) + + // TODO: declenation angle compensation + + if(heading < 0.0) // fix sign + heading += 2PI; + + if(heading > 2PI) // fix overflow + heading -= 2PI; + + return heading; } \ No newline at end of file