Merge branch 'master' of https://code.google.com/p/ardupilot-mega

ArduCopter/ArduCopter.pde

@@ -54,7 +54,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list
 #include <Arduino_Mega_ISR_Registry.h>
 #include <APM_RC.h>         // ArduPilot Mega RC Library
 #include <AP_GPS.h>         // ArduPilot GPS library
-#include <Wire.h>			// Arduino I2C lib
+#include <I2C.h>			// Arduino I2C lib
 #include <SPI.h>			// Arduino SPI lib
 #include <DataFlash.h>      // ArduPilot Mega Flash Memory Library
 #include <AP_ADC.h>         // ArduPilot Mega Analog to Digital Converter Library
@@ -703,9 +703,10 @@ static void medium_loop()
 
 			#if HIL_MODE != HIL_MODE_ATTITUDE					// don't execute in HIL mode
 				if(g.compass_enabled){
-					compass.read();		 						// Read magnetometer
+					if (compass.read()) {
                         compass.calculate(dcm.get_dcm_matrix());  	// Calculate heading
                         compass.null_offsets(dcm.get_dcm_matrix());
+                    }
 				}
 			#endif
 

ArduCopter/config.h

@@ -52,6 +52,7 @@
 # define CONFIG_RELAY      DISABLED
 # define MAG_ORIENTATION   AP_COMPASS_APM2_SHIELD
 # define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
+# define MAGNETOMETER ENABLED
 # ifdef APM2_BETA_HARDWARE
 #  define CONFIG_BARO     AP_BARO_BMP085
 # else // APM2 Production Hardware (default)

ArduCopter/system.pde

@@ -107,7 +107,8 @@ static void init_ardupilot()
 	// Initialize Wire and SPI libraries
 	//
 #ifndef DESKTOP_BUILD
-    Wire.begin();
+    I2c.begin();
+    I2c.timeOut(20);
 #endif
     SPI.begin();
     SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate

ArduCopter/test.pde

@@ -903,7 +903,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 
 		while(1){
 			delay(100);
-			compass.read();
+			if (compass.read()) {
                 compass.calculate(dcm.get_dcm_matrix());
                 Vector3f maggy = compass.get_offsets();
                 Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
@@ -911,6 +911,9 @@ test_mag(uint8_t argc, const Menu::arg *argv)
                                 compass.mag_x,
                                 compass.mag_y,
                                 compass.mag_z);
+            } else {
+                Serial.println_P(PSTR("not healthy"));
+            }
 
 			if(Serial.available() > 0){
 				return (0);

ArduPlane/ArduPlane.pde

@@ -29,7 +29,7 @@ version 2.1 of the License, or (at your option) any later version.
 #include <Arduino_Mega_ISR_Registry.h>
 #include <APM_RC.h>         // ArduPilot Mega RC Library
 #include <AP_GPS.h>         // ArduPilot GPS library
-#include <Wire.h>			// Arduino I2C lib
+#include <I2C.h>			// Wayne Truchsess I2C lib
 #include <SPI.h>			// Arduino SPI lib
 #include <DataFlash.h>      // ArduPilot Mega Flash Memory Library
 #include <AP_ADC.h>         // ArduPilot Mega Analog to Digital Converter Library
@@ -621,8 +621,7 @@ static void medium_loop()
             }
 
 			#if HIL_MODE != HIL_MODE_ATTITUDE
-				if(g.compass_enabled){
+            if (g.compass_enabled && compass.read()) {
-					compass.read();     // Read magnetometer
                 compass.calculate(dcm.get_dcm_matrix());  // Calculate heading
                 compass.null_offsets(dcm.get_dcm_matrix());
             }
@@ -979,8 +978,12 @@ static void update_alt()
 		// this function is in place to potentially add a sonar sensor in the future
 		//altitude_sensor = BARO;
 
+        if (barometer.healthy) {
             current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude;			// alt_MSL centimeters (meters * 100)
             current_loc.alt += g.altitude_mix * (read_barometer() + home.alt);
+        } else if (g_gps->fix) {
+            current_loc.alt = g_gps->altitude; // alt_MSL centimeters (meters * 100)            
+        }
 	#endif
 
         geofence_check(true);

ArduPlane/geofence.pde

@@ -235,13 +235,12 @@ static void geofence_check(bool altitude_check_only)
     }
 
     // we are outside the fence
