From f93c716126b993192aeb599c64d478632fe2650e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Dec 2011 20:31:36 +1100 Subject: [PATCH] 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 | 6 +- libraries/AP_Compass/AP_Compass_HIL.h | 6 +- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 107 ++++++++---------- libraries/AP_Compass/AP_Compass_HMC5843.h | 12 +- libraries/AP_Compass/Compass.cpp | 1 + libraries/AP_Compass/Compass.h | 36 +++--- .../AP_Compass_test/AP_Compass_test.pde | 12 +- .../examples/AP_Compass_test/Makefile | 1 - 8 files changed, 91 insertions(+), 90 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index a8554da41b..2eb7a069b5 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/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; } diff --git a/libraries/AP_Compass/AP_Compass_HIL.h b/libraries/AP_Compass/AP_Compass_HIL.h index 3949fc9750..b3cb985d91 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.h +++ b/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 diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index e3e7233d36..8096275623 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -16,8 +16,9 @@ // AVR LibC Includes #include #include "WConstants.h" +#include -#include +#include #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 *_orientation_matrix) // Read Sensor data bool AP_Compass_HMC5843::read_raw() { - int i = 0; - byte buff[6]; + uint8_t 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++; - } - - 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)) + 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 -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 diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index ec77fe753e..3dbd747eef 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/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); }; diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 86af0c21ce..d67c900ed0 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/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. diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index b16614fc01..a2550f2518 100644 --- a/libraries/AP_Compass/Compass.h +++ b/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 diff --git a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde index f1e7e3b05f..9987585a83 100644 --- a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde +++ b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde @@ -7,7 +7,7 @@ #include #include // Compass Library #include // ArduPilot Mega Vector/Matrix math Library -#include +#include 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(); } -} +} diff --git a/libraries/AP_Compass/examples/AP_Compass_test/Makefile b/libraries/AP_Compass/examples/AP_Compass_test/Makefile index 85b4d8856b..d1f40fd90f 100644 --- a/libraries/AP_Compass/examples/AP_Compass_test/Makefile +++ b/libraries/AP_Compass/examples/AP_Compass_test/Makefile @@ -1,2 +1 @@ -BOARD = mega include ../../../AP_Common/Arduino.mk