aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: split compass into frontend/backend

Browse Source
This commit is contained in:
Víctor Mayoral Vilches 2014-11-15 17:58:23 -08:00 committed by Andrew Tridgell
parent 774332ea02
commit d3b76cd8d3
13 changed files with 493 additions and 196 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -26,7 +26,6 @@
#include <AP_HAL.h>
#include "AP_Compass_AK8963.h"
#define READ_FLAG 0x80
#define MPUREG_I2C_SLV0_ADDR 0x25
#define MPUREG_I2C_SLV0_REG 0x26
@ -173,11 +172,26 @@ void AK8963_MPU9250_SPI_Backend::write(uint8_t address, const uint8_t *buf, uint
_spi->transaction(tx, NULL, count + 1);
}
AP_Compass_AK8963_MPU9250::AP_Compass_AK8963_MPU9250()
AP_Compass_AK8963_MPU9250::AP_Compass_AK8963_MPU9250(Compass &compass):
AP_Compass_AK8963(compass)
{
product_id = AP_COMPASS_TYPE_AK8963_MPU9250;
}
// detect the sensor
AP_Compass_Backend *AP_Compass_AK8963_MPU9250::detect(Compass &compass)
{
AP_Compass_AK8963_MPU9250 *sensor = new AP_Compass_AK8963_MPU9250(compass);
if (sensor == NULL) {
return NULL;
}
if (!sensor->init()) {
delete sensor;
return NULL;
}
return sensor;
}
void AP_Compass_AK8963_MPU9250::_dump_registers()
{
#if AK8963_DEBUG
@ -244,7 +258,7 @@ uint8_t AP_Compass_AK8963_MPU9250::_read_id()
return 1;
}
bool AP_Compass_AK8963_MPU9250::_read_raw()
bool AP_Compass_AK8963_MPU9250::read_raw()
{
uint8_t rx[14] = {0};
@ -271,11 +285,10 @@ bool AP_Compass_AK8963_MPU9250::_read_raw()
}
AP_Compass_AK8963::AP_Compass_AK8963() :
Compass(),
AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass) :
AP_Compass_Backend(compass),
_backend(NULL)
{
_healthy[0] = true;
_initialised = false;
_mag_x_accum =_mag_y_accum = _mag_z_accum = 0;
_mag_x =_mag_y = _mag_z = 0;
@ -306,7 +319,7 @@ bool AP_Compass_AK8963::_self_test()
_start_conversion();
hal.scheduler->delay(20);
_read_raw();
read_raw();
float hx = _mag_x;
float hy = _mag_y;
@ -353,11 +366,14 @@ bool AP_Compass_AK8963::_self_test()
bool AP_Compass_AK8963::init()
{
_healthy[0] = true;
// register the compass instance in the frontend
_compass_instance = _compass.register_compass();
_field[0].x = 0.0f;
_field[0].y = 0.0f;
_field[0].z = 0.0f;
_compass._healthy[_compass_instance] = true;
_compass._field[_compass_instance].x = 0.0f;
_compass._field[_compass_instance].y = 0.0f;
_compass._field[_compass_instance].z = 0.0f;
hal.scheduler->suspend_timer_procs();
if (!_backend->sem_take_blocking()) {
@ -485,36 +501,37 @@ bool AP_Compass_AK8963::read()
}
/* Update */
_field[0].x = _mag_x_accum * magnetometer_ASA[0] / _accum_count;
_field[0].y = _mag_y_accum * magnetometer_ASA[1] / _accum_count;
_field[0].z = _mag_z_accum * magnetometer_ASA[2] / _accum_count;
_compass._field[_compass_instance].x = _mag_x_accum * magnetometer_ASA[0] / _accum_count;
_compass._field[_compass_instance].y = _mag_y_accum * magnetometer_ASA[1] / _accum_count;
_compass._field[_compass_instance].z = _mag_z_accum * magnetometer_ASA[2] / _accum_count;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
// apply default board orientation for this compass type. This is
// a noop on most boards
_field[0].rotate(MAG_BOARD_ORIENTATION);
_compass._field[_compass_instance].rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
_field[0].rotate((enum Rotation)_orientation[0].get());
_compass._field[_compass_instance].rotate((enum Rotation)_compass._orientation[_compass_instance].get());
if (!_external[0]) {
if (!_compass._external[_compass_instance]) {
// and add in AHRS_ORIENTATION setting if not an external compass
_field[0].rotate(_board_orientation);
_compass._field[_compass_instance].rotate(_compass._board_orientation);
}
apply_corrections(_field[0],0);
_compass.apply_corrections(_compass._field[_compass_instance],_compass_instance);
#if 0
float x = _field[0].x;
float y = _field[0].y;
float z = _field[0].z;
float x = _compass._field[_compass_instance].x;
float y = _compass._field[_compass_instance].y;
float z = _compass._field[_compass_instance].z;
error("%f %f %f\n", x, y, z);
#endif
last_update = hal.scheduler->micros(); // record time of update
_compass.last_update = hal.scheduler->micros(); // record time of update
return true;
}
@ -538,8 +555,8 @@ void AP_Compass_AK8963::_collect_samples()
return;
}
if (!_read_raw()) {
error("_read_raw failed\n");
if (!read_raw()) {
error("read_raw failed\n");
} else {
_mag_x_accum += _mag_x;
_mag_y_accum += _mag_y;

21
libraries/AP_Compass/AP_Compass_AK8963.h
View File

@ -7,10 +7,12 @@
#include "../AP_Math/AP_Math.h"
#include "Compass.h"
#include "AP_Compass_Backend.h"
class AK8963_Backend
{
public:
virtual ~AK8963_Backend() {}
virtual void read(uint8_t address, uint8_t *buf, uint32_t count) = 0;
virtual void write(uint8_t address, const uint8_t *buf, uint32_t count) = 0;
virtual bool sem_take_nonblocking() = 0;
@ -29,7 +31,7 @@ class AK8963_Backend
}
};
class AP_Compass_AK8963 : public Compass
class AP_Compass_AK8963 : public AP_Compass_Backend
{
private:
typedef enum
@ -43,7 +45,6 @@ private:
virtual void _register_read(uint8_t address, uint8_t count, uint8_t *value) = 0;
virtual void _register_write(uint8_t address, uint8_t value) = 0;
virtual void _backend_reset() = 0;
virtual bool _read_raw() = 0;
virtual uint8_t _read_id() = 0;
virtual void _dump_registers() {}
@ -70,11 +71,13 @@ protected:
float _mag_x;
float _mag_y;
float _mag_z;
uint8_t _compass_instance;
AK8963_Backend *_backend;
AK8963_Backend *_backend; // Not to be confused with Compass (frontend) "_backends" attribute.
virtual bool re_initialise(void) {return false;}
public:
AP_Compass_AK8963();
AP_Compass_AK8963(Compass &compass);
virtual bool init(void);
virtual bool read(void);
@ -91,6 +94,7 @@ class AK8963_MPU9250_SPI_Backend: public AK8963_Backend
bool sem_take_nonblocking();
bool sem_take_blocking();
bool sem_give();
~AK8963_MPU9250_SPI_Backend() {}
private:
AP_HAL::SPIDeviceDriver *_spi;
@ -100,15 +104,20 @@ class AK8963_MPU9250_SPI_Backend: public AK8963_Backend
class AP_Compass_AK8963_MPU9250: public AP_Compass_AK8963
{
public:
AP_Compass_AK8963_MPU9250();
AP_Compass_AK8963_MPU9250(Compass &compass);
~AP_Compass_AK8963_MPU9250() {}
bool init();
// detect the sensor
static AP_Compass_Backend *detect(Compass &compass);
private:
bool _backend_init();
void _backend_reset();
void _register_read(uint8_t address, uint8_t count, uint8_t *value);
void _register_write(uint8_t address, uint8_t value);
void _dump_registers();
bool _read_raw();
bool read_raw();
uint8_t _read_id();
};

10
libraries/AP_Compass/AP_Compass_Backend.cpp Normal file
View File

@ -0,0 +1,10 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL.h>
#include "Compass.h"
#include "AP_Compass_Backend.h"
AP_Compass_Backend::AP_Compass_Backend(Compass &compass) :
_compass(compass),
product_id(AP_PRODUCT_ID_NONE)
{}

54
libraries/AP_Compass/AP_Compass_Backend.h Normal file
View File

@ -0,0 +1,54 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program 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 General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
Compass driver backend class. Each supported compass sensor type
needs to have an object derived from this class.
*/
#ifndef __AP_COMPASS_BACKEND_H__
#define __AP_COMPASS_BACKEND_H__
#include "Compass.h"
class Compass; // forward declaration
class AP_Compass_Backend
{
public:
AP_Compass_Backend(Compass &compass);
// we declare a virtual destructor so that drivers can
// override with a custom destructor if need be.
virtual ~AP_Compass_Backend(void) {}
// initialize the magnetometers
virtual bool init(void) = 0;
// read sensor data
virtual bool read(void) = 0;
// accumulate a reading from the magnetometer
virtual void accumulate(void) = 0;
int16_t product_id; /// product id
protected:
virtual bool read_raw(void) = 0;
virtual bool re_initialise(void) = 0;
Compass &_compass; ///< access to frontend
};
#endif // __AP_COMPASS_BACKEND_H__

96
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -27,90 +27,50 @@
extern const AP_HAL::HAL& hal;
// constructor
AP_Compass_HIL::AP_Compass_HIL() : Compass()
AP_Compass_HIL::AP_Compass_HIL(Compass &compass):
AP_Compass_Backend(compass)
{
product_id = AP_COMPASS_TYPE_HIL;
_setup_earth_field();
_compass._setup_earth_field();
}
// setup _Bearth
void AP_Compass_HIL::_setup_earth_field(void)
// detect the sensor
AP_Compass_Backend *AP_Compass_HIL::detect(Compass &compass)
{
// assume a earth field strength of 400
_Bearth(400, 0, 0);
// rotate _Bearth for inclination and declination. -66 degrees
// is the inclination in Canberra, Australia
Matrix3f R;
R.from_euler(0, ToRad(66), _declination.get());
_Bearth = R * _Bearth;
AP_Compass_HIL *sensor = new AP_Compass_HIL(compass);
if (sensor == NULL) {
return NULL;
}
if (!sensor->init()) {
delete sensor;
return NULL;
}
return sensor;
}
bool
AP_Compass_HIL::init(void)
{
// register the compass instance in the frontend
_compass_instance = _compass.register_compass();
return true;
}
// Public Methods //////////////////////////////////////////////////////////////
bool AP_Compass_HIL::read()
{
_field[0] = _hil_mag;
apply_corrections(_field[0],0);
_compass._field[_compass_instance] = _compass._hil_mag;
_compass.apply_corrections(_compass._field[_compass_instance],_compass_instance);
// values set by setHIL function
last_update = hal.scheduler->micros(); // record time of update
_compass.last_update = hal.scheduler->micros(); // record time of update
//_update_compass(_compass_instance, _compass._field[_compass_instance], _compass._healthy[_compass_instance])
return true;
}
#define MAG_OFS_X 5.0
#define MAG_OFS_Y 13.0
#define MAG_OFS_Z -18.0
// Update raw magnetometer values from HIL data
//
void AP_Compass_HIL::setHIL(float roll, float pitch, float yaw)
{
Matrix3f R;
// create a rotation matrix for the given attitude
R.from_euler(roll, pitch, yaw);
if (_last_declination != _declination.get()) {
_setup_earth_field();
_last_declination = _declination.get();
}
// convert the earth frame magnetic vector to body frame, and
// apply the offsets
_hil_mag = R.mul_transpose(_Bearth);
_hil_mag -= Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
// apply default board orientation for this compass type. This is
// a noop on most boards
_hil_mag.rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
_hil_mag.rotate((enum Rotation)_orientation[0].get());
if (!_external[0]) {
// and add in AHRS_ORIENTATION setting if not an external compass
_hil_mag.rotate(_board_orientation);
}
_healthy[0] = true;
}
// Update raw magnetometer values from HIL mag vector
//
void AP_Compass_HIL::setHIL(const Vector3f &mag)
{
_hil_mag.x = mag.x;
_hil_mag.y = mag.y;
_hil_mag.z = mag.z;
_healthy[0] = true;
}
const Vector3f& AP_Compass_HIL::getHIL() const {
return _hil_mag;
}
void AP_Compass_HIL::accumulate(void)
{
// nothing to do

20
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -4,21 +4,23 @@
#include "Compass.h"
class AP_Compass_HIL : public Compass
class AP_Compass_HIL : public AP_Compass_Backend
{
public:
AP_Compass_HIL();
AP_Compass_HIL(Compass &compass);
bool read(void);
void accumulate(void);
void setHIL(float roll, float pitch, float yaw);
void setHIL(const Vector3f &mag);
const Vector3f& getHIL() const;
bool init(void);
bool read_raw(void) {return true;}
bool re_initialise(void) {return true;}
bool read_register(uint8_t address, uint8_t *value) {return true;}
bool write_register(uint8_t address, uint8_t value) {return true;}
// detect the sensor
static AP_Compass_Backend *detect(Compass &compass);
private:
Vector3f _hil_mag;
Vector3f _Bearth;
float _last_declination;
void _setup_earth_field();
uint8_t _compass_instance;
};
#endif

64
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -57,11 +57,31 @@ extern const AP_HAL::HAL& hal;
#define DataOutputRate_30HZ 0x05
#define DataOutputRate_75HZ 0x06
// constructor
AP_Compass_HMC5843::AP_Compass_HMC5843(Compass &compass):
AP_Compass_Backend(compass),
_accum_count(0)
{}
// detect the sensor
AP_Compass_Backend *AP_Compass_HMC5843::detect(Compass &compass)
{
AP_Compass_HMC5843 *sensor = new AP_Compass_HMC5843(compass);
if (sensor == NULL) {
return NULL;
}
if (!sensor->init()) {
delete sensor;
return NULL;
}
return sensor;
}
// read_register - read a register value
bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
{
if (hal.i2c->readRegister((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
_healthy[0] = false;
_compass._healthy[_compass_instance] = false;
return false;
}
return true;
@ -71,7 +91,7 @@ bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
bool AP_Compass_HMC5843::write_register(uint8_t address, uint8_t value)
{
if (hal.i2c->writeRegister((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
_healthy[0] = false;
_compass._healthy[_compass_instance] = false;
return false;
}
return true;
@ -83,10 +103,10 @@ bool AP_Compass_HMC5843::read_raw()
uint8_t buff[6];
if (hal.i2c->readRegisters(COMPASS_ADDRESS, 0x03, 6, buff) != 0) {
if (_healthy[0]) {
if (_compass._healthy[_compass_instance]) {
hal.i2c->setHighSpeed(false);
}
_healthy[0] = false;
_compass._healthy[_compass_instance] = false;
_i2c_sem->give();
return false;
}
@ -123,7 +143,7 @@ void AP_Compass_HMC5843::accumulate(void)
return;
}
uint32_t tnow = hal.scheduler->micros();
if (_healthy[0] && _accum_count != 0 && (tnow - _last_accum_time) < 13333) {
if (_compass._healthy[_compass_instance] && _accum_count != 0 && (tnow - _last_accum_time) < 13333) {
// the compass gets new data at 75Hz
return;
}
@ -192,7 +212,7 @@ AP_Compass_HMC5843::init()
_base_config = 0;
if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
!read_register(ConfigRegA, &_base_config)) {
_healthy[0] = false;
_compass._healthy[_compass_instance] = false;
_i2c_sem->give();
hal.scheduler->resume_timer_procs();
return false;
@ -310,7 +330,7 @@ AP_Compass_HMC5843::init()
_initialised = true;
// perform an initial read
_healthy[0] = true;
_compass._healthy[_compass_instance] = true;
read();
#if 0
@ -318,6 +338,9 @@ AP_Compass_HMC5843::init()
calibration[0], calibration[1], calibration[2]);
#endif
// register the compass instance in the frontend
_compass_instance = _compass.register_compass();
return success;
}
@ -330,7 +353,7 @@ bool AP_Compass_HMC5843::read()
// have the right orientation!)
return false;
}
if (!_healthy[0]) {
if (!_compass._healthy[_compass_instance]) {
if (hal.scheduler->millis() < _retry_time) {
return false;
}
@ -343,7 +366,7 @@ bool AP_Compass_HMC5843::read()
if (_accum_count == 0) {
accumulate();
if (!_healthy[0] || _accum_count == 0) {
if (!_compass._healthy[_compass_instance] || _accum_count == 0) {
// try again in 1 second, and set I2c clock speed slower
_retry_time = hal.scheduler->millis() + 1000;
hal.i2c->setHighSpeed(false);
@ -351,34 +374,33 @@ bool AP_Compass_HMC5843::read()
}
}
_field[0].x = _mag_x_accum * calibration[0] / _accum_count;
_field[0].y = _mag_y_accum * calibration[1] / _accum_count;
_field[0].z = _mag_z_accum * calibration[2] / _accum_count;
_compass._field[_compass_instance].x = _mag_x_accum * calibration[0] / _accum_count;
_compass._field[_compass_instance].y = _mag_y_accum * calibration[1] / _accum_count;
_compass._field[_compass_instance].z = _mag_z_accum * calibration[2] / _accum_count;
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
last_update = hal.scheduler->micros(); // record time of update
_compass.last_update = hal.scheduler->micros(); // record time of update
// rotate to the desired orientation
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
_field[0].rotate(ROTATION_YAW_90);
_compass._field[_compass_instance].rotate(ROTATION_YAW_90);
}
// apply default board orientation for this compass type. This is
// a noop on most boards
_field[0].rotate(MAG_BOARD_ORIENTATION);
_compass._field[_compass_instance].rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
_field[0].rotate((enum Rotation)_orientation[0].get());
_compass._field[_compass_instance].rotate((enum Rotation)_compass.get_orientation(_compass_instance).get());
if (!_external[0]) {
if (!_compass._external[_compass_instance]) {
// and add in AHRS_ORIENTATION setting if not an external compass
_field[0].rotate(_board_orientation);
_compass._field[_compass_instance].rotate(_compass.get_board_orientation());
}
apply_corrections(_field[0],0);
_healthy[0] = true;
_compass.apply_corrections(_compass._field[_compass_instance], _compass_instance);
_compass._healthy[_compass_instance] = true;
return true;
}

11
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -7,8 +7,9 @@
#include "../AP_Math/AP_Math.h"
#include "Compass.h"
#include "AP_Compass_Backend.h"
class AP_Compass_HMC5843 : public Compass
class AP_Compass_HMC5843 : public AP_Compass_Backend
{
private:
float calibration[3];
@ -30,12 +31,16 @@ private:
uint8_t _accum_count;
uint32_t _last_accum_time;
uint8_t _compass_instance;
public:
AP_Compass_HMC5843() : Compass() {
}
AP_Compass_HMC5843(Compass &compass);
bool init(void);
bool read(void);
void accumulate(void);
// detect the sensor
static AP_Compass_Backend *detect(Compass &compass);
};
#endif

52
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -41,6 +41,26 @@ extern const AP_HAL::HAL& hal;
// Public Methods //////////////////////////////////////////////////////////////
// constructor
AP_Compass_PX4::AP_Compass_PX4(Compass &compass):
AP_Compass_Backend(compass),
_num_instances(0)
{}
// detect the sensor
AP_Compass_Backend *AP_Compass_PX4::detect(Compass &compass)
{
AP_Compass_PX4 *sensor = new AP_Compass_PX4(compass);
if (sensor == NULL) {
return NULL;
}
if (!sensor->init()) {
delete sensor;
return NULL;
}
return sensor;
}
bool AP_Compass_PX4::init(void)
{
_mag_fd[0] = open(MAG_BASE_DEVICE_PATH"0", O_RDONLY);
@ -60,7 +80,7 @@ bool AP_Compass_PX4::init(void)
for (uint8_t i=0; i<_num_instances; i++) {
// get device id
_dev_id[i] = ioctl(_mag_fd[i], DEVIOCGDEVICEID, 0);
_compass._dev_id[i] = ioctl(_mag_fd[i], DEVIOCGDEVICEID, 0);
// average over up to 20 samples
if (ioctl(_mag_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) {
@ -69,13 +89,13 @@ bool AP_Compass_PX4::init(void)
}
// remember if the compass is external
_external[i] = (ioctl(_mag_fd[i], MAGIOCGEXTERNAL, 0) > 0);
if (_external[i]) {
_compass._external[i] = (ioctl(_mag_fd[i], MAGIOCGEXTERNAL, 0) > 0);
if (_compass._external[i]) {
hal.console->printf("Using external compass[%u]\n", (unsigned)i);
}
_count[0] = 0;
_sum[i].zero();
_healthy[i] = false;
_compass._healthy[i] = false;
}
// give the driver a chance to run, and gather one sample
@ -84,6 +104,8 @@ bool AP_Compass_PX4::init(void)
if (_count[0] == 0) {
hal.console->printf("Failed initial compass accumulate\n");
}
_compass.product_id = AP_COMPASS_TYPE_PX4;
return true;
}
@ -94,7 +116,7 @@ bool AP_Compass_PX4::read(void)
// consider the compass healthy if we got a reading in the last 0.2s
for (uint8_t i=0; i<_num_instances; i++) {
_healthy[i] = (hal.scheduler->micros64() - _last_timestamp[i] < 200000);
_compass._healthy[i] = (hal.scheduler->micros64() - _last_timestamp[i] < 200000);
}
for (uint8_t i=0; i<_num_instances; i++) {
@ -108,24 +130,24 @@ bool AP_Compass_PX4::read(void)
// a noop on most boards
_sum[i].rotate(MAG_BOARD_ORIENTATION);
if (_external[i]) {
if (_compass._external[i]) {
// add user selectable orientation
_sum[i].rotate((enum Rotation)_orientation[i].get());
_sum[i].rotate((enum Rotation)_compass._orientation[i].get());
} else {
// add in board orientation from AHRS
_sum[i].rotate(_board_orientation);
_sum[i].rotate(_compass._board_orientation);
}
_field[i] = _sum[i];
apply_corrections(_field[i],i);
_compass._field[i] = _sum[i];
_compass.apply_corrections(_compass._field[i],i);
_sum[i].zero();
_count[i] = 0;
}
last_update = _last_timestamp[get_primary()];
_compass.last_update = _last_timestamp[get_primary()];
return _healthy[get_primary()];
return _compass._healthy[get_primary()];
}
void AP_Compass_PX4::accumulate(void)
@ -143,11 +165,11 @@ void AP_Compass_PX4::accumulate(void)
uint8_t AP_Compass_PX4::get_primary(void) const
{
if (_primary < _num_instances && _healthy[_primary] && use_for_yaw(_primary)) {
return _primary;
if (_compass._primary < _num_instances && _compass._healthy[_compass._primary] && _compass.use_for_yaw(_compass._primary)) {
return _compass._primary;
}
for (uint8_t i=0; i<_num_instances; i++) {
if (_healthy[i] && (use_for_yaw(i))) return i;
if (_compass._healthy[i] && (_compass.use_for_yaw(i))) return i;
}
return 0;
}

19
libraries/AP_Compass/AP_Compass_PX4.h
View File

@ -4,14 +4,11 @@
#define AP_Compass_PX4_H
#include "Compass.h"
#include "AP_Compass_Backend.h"
class AP_Compass_PX4 : public Compass
class AP_Compass_PX4 : public AP_Compass_Backend
{
public:
AP_Compass_PX4() : Compass() {
product_id = AP_COMPASS_TYPE_PX4;
_num_instances = 0;
}
bool init(void);
bool read(void);
void accumulate(void);
@ -22,12 +19,20 @@ public:
// return the primary compass
uint8_t get_primary(void) const;
AP_Compass_PX4(Compass &compass);
// detect the sensor
static AP_Compass_Backend *detect(Compass &compass);
private:
uint8_t _num_instances;
int _mag_fd[COMPASS_MAX_INSTANCES];
uint8_t _num_instances;
int _mag_fd[COMPASS_MAX_INSTANCES];
Vector3f _sum[COMPASS_MAX_INSTANCES];
uint32_t _count[COMPASS_MAX_INSTANCES];
uint64_t _last_timestamp[COMPASS_MAX_INSTANCES];
virtual bool read_raw(void) { return false;}
virtual bool re_initialise(void) {return false;}
};
#endif // AP_Compass_PX4_H

156
libraries/AP_Compass/Compass.cpp
View File

@ -269,9 +269,14 @@ Compass::Compass(void) :
last_update(0),
_null_init_done(false),
_thr_or_curr(0.0f),
_backend_count(0),
_compass_count(0),
_board_orientation(ROTATION_NONE)
{
AP_Param::setup_object_defaults(this, var_info);
for (uint8_t i=0; i<COMPASS_MAX_BACKEND; i++) {
_backends[i] = NULL;
}
#if COMPASS_MAX_INSTANCES > 1
// default device ids to zero. init() method will overwrite with the actual device ids
@ -281,14 +286,98 @@ Compass::Compass(void) :
#endif
}
// Default init method, just returns success.
// Default init method
//
bool
Compass::init()
{
if (_compass_count == 0) {
// detect available backends. Only called once
_detect_backends();
}
product_id = 0; // FIX
//TODO other initializations needed
return true;
}
// Register a new compass instance
//
uint8_t Compass::register_compass(void)
{
if (_compass_count == COMPASS_MAX_INSTANCES) {
hal.scheduler->panic(PSTR("Too many compass instances"));
}
return _compass_count++;
}
/*
try to load a backend
*/
void
Compass::_add_backend(AP_Compass_Backend *(detect)(Compass &))
{
if (_backend_count == COMPASS_MAX_BACKEND) {
hal.scheduler->panic(PSTR("Too many compass backends"));
}
_backends[_backend_count] = detect(*this);
if (_backends[_backend_count] != NULL) {
_backend_count++;
}
}
/*
detect available backends for this board
*/
void
Compass::_detect_backends(void)
{
// Values defined in AP_HAL/AP_HAL_Boards.h
#if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
_add_backend(AP_Compass_HIL::detect);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843
_add_backend(AP_Compass_HMC5843::detect);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4
_add_backend(AP_Compass_PX4::detect);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN
_add_backend(AP_Compass_VRBRAIN::detect);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250
_add_backend(AP_Compass_AK8963_MPU9250::detect);
#else
#error Unrecognised HAL_COMPASS_TYPE setting
#endif
if (_backend_count == 0 ||
_compass_count == 0) {
hal.scheduler->panic(PSTR("No Compass backends available"));
}
// set the product ID to the ID of the first backend
product_id = _backends[0]->product_id;
}
void
Compass::accumulate(void)
{
for (uint8_t i=0; i< _backend_count; i++) {
// call accumulate on each of the backend
_backends[i]->accumulate();
}
}
bool
Compass::read(void)
{
for (uint8_t i=0; i< _backend_count; i++) {
// call read on each of the backend. This call updates field[i]
_backends[i]->read();
}
return _healthy[get_primary()];
}
void
Compass::set_offsets(uint8_t i, const Vector3f &offsets)
{
@ -483,3 +572,68 @@ void Compass::apply_corrections(Vector3f &mag, uint8_t i)
_motor_offset[i].zero();
}
}
#define MAG_OFS_X 5.0
#define MAG_OFS_Y 13.0
#define MAG_OFS_Z -18.0
// Update raw magnetometer values from HIL data
//
void Compass::setHIL(float roll, float pitch, float yaw)
{
Matrix3f R;
// create a rotation matrix for the given attitude
R.from_euler(roll, pitch, yaw);
if (_last_declination != get_declination()) {
_setup_earth_field();
_last_declination = get_declination();
}
// convert the earth frame magnetic vector to body frame, and
// apply the offsets
_hil_mag = R.mul_transpose(_Bearth);
_hil_mag -= Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
// apply default board orientation for this compass type. This is
// a noop on most boards
_hil_mag.rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
_hil_mag.rotate((enum Rotation)get_orientation().get());
if (!_external[0]) {
// and add in AHRS_ORIENTATION setting if not an external compass
_hil_mag.rotate(get_board_orientation());
}
_healthy[0] = true;
}
// Update raw magnetometer values from HIL mag vector
//
void Compass::setHIL(const Vector3f &mag)
{
_hil_mag.x = mag.x;
_hil_mag.y = mag.y;
_hil_mag.z = mag.z;
_healthy[0] = true;
}
const Vector3f& Compass::getHIL() const {
return _hil_mag;
}
// setup _Bearth
void Compass::_setup_earth_field(void)
{
// assume a earth field strength of 400
_Bearth(400, 0, 0);
// rotate _Bearth for inclination and declination. -66 degrees
// is the inclination in Canberra, Australia
Matrix3f R;
R.from_euler(0, ToRad(66), get_declination());
_Bearth = R * _Bearth;
}

105
libraries/AP_Compass/Compass.h
View File

@ -7,6 +7,10 @@
#include <AP_Param.h>
#include <AP_Math.h>
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AP_HAL.h>
#include "AP_Compass_Backend.h"
extern const AP_HAL::HAL& hal;
// compass product id
#define AP_COMPASS_TYPE_UNKNOWN 0x00
@ -49,19 +53,22 @@
maximum number of compass instances available on this platform. If more
than 1 then redundent sensors may be available
*/
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#define COMPASS_MAX_INSTANCES 3
#define COMPASS_MAX_BACKEND 3
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#define COMPASS_MAX_INSTANCES 2
#define COMPASS_MAX_BACKEND 2
#else
#define COMPASS_MAX_INSTANCES 1
#define COMPASS_MAX_BACKEND 1
#endif
class Compass
{
public:
int16_t product_id; /// product id
uint32_t last_update; ///< micros() time of last update
int16_t product_id; /// product id
/// Constructor
///
@ -72,17 +79,25 @@ public:
/// @returns True if the compass was initialized OK, false if it was not
/// found or is not functioning.
///
virtual bool init();
bool init();
/// Read the compass and update the mag_ variables.
///
virtual bool read(void) = 0;
bool read();
/// use spare CPU cycles to accumulate values from the compass if
/// possible
virtual void accumulate(void) = 0;
/// possible (this method should also be implemented in the backends)
void accumulate();
/// Register a new compas driver, allocating an instance number
///
/// @return number of compass instances
uint8_t register_compass(void);
// load backend drivers
void _add_backend(AP_Compass_Backend *(detect)(Compass &));
void _detect_backends(void);
/// Calculate the tilt-compensated heading_ variables.
///
@ -117,7 +132,7 @@ public:
void save_offsets(void);
// return the number of compass instances
virtual uint8_t get_count(void) const { return 1; }
uint8_t get_count(void) const { return _compass_count; }
/// Return the current field as a Vector3f
const Vector3f &get_field(uint8_t i) const { return _field[i]; }
@ -178,6 +193,10 @@ public:
void set_board_orientation(enum Rotation orientation) {
_board_orientation = orientation;
}
enum Rotation get_board_orientation() { return _board_orientation;}
AP_Int8 get_orientation() { return _orientation[0];}
AP_Int8 get_orientation(uint8_t instance) { return _orientation[instance];}
/// Set the motor compensation type
///
@ -249,35 +268,57 @@ public:
///
/// @returns the instance number of the primary compass
///
virtual uint8_t get_primary(void) const { return 0; }
uint8_t get_primary(void) const { return 0; }
void apply_corrections(Vector3f &mag, uint8_t i);
// HIL methods
void setHIL(float roll, float pitch, float yaw);
void setHIL(const Vector3f &mag);
const Vector3f& getHIL() const;
void _setup_earth_field();
Vector3f _hil_mag;
static const struct AP_Param::GroupInfo var_info[];
// settable parameters
AP_Int8 _learn; ///<enable calibration learning
protected:
bool _healthy[COMPASS_MAX_INSTANCES];
Vector3f _field[COMPASS_MAX_INSTANCES]; ///< magnetic field strength
AP_Int8 _orientation[COMPASS_MAX_INSTANCES];
/// Return the product ID
///
/// @return the product ID
///
// int16_t product_id(void) const { return product_id; }
AP_Int8 _external[COMPASS_MAX_INSTANCES]; ///<compass is external
bool _healthy[COMPASS_MAX_INSTANCES];
Vector3f _field[COMPASS_MAX_INSTANCES]; ///< magnetic field strength
// board orientation from AHRS
enum Rotation _board_orientation;
AP_Int8 _orientation[COMPASS_MAX_INSTANCES];
AP_Vector3f _offset[COMPASS_MAX_INSTANCES];
AP_Float _declination;
AP_Int8 _use_for_yaw[COMPASS_MAX_INSTANCES];///<enable use for yaw calculation
AP_Int8 _auto_declination; ///<enable automatic declination code
AP_Int8 _external[COMPASS_MAX_INSTANCES]; ///<compass is external
#if COMPASS_MAX_INSTANCES > 1
AP_Int8 _primary; ///primary instance
AP_Int32 _dev_id[COMPASS_MAX_INSTANCES]; // device id detected at init. saved to eeprom when offsets are saved allowing ram & eeprom values to be compared as consistency check
AP_Int8 _primary; ///primary instance
AP_Int32 _dev_id[COMPASS_MAX_INSTANCES]; // device id detected at init. saved to eeprom when offsets are saved allowing ram & eeprom values to be compared as consistency check
#endif
bool _null_init_done; ///< first-time-around flag used by offset nulling
protected:
// backend objects
AP_Compass_Backend *_backends[COMPASS_MAX_INSTANCES];
// number of compasses
uint8_t _compass_count;
uint8_t _backend_count;
AP_Float _declination;
AP_Int8 _use_for_yaw[COMPASS_MAX_INSTANCES];///<enable use for yaw calculation
AP_Int8 _auto_declination; ///<enable automatic declination code
bool _null_init_done; ///< first-time-around flag used by offset nulling
///< used by offset correction
static const uint8_t _mag_history_size = 20;
uint8_t _mag_history_index[COMPASS_MAX_INSTANCES];
Vector3i _mag_history[COMPASS_MAX_INSTANCES][_mag_history_size];
uint8_t _mag_history_index[COMPASS_MAX_INSTANCES];
Vector3i _mag_history[COMPASS_MAX_INSTANCES][_mag_history_size];
// motor compensation
AP_Int8 _motor_comp_type; // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
@ -285,9 +326,17 @@ protected:
Vector3f _motor_offset[COMPASS_MAX_INSTANCES]; // latest compensation added to compass
float _thr_or_curr; // throttle expressed as a percentage from 0 ~ 1.0 or current expressed in amps
// board orientation from AHRS
enum Rotation _board_orientation;
// HIL variables
Vector3f _Bearth;
float _last_declination;
void apply_corrections(Vector3f &mag, uint8_t i);
};
#include "AP_Compass_Backend.h"
#include "AP_Compass_HMC5843.h"
#include "AP_Compass_HIL.h"
#include "AP_Compass_AK8963.h"
#include "AP_Compass_PX4.h"
#endif

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

@ -41,19 +41,7 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
#define CONFIG_COMPASS HAL_COMPASS_DEFAULT
#if CONFIG_COMPASS == HAL_COMPASS_PX4
static AP_Compass_PX4 compass;
#elif CONFIG_COMPASS == HAL_COMPASS_VRBRAIN
static AP_Compass_VRBRAIN compass;
#elif CONFIG_COMPASS == HAL_COMPASS_HMC5843
static AP_Compass_HMC5843 compass;
#elif CONFIG_COMPASS == HAL_COMPASS_HIL
static AP_Compass_HIL compass;
#elif CONFIG_COMPASS == HAL_COMPASS_AK8963
static AP_Compass_AK8963_MPU9250 compass;
#else
#error Unrecognized CONFIG_COMPASS setting
#endif
static Compass compass;
uint32_t timer;