/** * 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 } }