/**
* Initialize compass module.
*/
void init_compass(void)
{
DEBUG_STATUS(DEBUG_INIT_COMPASS);
compass_set(COMPASS_RAMP);
init_single_compass();
compass_set(COMPASS_FLAT);
init_single_compass();
// for(ms_timer = 0; ms_timer < (40/MS_TIMER_PER);); // Wait for compass to stabilize
while(! compass_read(&compass_north));
DEBUG_CLEAR_STATUS();
}
int compass_get_bearing(void)
{
uint16_t abs_heading;
int bearing;
while(! compass_read(&abs_heading));
bearing = abs_heading - compass_north;
if(current_compass_addr == COMPASS_RAMP_TWI_ADDRESS)
bearing -= 1800;
while(bearing < 0)
bearing += 3600;
return bearing;
}
int main() // main function
{
int x, y, z; // Declare x, y, & z axis variables
i2c *bus = i2c_newbus(3, 2, 0); // New I2C bus SCL=P3, SDA=P2
compass_init(bus); // Initialize compass on bus.
while(1) // Repeat indefinitely
{
compass_read(bus, &x, &y, &z); // Compass vals -> variables
print("%cx=%d, y=%d, z=%d%c\n", // Display compass variables
HOME, x, y, z, CLREOL);
float fy = (float) y; // Ints to floats
float fx = (float) x;
float heading = atan2(fy, fx) * 180.0/PI; // Calculate heading with floats
if(heading < 0.0) // Convert -180...+180 to 0...360
heading = (360.0 + heading);
print("heading = %.2f%c\n", // Display heading
heading, CLREOL);
pause(500); // Wait 1/2 second
}
}
