mirror of https://github.com/ArduPilot/ardupilot
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:
parent
8df91ea885
commit
f93c716126
|
@ -13,9 +13,10 @@
|
||||||
|
|
||||||
// Public Methods //////////////////////////////////////////////////////////////
|
// 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
|
// 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_x = _mag_x;
|
||||||
mag_y = _mag_y;
|
mag_y = _mag_y;
|
||||||
mag_z = _mag_z;
|
mag_z = _mag_z;
|
||||||
|
healthy = true;
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,3 +1,4 @@
|
||||||
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
#ifndef AP_Compass_HIL_H
|
#ifndef AP_Compass_HIL_H
|
||||||
#define AP_Compass_HIL_H
|
#define AP_Compass_HIL_H
|
||||||
|
|
||||||
|
@ -7,9 +8,8 @@ class AP_Compass_HIL : public Compass
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) { product_id = AP_COMPASS_TYPE_HIL; }
|
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);
|
||||||
void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -16,8 +16,9 @@
|
||||||
// AVR LibC Includes
|
// AVR LibC Includes
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include "WConstants.h"
|
#include "WConstants.h"
|
||||||
|
#include <FastSerial.h>
|
||||||
|
|
||||||
#include <Wire.h>
|
#include <I2C.h>
|
||||||
#include "AP_Compass_HMC5843.h"
|
#include "AP_Compass_HMC5843.h"
|
||||||
|
|
||||||
#define COMPASS_ADDRESS 0x1E
|
#define COMPASS_ADDRESS 0x1E
|
||||||
|
@ -47,40 +48,23 @@
|
||||||
#define DataOutputRate_75HZ 0x06
|
#define DataOutputRate_75HZ 0x06
|
||||||
|
|
||||||
// read_register - read a register value
|
// 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;
|
return false;
|
||||||
|
}
|
||||||
Wire.beginTransmission(COMPASS_ADDRESS);
|
return true;
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// write_register - update a register value
|
// 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);
|
return false;
|
||||||
if (0 != Wire.endTransmission())
|
}
|
||||||
return false;
|
return true;
|
||||||
delay(10);
|
|
||||||
return true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -96,25 +80,10 @@ static void rotate_for_5883L(AP_VarS<Matrix3f> *_orientation_matrix)
|
||||||
// Read Sensor data
|
// Read Sensor data
|
||||||
bool AP_Compass_HMC5843::read_raw()
|
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;
|
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 //////////////////////////////////////////////////////////////
|
// Public Methods //////////////////////////////////////////////////////////////
|
||||||
bool
|
bool
|
||||||
AP_Compass_HMC5843::init()
|
AP_Compass_HMC5843::init()
|
||||||
{
|
{
|
||||||
int numAttempts = 0, good_count = 0;
|
int numAttempts = 0, good_count = 0;
|
||||||
bool success = false;
|
bool success = false;
|
||||||
byte base_config; // used to test compass type
|
|
||||||
byte calibration_gain = 0x20;
|
byte calibration_gain = 0x20;
|
||||||
uint16_t expected_x = 715;
|
uint16_t expected_x = 715;
|
||||||
uint16_t expected_yz = 715;
|
uint16_t expected_yz = 715;
|
||||||
|
@ -156,10 +138,11 @@ AP_Compass_HMC5843::init()
|
||||||
|
|
||||||
// determine if we are using 5843 or 5883L
|
// determine if we are using 5843 or 5883L
|
||||||
if (! write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
|
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;
|
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
|
// a 5883L supports the sample averaging config
|
||||||
int old_product_id = product_id;
|
int old_product_id = product_id;
|
||||||
|
|
||||||
|
@ -175,7 +158,7 @@ AP_Compass_HMC5843::init()
|
||||||
*/
|
*/
|
||||||
rotate_for_5883L(&_orientation_matrix);
|
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;
|
product_id = AP_COMPASS_TYPE_HMC5843;
|
||||||
} else {
|
} else {
|
||||||
// not behaving like either supported compass type
|
// not behaving like either supported compass type
|
||||||
|
@ -253,23 +236,22 @@ AP_Compass_HMC5843::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
// leave test mode
|
// leave test mode
|
||||||
if (! write_register(ConfigRegA, base_config))
|
if (!re_initialise()) {
|
||||||
return false;
|
return false;
|
||||||
delay(50);
|
}
|
||||||
if (! write_register(ConfigRegB, magGain) ||
|
|
||||||
! write_register(ModeRegister, ContinuousConversion))
|
|
||||||
return false;
|
|
||||||
delay(50);
|
|
||||||
|
|
||||||
return success;
|
return success;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read Sensor data
|
// Read Sensor data
|
||||||
void
|
bool AP_Compass_HMC5843::read()
|
||||||
AP_Compass_HMC5843::read()
|
|
||||||
{
|
{
|
||||||
|
if (!healthy && !re_initialise()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
if (!read_raw()) {
|
if (!read_raw()) {
|
||||||
return;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
mag_x *= calibration[0];
|
mag_x *= calibration[0];
|
||||||
|
@ -285,6 +267,9 @@ AP_Compass_HMC5843::read()
|
||||||
mag_x = rot_mag.x;
|
mag_x = rot_mag.x;
|
||||||
mag_y = rot_mag.y;
|
mag_y = rot_mag.y;
|
||||||
mag_z = rot_mag.z;
|
mag_z = rot_mag.z;
|
||||||
|
healthy = true;
|
||||||
|
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// set orientation
|
// set orientation
|
||||||
|
|
|
@ -1,3 +1,4 @@
|
||||||
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
#ifndef AP_Compass_HMC5843_H
|
#ifndef AP_Compass_HMC5843_H
|
||||||
#define AP_Compass_HMC5843_H
|
#define AP_Compass_HMC5843_H
|
||||||
|
|
||||||
|
@ -47,11 +48,16 @@ class AP_Compass_HMC5843 : public Compass
|
||||||
{
|
{
|
||||||
private:
|
private:
|
||||||
float calibration[3];
|
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:
|
public:
|
||||||
AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
|
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);
|
virtual void set_orientation(const Matrix3f &rotation_matrix);
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
|
@ -1,3 +1,4 @@
|
||||||
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
#include "Compass.h"
|
#include "Compass.h"
|
||||||
|
|
||||||
// Default constructor.
|
// Default constructor.
|
||||||
|
|
|
@ -1,3 +1,4 @@
|
||||||
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
#ifndef Compass_h
|
#ifndef Compass_h
|
||||||
#define Compass_h
|
#define Compass_h
|
||||||
|
|
||||||
|
@ -40,6 +41,7 @@ public:
|
||||||
float heading_x; ///< compass vector X magnitude
|
float heading_x; ///< compass vector X magnitude
|
||||||
float heading_y; ///< compass vector Y magnitude
|
float heading_y; ///< compass vector Y magnitude
|
||||||
unsigned long last_update; ///< millis() time of last update
|
unsigned long last_update; ///< millis() time of last update
|
||||||
|
bool healthy; ///< true if last read OK
|
||||||
|
|
||||||
/// Constructor
|
/// Constructor
|
||||||
///
|
///
|
||||||
|
@ -56,7 +58,7 @@ public:
|
||||||
|
|
||||||
/// Read the compass and update the mag_ variables.
|
/// Read the compass and update the mag_ variables.
|
||||||
///
|
///
|
||||||
virtual void read() = 0;
|
virtual bool read(void) = 0;
|
||||||
|
|
||||||
/// Calculate the tilt-compensated heading_ variables.
|
/// Calculate the tilt-compensated heading_ variables.
|
||||||
///
|
///
|
||||||
|
@ -98,21 +100,21 @@ public:
|
||||||
///
|
///
|
||||||
virtual Vector3f &get_offsets();
|
virtual Vector3f &get_offsets();
|
||||||
|
|
||||||
/// Program new offset values.
|
/// Program new offset values.
|
||||||
///
|
///
|
||||||
/// XXX DEPRECATED
|
/// XXX DEPRECATED
|
||||||
///
|
///
|
||||||
/// @param x Offset to the raw mag_x value.
|
/// @param x Offset to the raw mag_x value.
|
||||||
/// @param y Offset to the raw mag_y value.
|
/// @param y Offset to the raw mag_y value.
|
||||||
/// @param z Offset to the raw mag_z 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)); }
|
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.
|
/// Perform automatic offset updates using the results of the DCM matrix.
|
||||||
///
|
///
|
||||||
/// @param dcm_matrix The DCM result matrix.
|
/// @param dcm_matrix The DCM result matrix.
|
||||||
///
|
///
|
||||||
void null_offsets(const Matrix3f &dcm_matrix);
|
void null_offsets(const Matrix3f &dcm_matrix);
|
||||||
|
|
||||||
|
|
||||||
/// Sets the local magnetic field declination.
|
/// Sets the local magnetic field declination.
|
||||||
|
@ -129,7 +131,7 @@ protected:
|
||||||
AP_Float _declination;
|
AP_Float _declination;
|
||||||
|
|
||||||
bool _null_init_done; ///< first-time-around flag used by offset nulling
|
bool _null_init_done; ///< first-time-around flag used by offset nulling
|
||||||
Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling
|
Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling
|
||||||
Vector3f _mag_body_last; ///< ?? used by offset nulling
|
Vector3f _mag_body_last; ///< ?? used by offset nulling
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -7,7 +7,7 @@
|
||||||
#include <AP_Common.h>
|
#include <AP_Common.h>
|
||||||
#include <AP_Compass.h> // Compass Library
|
#include <AP_Compass.h> // Compass Library
|
||||||
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||||
#include <Wire.h>
|
#include <I2C.h>
|
||||||
|
|
||||||
FastSerialPort0(Serial);
|
FastSerialPort0(Serial);
|
||||||
|
|
||||||
|
@ -23,7 +23,8 @@ void setup()
|
||||||
{
|
{
|
||||||
Serial.begin(115200);
|
Serial.begin(115200);
|
||||||
Serial.println("Compass library test (HMC5843 and HMC5883L)");
|
Serial.println("Compass library test (HMC5843 and HMC5883L)");
|
||||||
Wire.begin();
|
I2c.begin();
|
||||||
|
I2c.timeOut(20);
|
||||||
if (!compass.init()) {
|
if (!compass.init()) {
|
||||||
Serial.println("compass initialisation failed!");
|
Serial.println("compass initialisation failed!");
|
||||||
while (1) ;
|
while (1) ;
|
||||||
|
@ -61,6 +62,11 @@ void loop()
|
||||||
{
|
{
|
||||||
timer = millis();
|
timer = millis();
|
||||||
compass.read();
|
compass.read();
|
||||||
|
|
||||||
|
if (!compass.healthy) {
|
||||||
|
Serial.println("not healthy");
|
||||||
|
return;
|
||||||
|
}
|
||||||
compass.calculate(0,0); // roll = 0, pitch = 0 for this example
|
compass.calculate(0,0); // roll = 0, pitch = 0 for this example
|
||||||
|
|
||||||
// capture min
|
// capture min
|
||||||
|
@ -106,4 +112,4 @@ void loop()
|
||||||
|
|
||||||
Serial.println();
|
Serial.println();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,2 +1 @@
|
||||||
BOARD = mega
|
|
||||||
include ../../../AP_Common/Arduino.mk
|
include ../../../AP_Common/Arduino.mk
|
||||||
|
|
Loading…
Reference in New Issue