Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

I2C: convert compass code to new I2C library 

this also adds a healthy attribute, and error checking on all I2C
calls
This commit is contained in:
Andrew Tridgell 2011-12-28 20:31:36 +11:00
parent 8df91ea885
commit f93c716126
8 changed files with 91 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -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;
}

6
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -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

107
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -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];
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

12
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -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);
};

1
libraries/AP_Compass/Compass.cpp
View File

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

36
libraries/AP_Compass/Compass.h
View File

@ -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

12
libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde
View File

@ -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();
}
}
}

1
libraries/AP_Compass/examples/AP_Compass_test/Makefile
View File

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