![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Compass Working
Compass.cpp
- Committer:
- pstephens18
- Date:
- 2016-01-19
- Revision:
- 5:f6762034b4c0
- Parent:
- 2:ef6778c2438e
File content as of revision 5:f6762034b4c0:
#include "Compass.h" I2C i2c_port(p9,p10); char rawBytes[6]; short data[6]; short magComponents[3]; double heading; const int addr1 = (0x1E << 1); char config[2]; char init[2]; double Compass() { config[0]= 0x02; config[1] = 0x00; i2c_port.write(addr1, config,2); init[0]=0x03; while(1) { i2c_port.write(addr1,init,1); i2c_port.read(addr1,rawBytes,6); for (int i = 0; i<6; i++) { data[i] = rawBytes[i]; } magComponents[0] = data[0]<<8 | data[1]; magComponents[1] = data[4]<<8 | data[5]; magComponents[2] = data[2]<<8 | data[3]; heading = atan2((double)magComponents[1],(double)magComponents[0]); // Angle in Radians heading = (heading*180)/3.14159; // Convert to Degrees heading = heading + 14.85; // Find True North if(heading > 180) { heading -= 360; } return heading; } }