示例#1
0
/**
 * 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();
}
示例#2
0
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
  }
}