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

AP_Compass: use state array for compass variables

This commit is contained in:
Andrew Tridgell 2015-02-24 10:17:44 +11:00
parent 13f0aa5ecd
commit a871c87cad
16 changed files with 352 additions and 577 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Compass/AP_Compass.h
View File

@ -6,5 +6,4 @@
#include "AP_Compass_HMC5843.h"
#include "AP_Compass_HIL.h"
#include "AP_Compass_PX4.h"
#include "AP_Compass_VRBRAIN.h"
#include "AP_Compass_AK8963.h"

37
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -175,7 +175,6 @@ void AK8963_MPU9250_SPI_Backend::write(uint8_t address, const uint8_t *buf, uint
AP_Compass_AK8963_MPU9250::AP_Compass_AK8963_MPU9250(Compass &compass):
AP_Compass_AK8963(compass)
{
product_id = AP_COMPASS_TYPE_AK8963_MPU9250;
}
// detect the sensor
@ -367,13 +366,7 @@ bool AP_Compass_AK8963::_self_test()
bool AP_Compass_AK8963::init()
{
// register the compass instance in the frontend
_compass_instance = _compass.register_compass();
_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;
_compass_instance = register_compass();
hal.scheduler->suspend_timer_procs();
if (!_backend->sem_take_blocking()) {
@ -489,39 +482,27 @@ bool AP_Compass_AK8963::_calibrate()
return true;
}
bool AP_Compass_AK8963::read()
void AP_Compass_AK8963::read()
{
if (!_initialised) {
return false;
return;
}
if (_accum_count == 0) {
/* We're not ready to publish*/
return true;
return;
}
/* Update */
_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;
Vector3f field(_mag_x_accum * magnetometer_ASA[0],
_mag_y_accum * magnetometer_ASA[1],
_mag_z_accum * magnetometer_ASA[2]);
field /= _accum_count;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
_copy_to_frontend(_compass_instance);
#if 0
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
_compass.last_update = hal.scheduler->micros(); // record time of update
return true;
publish_field(field, _compass_instance);
}
void AP_Compass_AK8963::_start_conversion()

35
libraries/AP_Compass/AP_Compass_AK8963.h
View File

@ -33,6 +33,23 @@ class AK8963_Backend
class AP_Compass_AK8963 : public AP_Compass_Backend
{
public:
AP_Compass_AK8963(Compass &compass);
bool init(void);
void read(void);
void accumulate(void);
protected:
AK8963_Backend *_backend; // Not to be confused with Compass (frontend) "_backends" attribute.
float magnetometer_ASA[3];
float _mag_x;
float _mag_y;
float _mag_z;
uint8_t _compass_instance;
virtual bool read_raw() = 0;
private:
typedef enum
{
@ -65,24 +82,6 @@ private:
uint8_t _magnetometer_adc_resolution;
uint32_t _last_update_timestamp;
uint32_t _last_accum_time;
protected:
float magnetometer_ASA[3];
float _mag_x;
float _mag_y;
float _mag_z;
uint8_t _compass_instance;
AK8963_Backend *_backend; // Not to be confused with Compass (frontend) "_backends" attribute.
virtual bool re_initialise(void) {return false;}
public:
AP_Compass_AK8963(Compass &compass);
virtual bool init(void);
virtual bool read(void);
virtual void accumulate(void);
};
class AK8963_MPU9250_SPI_Backend: public AK8963_Backend

75
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -4,27 +4,84 @@
#include "Compass.h"
#include "AP_Compass_Backend.h"
extern const AP_HAL::HAL& hal;
AP_Compass_Backend::AP_Compass_Backend(Compass &compass) :
_compass(compass),
product_id(AP_PRODUCT_ID_NONE)
_compass(compass)
{}
/*
copy latest data to the frontend from a backend
*/
void AP_Compass_Backend::_copy_to_frontend(uint8_t instance) {
void AP_Compass_Backend::publish_field(const Vector3f &mag, uint8_t instance)
{
Compass::mag_state &state = _compass._state[instance];
state.field = mag;
// apply default board orientation for this compass type. This is
// a noop on most boards
_compass._field[instance].rotate(MAG_BOARD_ORIENTATION);
state.field.rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
_compass._field[instance].rotate((enum Rotation)_compass._orientation[instance].get());
state.field.rotate((enum Rotation)state.orientation.get());
if (!_compass._external[instance]) {
if (!state.external) {
// and add in AHRS_ORIENTATION setting if not an external compass
_compass._field[instance].rotate(_compass.get_board_orientation());
state.field.rotate(_compass._board_orientation);
}
_compass.apply_corrections(_compass._field[instance],instance);
}
apply_corrections(state.field, instance);
state.last_update_ms = hal.scheduler->millis();
}
/*
register a new backend with frontend, returning instance which
should be used in publish_field()
*/
uint8_t AP_Compass_Backend::register_compass(void) const
{
return _compass.register_compass();
}
/*
apply offset and motor compensation corrections
*/
void AP_Compass_Backend::apply_corrections(Vector3f &mag, uint8_t i)
{
Compass::mag_state &state = _compass._state[i];
const Vector3f &offsets = state.offset.get();
const Vector3f &mot = state.motor_compensation.get();
/*
note that _motor_offset[] is kept even if compensation is not
being applied so it can be logged correctly
*/
mag += offsets;
if(_compass._motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _compass._thr_or_curr != 0.0f) {
state.motor_offset = mot * _compass._thr_or_curr;
mag += state.motor_offset;
} else {
state.motor_offset.zero();
}
}
/*
set dev_id for an instance
*/
void AP_Compass_Backend::set_dev_id(uint8_t instance, uint32_t dev_id)
{
#if COMPASS_MAX_INSTANCES > 1
_compass._state[instance].dev_id.set(dev_id);
#endif
}
/*
set external for an instance
*/
void AP_Compass_Backend::set_external(uint8_t instance, bool external)
{
_compass._state[instance].external.set(external);
}

31
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -37,19 +37,30 @@ public:
virtual bool init(void) = 0;
// read sensor data
virtual bool read(void) = 0;
virtual void read(void) = 0;
// accumulate a reading from the magnetometer
virtual void accumulate(void) = 0;
int16_t product_id; /// product id
// accumulate a reading from the magnetometer. Optional in
// backends
virtual void accumulate(void) {};
protected:
virtual bool read_raw(void) = 0;
virtual bool re_initialise(void) = 0;
Compass &_compass; ///< access to frontend
void _copy_to_frontend(uint8_t instance);
// publish a magnetic field vector to the frontend
void publish_field(const Vector3f &mag, uint8_t instance);
// register a new compass instance with the frontend
uint8_t register_compass(void) const;
// set dev_id for an instance
void set_dev_id(uint8_t instance, uint32_t dev_id);
// set external state for an instance
void set_external(uint8_t instance, bool external);
// access to frontend
Compass &_compass;
private:
void apply_corrections(Vector3f &mag, uint8_t i);
};
#endif // __AP_COMPASS_BACKEND_H__

25
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -15,8 +15,7 @@
*/
/*
* AP_Compass_HIL.cpp - Arduino Library for HIL model of HMC5843 I2C Magnetometer
* Code by James Goppert. DIYDrones.com
* AP_Compass_HIL.cpp - HIL backend for AP_Compass
*
*/
@ -30,9 +29,7 @@ extern const AP_HAL::HAL& hal;
AP_Compass_HIL::AP_Compass_HIL(Compass &compass):
AP_Compass_Backend(compass)
{
product_id = AP_COMPASS_TYPE_HIL;
_compass._setup_earth_field();
}
// detect the sensor
@ -53,25 +50,11 @@ bool
AP_Compass_HIL::init(void)
{
// register the compass instance in the frontend
_compass_instance = _compass.register_compass();
_compass_instance = register_compass();
return true;
}
// Public Methods //////////////////////////////////////////////////////////////
bool AP_Compass_HIL::read()
void AP_Compass_HIL::read()
{
_compass._field[_compass_instance] = _compass._hil_mag;
_compass.apply_corrections(_compass._field[_compass_instance],_compass_instance);
// values set by setHIL function
_compass.last_update = hal.scheduler->micros(); // record time of update
//_update_compass(_compass_instance, _compass._field[_compass_instance], _compass._healthy[_compass_instance])
return true;
}
void AP_Compass_HIL::accumulate(void)
{
// nothing to do
publish_field(_compass._hil.field, _compass_instance);
}

7
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -8,13 +8,8 @@ class AP_Compass_HIL : public AP_Compass_Backend
{
public:
AP_Compass_HIL(Compass &compass);
bool read(void);
void accumulate(void);
void read(void);
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);

60
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -60,7 +60,9 @@ extern const AP_HAL::HAL& hal;
// constructor
AP_Compass_HMC5843::AP_Compass_HMC5843(Compass &compass):
AP_Compass_Backend(compass),
_accum_count(0)
_accum_count(0),
_retry_time(0),
_product_id(0)
{}
// detect the sensor
@ -81,7 +83,7 @@ AP_Compass_Backend *AP_Compass_HMC5843::detect(Compass &compass)
bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
{
if (hal.i2c->readRegister((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
_compass._healthy[_compass_instance] = false;
_retry_time = hal.scheduler->millis() + 1000;
return false;
}
return true;
@ -91,7 +93,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) {
_compass._healthy[_compass_instance] = false;
_retry_time = hal.scheduler->millis() + 1000;
return false;
}
return true;
@ -103,17 +105,15 @@ bool AP_Compass_HMC5843::read_raw()
uint8_t buff[6];
if (hal.i2c->readRegisters(COMPASS_ADDRESS, 0x03, 6, buff) != 0) {
if (_compass._healthy[_compass_instance]) {
hal.i2c->setHighSpeed(false);
}
_compass._healthy[_compass_instance] = false;
hal.i2c->setHighSpeed(false);
_retry_time = hal.scheduler->millis() + 1000;
_i2c_sem->give();
return false;
}
int16_t rx, ry, rz;
rx = (((int16_t)buff[0]) << 8) | buff[1];
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
rz = (((int16_t)buff[2]) << 8) | buff[3];
ry = (((int16_t)buff[4]) << 8) | buff[5];
} else {
@ -143,7 +143,7 @@ void AP_Compass_HMC5843::accumulate(void)
return;
}
uint32_t tnow = hal.scheduler->micros();
if (_compass._healthy[_compass_instance] && _accum_count != 0 && (tnow - _last_accum_time) < 13333) {
if (_accum_count != 0 && (tnow - _last_accum_time) < 13333) {
// the compass gets new data at 75Hz
return;
}
@ -212,14 +212,13 @@ AP_Compass_HMC5843::init()
_base_config = 0;
if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
!read_register(ConfigRegA, &_base_config)) {
_compass._healthy[_compass_instance] = false;
_i2c_sem->give();
hal.scheduler->resume_timer_procs();
return false;
}
if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
// a 5883L supports the sample averaging config
product_id = AP_COMPASS_TYPE_HMC5883L;
_product_id = AP_COMPASS_TYPE_HMC5883L;
calibration_gain = 0x60;
/*
note that the HMC5883 datasheet gives the x and y expected
@ -230,7 +229,7 @@ AP_Compass_HMC5843::init()
expected_yz = 713;
gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
product_id = AP_COMPASS_TYPE_HMC5843;
_product_id = AP_COMPASS_TYPE_HMC5843;
} else {
// not behaving like either supported compass type
_i2c_sem->give();
@ -330,7 +329,6 @@ AP_Compass_HMC5843::init()
_initialised = true;
// perform an initial read
_compass._healthy[_compass_instance] = true;
read();
#if 0
@ -339,56 +337,54 @@ AP_Compass_HMC5843::init()
#endif
// register the compass instance in the frontend
_compass_instance = _compass.register_compass();
_compass_instance = register_compass();
return success;
}
// Read Sensor data
bool AP_Compass_HMC5843::read()
void AP_Compass_HMC5843::read()
{
if (!_initialised) {
// someone has tried to enable a compass for the first time
// mid-flight .... we can't do that yet (especially as we won't
// have the right orientation!)
return false;
return;
}
if (!_compass._healthy[_compass_instance]) {
if (_retry_time != 0) {
if (hal.scheduler->millis() < _retry_time) {
return false;
return;
}
if (!re_initialise()) {
_retry_time = hal.scheduler->millis() + 1000;
hal.i2c->setHighSpeed(false);
return false;
return;
}
}
if (_accum_count == 0) {
accumulate();
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;
if (_retry_time != 0) {
hal.i2c->setHighSpeed(false);
return false;
return;
}
}
_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;
Vector3f field(_mag_x_accum * calibration[0],
_mag_y_accum * calibration[1],
_mag_z_accum * calibration[2]);
field /= _accum_count;
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_compass.last_update = hal.scheduler->micros(); // record time of update
// rotate to the desired orientation
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
_compass._field[_compass_instance].rotate(ROTATION_YAW_90);
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
field.rotate(ROTATION_YAW_90);
}
_copy_to_frontend(_compass_instance);
_compass._healthy[_compass_instance] = true;
return true;
publish_field(field, _compass_instance);
_retry_time = 0;
}

3
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -32,11 +32,12 @@ private:
uint32_t _last_accum_time;
uint8_t _compass_instance;
uint8_t _product_id;
public:
AP_Compass_HMC5843(Compass &compass);
bool init(void);
bool read(void);
void read(void);
void accumulate(void);
// detect the sensor

61
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -22,7 +22,7 @@
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "AP_Compass_PX4.h"
#include <sys/types.h>
@ -43,8 +43,7 @@ extern const AP_HAL::HAL& hal;
// constructor
AP_Compass_PX4::AP_Compass_PX4(Compass &compass):
AP_Compass_Backend(compass),
_num_instances(0)
AP_Compass_Backend(compass)
{}
// detect the sensor
@ -67,20 +66,22 @@ bool AP_Compass_PX4::init(void)
_mag_fd[1] = open(MAG_BASE_DEVICE_PATH"1", O_RDONLY);
_mag_fd[2] = open(MAG_BASE_DEVICE_PATH"2", O_RDONLY);
_num_instances = 0;
_num_sensors = 0;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if (_mag_fd[i] >= 0) {
_num_instances = i+1;
_num_sensors = i+1;
}
}
if (_num_instances == 0) {
if (_num_sensors == 0) {
hal.console->printf("Unable to open " MAG_BASE_DEVICE_PATH"0" "\n");
return false;
}
for (uint8_t i=0; i<_num_instances; i++) {
for (uint8_t i=0; i<_num_sensors; i++) {
// get device id
_compass._dev_id[i] = ioctl(_mag_fd[i], DEVIOCGDEVICEID, 0);
set_dev_id(_instance[i], ioctl(_mag_fd[i], DEVIOCGDEVICEID, 0));
_instance[i] = register_compass();
// average over up to 20 samples
if (ioctl(_mag_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) {
@ -89,13 +90,9 @@ bool AP_Compass_PX4::init(void)
}
// remember if the compass is external
_compass._external[i] = (ioctl(_mag_fd[i], MAGIOCGEXTERNAL, 0) > 0);
if (_compass._external[i]) {
hal.console->printf("Using external compass[%u]\n", (unsigned)i);
}
set_external(_instance[i], ioctl(_mag_fd[i], MAGIOCGEXTERNAL, 0) > 0);
_count[0] = 0;
_sum[i].zero();
_compass._healthy[i] = false;
}
// give the driver a chance to run, and gather one sample
@ -105,21 +102,15 @@ bool AP_Compass_PX4::init(void)
hal.console->printf("Failed initial compass accumulate\n");
}
_compass.product_id = AP_COMPASS_TYPE_PX4;
return true;
}
bool AP_Compass_PX4::read(void)
void AP_Compass_PX4::read(void)
{
// try to accumulate one more sample, so we have the latest data
accumulate();
// consider the compass healthy if we got a reading in the last 0.2s
for (uint8_t i=0; i<_num_instances; i++) {
_compass._healthy[i] = (hal.scheduler->micros64() - _last_timestamp[i] < 200000);
}
for (uint8_t i=0; i<_num_instances; i++) {
for (uint8_t i=0; i<_num_sensors; i++) {
// avoid division by zero if we haven't received any mag reports
if (_count[i] == 0) continue;
@ -130,30 +121,17 @@ bool AP_Compass_PX4::read(void)
// a noop on most boards
_sum[i].rotate(MAG_BOARD_ORIENTATION);
if (_compass._external[i]) {
// add user selectable orientation
_sum[i].rotate((enum Rotation)_compass._orientation[i].get());
} else {
// add in board orientation from AHRS
_sum[i].rotate(_compass._board_orientation);
}
_compass._field[i] = _sum[i];
_compass.apply_corrections(_compass._field[i],i);
publish_field(_sum[i], _instance[i]);
_sum[i].zero();
_count[i] = 0;
}
_compass.last_update = _last_timestamp[get_primary()];
return _compass._healthy[get_primary()];
}
void AP_Compass_PX4::accumulate(void)
{
struct mag_report mag_report;
for (uint8_t i=0; i<_num_instances; i++) {
for (uint8_t i=0; i<_num_sensors; i++) {
while (::read(_mag_fd[i], &mag_report, sizeof(mag_report)) == sizeof(mag_report) &&
mag_report.timestamp != _last_timestamp[i]) {
_sum[i] += Vector3f(mag_report.x, mag_report.y, mag_report.z);
@ -163,15 +141,4 @@ void AP_Compass_PX4::accumulate(void)
}
}
uint8_t AP_Compass_PX4::get_primary(void) const
{
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 (_compass._healthy[i] && (_compass.use_for_yaw(i))) return i;
}
return 0;
}
#endif // CONFIG_HAL_BOARD

12
libraries/AP_Compass/AP_Compass_PX4.h
View File

@ -10,21 +10,17 @@ class AP_Compass_PX4 : public AP_Compass_Backend
{
public:
bool init(void);
bool read(void);
void read(void);
void accumulate(void);
// return the number of compass instances
uint8_t get_count(void) const { return _num_instances; }
// 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;
uint8_t _num_sensors;
uint8_t _instance[COMPASS_MAX_INSTANCES];
int _mag_fd[COMPASS_MAX_INSTANCES];
Vector3f _sum[COMPASS_MAX_INSTANCES];
uint32_t _count[COMPASS_MAX_INSTANCES];

164
libraries/AP_Compass/AP_Compass_VRBRAIN.cpp
View File

@ -1,164 +0,0 @@
/// -*- 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/>.
*/
/*
* AP_Compass_VRBRAIN.cpp - Arduino Library for VRBRAIN magnetometer
*
*/
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "AP_Compass_VRBRAIN.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <drivers/drv_device.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <stdio.h>
#include <errno.h>
extern const AP_HAL::HAL& hal;
// Public Methods //////////////////////////////////////////////////////////////
bool AP_Compass_VRBRAIN::init(void)
{
_mag_fd[0] = open(MAG_DEVICE_PATH, O_RDONLY);
_mag_fd[1] = open(MAG_DEVICE_PATH "1", O_RDONLY);
_num_instances = 0;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if (_mag_fd[i] >= 0) {
_num_instances = i+1;
}
}
if (_num_instances == 0) {
hal.console->printf("Unable to open " MAG_DEVICE_PATH "\n");
return false;
}
for (uint8_t i=0; i<_num_instances; i++) {
// get device id
_dev_id[i] = ioctl(_mag_fd[i], DEVIOCGDEVICEID, 0);
// average over up to 20 samples
if (ioctl(_mag_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) {
hal.console->printf("Failed to setup compass queue\n");
return false;
}
// remember if the compass is external
_external[i] = (ioctl(_mag_fd[i], MAGIOCGEXTERNAL, 0) > 0);
#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52)
//deal with situations where user has cut internal mag on VRBRAIN 4.5
//and uses only one external mag attached to the internal I2C bus
bool external_tmp = _external[i];
if (!_external[i].load()) {
_external[i] = external_tmp;
}
#endif
if (_external[i]) {
hal.console->printf("Using external compass[%u]\n", (unsigned)i);
}
_count[0] = 0;
_sum[i].zero();
_healthy[i] = false;
}
// give the driver a chance to run, and gather one sample
hal.scheduler->delay(40);
accumulate();
if (_count[0] == 0) {
hal.console->printf("Failed initial compass accumulate\n");
}
return true;
}
bool AP_Compass_VRBRAIN::read(void)
{
// try to accumulate one more sample, so we have the latest data
accumulate();
// 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);
}
for (uint8_t i=0; i<_num_instances; i++) {
// avoid division by zero if we haven't received any mag reports
if (_count[i] == 0) continue;
_sum[i] /= _count[i];
_sum[i] *= 1000;
// apply default board orientation for this compass type. This is
// a noop on most boards
_sum[i].rotate(MAG_BOARD_ORIENTATION);
// override any user setting of COMPASS_EXTERNAL
//_external.set(_is_external[0]);
if (_external[i]) {
// add user selectable orientation
_sum[i].rotate((enum Rotation)_orientation[i].get());
} else {
// add in board orientation from AHRS
_sum[i].rotate(_board_orientation);
}
_field[i] = _sum[i];
apply_corrections(_field[i],i);
_sum[i].zero();
_count[i] = 0;
}
last_update = _last_timestamp[get_primary()];
return _healthy[get_primary()];
}
void AP_Compass_VRBRAIN::accumulate(void)
{
struct mag_report mag_report;
for (uint8_t i=0; i<_num_instances; i++) {
while (::read(_mag_fd[i], &mag_report, sizeof(mag_report)) == sizeof(mag_report) &&
mag_report.timestamp != _last_timestamp[i]) {
_sum[i] += Vector3f(mag_report.x, mag_report.y, mag_report.z);
_count[i]++;
_last_timestamp[i] = mag_report.timestamp;
}
}
}
uint8_t AP_Compass_VRBRAIN::get_primary(void) const
{
if (_primary < _num_instances && _healthy[_primary] && use_for_yaw(_primary)) {
return _primary;
}
for (uint8_t i=0; i<_num_instances; i++) {
if (_healthy[i] && (use_for_yaw(i))) return i;
}
return 0;
}
#endif // CONFIG_HAL_BOARD

34
libraries/AP_Compass/AP_Compass_VRBRAIN.h
View File

@ -1,34 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef AP_Compass_VRBRAIN_H
#define AP_Compass_VRBRAIN_H
#include "Compass.h"
class AP_Compass_VRBRAIN : public Compass
{
public:
AP_Compass_VRBRAIN() : Compass() {
product_id = AP_COMPASS_TYPE_VRBRAIN;
_num_instances = 0;
}
bool init(void);
bool read(void);
void accumulate(void);
// return the number of compass instances
uint8_t get_count(void) const { return _num_instances; }
// return the primary compass
uint8_t get_primary(void) const;
private:
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];
};
#endif // AP_Compass_VRBRAIN_H

162
libraries/AP_Compass/Compass.cpp
View File

@ -1,6 +1,16 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL.h>
#include <AP_Progmem.h>
#include "Compass.h"
#include <AP_Vehicle.h>
extern AP_HAL::HAL& hal;
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
#define COMPASS_LEARN_DEFAULT 0
#else
#define COMPASS_LEARN_DEFAULT 1
#endif
const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// index 0 was used for the old orientation matrix
@ -22,7 +32,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @Description: Offset to be added to the compass z-axis values to compensate for metal in the frame
// @Range: -400 400
// @Increment: 1
AP_GROUPINFO("OFS", 1, Compass, _offset[0], 0),
AP_GROUPINFO("OFS", 1, Compass, _state[0].offset, 0),
// @Param: DEC
// @DisplayName: Compass declination
@ -38,14 +48,14 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @Description: Enable or disable the automatic learning of compass offsets
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("LEARN", 3, Compass, _learn, 1), // true if learning calibration
AP_GROUPINFO("LEARN", 3, Compass, _learn, COMPASS_LEARN_DEFAULT),
// @Param: USE
// @DisplayName: Use compass for yaw
// @Description: Enable or disable the use of the compass (instead of the GPS) for determining heading
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("USE", 4, Compass, _use_for_yaw[0], 1), // true if used for DCM yaw
AP_GROUPINFO("USE", 4, Compass, _state[0].use_for_yaw, 1), // true if used for DCM yaw
#if !defined( __AVR_ATmega1280__ )
// @Param: AUTODEC
@ -83,20 +93,20 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @Range: -1000 1000
// @Units: Offset per Amp or at Full Throttle
// @Increment: 1
AP_GROUPINFO("MOT", 7, Compass, _motor_compensation[0], 0),
AP_GROUPINFO("MOT", 7, Compass, _state[0].motor_compensation, 0),
// @Param: ORIENT
// @DisplayName: Compass orientation
// @Description: The orientation of the compass relative to the autopilot board. This will default to the right value for each board type, but can be changed if you have an external compass. See the documentation for your external compass for the right value. The correct orientation should give the X axis forward, the Y axis to the right and the Z axis down. So if your aircraft is pointing west it should show a positive value for the Y axis, and a value close to zero for the X axis. On a PX4 or Pixhawk with an external compass the correct value is zero if the compass is correctly oriented. NOTE: This orientation is combined with any AHRS_ORIENTATION setting.
// @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll90
AP_GROUPINFO("ORIENT", 8, Compass, _orientation[0], ROTATION_NONE),
AP_GROUPINFO("ORIENT", 8, Compass, _state[0].orientation, ROTATION_NONE),
// @Param: EXTERNAL
// @DisplayName: Compass is attached via an external cable
// @Description: Configure compass so it is attached externally. This is auto-detected on PX4 and Pixhawk, but must be set correctly on an APM2. Set to 1 if the compass is externally connected. When externally connected the COMPASS_ORIENT option operates independently of the AHRS_ORIENTATION board orientation option
// @Values: 0:Internal,1:External
// @User: Advanced
AP_GROUPINFO("EXTERNAL", 9, Compass, _external[0], 0),
AP_GROUPINFO("EXTERNAL", 9, Compass, _state[0].external, 0),
#if COMPASS_MAX_INSTANCES > 1
// @Param: OFS2_X
@ -116,7 +126,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @Description: Offset to be added to compass2's z-axis values to compensate for metal in the frame
// @Range: -400 400
// @Increment: 1
AP_GROUPINFO("OFS2", 10, Compass, _offset[1], 0),
AP_GROUPINFO("OFS2", 10, Compass, _state[1].offset, 0),
// @Param: MOT2_X
// @DisplayName: Motor interference compensation to compass2 for body frame X axis
@ -138,7 +148,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @Range: -1000 1000
// @Units: Offset per Amp or at Full Throttle
// @Increment: 1
AP_GROUPINFO("MOT2", 11, Compass, _motor_compensation[1], 0),
AP_GROUPINFO("MOT2", 11, Compass, _state[1].motor_compensation, 0),
// @Param: PRIMARY
// @DisplayName: Choose primary compass
@ -166,7 +176,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @Description: Offset to be added to compass3's z-axis values to compensate for metal in the frame
// @Range: -400 400
// @Increment: 1
AP_GROUPINFO("OFS3", 13, Compass, _offset[2], 0),
AP_GROUPINFO("OFS3", 13, Compass, _state[2].offset, 0),
// @Param: MOT3_X
// @DisplayName: Motor interference compensation to compass3 for body frame X axis
@ -188,7 +198,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @Range: -1000 1000
// @Units: Offset per Amp or at Full Throttle
// @Increment: 1
AP_GROUPINFO("MOT3", 14, Compass, _motor_compensation[2], 0),
AP_GROUPINFO("MOT3", 14, Compass, _state[2].motor_compensation, 0),
#endif
#if COMPASS_MAX_INSTANCES > 1
@ -196,13 +206,13 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @DisplayName: Compass device id
// @Description: Compass device id. Automatically detected, do not set manually
// @User: Advanced
AP_GROUPINFO("DEV_ID", 15, Compass, _dev_id[0], 0),
AP_GROUPINFO("DEV_ID", 15, Compass, _state[0].dev_id, 0),
// @Param: DEV_ID2
// @DisplayName: Compass2 device id
// @Description: Second compass's device id. Automatically detected, do not set manually
// @User: Advanced
AP_GROUPINFO("DEV_ID2", 16, Compass, _dev_id[1], 0),
AP_GROUPINFO("DEV_ID2", 16, Compass, _state[1].dev_id, 0),
#endif
#if COMPASS_MAX_INSTANCES > 2
@ -210,7 +220,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @DisplayName: Compass3 device id
// @Description: Third compass's device id. Automatically detected, do not set manually
// @User: Advanced
AP_GROUPINFO("DEV_ID3", 17, Compass, _dev_id[2], 0),
AP_GROUPINFO("DEV_ID3", 17, Compass, _state[2].dev_id, 0),
#endif
#if COMPASS_MAX_INSTANCES > 1
@ -219,20 +229,20 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @Description: Enable or disable the second compass for determining heading.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("USE2", 18, Compass, _use_for_yaw[1], 1),
AP_GROUPINFO("USE2", 18, Compass, _state[1].use_for_yaw, 1),
// @Param: ORIENT2
// @DisplayName: Compass2 orientation
// @Description: The orientation of the second compass relative to the frame (if external) or autopilot board (if internal).
// @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll90
AP_GROUPINFO("ORIENT2", 19, Compass, _orientation[1], ROTATION_NONE),
AP_GROUPINFO("ORIENT2", 19, Compass, _state[1].orientation, ROTATION_NONE),
// @Param: EXTERN2
// @DisplayName: Compass2 is attached via an external cable
// @Description: Configure second compass so it is attached externally. This is auto-detected on PX4 and Pixhawk.
// @Values: 0:Internal,1:External
// @User: Advanced
AP_GROUPINFO("EXTERN2",20, Compass, _external[1], 0),
AP_GROUPINFO("EXTERN2",20, Compass, _state[1].external, 0),
#endif
#if COMPASS_MAX_INSTANCES > 2
@ -241,20 +251,20 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @Description: Enable or disable the third compass for determining heading.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("USE3", 21, Compass, _use_for_yaw[2], 1),
AP_GROUPINFO("USE3", 21, Compass, _state[2].use_for_yaw, 1),
// @Param: ORIENT3
// @DisplayName: Compass3 orientation
// @Description: The orientation of the third compass relative to the frame (if external) or autopilot board (if internal).
// @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll90
AP_GROUPINFO("ORIENT3", 22, Compass, _orientation[2], ROTATION_NONE),
AP_GROUPINFO("ORIENT3", 22, Compass, _state[2].orientation, ROTATION_NONE),
// @Param: EXTERN3
// @DisplayName: Compass3 is attached via an external cable
// @Description: Configure third compass so it is attached externally. This is auto-detected on PX4 and Pixhawk.
// @Values: 0:Internal,1:External
// @User: Advanced
AP_GROUPINFO("EXTERN3",23, Compass, _external[2], 0),
AP_GROUPINFO("EXTERN3",23, Compass, _state[2].external, 0),
#endif
AP_GROUPEND
@ -265,7 +275,6 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// their values.
//
Compass::Compass(void) :
product_id(AP_COMPASS_TYPE_UNKNOWN),
last_update(0),
_null_init_done(false),
_thr_or_curr(0.0f),
@ -281,7 +290,7 @@ Compass::Compass(void) :
#if COMPASS_MAX_INSTANCES > 1
// default device ids to zero. init() method will overwrite with the actual device ids
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
_dev_id[i] = 0;
_state[i].dev_id = 0;
}
#endif
}
@ -295,11 +304,6 @@ Compass::init()
// detect available backends. Only called once
_detect_backends();
}
product_id = 0; // FIX
//TODO other initializations needed
return true;
}
@ -340,10 +344,8 @@ Compass::_detect_backends(void)
_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
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN
_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
@ -354,9 +356,6 @@ Compass::_detect_backends(void)
_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
@ -371,11 +370,12 @@ Compass::accumulate(void)
bool
Compass::read(void)
{
for (uint8_t i=0; i< _backend_count; i++) {
for (uint8_t i=0; i< _backend_count; i++) {
// call read on each of the backend. This call updates field[i]
_backends[i]->read();
_state[i].healthy = (hal.scheduler->millis() - _state[i].last_update_ms < 500);
}
return _healthy[get_primary()];
return healthy();
}
void
@ -383,7 +383,7 @@ Compass::set_offsets(uint8_t i, const Vector3f &offsets)
{
// sanity check compass instance provided
if (i < COMPASS_MAX_INSTANCES) {
_offset[i].set(offsets);
_state[i].offset.set(offsets);
}
}
@ -392,7 +392,7 @@ Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets)
{
// sanity check compass instance provided
if (i < COMPASS_MAX_INSTANCES) {
_offset[i].set(offsets);
_state[i].offset.set(offsets);
save_offsets(i);
}
}
@ -400,9 +400,9 @@ Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets)
void
Compass::save_offsets(uint8_t i)
{
_offset[i].save(); // save offsets
_state[i].offset.save(); // save offsets
#if COMPASS_MAX_INSTANCES > 1
_dev_id[i].save(); // save device id corresponding to these offsets
_state[i].dev_id.save(); // save device id corresponding to these offsets
#endif
}
@ -417,7 +417,7 @@ Compass::save_offsets(void)
void
Compass::set_motor_compensation(uint8_t i, const Vector3f &motor_comp_factor)
{
_motor_compensation[i].set(motor_comp_factor);
_state[i].motor_compensation.set(motor_comp_factor);
}
void
@ -425,7 +425,7 @@ Compass::save_motor_compensation()
{
_motor_comp_type.save();
for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
_motor_compensation[k].save();
_state[k].motor_compensation.save();
}
}
@ -434,7 +434,6 @@ Compass::set_initial_location(int32_t latitude, int32_t longitude)
{
// if automatic declination is configured, then compute
// the declination based on the initial GPS fix
#if !defined( __AVR_ATmega1280__ )
if (_auto_declination) {
// Set the declination based on the lat/lng from GPS
_declination.set(radians(
@ -442,7 +441,6 @@ Compass::set_initial_location(int32_t latitude, int32_t longitude)
(float)latitude / 10000000,
(float)longitude / 10000000)));
}
#endif
}
/// return true if the compass should be used for yaw calculations
@ -457,7 +455,7 @@ Compass::use_for_yaw(void) const
bool
Compass::use_for_yaw(uint8_t i) const
{
return _use_for_yaw[i];
return _state[i].use_for_yaw;
}
void
@ -485,10 +483,12 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix) const
float cos_pitch_sq = 1.0f-(dcm_matrix.c.x*dcm_matrix.c.x);
// Tilt compensated magnetic field Y component:
float headY = _field[0].y * dcm_matrix.c.z - _field[0].z * dcm_matrix.c.y;
const Vector3f &field = get_field();
float headY = field.y * dcm_matrix.c.z - field.z * dcm_matrix.c.y;
// Tilt compensated magnetic field X component:
float headX = _field[0].x * cos_pitch_sq - dcm_matrix.c.x * (_field[0].y * dcm_matrix.c.y + _field[0].z * dcm_matrix.c.z);
float headX = field.x * cos_pitch_sq - dcm_matrix.c.x * (field.y * dcm_matrix.c.y + field.z * dcm_matrix.c.z);
// magnetic heading
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
@ -525,15 +525,15 @@ bool Compass::configured(uint8_t i)
#if COMPASS_MAX_INSTANCES > 1
// backup detected dev_id
int32_t dev_id_orig = _dev_id[i];
int32_t dev_id_orig = _state[i].dev_id;
// load dev_id from eeprom
_dev_id[i].load();
_state[i].dev_id.load();
// if different then the device has not been configured
if (_dev_id[i] != dev_id_orig) {
if (_state[i].dev_id != dev_id_orig) {
// restore device id
_dev_id[i] = dev_id_orig;
_state[i].dev_id = dev_id_orig;
// return failure
return false;
}
@ -552,27 +552,6 @@ bool Compass::configured(void)
return all_configured;
}
/*
apply offset and motor compensation corrections
*/
void Compass::apply_corrections(Vector3f &mag, uint8_t i)
{
const Vector3f &offsets = _offset[i].get();
const Vector3f &mot = _motor_compensation[i].get();
/*
note that _motor_offset[] is kept even if compensation is not
being applied so it can be logged correctly
*/
mag += offsets;
if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset[i] = mot * _thr_or_curr;
mag += _motor_offset[i];
} else {
_motor_offset[i].zero();
}
}
#define MAG_OFS_X 5.0
#define MAG_OFS_Y 13.0
#define MAG_OFS_Z -18.0
@ -586,54 +565,63 @@ void Compass::setHIL(float roll, float pitch, float yaw)
// create a rotation matrix for the given attitude
R.from_euler(roll, pitch, yaw);
if (_last_declination != get_declination()) {
if (_hil.last_declination != get_declination()) {
_setup_earth_field();
_last_declination = get_declination();
_hil.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);
_hil.field = R.mul_transpose(_hil.Bearth);
_hil.field -= 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);
_hil.field.rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
_hil_mag.rotate((enum Rotation)get_orientation().get());
_hil.field.rotate((enum Rotation)_state[0].orientation.get());
if (!_external[0]) {
if (!_state[0].external) {
// and add in AHRS_ORIENTATION setting if not an external compass
_hil_mag.rotate(get_board_orientation());
_hil.field.rotate(_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;
_hil.field = mag;
}
const Vector3f& Compass::getHIL() const {
return _hil_mag;
return _hil.field;
}
// setup _Bearth
void Compass::_setup_earth_field(void)
{
// assume a earth field strength of 400
_Bearth(400, 0, 0);
_hil.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;
_hil.Bearth = R * _hil.Bearth;
}
/*
set the type of motor compensation to use
*/
void Compass::motor_compensation_type(const uint8_t comp_type)
{
if (_motor_comp_type <= AP_COMPASS_MOT_COMP_CURRENT && _motor_comp_type != (int8_t)comp_type) {
_motor_comp_type = (int8_t)comp_type;
_thr_or_curr = 0; // set current current or throttle to zero
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
set_motor_compensation(i, Vector3f(0,0,0)); // clear out invalid compensation vectors
}
}
}

188
libraries/AP_Compass/Compass.h
View File

@ -10,8 +10,6 @@
#include <AP_HAL.h>
#include "AP_Compass_Backend.h"
extern const AP_HAL::HAL& hal;
// compass product id
#define AP_COMPASS_TYPE_UNKNOWN 0x00
#define AP_COMPASS_TYPE_HIL 0x01
@ -67,14 +65,8 @@ extern const AP_HAL::HAL& hal;
class Compass
{
friend class AP_Compass_Backend;
friend class AP_Compass_HIL;
friend class AP_Compass_VRBRAIN;
friend class AP_Compass_PX4;
friend class AP_Compass_AK8963;
friend class AP_Compass_HMC5843;
public:
uint32_t last_update; ///< micros() time of last update
int16_t product_id; /// product id
uint32_t last_update; //< micros() time of last update
/// Constructor
///
@ -95,16 +87,6 @@ public:
/// 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.
///
/// @param dcm_matrix The current orientation rotation matrix
@ -141,21 +123,18 @@ public:
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]; }
const Vector3f &get_field(uint8_t i) const { return _state[i].field; }
const Vector3f &get_field(void) const { return get_field(get_primary()); }
/// Return the health of a compass
bool healthy(uint8_t i) const { return _healthy[i]; }
bool healthy(uint8_t i) const { return _state[i].healthy; }
bool healthy(void) const { return healthy(get_primary()); }
/// set the current field as a Vector3f
void set_field(const Vector3f &field) { _field[0] = field; }
/// Returns the current offset values
///
/// @returns The current compass offsets.
///
const Vector3f &get_offsets(uint8_t i) const { return _offset[i]; }
const Vector3f &get_offsets(uint8_t i) const { return _state[i].offset; }
const Vector3f &get_offsets(void) const { return get_offsets(get_primary()); }
/// Sets the initial location used to get declination
@ -199,27 +178,15 @@ 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
///
/// @param comp_type 0 = disabled, 1 = enabled use throttle, 2 = enabled use current
///
void motor_compensation_type(const uint8_t comp_type) {
if (_motor_comp_type <= AP_COMPASS_MOT_COMP_CURRENT && _motor_comp_type != (int8_t)comp_type) {
_motor_comp_type = (int8_t)comp_type;
_thr_or_curr = 0; // set current current or throttle to zero
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
set_motor_compensation(i, Vector3f(0,0,0)); // clear out invalid compensation vectors
}
}
}
void motor_compensation_type(const uint8_t comp_type);
/// get the motor compensation value.
uint8_t motor_compensation_type() const {
uint8_t get_motor_compensation_type() const {
return _motor_comp_type;
}
@ -231,7 +198,7 @@ public:
void set_motor_compensation(uint8_t i, const Vector3f &motor_comp_factor);
/// get motor compensation factors as a vector
const Vector3f& get_motor_compensation(uint8_t i) const { return _motor_compensation[i]; }
const Vector3f& get_motor_compensation(uint8_t i) const { return _state[i].motor_compensation; }
const Vector3f& get_motor_compensation(void) const { return get_motor_compensation(get_primary()); }
/// Saves the current motor compensation x/y/z values.
@ -244,13 +211,13 @@ public:
///
/// @returns The current compass offsets.
///
const Vector3f &get_motor_offsets(uint8_t i) const { return _motor_offset[i]; }
const Vector3f &get_motor_offsets(uint8_t i) const { return _state[i].motor_offset; }
const Vector3f &get_motor_offsets(void) const { return get_motor_offsets(get_primary()); }
/// Set the throttle as a percentage from 0.0 to 1.0
/// @param thr_pct throttle expressed as a percentage from 0 to 1.0
void set_throttle(float thr_pct) {
if(_motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
if (_motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
_thr_or_curr = thr_pct;
}
}
@ -258,7 +225,7 @@ public:
/// Set the current used by system in amps
/// @param amps current flowing to the motors expressed in amps
void set_current(float amps) {
if(_motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
if (_motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
_thr_or_curr = amps;
}
}
@ -274,71 +241,102 @@ public:
///
/// @returns the instance number of the primary compass
///
uint8_t get_primary(void) const { return 0; }
void apply_corrections(Vector3f &mag, uint8_t i);
uint8_t get_primary(void) const { return _primary; }
// 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
/// 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];
// board orientation from AHRS
enum Rotation _board_orientation;
AP_Int8 _orientation[COMPASS_MAX_INSTANCES];
AP_Vector3f _offset[COMPASS_MAX_INSTANCES];
#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
#endif
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];
// motor compensation
AP_Int8 _motor_comp_type; // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
AP_Vector3f _motor_compensation[COMPASS_MAX_INSTANCES]; // factors multiplied by throttle and added to compass outputs
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
// HIL variables
Vector3f _Bearth;
float _last_declination;
struct {
Vector3f Bearth;
float last_declination;
Vector3f field;
} _hil;
private:
Vector3f _field[COMPASS_MAX_INSTANCES]; ///< magnetic field strength
/// 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);
// backend objects
AP_Compass_Backend *_backends[COMPASS_MAX_BACKEND];
uint8_t _backend_count;
// primary product id
int16_t _product_id;
// number of registered compasses.
uint8_t _compass_count;
// settable parameters
AP_Int8 _learn;
// board orientation from AHRS
enum Rotation _board_orientation;
// primary instance
AP_Int8 _primary;
// declination in radians
AP_Float _declination;
// enable automatic declination code
AP_Int8 _auto_declination;
// first-time-around flag used by offset nulling
bool _null_init_done;
// used by offset correction
static const uint8_t _mag_history_size = 20;
// motor compensation type
// 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
AP_Int8 _motor_comp_type;
// throttle expressed as a percentage from 0 ~ 1.0 or current expressed in amps
float _thr_or_curr;
struct mag_state {
AP_Int8 external;
bool healthy;
AP_Int8 orientation;
AP_Vector3f offset;
#if COMPASS_MAX_INSTANCES > 1
// device id detected at init.
// saved to eeprom when offsets are saved allowing ram &
// eeprom values to be compared as consistency check
AP_Int32 dev_id;
#endif
AP_Int8 use_for_yaw;
uint8_t mag_history_index;
Vector3i mag_history[_mag_history_size];
// factors multiplied by throttle and added to compass outputs
AP_Vector3f motor_compensation;
// latest compensation added to compass
Vector3f motor_offset;
// corrected magnetic field strength
Vector3f field;
// when we last got data
uint32_t last_update_ms;
} _state[COMPASS_MAX_INSTANCES];
};
#include "AP_Compass_Backend.h"

34
libraries/AP_Compass/Compass_learn.cpp
View File

@ -42,37 +42,39 @@ Compass::learn_offsets(void)
// first time through
_null_init_done = true;
for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
const Vector3f &ofs = _offset[k].get();
const Vector3f &field = _state[k].field;
const Vector3f &ofs = _state[k].offset.get();
for (uint8_t i=0; i<_mag_history_size; i++) {
// fill the history buffer with the current mag vector,
// with the offset removed
_mag_history[k][i] = Vector3i((_field[k].x+0.5f) - ofs.x, (_field[k].y+0.5f) - ofs.y, (_field[k].z+0.5f) - ofs.z);
_state[k].mag_history[i] = Vector3i((field.x+0.5f) - ofs.x, (field.y+0.5f) - ofs.y, (field.z+0.5f) - ofs.z);
}
_mag_history_index[k] = 0;
_state[k].mag_history_index = 0;
}
return;
}
for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
const Vector3f &ofs = _offset[k].get();
const Vector3f &ofs = _state[k].offset.get();
const Vector3f &field = _state[k].field;
Vector3f b1, diff;
float length;
if (ofs.is_nan()) {
// offsets are bad possibly due to a past bug - zero them
_offset[k].set(Vector3f());
_state[k].offset.set(Vector3f());
}
// get a past element
b1 = Vector3f(_mag_history[k][_mag_history_index[k]].x,
_mag_history[k][_mag_history_index[k]].y,
_mag_history[k][_mag_history_index[k]].z);
b1 = Vector3f(_state[k].mag_history[_state[k].mag_history_index].x,
_state[k].mag_history[_state[k].mag_history_index].y,
_state[k].mag_history[_state[k].mag_history_index].z);
// the history buffer doesn't have the offsets
b1 += ofs;
// get the current vector
const Vector3f &b2 = _field[k];
const Vector3f &b2 = field;
// calculate the delta for this sample
diff = b2 - b1;
@ -85,15 +87,15 @@ Compass::learn_offsets(void)
// build up before calculating an offset change, as accuracy
// of the offset change is highly dependent on the size of the
// rotation.
_mag_history_index[k] = (_mag_history_index[k] + 1) % _mag_history_size;
_state[k].mag_history_index = (_state[k].mag_history_index + 1) % _mag_history_size;
continue;
}
// put the vector in the history
_mag_history[k][_mag_history_index[k]] = Vector3i((_field[k].x+0.5f) - ofs.x,
(_field[k].y+0.5f) - ofs.y,
(_field[k].z+0.5f) - ofs.z);
_mag_history_index[k] = (_mag_history_index[k] + 1) % _mag_history_size;
_state[k].mag_history[_state[k].mag_history_index] = Vector3i((field.x+0.5f) - ofs.x,
(field.y+0.5f) - ofs.y,
(field.z+0.5f) - ofs.z);
_state[k].mag_history_index = (_state[k].mag_history_index + 1) % _mag_history_size;
// equation 6 of Bills paper
diff = diff * (gain * (b2.length() - b1.length()) / length);
@ -106,7 +108,7 @@ Compass::learn_offsets(void)
diff *= max_change / length;
}
Vector3f new_offsets = _offset[k].get() - diff;
Vector3f new_offsets = _state[k].offset.get() - diff;
if (new_offsets.is_nan()) {
// don't apply bad offsets
@ -119,6 +121,6 @@ Compass::learn_offsets(void)
new_offsets.z = constrain_float(new_offsets.z, -COMPASS_OFS_LIMIT, COMPASS_OFS_LIMIT);
// set the new offsets
_offset[k].set(new_offsets);
_state[k].offset.set(new_offsets);
}
}