I2C: convert compass code to new I2C library

this also adds a healthy attribute, and error checking on all I2C
calls

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
+	// 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,9 +8,8 @@ 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; }
-
-	void 			read();
-    void 	    	setHIL(float Mag_X, float Mag_Y, float Mag_Z);
+	bool read(void);
+	void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
 };
 
 #endif

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,40 +48,23 @@
 #define DataOutputRate_75HZ   0x06
 
 // read_register - read a register value
-static bool
-read_register(int address, byte *value)
+bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
 {
-  bool ret = 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 false;
-  
-  return ret;
+   if (I2c.read((uint8_t)COMPASS_ADDRESS, address, 1, value) != 0) {
+	  healthy = false;
+	  return false;
+   }
+   return true;
 }
 
 // write_register - update a register value
-static bool
-write_register(int address, byte value)
+bool AP_Compass_HMC5843::write_register(uint8_t address, byte value)
 {
-  Wire.beginTransmission(COMPASS_ADDRESS);
-  Wire.send(address);
-  Wire.send(value);
-  if (0 != Wire.endTransmission())
-	 return false;
-  delay(10);
-  return true;
+   if (I2c.write((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
+	  healthy = false;
+	  return false;
+   }
+   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;
-   byte buff[6];
-
-   Wire.beginTransmission(COMPASS_ADDRESS);
-   Wire.send(0x03);        //sends address to read from
-   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++;
-   }
+   uint8_t buff[6];
 
-   if (0 != Wire.endTransmission())
-	  return false;
-
-   if (i != 6) {
-	  /* we didn't get enough bytes */
+   if (I2c.read(COMPASS_ADDRESS, 0x03, 6, buff) != 0) {
+	  healthy = false;
 	  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))
-	 return false;
-  delay(50);
-  if (! write_register(ConfigRegB, magGain) ||
-	  ! write_register(ModeRegister, ContinuousConversion))
+  if (!re_initialise()) {
 	 return false;
-  delay(50);
+  }
 
   return success;
 }
 
 // Read Sensor data
-void 
-AP_Compass_HMC5843::read()
+bool 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 void read();
-	virtual bool read_raw();
+	virtual bool init(void);
+	virtual bool read(void);
 	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.
 	///
@@ -98,21 +100,21 @@ public:
 	///
 	virtual Vector3f &get_offsets();
 
-    /// Program new offset values.
-    ///
-    /// XXX DEPRECATED
-    ///
-    /// @param  x                   Offset to the raw mag_x value.
-    /// @param  y                   Offset to the raw mag_y value.
-    /// @param  z                   Offset to the raw mag_z value.
-    ///
-    void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); }
+	/// Program new offset values.
+	///
+	/// XXX DEPRECATED
+	///
+	/// @param  x                   Offset to the raw mag_x value.
+	/// @param  y                   Offset to the raw mag_y value.
+	/// @param  z                   Offset to the raw mag_z value.
+	///
+	void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); }
 
-    /// Perform automatic offset updates using the results of the DCM matrix.
-    ///
-    /// @param  dcm_matrix          The DCM result matrix.
-    ///
-    void null_offsets(const Matrix3f &dcm_matrix);
+	/// Perform automatic offset updates using the results of the DCM matrix.
+	///
+	/// @param  dcm_matrix          The DCM result matrix.
+	///
+	void null_offsets(const Matrix3f &dcm_matrix);
 
 
 	/// Sets the local magnetic field declination.
@@ -129,7 +131,7 @@ protected:
 	AP_Float            _declination;
 
 	bool                _null_init_done;        ///< first-time-around flag used by offset nulling
-    Matrix3f            _last_dcm_matrix;       ///< previous DCM matrix used by offset nulling
-    Vector3f            _mag_body_last;         ///< ?? used by offset nulling
+	Matrix3f            _last_dcm_matrix;       ///< previous DCM matrix used by offset nulling
+	Vector3f            _mag_body_last;         ///< ?? used by offset nulling
 };
 #endif

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,8 @@ void setup()
 {
   Serial.begin(115200);
   Serial.println("Compass library test (HMC5843 and HMC5883L)");
-  Wire.begin();
+  I2c.begin();
+  I2c.timeOut(20);
   if (!compass.init()) {
 	  Serial.println("compass initialisation failed!");
 	  while (1) ;
@@ -61,6 +62,11 @@ void loop()
   {
     timer = millis();
     compass.read();
+
+    if (!compass.healthy) {
+	    Serial.println("not healthy");
+	    return;
+    }
     compass.calculate(0,0);  // roll = 0, pitch = 0 for this example
 
     // capture min
@@ -106,4 +112,4 @@ void loop()
 
     Serial.println();
   }
-}
+}

libraries/AP_Compass/examples/AP_Compass_test/Makefile

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