-    if (geofence_state->fence_triggered) {
+    if (geofence_state->fence_triggered && control_mode == GUIDED) {
         // we have already triggered, don't trigger again until the
         // user disables/re-enables using the fence channel switch
         return;
     }
 
-
     // we are outside, and have not previously triggered.
     geofence_state->fence_triggered = true;
     geofence_state->breach_count++;

ArduPlane/sensors.pde

@@ -11,7 +11,7 @@ static void init_barometer(void)
 	long 			ground_pressure = 0;
 	int 			ground_temperature;
 
-	while(ground_pressure == 0){
+	while (ground_pressure == 0 || !barometer.healthy) {
 		barometer.read(); 					// Get initial data from absolute pressure sensor
 		ground_pressure 	= barometer.get_pressure();
 		ground_temperature 	= barometer.get_temperature();
@@ -25,7 +25,9 @@ static void init_barometer(void)
         gcs_update(); 				// look for inbound hil packets
 		#endif
         
-		barometer.read(); 				// Get initial data from absolute pressure sensor
+        do {
+            barometer.read(); 				// Get pressure sensor
+        } while (!barometer.healthy);
 		ground_pressure		= (ground_pressure * 9l   + barometer.get_pressure()) / 10l;
 		ground_temperature	= (ground_temperature * 9 + barometer.get_temperature()) / 10;
 
@@ -59,7 +61,6 @@ static long read_barometer(void)
 
 	barometer.read();		// Get new data from absolute pressure sensor
 
-
 	//abs_pressure 			= (abs_pressure + barometer.get_pressure()) >> 1;		// Small filtering
 	abs_pressure 			= ((float)abs_pressure * .7) + ((float)barometer.get_pressure() * .3);		// large filtering
 	scaling 				= (float)g.ground_pressure / (float)abs_pressure;

ArduPlane/system.pde

@@ -107,7 +107,8 @@ static void init_ardupilot()
 	// Initialize Wire and SPI libraries
 	//
 #ifndef DESKTOP_BUILD
-    Wire.begin();
+    I2c.begin();
+    I2c.timeOut(20);
 #endif
     SPI.begin();
     SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate

ArduPlane/test.pde

@@ -547,8 +547,9 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 			if(g.compass_enabled) {
 				medium_loopCounter++;
 				if(medium_loopCounter == 5){
-					compass.read();		 				// Read magnetometer
+					if (compass.read()) {
                         compass.calculate(dcm.get_dcm_matrix());		// Calculate heading
+                    }
                     medium_loopCounter = 0;
 				}
 			}
@@ -610,14 +611,16 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 
             medium_loopCounter++;
             if(medium_loopCounter == 5){
-                compass.read();		 				// Read magnetometer
+                if (compass.read()) {
                     compass.calculate(dcm.get_dcm_matrix());		// Calculate heading
                     compass.null_offsets(dcm.get_dcm_matrix());
+                }
                 medium_loopCounter = 0;
             }
 
 			counter++;
 			if (counter>20) {
+                if (compass.healthy) {
                     Vector3f maggy = compass.get_offsets();
                     Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
                                     (wrap_360(ToDeg(compass.heading) * 100)) /100,
@@ -627,6 +630,9 @@ test_mag(uint8_t argc, const Menu::arg *argv)
                                     maggy.x,
                                     maggy.y,
                                     maggy.z);
+                } else {
+                    Serial.println_P(PSTR("compass not healthy"));
+                }
                 counter=0;
             }
 		}
@@ -694,9 +700,13 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
 		delay(100);
 		current_loc.alt = read_barometer() + home.alt;
 
+        if (!barometer.healthy) {
+            Serial.println_P(PSTR("not healthy"));
+        } else {
             Serial.printf_P(PSTR("Alt: %0.2fm, Raw: %ld Temperature: %.1f\n"),
                             current_loc.alt / 100.0,
                             abs_pressure, 0.1*barometer.get_temperature());
+        }
 
 		if(Serial.available() > 0){
 			return (0);

libraries/AP_Baro/AP_Baro.h

@@ -1,3 +1,4 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 #ifndef __AP_BARO_H__
 #define __AP_BARO_H__
@@ -7,8 +8,9 @@
 class AP_Baro
 {
     public:
+	bool healthy;
 	AP_Baro() {}
-    virtual void    init(AP_PeriodicProcess *scheduler)=0;
+	virtual bool    init(AP_PeriodicProcess *scheduler)=0;
 	virtual uint8_t read() = 0;
 	virtual int32_t get_pressure() = 0;
 	virtual int16_t get_temperature() = 0;

libraries/AP_Baro/AP_Baro_BMP085.cpp

@@ -1,3 +1,4 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 /*
 	APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor
 	Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com
@@ -41,7 +42,9 @@ extern "C" {
   #include "WConstants.h"
 }
 
-#include <Wire.h>
+#include <AP_Common.h>
+#include <AP_Math.h>		// ArduPilot Mega Vector/Matrix math Library
+#include <I2C.h>
 #include "AP_Baro_BMP085.h"
 
 #define BMP085_ADDRESS 0x77  //(0xEE >> 1)
@@ -55,10 +58,9 @@ extern "C" {
 
 
 // Public Methods //////////////////////////////////////////////////////////////
-void AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
+bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
 {
 	byte buff[22];
-	int i = 0;
 	
 	pinMode(BMP085_EOC, INPUT);	 // End Of Conversion (PC7) input
 
@@ -66,23 +68,9 @@ void AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
 	BMP085_State = 0;		 // Initial state
 
 	// We read the calibration data registers
-	Wire.beginTransmission(BMP085_ADDRESS);
+	if (I2c.read(BMP085_ADDRESS, 0xAA, 22, buff) != 0) {
-	Wire.send(0xAA);
+		healthy = false;
-	if (Wire.endTransmission() != 0) {
+		return false;
-        // Error!
-		return;
-	}
-
-	Wire.requestFrom(BMP085_ADDRESS, 22);
-
-  //Wire.endTransmission();
-	while(Wire.available()){
-		buff[i] = Wire.receive();	// receive one byte
-		i++;
-	}
-	if (i != 22) {
-        // Error!
-		return;
 	}
 
 	ac1 = ((int)buff[0] << 8) | buff[1];
@@ -100,7 +88,9 @@ void AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
 	//Send a command to read Temp
 	Command_ReadTemp();
 	BMP085_State = 1;
-	return;
+
+	healthy = true;
+	return true;
 }
 
 // Read the sensor. This is a state machine
@@ -152,43 +142,22 @@ int32_t AP_Baro_BMP085::get_raw_temp() {
 // Send command to Read Pressure
 void AP_Baro_BMP085::Command_ReadPress()
 {
-	Wire.beginTransmission(BMP085_ADDRESS);
+	if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(oss << 6)) != 0) {
-	Wire.send(0xF4);
+		healthy = false;
-	Wire.send(0x34+(oss << 6));	// write_register(0xF4, 0x34+(oversampling_setting << 6));
+	}
-	Wire.endTransmission();
 }
 
 // Read Raw Pressure values
 void AP_Baro_BMP085::ReadPress()
 {
-	byte msb;
+	uint8_t buf[3];
-	byte lsb;
-	byte xlsb;
 
-	Wire.beginTransmission(BMP085_ADDRESS);
+	if (I2c.read(BMP085_ADDRESS, 0xF6, 3, buf) != 0) {
-	Wire.send(0xF6);
+		healthy = false;
-	Wire.endTransmission();
+		return;
-
-	Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte
-
-	while(!Wire.available()) {
-    // waiting
 	}
 
-	msb = Wire.receive();
+	RawPress = (((long)buf[0] << 16) | ((long)buf[1] << 8) | ((long)buf[2])) >> (8 - oss);
-
-	while(!Wire.available()) {
-    // waiting
-	}
-
-	lsb = Wire.receive();
-
-	while(!Wire.available()) {
-    // waiting
-	}
-
-	xlsb = Wire.receive();
-	RawPress = (((long)msb << 16) | ((long)lsb << 8) | ((long)xlsb)) >> (8 - oss);
 
 	if(_offset_press == 0){
 		_offset_press = RawPress;
@@ -203,6 +172,7 @@ void AP_Baro_BMP085::ReadPress()
 		_press_index = 0;
 
 	RawPress = 0;
+
 	// sum our filter
 	for (uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){
 		RawPress += _press_filter[i];
@@ -217,30 +187,22 @@ void AP_Baro_BMP085::ReadPress()
 // Send Command to Read Temperature
 void AP_Baro_BMP085::Command_ReadTemp()
 {
-	Wire.beginTransmission(BMP085_ADDRESS);
+	if (I2c.write(BMP085_ADDRESS, 0xF4, 0x2E) != 0) {
-	Wire.send(0xF4);
+		healthy = false;
-	Wire.send(0x2E);
+	}
-	Wire.endTransmission();
 }
 
 // Read Raw Temperature values
 void AP_Baro_BMP085::ReadTemp()
 {
-	byte tmp;
+	uint8_t buf[2];
-	Wire.beginTransmission(BMP085_ADDRESS);
-	Wire.send(0xF6);
-	Wire.endTransmission();
 
-	Wire.beginTransmission(BMP085_ADDRESS);
+	if (I2c.read(BMP085_ADDRESS, 0xF6, 2, buf) != 0) {
-	Wire.requestFrom(BMP085_ADDRESS,2);
+		healthy = false;
-
+		return;
-	while(!Wire.available());	// wait
+	}
-	RawTemp = Wire.receive();
+	RawTemp = buf[0];
-
+	RawTemp = (RawTemp << 8) | buf[1];
-	while(!Wire.available());	// wait
-	tmp 	= Wire.receive();
-
-	RawTemp = RawTemp << 8 | tmp;
 
 	if (_offset_temp == 0){
 		_offset_temp = RawTemp;

libraries/AP_Baro/AP_Baro_BMP085.h

@@ -1,3 +1,4 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 #ifndef __AP_BARO_BMP085_H__
 #define __AP_BARO_BMP085_H__
 
@@ -16,7 +17,7 @@ class AP_Baro_BMP085 : public AP_Baro
 
 
     /* AP_Baro public interface: */
-    void    init(AP_PeriodicProcess * scheduler);
+    bool    init(AP_PeriodicProcess * scheduler);
     uint8_t read();
     int32_t get_pressure();
     int16_t get_temperature();

libraries/AP_Baro/AP_Baro_BMP085_hil.cpp

@@ -1,3 +1,4 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 extern "C" {
   // AVR LibC Includes
@@ -14,9 +15,10 @@ AP_Baro_BMP085_HIL::AP_Baro_BMP085_HIL()
 }
 
 // Public Methods //////////////////////////////////////////////////////////////
-void AP_Baro_BMP085_HIL::init(AP_PeriodicProcess * scheduler)
+bool AP_Baro_BMP085_HIL::init(AP_PeriodicProcess * scheduler)
 {
 	BMP085_State=1;
+	return true;
 }
 
 
@@ -46,6 +48,7 @@ void AP_Baro_BMP085_HIL::setHIL(float _Temp, float _Press)
     // TODO: map floats to raw
 	Temp 	= _Temp;
 	Press 	= _Press;
+	healthy = true;
 }
 
 int32_t AP_Baro_BMP085_HIL::get_pressure() {

libraries/AP_Baro/AP_Baro_BMP085_hil.h

@@ -1,3 +1,4 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 #ifndef __AP_BARO_BMP085_HIL_H__
 #define __AP_BARO_BMP085_HIL_H__
@@ -14,8 +15,10 @@ class AP_Baro_BMP085_HIL
 public:
     AP_Baro_BMP085_HIL();  // Constructor
 	//int Altitude;
+    bool healthy;
+
 	uint8_t oss;
-	void init(AP_PeriodicProcess * scheduler);
+	bool init(AP_PeriodicProcess * scheduler);
 	uint8_t read();
 	int32_t get_pressure();
 	int16_t get_temperature();

libraries/AP_Baro/AP_Baro_MS5611.cpp

@@ -1,3 +1,4 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 /*
 	APM_MS5611.cpp - Arduino Library for MS5611-01BA01 absolute pressure sensor
 	Code by Jose Julio, Pat Hickey and Jordi Muñoz. DIYDrones.com
@@ -110,7 +111,7 @@ void AP_Baro_MS5611::_spi_write(uint8_t reg)
 
 // Public Methods //////////////////////////////////////////////////////////////
 // SPI should be initialized externally
-void AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
+bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
 {
 	pinMode(MS5611_CS, OUTPUT);	 // Chip select Pin
 	digitalWrite(MS5611_CS, HIGH);
@@ -136,6 +137,9 @@ void AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
 	Press=0;
 	
 	scheduler->register_process( AP_Baro_MS5611::_update );
+
+	healthy = true;
+    return true;
 }
 
 

libraries/AP_Baro/AP_Baro_MS5611.h

@@ -1,3 +1,4 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 #ifndef __AP_BARO_MS5611_H__
 #define __AP_BARO_MS5611_H__
@@ -10,7 +11,7 @@ class AP_Baro_MS5611 : public AP_Baro
   AP_Baro_MS5611() {}  // Constructor
 
   /* AP_Baro public interface: */
-  void init(AP_PeriodicProcess *scheduler);
+  bool init(AP_PeriodicProcess *scheduler);
   uint8_t read();
   int32_t get_pressure();     // in mbar*100 units
   int16_t get_temperature();  // in celsius degrees * 100 units

libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde

@@ -4,52 +4,74 @@
 */
 
 #include <FastSerial.h>
-#include <Wire.h>
+#include <I2C.h>
-#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
+#include <SPI.h>
+#include <AP_Baro.h> // ArduPilot Mega BMP085 Library
+#include <Arduino_Mega_ISR_Registry.h>
+#include <AP_PeriodicProcess.h>
+#include <AP_InertialSensor.h>
+#include <AP_Math.h>
+#include <AP_Common.h>
 
-APM_BMP085_Class APM_BMP085;
+#ifndef APM2_HARDWARE
+# define APM2_HARDWARE 0
+#endif
+
+AP_Baro_BMP085 APM_BMP085(APM2_HARDWARE);
 
 unsigned long timer;
 
 FastSerialPort0(Serial);
 
-#ifdef APM2_HARDWARE
+
-static bool apm2_hardware = true;
+Arduino_Mega_ISR_Registry isr_registry;
-#else
+AP_TimerProcess  scheduler;
-static bool apm2_hardware = false;
-#endif
 
 void setup()
 {	
 	Serial.begin(115200);
 	Serial.println("ArduPilot Mega BMP085 library test");
 	Serial.println("Initialising barometer..."); delay(100);
-	if (!APM_BMP085.Init(1, apm2_hardware)) {
+
+	I2c.begin();
+	I2c.timeOut(20);
+
+	//I2c.setSpeed(true);
+
+	isr_registry.init();
+	scheduler.init(&isr_registry);
+
+	if (!APM_BMP085.init(&scheduler)) {
 		Serial.println("Barometer initialisation FAILED\n");
 	}
 	Serial.println("initialisation complete."); delay(100);
 	delay(1000);
-	timer = millis();
+	timer = micros();
 }
 
 void loop()
 {
-	int ch;
 	float tmp_float;
 	float Altitude;
 	
-	if((millis()- timer) > 50){
+	if((micros()- timer) > 50000L){
-		timer = millis();
+		timer = micros();
-		APM_BMP085.Read();
+		APM_BMP085.read();
+		unsigned long read_time = micros() - timer;
+		if (!APM_BMP085.healthy) {
+			Serial.println("not healthy");
+			return;
+		}
 		Serial.print("Pressure:");
-		Serial.print(APM_BMP085.Press);
+		Serial.print(APM_BMP085.get_pressure());
 		Serial.print(" Temperature:");
-		Serial.print(APM_BMP085.Temp / 10.0);
+		Serial.print(APM_BMP085.get_temperature());
 		Serial.print(" Altitude:");
-		tmp_float = (APM_BMP085.Press / 101325.0);
+		tmp_float = (APM_BMP085.get_pressure() / 101325.0);
 		tmp_float = pow(tmp_float, 0.190295);
 		Altitude = 44330 * (1.0 - tmp_float);
 		Serial.print(Altitude);
+		Serial.printf(" t=%u", (unsigned)read_time);
 		Serial.println();
 	}
 }

libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde

@@ -1,12 +1,21 @@
 
 #include <stdint.h>
+#include <AP_Common.h>
+#include <AP_Math.h>		// ArduPilot Mega Vector/Matrix math Library
 #include <FastSerial.h>
+#include <I2C.h>
 #include <SPI.h>
+#include <Arduino_Mega_ISR_Registry.h>
+#include <AP_PeriodicProcess.h>
 #include <AP_Baro.h> // ArduPilot Mega ADC Library
 
 FastSerialPort0(Serial);
 
 AP_Baro_MS5611 baro;
+Arduino_Mega_ISR_Registry isr_registry;
+AP_TimerProcess  scheduler;
+
+unsigned long timer;
 
 void setup()
 {  
@@ -15,54 +24,41 @@ void setup()
 
 	delay(1000);
 
+	isr_registry.init();
+	scheduler.init(&isr_registry);
+
 	pinMode(63, OUTPUT);
 	digitalWrite(63, HIGH);
 	SPI.begin();
 	SPI.setClockDivider(SPI_CLOCK_DIV32); // 500khz for debugging, increase later
 
-  baro.init();
+	baro.init(&scheduler);
+	timer = micros();
 }
 
 void loop()
 {
-    int32_t pres;
+	float tmp_float;
-    int32_t temp;
+	float Altitude;
 	
-    Serial.println("Start Conversions");
+	if((micros()- timer) > 50000L){
-
+		timer = micros();
-    baro._start_conversion_D1();
+		baro.read();
-    delay(10);
+		unsigned long read_time = micros() - timer;
-    bool res1 = baro._adc_read(&pres);
+		if (!baro.healthy) {
-    baro._start_conversion_D2();
+			Serial.println("not healthy");
-    delay(10);
+			return;
-    bool res2 = baro._adc_read(&temp);
-
-    if (res1) {
-        Serial.printf("Pressure raw value %ld\r\n",pres);
-    } else {
-        Serial.println("ADC conversion D1 unsuccessful");
 		}
-
+		Serial.print("Pressure:");
-    if (res2) {
+		Serial.print(baro.get_pressure());
-        Serial.printf("Temp raw value %ld\r\n",pres);
+		Serial.print(" Temperature:");
-    } else {
+		Serial.print(baro.get_temperature());
-        Serial.println("ADC conversion D2 unsuccessful");
+		Serial.print(" Altitude:");
+		tmp_float = (baro.get_pressure() / 101325.0);
+		tmp_float = pow(tmp_float, 0.190295);
+		Altitude = 44330 * (1.0 - tmp_float);
+		Serial.print(Altitude);
+		Serial.printf(" t=%u", (unsigned)read_time);
+		Serial.println();
 	}
-    
-    Serial.println("---");
-    delay(250);
-}
-
-void update_and_print()
-{
-    int32_t pres;
-    float temp;
-
-    baro.update();
-
-    pres = baro.get_pressure();
-    temp = baro.get_temp();
-    Serial.printf("p: %ld t: %f \r\n", pres, temp);
-
-    delay(100);
 }

libraries/AP_Compass/AP_Compass_HIL.cpp

@@ -13,9 +13,10 @@
 
 // Public Methods //////////////////////////////////////////////////////////////
 
-void AP_Compass_HIL::read()
+bool AP_Compass_HIL::read()
 {
 	// values set by setHIL function
+	return true;
 }
 
 // Update raw magnetometer values from HIL data
@@ -26,4 +27,5 @@ void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
     mag_x = _mag_x;
     mag_y = _mag_y;
     mag_z = _mag_z;
+    healthy = true;
 }

libraries/AP_Compass/AP_Compass_HIL.h

@@ -1,3 +1,4 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 #ifndef AP_Compass_HIL_H
 #define AP_Compass_HIL_H
 
@@ -7,8 +8,7 @@ class AP_Compass_HIL : public Compass
 {
   public:
 	AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) { product_id = AP_COMPASS_TYPE_HIL; }
-
+	bool read(void);
-	void 			read();
 	void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
 };
 

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -16,8 +16,9 @@
 // AVR LibC Includes
 #include <math.h>
 #include "WConstants.h"
+#include <FastSerial.h>
 
-#include <Wire.h>
+#include <I2C.h>
 #include "AP_Compass_HMC5843.h"
 
 #define COMPASS_ADDRESS       0x1E
@@ -47,39 +48,22 @@
 #define DataOutputRate_75HZ   0x06
 
 // read_register - read a register value
-static bool
+bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
-read_register(int address, byte *value)
 {
-  bool ret = false;
+   if (I2c.read((uint8_t)COMPASS_ADDRESS, address, 1, value) != 0) {
-
+	  healthy = false;
-  *value = 0;
-
-  Wire.beginTransmission(COMPASS_ADDRESS);
-  Wire.send(address);     //sends address to read from
-  if (0 != Wire.endTransmission())
 	  return false;
-
-  Wire.requestFrom(COMPASS_ADDRESS, 1);    // request 1 byte from device
-  if( Wire.available() ) {
-      *value = Wire.receive();  // receive one byte
-	  ret = true;
    }
-  if (0 != Wire.endTransmission())
+   return true;
-	 return false;
-  
-  return ret;
 }
 
 // write_register - update a register value
-static bool
+bool AP_Compass_HMC5843::write_register(uint8_t address, byte value)
-write_register(int address, byte value)
 {
-  Wire.beginTransmission(COMPASS_ADDRESS);
+   if (I2c.write((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
-  Wire.send(address);
+	  healthy = false;
-  Wire.send(value);
-  if (0 != Wire.endTransmission())
 	  return false;
-  delay(10);
+   }
    return true;
 }
 
@@ -96,25 +80,10 @@ static void rotate_for_5883L(AP_VarS<Matrix3f> *_orientation_matrix)
 // Read Sensor data
 bool AP_Compass_HMC5843::read_raw()
 {
-   int i = 0;
+   uint8_t buff[6];
-   byte buff[6];
 
-   Wire.beginTransmission(COMPASS_ADDRESS);
+   if (I2c.read(COMPASS_ADDRESS, 0x03, 6, buff) != 0) {
-   Wire.send(0x03);        //sends address to read from
+	  healthy = false;
-   if (0 != Wire.endTransmission())
-	  return false;
-
-   Wire.requestFrom(COMPASS_ADDRESS, 6);    // request 6 bytes from device
-   while (Wire.available()) {
-	  buff[i] = Wire.receive();  // receive one byte
-	  i++;
-   }
-
-   if (0 != Wire.endTransmission())
-	  return false;
-
-   if (i != 6) {
-	  /* we didn't get enough bytes */
 	  return false;
    }
 
@@ -140,13 +109,26 @@ bool AP_Compass_HMC5843::read_raw()
 }
 
 
+
+/*
+  re-initialise after a IO error
+ */
+bool AP_Compass_HMC5843::re_initialise()
+{
+   if (! write_register(ConfigRegA, _base_config) ||
+	   ! write_register(ConfigRegB, magGain) ||
+	   ! write_register(ModeRegister, ContinuousConversion))
+	  return false;
+   return true;
+}
+
+
 // Public Methods //////////////////////////////////////////////////////////////
 bool 
 AP_Compass_HMC5843::init()
 {
   int numAttempts = 0, good_count = 0;
   bool success = false;
-  byte base_config;  // used to test compass type
   byte calibration_gain = 0x20;
   uint16_t expected_x = 715;
   uint16_t expected_yz = 715;
@@ -156,10 +138,11 @@ AP_Compass_HMC5843::init()
 
   // determine if we are using 5843 or 5883L
   if (! write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
-	  ! read_register(ConfigRegA, &base_config)) {
+	  ! read_register(ConfigRegA, &_base_config)) {
+	 healthy = false;
 	 return false;
   }
-  if ( base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
+  if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
 	 // a 5883L supports the sample averaging config
 	 int old_product_id = product_id;
 
@@ -175,7 +158,7 @@ AP_Compass_HMC5843::init()
 		 */
 		rotate_for_5883L(&_orientation_matrix);
 	 }
-  } else if (base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
+  } else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
       product_id = AP_COMPASS_TYPE_HMC5843;
   } else {
 	 // not behaving like either supported compass type
@@ -253,23 +236,22 @@ AP_Compass_HMC5843::init()
   }
 
   // leave test mode
-  if (! write_register(ConfigRegA, base_config))
+  if (!re_initialise()) {
 	 return false;
-  delay(50);
+  }
-  if (! write_register(ConfigRegB, magGain) ||
-	  ! write_register(ModeRegister, ContinuousConversion))
-	 return false;
-  delay(50);
 
   return success;
 }
 
 // Read Sensor data
-void 
+bool AP_Compass_HMC5843::read()
-AP_Compass_HMC5843::read()
 {
+   if (!healthy && !re_initialise()) {
+	  return false;
+   }
+
    if (!read_raw()) {
-	  return;
+	  return false;
    }
 
    mag_x *= calibration[0];
@@ -285,6 +267,9 @@ AP_Compass_HMC5843::read()
    mag_x = rot_mag.x;
    mag_y = rot_mag.y;
    mag_z = rot_mag.z;
+   healthy = true;
+
+   return true;
 }
 
 // set orientation

libraries/AP_Compass/AP_Compass_HMC5843.h

@@ -1,3 +1,4 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 #ifndef AP_Compass_HMC5843_H
 #define AP_Compass_HMC5843_H
 
@@ -47,11 +48,16 @@ class AP_Compass_HMC5843 : public Compass
 {
   private:
 	float calibration[3];
+	virtual bool read_raw(void);
+    uint8_t _base_config;
+	virtual bool re_initialise(void);
+    bool read_register(uint8_t address, uint8_t *value);
+    bool write_register(uint8_t address, byte value);
+
   public:
 	AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
-	virtual bool init();
+	virtual bool init(void);
-	virtual void read();
+	virtual bool read(void);
-	virtual bool read_raw();
 	virtual void set_orientation(const Matrix3f &rotation_matrix);
 
 };

libraries/AP_Compass/Compass.cpp

@@ -1,3 +1,4 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 #include "Compass.h"
 
 // Default constructor.

libraries/AP_Compass/Compass.h

@@ -1,3 +1,4 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 #ifndef Compass_h
 #define Compass_h
 
@@ -40,6 +41,7 @@ public:
 	float           heading_x;      ///< compass vector X magnitude
 	float           heading_y;      ///< compass vector Y magnitude
 	unsigned long   last_update;    ///< millis() time of last update
+	bool		healthy;        ///< true if last read OK
 
 	/// Constructor
 	///
@@ -56,7 +58,7 @@ public:
 	
 	/// Read the compass and update the mag_ variables.
 	///
-	virtual void read() = 0;
+	virtual bool read(void) = 0;
 
 	/// Calculate the tilt-compensated heading_ variables.
 	///

libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde

@@ -7,7 +7,7 @@
 #include <AP_Common.h>
 #include <AP_Compass.h> // Compass Library
 #include <AP_Math.h>		// ArduPilot Mega Vector/Matrix math Library
-#include <Wire.h>
+#include <I2C.h>
 
 FastSerialPort0(Serial);
 
@@ -23,7 +23,11 @@ void setup()
 {
   Serial.begin(115200);
   Serial.println("Compass library test (HMC5843 and HMC5883L)");
-  Wire.begin();
+  I2c.begin();
+  I2c.timeOut(20);
+
+  // I2c.setSpeed(true);
+
   if (!compass.init()) {
 	  Serial.println("compass initialisation failed!");
 	  while (1) ;
@@ -50,17 +54,23 @@ void setup()
   }
   
   delay(3000);
-  timer = millis();
+  timer = micros();
 }
 
 void loop()
 {
   static float min[3], max[3], offset[3];
 
-  if((millis()- timer) > 100)
+  if((micros()- timer) > 100000L)
   {
-    timer = millis();
+    timer = micros();
     compass.read();
+    unsigned long read_time = micros() - timer;
+
+    if (!compass.healthy) {
+	    Serial.println("not healthy");
+	    return;
+    }
     compass.calculate(0,0);  // roll = 0, pitch = 0 for this example
 
     // capture min
@@ -85,24 +95,17 @@ void loop()
     offset[2] = -(max[2]+min[2])/2;
 
     // display all to user
-    Serial.print("Heading:");
+    Serial.printf("Heading: %.2f (%3u,%3u,%3u) ",
-    Serial.print(ToDeg(compass.heading));
+		  ToDeg(compass.heading),
-    Serial.print("  (");
+		  compass.mag_x,
-    Serial.print(compass.mag_x);
+		  compass.mag_y,
-    Serial.print(",");
+		  compass.mag_z);
-    Serial.print(compass.mag_y);
-    Serial.print(",");
-    Serial.print(compass.mag_z);
-    Serial.print(")");
 
     // display offsets
-    Serial.print("\t offsets(");
+    Serial.printf("\t offsets(%.2f, %.2f, %.2f)",
-    Serial.print(offset[0]);
+		  offset[0], offset[1], offset[2]);
-    Serial.print(",");
+
-    Serial.print(offset[1]);
+    Serial.printf(" t=%u", (unsigned)read_time);
-    Serial.print(",");
-    Serial.print(offset[2]);
-    Serial.print(")");
 
     Serial.println();
   }

libraries/AP_Compass/examples/AP_Compass_test/Makefile

@@ -1,2 +1 @@
-BOARD	=	mega
 include ../../../AP_Common/Arduino.mk

libraries/AP_DCM/AP_DCM.cpp

@@ -309,7 +309,7 @@ AP_DCM::drift_correction(void)
 
 	//*****YAW***************
 
-	if (_compass) {
+	if (_compass && _compass->healthy) {
 		// We make the gyro YAW drift correction based on compass magnetic heading
 		error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);	// Equation 23, Calculating YAW error
 

libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde

@@ -5,7 +5,6 @@
 //
 
 #include <FastSerial.h>
-#include <Wire.h>
 #include <SPI.h>
 #include <Arduino_Mega_ISR_Registry.h>
 #include <AP_PeriodicProcess.h>
@@ -24,7 +23,6 @@ void setup(void)
 	Serial.begin(115200);
 	Serial.println("Doing INS startup...");
 
-	Wire.begin();
     SPI.begin();
     SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
 

libraries/Desktop/Desktop.mk

@@ -185,7 +185,7 @@ else
 endif
 
 # these are library objects we don't want in the desktop build (maybe we'll add them later)
-NODESKTOP		:= DataFlash/DataFlash_APM1.cpp FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp AP_Baro/AP_Baro_BMP085.cpp
+NODESKTOP		:= I2C/I2C.cpp DataFlash/DataFlash_APM1.cpp FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp AP_Baro/AP_Baro_BMP085.cpp
 
 #
 # Find sketchbook libraries referenced by the sketch.

libraries/I2C/I2C.cpp

@@ -0,0 +1,557 @@
+/*
+  I2C.cpp - I2C library
+  Copyright (c) 2011 Wayne Truchsess.  All right reserved.
+  Rev 2.0 - September 19th, 2011
+          - Added support for timeout function to prevent 
+            and recover from bus lockup (thanks to PaulS
+            and CrossRoads on the Arduino forum)
+          - Changed return type for stop() from void to
+            uint8_t to handle timeOut function 
+  Rev 1.0 - August 8th, 2011
+  
+  This is a modified version of the Arduino Wire/TWI 
+  library.  Functions were rewritten to provide more functionality
+  and also the use of Repeated Start.  Some I2C devices will not
+  function correctly without the use of a Repeated Start.  The 
+  initial version of this library only supports the Master.
+
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include "WProgram.h"
+#include <inttypes.h>
+#include "I2C.h"
+
+
+
+uint8_t I2C::bytesAvailable = 0;
+uint8_t I2C::bufferIndex = 0;
+uint8_t I2C::totalBytes = 0;
+uint16_t I2C::timeOutDelay = 0;
+
+I2C::I2C()
+{
+}
+
+
+////////////// Public Methods ////////////////////////////////////////
+
+
+
+void I2C::begin()
+{
+  #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
+    // activate internal pull-ups for twi
+    // as per note from atmega8 manual pg167
+    sbi(PORTC, 4);
+    sbi(PORTC, 5);
+  #else
+    // activate internal pull-ups for twi
+    // as per note from atmega128 manual pg204
+    sbi(PORTD, 0);
+    sbi(PORTD, 1);
+  #endif
+  // initialize twi prescaler and bit rate
+  cbi(TWSR, TWPS0);
+  cbi(TWSR, TWPS1);
+  TWBR = ((CPU_FREQ / 100000) - 16) / 2;
+  // enable twi module, acks, and twi interrupt
+  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);
+}
+
+void I2C::end()
+{
+  TWCR = 0;
+}
+
+void I2C::timeOut(uint16_t _timeOut)
+{
+  timeOutDelay = _timeOut;
+}
+
+void I2C::setSpeed(boolean _fast)
+{
+  if(!_fast)
+  {
+    TWBR = ((CPU_FREQ / 100000) - 16) / 2;
+  }
+  else
+  {
+    TWBR = ((CPU_FREQ / 400000) - 16) / 2;
+  }
+}
+  
+void I2C::pullup(boolean activate)
+{
+  if(activate)
+  {
+    #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
+      // activate internal pull-ups for twi
+      // as per note from atmega8 manual pg167
+      sbi(PORTC, 4);
+      sbi(PORTC, 5);
+    #else
+      // activate internal pull-ups for twi
+      // as per note from atmega128 manual pg204
+      sbi(PORTD, 0);
+      sbi(PORTD, 1);
+    #endif
+  }
+  else
+  {
+    #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
+      // deactivate internal pull-ups for twi
+      // as per note from atmega8 manual pg167
+      cbi(PORTC, 4);
+      cbi(PORTC, 5);
+    #else
+      // deactivate internal pull-ups for twi
+      // as per note from atmega128 manual pg204
+      cbi(PORTD, 0);
+      cbi(PORTD, 1);
+    #endif
+  }
+}
+
+/////////////carry over from Wire library ///////////
+
+uint8_t I2C::beginTransmission(uint8_t address)
+{
+  returnStatusWire = 0;
+  returnStatus = 0;
+  returnStatus = start();
+  returnStatusWire = returnStatus;
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendAddress(SLA_W(address));
+  returnStatusWire = returnStatus;
+  return(returnStatus);
+}
+
+uint8_t I2C::beginTransmission(int address)
+{
+  return(beginTransmission((uint8_t) address));
+}
+
+uint8_t I2C::send(uint8_t databyte)
+{
+  if(returnStatusWire)
+  {
+    return(returnStatusWire);
+  }
+  returnStatus = 0;
+  returnStatus = sendByte(databyte);
+  returnStatusWire = returnStatus;
+  return(returnStatus);
+}
+
+uint8_t I2C::send(int databyte)
+{
+  return(send((uint8_t) databyte));
+}
+
+uint8_t I2C::endTransmission()
+{
+  stop();
+  return(returnStatusWire);
+}
+
+uint8_t I2C::requestFrom(int address, int numberBytes)
+{
+  return(requestFrom((uint8_t) address, (uint8_t) numberBytes));
+}
+
+uint8_t I2C::requestFrom(uint8_t address, uint8_t numberBytes)
+{
+  returnStatus = 0;
+  returnStatus = read(address,numberBytes);
+  if(!returnStatus)
+  {
+    return(numberBytes);
+  }
+  return(0);
+}
+
+uint8_t I2C::available()
+{
+  return(bytesAvailable);
+}
+
+uint8_t I2C::receive()
+{
+  bufferIndex = totalBytes - bytesAvailable;
+  if(!bytesAvailable)
+  {
+    bufferIndex = 0;
+    return(0);
+  }
+  bytesAvailable--;
+  return(data[bufferIndex]);
+}
+
+  
+
+
+
+/////////////////////////////////////////////////////
+
+uint8_t I2C::write(uint8_t address, uint8_t registerAddress)
+{
+  returnStatus = 0;
+  returnStatus = start();
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendAddress(SLA_W(address));
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendByte(registerAddress);
+  if(returnStatus){return(returnStatus);}
+  returnStatus = stop();
+  return(returnStatus);
+}
+
+uint8_t I2C::write(int address, int registerAddress)
+{
+  return(write((uint8_t) address, (uint8_t) registerAddress));
+}
+
+uint8_t I2C::write(uint8_t address, uint8_t registerAddress, uint8_t databyte)
+{
+  returnStatus = 0;
+  returnStatus = start(); 
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendAddress(SLA_W(address));
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendByte(registerAddress);
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendByte(databyte);
+  if(returnStatus){return(returnStatus);}
+  returnStatus = stop();
+  return(returnStatus);
+}
+
+uint8_t I2C::write(int address, int registerAddress, int databyte)
+{
+  return(write((uint8_t) address, (uint8_t) registerAddress, (uint8_t) databyte));
+}
+
+uint8_t I2C::write(uint8_t address, uint8_t registerAddress, char *databytes)
+{
+  uint8_t bufferLength = strlen(databytes);
+  returnStatus = 0;
+  returnStatus = write(address, registerAddress, (uint8_t*)databytes, bufferLength);
+  return(returnStatus);
+}
+
+uint8_t I2C::write(uint8_t address, uint8_t registerAddress, uint8_t *databytes, uint8_t numberBytes)
+{
+  returnStatus = 0;
+  returnStatus = start();
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendAddress(SLA_W(address));
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendByte(registerAddress);
+  if(returnStatus){return(returnStatus);}
+  for (uint8_t i = 0; i < numberBytes; i++)
+  {
+    returnStatus = sendByte(databytes[i]);
+    if(returnStatus){return(returnStatus);}
+  }
+  returnStatus = stop();
+  return(returnStatus);
+}
+
+uint8_t I2C::read(int address, int numberBytes)
+{
+  return(read((uint8_t) address, (uint8_t) numberBytes));
+}
+
+uint8_t I2C::read(uint8_t address, uint8_t numberBytes)
+{
+  bytesAvailable = 0;
+  bufferIndex = 0;
+  if(numberBytes == 0){numberBytes++;}
+  nack = numberBytes - 1;
+  returnStatus = 0;
+  returnStatus = start();
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendAddress(SLA_R(address));
+  if(returnStatus){return(returnStatus);}
+  for(uint8_t i = 0; i < numberBytes; i++)
+  {
+    if( i == nack )
+    {
+      returnStatus = receiveByte(0);
+      if(returnStatus != MR_DATA_NACK){return(returnStatus);}
+    }
+    else
+    {
+      returnStatus = receiveByte(1);
+      if(returnStatus != MR_DATA_ACK){return(returnStatus);}
+    }
+    data[i] = TWDR;
+    bytesAvailable = i+1;
+    totalBytes = i+1;
+  }
+  returnStatus = stop();
+  return(returnStatus);
+}
+
+uint8_t I2C::read(int address, int registerAddress, int numberBytes)
+{
+  return(read((uint8_t) address, (uint8_t) registerAddress, (uint8_t) numberBytes));
+}
+
+uint8_t I2C::read(uint8_t address, uint8_t registerAddress, uint8_t numberBytes)
+{
+  bytesAvailable = 0;
+  bufferIndex = 0;
+  if(numberBytes == 0){numberBytes++;}
+  nack = numberBytes - 1;
+  returnStatus = 0;
+  returnStatus = start();
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendAddress(SLA_W(address));
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendByte(registerAddress);
+  if(returnStatus){return(returnStatus);}
+  returnStatus = start();
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendAddress(SLA_R(address));
+  if(returnStatus){return(returnStatus);}
+  for(uint8_t i = 0; i < numberBytes; i++)
+  {
+    if( i == nack )
+    {
+      returnStatus = receiveByte(0);
+      if(returnStatus != MR_DATA_NACK){return(returnStatus);}
+    }
+    else
+    {
+      returnStatus = receiveByte(1);
+      if(returnStatus != MR_DATA_ACK){return(returnStatus);}
+    }
+    data[i] = TWDR;
+    bytesAvailable = i+1;
+    totalBytes = i+1;
+  }
+  returnStatus = stop();
+  return(returnStatus);
+}
+
+uint8_t I2C::read(uint8_t address, uint8_t numberBytes, uint8_t *dataBuffer)
+{
+  bytesAvailable = 0;
+  bufferIndex = 0;
+  if(numberBytes == 0){numberBytes++;}
+  nack = numberBytes - 1;
+  returnStatus = 0;
+  returnStatus = start();
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendAddress(SLA_R(address));
+  if(returnStatus){return(returnStatus);}
+  for(uint8_t i = 0; i < numberBytes; i++)
+  {
+    if( i == nack )
+    {
+      returnStatus = receiveByte(0);
+      if(returnStatus != MR_DATA_NACK){return(returnStatus);}
+    }
+    else
+    {
+      returnStatus = receiveByte(1);
+      if(returnStatus != MR_DATA_ACK){return(returnStatus);}
+    }
+    dataBuffer[i] = TWDR;
+    bytesAvailable = i+1;
+    totalBytes = i+1;
+  }
+  returnStatus = stop();
+  return(returnStatus);
+}
+
+uint8_t I2C::read(uint8_t address, uint8_t registerAddress, uint8_t numberBytes, uint8_t *dataBuffer)
+{
+  bytesAvailable = 0;
+  bufferIndex = 0;
+  if(numberBytes == 0){numberBytes++;}
+  nack = numberBytes - 1;
+  returnStatus = 0;
+  returnStatus = start();
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendAddress(SLA_W(address));
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendByte(registerAddress);
+  if(returnStatus){return(returnStatus);}
+  returnStatus = start();
+  if(returnStatus){return(returnStatus);}
+  returnStatus = sendAddress(SLA_R(address));
+  if(returnStatus){return(returnStatus);}
+  for(uint8_t i = 0; i < numberBytes; i++)
+  {
+    if( i == nack )
+    {
+      returnStatus = receiveByte(0);
+      if(returnStatus != MR_DATA_NACK){return(returnStatus);}
+    }
+    else
+    {
+      returnStatus = receiveByte(1);
+      if(returnStatus != MR_DATA_ACK){return(returnStatus);}
+    }
+    dataBuffer[i] = TWDR;
+    bytesAvailable = i+1;
+    totalBytes = i+1;
+  }
+  returnStatus = stop();
+  return(returnStatus);
+}
+
+
+/////////////// Private Methods ////////////////////////////////////////
+
+
+uint8_t I2C::start()
+{
+  unsigned long startingTime = millis();
+  TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);
+  while (!(TWCR & (1<<TWINT)))
+  {
+    if(!timeOutDelay){continue;}
+    if((millis() - startingTime) >= timeOutDelay)
+    {
+      lockUp();
+      return(1);
+    }
+       
+  }
+  if ((TWI_STATUS == START) || (TWI_STATUS == REPEATED_START))
+  {
+    return(0);
+  }
+  return(TWI_STATUS);
+}
+
+uint8_t I2C::sendAddress(uint8_t i2cAddress)
+{
+  TWDR = i2cAddress;
+  unsigned long startingTime = millis();
+  TWCR = (1<<TWINT) | (1<<TWEN);
+  while (!(TWCR & (1<<TWINT)))
+  {
+    if(!timeOutDelay){continue;}
+    if((millis() - startingTime) >= timeOutDelay)
+    {
+      lockUp();
+      return(1);
+    }
+       
+  }
+  if ((TWI_STATUS == MT_SLA_ACK) || (TWI_STATUS == MR_SLA_ACK))
+  {
+    return(0);
+  }
+  return(TWI_STATUS);
+}
+
+uint8_t I2C::sendByte(uint8_t i2cData)
+{
+  TWDR = i2cData;
+  unsigned long startingTime = millis();
+  TWCR = (1<<TWINT) | (1<<TWEN);
+  while (!(TWCR & (1<<TWINT)))
+  {
+    if(!timeOutDelay){continue;}
+    if((millis() - startingTime) >= timeOutDelay)
+    {
+      lockUp();
+      return(1);
+    }
+       
+  }
+  if (TWI_STATUS == MT_DATA_ACK)
+  {
+    return(0);
+  }
+  return(TWI_STATUS);
+}
+
+uint8_t I2C::receiveByte(boolean ack)
+{
+  unsigned long startingTime = millis();
+  if(ack)
+  {
+    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);
+
+  }
+  else
+  {
+    TWCR = (1<<TWINT) | (1<<TWEN);
+  }
+  while (!(TWCR & (1<<TWINT)))
+  {
+    if(!timeOutDelay){continue;}
+    if((millis() - startingTime) >= timeOutDelay)
+    {
+      lockUp();
+      return(1);
+    }
+       
+  }
+  return(TWI_STATUS);
+}
+
+uint8_t I2C::stop()
+{
+  unsigned long startingTime = millis();
+  TWCR = (1<<TWINT)|(1<<TWEN)| (1<<TWSTO);
+  while ((TWCR & (1<<TWSTO)))
+  {
+    if(!timeOutDelay){continue;}
+    if((millis() - startingTime) >= timeOutDelay)
+    {
+      lockUp();
+      return(1);
+    }
+       
+  }
+  return(0);
+}
+
+void I2C::lockUp()
+{
+  TWCR = 0; //releases SDA and SCL lines to high impedance
+  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); //reinitialize TWI
+}
+
+SIGNAL(TWI_vect)
+{
+  switch(TWI_STATUS){
+    case 0x20:
+    case 0x30:
+    case 0x48:
+         TWCR = (1<<TWINT)|(1<<TWEN)| (1<<TWSTO); // send a stop
+	 break;
+    case 0x38:
+    case 0x68:
+    case 0x78:
+    case 0xB0:
+         TWCR = 0; //releases SDA and SCL lines to high impedance
+  	 TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); //reinitialize TWI
+	 break;
+  }
+}
+
+
+I2C I2c = I2C();
+

libraries/I2C/I2C.h

@@ -0,0 +1,116 @@
+/*
+  I2C.h - I2C library
+  Copyright (c) 2011 Wayne Truchsess.  All right reserved.
+  Rev 2.0 - September 19th, 2011
+          - Added support for timeout function to prevent 
+            and recover from bus lockup (thanks to PaulS
+            and CrossRoads on the Arduino forum)
+          - Changed return type for stop() from void to
+            uint8_t to handle timeOut function 
+  Rev 1.0 - August 8th, 2011
+  
+  This is a modified version of the Arduino Wire/TWI 
+  library.  Functions were rewritten to provide more functionality
+  and also the use of Repeated Start.  Some I2C devices will not
+  function correctly without the use of a Repeated Start.  The 
+  initial version of this library only supports the Master.
+
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include "WProgram.h"
+#include <inttypes.h>
+
+#ifndef I2C_h
+#define I2C_h
+
+
+#define START           0x08
+#define REPEATED_START  0x10
+#define MT_SLA_ACK	0x18
+#define MT_DATA_ACK     0x28
+#define MR_SLA_ACK	0x40
+#define MR_DATA_ACK     0x50
+#define MR_DATA_NACK    0x58
+#define TWI_STATUS      (TWSR & 0xF8)
+#define SLA_W(address)  (address << 1)
+#define SLA_R(address)  ((address << 1) + 0x01)
+#define cbi(sfr, bit)   (_SFR_BYTE(sfr) &= ~_BV(bit))
+#define sbi(sfr, bit)   (_SFR_BYTE(sfr) |= _BV(bit))
+
+#define MAX_BUFFER_SIZE 32
+
+
+#ifndef CPU_FREQ
+#define CPU_FREQ 16000000L
+#endif
+
+
+
+class I2C
+{
+  public:
+    I2C();
+    void begin();
+    void end();
+    void timeOut(uint16_t);
+    void setSpeed(boolean); 
+    void pullup(boolean);
+    ///////carry over from Wire library//////// 
+    uint8_t returnStatusWire; 
+    uint8_t beginTransmission(uint8_t);
+    uint8_t beginTransmission(int);
+    uint8_t send(uint8_t);
+    uint8_t send(int);
+    uint8_t endTransmission();
+    uint8_t requestFrom(uint8_t, uint8_t);
+    uint8_t requestFrom(int, int);
+    uint8_t available();
+    ///////////////////////////////////////////
+    uint8_t write(uint8_t, uint8_t);
+    uint8_t write(int, int); 
+    uint8_t write(uint8_t, uint8_t, uint8_t);
+    uint8_t write(int, int, int);
+    uint8_t write(uint8_t, uint8_t, char*);
+    uint8_t write(uint8_t, uint8_t, uint8_t*, uint8_t);
+    uint8_t read(uint8_t, uint8_t);
+    uint8_t read(int, int);
+    uint8_t read(uint8_t, uint8_t, uint8_t);
+    uint8_t read(int, int, int);
+    uint8_t read(uint8_t, uint8_t, uint8_t*);
+    uint8_t read(uint8_t, uint8_t, uint8_t, uint8_t*);
+    uint8_t receive();
+
+  private:
+    uint8_t start();
+    uint8_t sendAddress(uint8_t);
+    uint8_t sendByte(uint8_t);
+    uint8_t receiveByte(boolean);
+    uint8_t stop();
+    void lockUp();
+    uint8_t returnStatus;
+    uint8_t nack;
+    uint8_t data[MAX_BUFFER_SIZE];
+    static uint8_t bytesAvailable;
+    static uint8_t bufferIndex;
+    static uint8_t totalBytes;
+    static uint16_t timeOutDelay;
+
+};
+
+extern I2C I2c;
+
+#endif

libraries/I2C/examples/HMC5883L/HMC5883L.pde

@@ -0,0 +1,70 @@
+/*******************************************
+ Sample sketch that configures an HMC5883L 3 axis
+ magnetometer to continuous mode and reads back 
+ the three axis of data.
+ Code compiles to a size of 1500 bytes
+ Equivalent Wire Library code compiles to 2032 bytes
+ *******************************************/
+
+#include <I2C.h>
+
+#define HMC5883L  0x1E 
+
+int x = 0;
+int y = 0;
+int z = 0;
+
+
+void setup()
+{
+  I2c.begin();
+  I2c.write(HMC5883L,0x02,0x00); //configure device for continuous mode
+}
+
+void loop()
+{
+  I2c.read(HMC5883L,0x03,6); //read 6 bytes (x,y,z) from the device
+  x = I2c.receive() << 8;
+  x |= I2c.receive();
+  y = I2c.receive() << 8;
+  y |= I2c.receive();
+  z = I2c.receive() << 8;
+  z |= I2c.receive(); 
+}
+
+
+/* Wire library equivalent would be this 
+
+//#include <Wire.h>
+
+#define HMC5883L  0x1E 
+
+int x = 0;
+int y = 0;
+int z = 0;
+
+
+void setup()
+{
+  Wire.begin();
+  Wire.beginTransmission(HMC5883L);
+  Wire.send(0x02);
+  Wire.send(0x00);
+  Wire.endTransmission();
+}
+
+void loop()
+{
+  Wire.beginTransmission(HMC5883L);
+  Wire.send(0x03);
+  Wire.endTransmission();
+  Wire.requestFrom(HMC5883L,6);
+  x = Wire.receive() << 8;
+  x |= Wire.receive();
+  y = Wire.receive() << 8;
+  y |= Wire.receive();
+  z = Wire.receive() << 8;
+  z |= Wire.receive(); 
+}
+
+********************************************/

libraries/I2C/keywords.txt

@@ -0,0 +1,36 @@
+#######################################
+# Syntax Coloring Map For I2C
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+begin	KEYWORD2
+end	KEYWORD2
+timeOut	KEYWORD2
+setSpeed	KEYWORD2
+pullup	KEYWORD2
+write	KEYWORD2
+read	KEYWORD2
+beginTransmission	KEYWORD2
+send	KEYWORD2
+endTransmission	KEYWORD2
+requestFrom	KEYWORD2
+available	KEYWORD2
+receive	KEYWORD2
+
+#######################################
+# Instances (KEYWORD2)
+#######################################
+
+I2c	KEYWORD2
+
+#######################################
+# Constants (LITERAL1)
+#######################################