Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: merged in compass device ID changes from master

Browse Source
This commit is contained in:
Andrew Tridgell 2020-05-11 19:38:28 +10:00
parent 03204ac0eb
commit cbb1512ebc
27 changed files with 978 additions and 454 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

799
libraries/AP_Compass/AP_Compass.cpp
View File

File diff suppressed because it is too large Load Diff

151
libraries/AP_Compass/AP_Compass.h
View File

@ -12,6 +12,7 @@
#include "CompassCalibrator.h"
#include "AP_Compass_Backend.h"
#include "Compass_PerMotor.h"
#include <AP_Common/TSIndex.h>
// motor compensation types (for use with motor_comp_enabled)
#define AP_COMPASS_MOT_COMP_DISABLED 0x00
@ -45,8 +46,22 @@
maximum number of compass instances available on this platform. If more
than 1 then redundant sensors may be available
*/
#define COMPASS_MAX_INSTANCES 3
#define COMPASS_MAX_BACKEND 3
#ifndef HAL_BUILD_AP_PERIPH
#ifndef HAL_COMPASS_MAX_SENSORS
#define HAL_COMPASS_MAX_SENSORS 3
#endif
#define COMPASS_MAX_UNREG_DEV 5
#else
#ifndef HAL_COMPASS_MAX_SENSORS
#define HAL_COMPASS_MAX_SENSORS 1
#endif
#define COMPASS_MAX_UNREG_DEV 0
#endif
#define COMPASS_MAX_INSTANCES HAL_COMPASS_MAX_SENSORS
#define COMPASS_MAX_BACKEND HAL_COMPASS_MAX_SENSORS
#define MAX_CONNECTED_MAGS (COMPASS_MAX_UNREG_DEV+COMPASS_MAX_INSTANCES)
class CompassLearn;
@ -87,7 +102,7 @@ public:
/// @returns heading in radians
///
float calculate_heading(const Matrix3f &dcm_matrix) const {
return calculate_heading(dcm_matrix, get_primary());
return calculate_heading(dcm_matrix, 0);
}
float calculate_heading(const Matrix3f &dcm_matrix, uint8_t i) const;
@ -107,6 +122,7 @@ public:
void set_and_save_diagonals(uint8_t i, const Vector3f &diagonals);
void set_and_save_offdiagonals(uint8_t i, const Vector3f &diagonals);
void set_and_save_scale_factor(uint8_t i, float scale_factor);
void set_and_save_orientation(uint8_t i, Rotation orientation);
/// Saves the current offset x/y/z values for one or all compasses
///
@ -121,9 +137,12 @@ public:
// return the number of compass instances
uint8_t get_count(void) const { return _compass_count; }
// return the number of enabled sensors
uint8_t get_num_enabled(void) const;
/// Return the current field as a Vector3f in milligauss
const Vector3f &get_field(uint8_t i) const { return _state[i].field; }
const Vector3f &get_field(void) const { return get_field(get_primary()); }
const Vector3f &get_field(uint8_t i) const { return _get_state(Priority(i)).field; }
const Vector3f &get_field(void) const { return get_field(0); }
/// Return true if we have set a scale factor for a compass
bool have_scale_factor(uint8_t i) const;
@ -166,22 +185,22 @@ public:
bool consistent() const;
/// Return the health of a compass
bool healthy(uint8_t i) const { return _state[i].healthy; }
bool healthy(void) const { return healthy(get_primary()); }
bool healthy(uint8_t i) const { return _get_state(Priority(i)).healthy; }
bool healthy(void) const { return healthy(0); }
uint8_t get_healthy_mask() const;
/// Returns the current offset values
///
/// @returns The current compass offsets in milligauss.
///
const Vector3f &get_offsets(uint8_t i) const { return _state[i].offset; }
const Vector3f &get_offsets(void) const { return get_offsets(get_primary()); }
const Vector3f &get_offsets(uint8_t i) const { return _get_state(Priority(i)).offset; }
const Vector3f &get_offsets(void) const { return get_offsets(0); }
const Vector3f &get_diagonals(uint8_t i) const { return _state[i].diagonals; }
const Vector3f &get_diagonals(void) const { return get_diagonals(get_primary()); }
const Vector3f &get_diagonals(uint8_t i) const { return _get_state(Priority(i)).diagonals; }
const Vector3f &get_diagonals(void) const { return get_diagonals(0); }
const Vector3f &get_offdiagonals(uint8_t i) const { return _state[i].offdiagonals; }
const Vector3f &get_offdiagonals(void) const { return get_offdiagonals(get_primary()); }
const Vector3f &get_offdiagonals(uint8_t i) const { return _get_state(Priority(i)).offdiagonals; }
const Vector3f &get_offdiagonals(void) const { return get_offdiagonals(0); }
// learn offsets accessor
bool learn_offsets_enabled() const { return _learn == LEARN_INFLIGHT; }
@ -190,9 +209,7 @@ public:
bool use_for_yaw(uint8_t i) const;
bool use_for_yaw(void) const;
void set_use_for_yaw(uint8_t i, bool use) {
_state[i].use_for_yaw.set(use);
}
void set_use_for_yaw(uint8_t i, bool use);
/// Sets the local magnetic field declination.
///
@ -229,8 +246,8 @@ 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 _state[i].motor_compensation; }
const Vector3f& get_motor_compensation(void) const { return get_motor_compensation(get_primary()); }
const Vector3f& get_motor_compensation(uint8_t i) const { return _get_state(Priority(i)).motor_compensation; }
const Vector3f& get_motor_compensation(void) const { return get_motor_compensation(0); }
/// Saves the current motor compensation x/y/z values.
///
@ -242,8 +259,8 @@ public:
///
/// @returns The current compass offsets in milligauss.
///
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()); }
const Vector3f &get_motor_offsets(uint8_t i) const { return _get_state(Priority(i)).motor_offset; }
const Vector3f &get_motor_offsets(void) const { return get_motor_offsets(0); }
/// 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
@ -265,13 +282,7 @@ public:
/// @returns True if compass has been configured
///
bool configured(uint8_t i);
bool configured(void);
/// Returns the instance of the primary compass
///
/// @returns the instance number of the primary compass
///
uint8_t get_primary(void) const { return _primary; }
bool configured(char *failure_msg, uint8_t failure_msg_len);
// HIL methods
void setHIL(uint8_t instance, float roll, float pitch, float yaw);
@ -283,11 +294,11 @@ public:
void set_hil_mode(void) { _hil_mode = true; }
// return last update time in microseconds
uint32_t last_update_usec(void) const { return _state[get_primary()].last_update_usec; }
uint32_t last_update_usec(uint8_t i) const { return _state[i].last_update_usec; }
uint32_t last_update_usec(void) const { return last_update_usec(0); }
uint32_t last_update_usec(uint8_t i) const { return _get_state(Priority(i)).last_update_usec; }
uint32_t last_update_ms(void) const { return _state[get_primary()].last_update_ms; }
uint32_t last_update_ms(uint8_t i) const { return _state[i].last_update_ms; }
uint32_t last_update_ms(void) const { return last_update_ms(0); }
uint32_t last_update_ms(uint8_t i) const { return _get_state(Priority(i)).last_update_ms; }
static const struct AP_Param::GroupInfo var_info[];
@ -335,10 +346,20 @@ public:
private:
static Compass *_singleton;
// Use Priority and StateIndex typesafe index types
// to distinguish between two different type of indexing
// We use StateIndex for access by Backend
// and Priority for access by Frontend
DECLARE_TYPESAFE_INDEX(Priority, uint8_t);
DECLARE_TYPESAFE_INDEX(StateIndex, uint8_t);
/// Register a new compas driver, allocating an instance number
///
/// @return number of compass instances
uint8_t register_compass(void);
/// @param dev_id Dev ID of compass to register against
///
/// @return instance number saved against the dev id or first available empty instance number
bool register_compass(int32_t dev_id, uint8_t& instance);
// load backend drivers
bool _add_backend(AP_Compass_Backend *backend);
@ -366,7 +387,7 @@ private:
#if COMPASS_CAL_ENABLED
//keep track of which calibrators have been saved
bool _cal_saved[COMPASS_MAX_INSTANCES];
RestrictIDTypeArray<bool, COMPASS_MAX_INSTANCES, Priority> _cal_saved;
bool _cal_autosave;
#endif
@ -407,16 +428,17 @@ private:
// number of registered compasses.
uint8_t _compass_count;
// number of unregistered compasses.
uint8_t _unreg_compass_count;
// settable parameters
AP_Int8 _learn;
// board orientation from AHRS
enum Rotation _board_orientation = ROTATION_NONE;
// custom rotation matrix
Matrix3f* _custom_rotation;
// primary instance
AP_Int8 _primary;
// declination in radians
AP_Float _declination;
@ -429,15 +451,17 @@ private:
// stores which bit is used to indicate we should log compass readings
uint32_t _log_bit = -1;
// 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;
// automatic compass orientation on calibration
AP_Int8 _rotate_auto;
// custom compass rotation
AP_Float _custom_roll;
AP_Float _custom_pitch;
AP_Float _custom_yaw;
// throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation
float _thr;
@ -445,6 +469,8 @@ private:
struct mag_state {
AP_Int8 external;
bool healthy;
bool registered;
Compass::Priority priority;
AP_Int8 orientation;
AP_Vector3f offset;
AP_Vector3f diagonals;
@ -455,13 +481,8 @@ private:
// saved to eeprom when offsets are saved allowing ram &
// eeprom values to be compared as consistency check
AP_Int32 dev_id;
AP_Int32 expected_dev_id;
int32_t detected_dev_id;
AP_Int8 use_for_yaw;
uint8_t mag_history_index;
Vector3i mag_history[_mag_history_size];
int32_t expected_dev_id;
// factors multiplied by throttle and added to compass outputs
AP_Vector3f motor_compensation;
@ -482,7 +503,34 @@ private:
// accumulated samples, protected by _sem, used by AP_Compass_Backend
Vector3f accum;
uint32_t accum_count;
} _state[COMPASS_MAX_INSTANCES];
// We only copy persistent params
void copy_from(const mag_state& state);
};
//Create an Array of mag_state to be accessible by StateIndex only
RestrictIDTypeArray<mag_state, COMPASS_MAX_INSTANCES+1, StateIndex> _state;
//Convert Priority to StateIndex
StateIndex _get_state_id(Priority priority) const;
//Get State Struct by Priority
const struct mag_state& _get_state(Priority priority) const { return _state[_get_state_id(priority)]; }
//Convert StateIndex to Priority
Priority _get_priority(StateIndex state_id) { return _state[state_id].priority; }
//Method to detect compass beyond initialisation stage
void _detect_runtime(void);
// This method reorganises devid list to match
// priority list, only call before detection at boot
void _reorder_compass_params();
// Update Priority List for Mags, by default, we just
// load them as they come up the first time
Priority _update_priority_list(int32_t dev_id);
//Create Arrays to be accessible by Priority only
RestrictIDTypeArray<AP_Int8, COMPASS_MAX_INSTANCES, Priority> _use_for_yaw;
#if COMPASS_MAX_INSTANCES > 1
RestrictIDTypeArray<AP_Int32, COMPASS_MAX_INSTANCES, Priority> _priority_did_stored_list;
RestrictIDTypeArray<int32_t, COMPASS_MAX_INSTANCES, Priority> _priority_did_list;
#endif
AP_Int16 _offset_max;
@ -493,7 +541,7 @@ private:
AP_Int16 _options;
#if COMPASS_CAL_ENABLED
CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
RestrictIDTypeArray<CompassCalibrator, COMPASS_MAX_INSTANCES, Priority> _calibrator;
#endif
#if COMPASS_MOT_ENABLED
@ -508,7 +556,12 @@ private:
// mask of driver types to not load. Bit positions match DEVTYPE_ in backend
AP_Int32 _driver_type_mask;
#if COMPASS_MAX_UNREG_DEV
// Put extra dev ids detected
AP_Int32 extra_dev_id[COMPASS_MAX_UNREG_DEV];
#endif
AP_Int8 _filter_range;
CompassLearn *learn;

17
libraries/AP_Compass/AP_Compass_AK09916.cpp
View File

@ -104,9 +104,7 @@ AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(AP_HAL::OwnPtr<AP_HAL::I2
return nullptr;
}
if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return nullptr;
}
dev->get_semaphore()->take_blocking();
/* Allow ICM20x48 to shortcut auxiliary bus and host bus */
uint8_t rval;
@ -192,9 +190,10 @@ bool AP_Compass_AK09916::init()
{
AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
if (!bus_sem || !_bus->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (!bus_sem) {
return false;
}
_bus->get_semaphore()->take_blocking();
if (!_bus->configure()) {
hal.console->printf("AK09916: Could not configure the bus\n");
@ -202,7 +201,6 @@ bool AP_Compass_AK09916::init()
}
if (!_reset()) {
hal.console->printf("AK09916: Reset Failed\n");
goto fail;
}
@ -224,7 +222,11 @@ bool AP_Compass_AK09916::init()
_initialized = true;
/* register the compass instance in the frontend */
_compass_instance = register_compass();
_bus->set_device_type(DEVTYPE_AK09916);
if (!register_compass(_bus->get_bus_id(), _compass_instance)) {
goto fail;
}
set_dev_id(_compass_instance, _bus->get_bus_id());
if (_force_external) {
set_external(_compass_instance, true);
@ -232,9 +234,6 @@ bool AP_Compass_AK09916::init()
set_rotation(_compass_instance, _rotation);
_bus->set_device_type(DEVTYPE_AK09916);
set_dev_id(_compass_instance, _bus->get_bus_id());
bus_sem->give();
_bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK09916::_update, void));

12
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -122,10 +122,10 @@ bool AP_Compass_AK8963::init()
{
AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
if (!bus_sem || !_bus->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.console->printf("AK8963: Unable to get bus semaphore\n");
if (!bus_sem) {
return false;
}
_bus->get_semaphore()->take_blocking();
if (!_bus->configure()) {
hal.console->printf("AK8963: Could not configure the bus\n");
@ -155,13 +155,13 @@ bool AP_Compass_AK8963::init()
_initialized = true;
/* register the compass instance in the frontend */
_compass_instance = register_compass();
set_rotation(_compass_instance, _rotation);
_bus->set_device_type(DEVTYPE_AK8963);
if (!register_compass(_bus->get_bus_id(), _compass_instance)) {
goto fail;
}
set_dev_id(_compass_instance, _bus->get_bus_id());
set_rotation(_compass_instance, _rotation);
bus_sem->give();
_bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void));

13
libraries/AP_Compass/AP_Compass_BMM150.cpp
View File

@ -142,10 +142,7 @@ bool AP_Compass_BMM150::init()
uint8_t val = 0;
bool ret;
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.console->printf("BMM150: Unable to get bus semaphore\n");
return false;
}
_dev->get_semaphore()->take_blocking();
// 10 retries for init
_dev->set_retries(10);
@ -214,12 +211,14 @@ bool AP_Compass_BMM150::init()
_dev->get_semaphore()->give();
/* register the compass instance in the frontend */
_compass_instance = register_compass();
_dev->set_device_type(DEVTYPE_BMM150);
if (!register_compass(_dev->get_bus_id(), _compass_instance)) {
return false;
}
set_dev_id(_compass_instance, _dev->get_bus_id());
set_rotation(_compass_instance, _rotation);
_dev->set_device_type(DEVTYPE_BMM150);
set_dev_id(_compass_instance, _dev->get_bus_id());
_perf_err = hal.util->perf_alloc(AP_HAL::Util::PC_COUNT, "BMM150_err");

56
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -4,6 +4,7 @@
#include "AP_Compass_Backend.h"
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
extern const AP_HAL::HAL& hal;
@ -14,8 +15,10 @@ AP_Compass_Backend::AP_Compass_Backend()
void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
{
Compass::mag_state &state = _compass._state[instance];
mag.rotate(MAG_BOARD_ORIENTATION);
Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
if (MAG_BOARD_ORIENTATION != ROTATION_NONE) {
mag.rotate(MAG_BOARD_ORIENTATION);
}
mag.rotate(state.rotation);
if (!state.external) {
@ -27,26 +30,35 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
}
} else {
// add user selectable orientation
#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
Rotation rotation = Rotation(state.orientation.get());
if (rotation == ROTATION_CUSTOM && _compass._custom_rotation) {
mag = *_compass._custom_rotation * mag;
} else {
mag.rotate(rotation);
}
#else
mag.rotate((enum Rotation)state.orientation.get());
#endif
}
}
void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint8_t instance)
{
Compass::mag_state &state = _compass._state[instance];
Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
// note that we do not set last_update_usec here as otherwise the
// EKF and DCM would end up consuming compass data at the full
// sensor rate. We want them to consume only the filtered fields
state.last_update_ms = AP_HAL::millis();
#if COMPASS_CAL_ENABLED
_compass._calibrator[instance].new_sample(mag);
_compass._calibrator[_compass._get_priority(Compass::StateIndex(instance))].new_sample(mag);
#endif
}
void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
{
Compass::mag_state &state = _compass._state[i];
Compass::mag_state &state = _compass._state[Compass::StateIndex(i)];
if (state.diagonals.get().is_zero()) {
state.diagonals.set(Vector3f(1.0f,1.0f,1.0f));
@ -123,7 +135,7 @@ void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint8_t instance,
WITH_SEMAPHORE(_sem);
Compass::mag_state &state = _compass._state[instance];
Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
state.accum += field;
state.accum_count++;
if (max_samples && state.accum_count >= max_samples) {
@ -137,7 +149,7 @@ void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance,
{
WITH_SEMAPHORE(_sem);
Compass::mag_state &state = _compass._state[instance];
Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
if (state.accum_count == 0) {
return;
@ -159,7 +171,7 @@ void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance,
*/
void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t instance)
{
Compass::mag_state &state = _compass._state[instance];
Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
state.field = mag;
@ -169,7 +181,7 @@ void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t ins
void AP_Compass_Backend::set_last_update_usec(uint32_t last_update, uint8_t instance)
{
Compass::mag_state &state = _compass._state[instance];
Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
state.last_update_usec = last_update;
}
@ -177,9 +189,9 @@ void AP_Compass_Backend::set_last_update_usec(uint32_t last_update, uint8_t inst
register a new backend with frontend, returning instance which
should be used in publish_field()
*/
uint8_t AP_Compass_Backend::register_compass(void) const
bool AP_Compass_Backend::register_compass(int32_t dev_id, uint8_t& instance) const
{
return _compass.register_compass();
return _compass.register_compass(dev_id, instance);
}
@ -188,8 +200,9 @@ uint8_t AP_Compass_Backend::register_compass(void) const
*/
void AP_Compass_Backend::set_dev_id(uint8_t instance, uint32_t dev_id)
{
_compass._state[instance].dev_id.set_and_notify(dev_id);
_compass._state[instance].detected_dev_id = dev_id;
_compass._state[Compass::StateIndex(instance)].dev_id.set_and_notify(dev_id);
_compass._state[Compass::StateIndex(instance)].detected_dev_id = dev_id;
_compass._state[Compass::StateIndex(instance)].expected_dev_id = dev_id;
}
/*
@ -197,7 +210,7 @@ void AP_Compass_Backend::set_dev_id(uint8_t instance, uint32_t dev_id)
*/
void AP_Compass_Backend::save_dev_id(uint8_t instance)
{
_compass._state[instance].dev_id.save();
_compass._state[Compass::StateIndex(instance)].dev_id.save();
}
/*
@ -205,20 +218,27 @@ void AP_Compass_Backend::save_dev_id(uint8_t instance)
*/
void AP_Compass_Backend::set_external(uint8_t instance, bool external)
{
if (_compass._state[instance].external != 2) {
_compass._state[instance].external.set_and_notify(external);
if (_compass._state[Compass::StateIndex(instance)].external != 2) {
_compass._state[Compass::StateIndex(instance)].external.set_and_notify(external);
}
}
bool AP_Compass_Backend::is_external(uint8_t instance)
{
return _compass._state[instance].external;
return _compass._state[Compass::StateIndex(instance)].external;
}
// set rotation of an instance
void AP_Compass_Backend::set_rotation(uint8_t instance, enum Rotation rotation)
{
_compass._state[instance].rotation = rotation;
_compass._state[Compass::StateIndex(instance)].rotation = rotation;
#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
// lazily create the custom rotation matrix
if (!_compass._custom_rotation && Rotation(_compass._state[Compass::StateIndex(instance)].orientation.get()) == ROTATION_CUSTOM) {
_compass._custom_rotation = new Matrix3f();
_compass._custom_rotation->from_euler(radians(_compass._custom_roll), radians(_compass._custom_pitch), radians(_compass._custom_yaw));
}
#endif
}
static constexpr float FILTER_KOEF = 0.1f;

4
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -89,7 +89,7 @@ protected:
void drain_accumulated_samples(uint8_t instance, const Vector3f *scale = NULL);
// register a new compass instance with the frontend
uint8_t register_compass(void) const;
bool register_compass(int32_t dev_id, uint8_t& instance) const;
// set dev_id for an instance
void set_dev_id(uint8_t instance, uint32_t dev_id);
@ -113,7 +113,7 @@ protected:
Compass &_compass;
// semaphore for access to shared frontend data
HAL_Semaphore_Recursive _sem;
HAL_Semaphore _sem;
// Check that the compass field is valid by using a mean filter on the vector length
bool field_ok(const Vector3f &field);

66
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -7,7 +7,7 @@
#include "AP_Compass.h"
extern AP_HAL::HAL& hal;
const extern AP_HAL::HAL& hal;
#if COMPASS_CAL_ENABLED
@ -19,7 +19,7 @@ void Compass::cal_update()
bool running = false;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
bool failure;
_calibrator[i].update(failure);
if (failure) {
@ -34,7 +34,7 @@ void Compass::cal_update()
if (_calibrator[i].running()) {
running = true;
} else if (_cal_autosave && !_cal_saved[i] && _calibrator[i].get_status() == CompassCalibrator::Status::SUCCESS) {
_accept_calibration(i);
_accept_calibration(uint8_t(i));
}
}
@ -57,6 +57,11 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
if (!use_for_yaw(i)) {
return false;
}
Priority prio = Priority(i);
if (_priority_did_list[prio] != _priority_did_stored_list[prio]) {
gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires reboot after priority change");
return false;
}
if (_options.get() & uint16_t(Option::CAL_REQUIRE_GPS)) {
if (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) {
gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires GPS lock");
@ -66,22 +71,22 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
if (!is_calibrating()) {
AP_Notify::events.initiated_compass_cal = 1;
}
if (i == get_primary() && _state[i].external != 0) {
_calibrator[i].set_tolerance(_calibration_threshold);
if (i == 0 && _get_state(prio).external != 0) {
_calibrator[prio].set_tolerance(_calibration_threshold);
} else {
// internal compasses or secondary compasses get twice the
// threshold. This is because internal compasses tend to be a
// lot noisier
_calibrator[i].set_tolerance(_calibration_threshold*2);
_calibrator[prio].set_tolerance(_calibration_threshold*2);
}
if (_rotate_auto) {
enum Rotation r = _state[i].external?(enum Rotation)_state[i].orientation.get():ROTATION_NONE;
enum Rotation r = _get_state(prio).external?(enum Rotation)_get_state(prio).orientation.get():ROTATION_NONE;
if (r != ROTATION_CUSTOM) {
_calibrator[i].set_orientation(r, _state[i].external, _rotate_auto>=2);
_calibrator[prio].set_orientation(r, _get_state(prio).external, _rotate_auto>=2);
}
}
_cal_saved[i] = false;
_calibrator[i].start(retry, delay, get_offsets_max(), i);
_cal_saved[prio] = false;
_calibrator[prio].start(retry, delay, get_offsets_max(), i);
// disable compass learning both for calibration and after completion
_learn.set_and_save(0);
@ -120,12 +125,13 @@ void Compass::start_calibration_all(bool retry, bool autosave, float delay, bool
void Compass::_cancel_calibration(uint8_t i)
{
AP_Notify::events.initiated_compass_cal = 0;
Priority prio = Priority(i);
if (_calibrator[i].running() || _calibrator[i].get_status() == CompassCalibrator::Status::WAITING_TO_START) {
if (_calibrator[prio].running() || _calibrator[prio].get_status() == CompassCalibrator::Status::WAITING_TO_START) {
AP_Notify::events.compass_cal_canceled = 1;
}
_cal_saved[i] = false;
_calibrator[i].stop();
_cal_saved[prio] = false;
_calibrator[prio].stop();
}
void Compass::_cancel_calibration_mask(uint8_t mask)
@ -144,14 +150,16 @@ void Compass::cancel_calibration_all()
bool Compass::_accept_calibration(uint8_t i)
{
CompassCalibrator& cal = _calibrator[i];
Priority prio = Priority(i);
CompassCalibrator& cal = _calibrator[prio];
const CompassCalibrator::Status cal_status = cal.get_status();
if (_cal_saved[i] || cal_status == CompassCalibrator::Status::NOT_STARTED) {
if (_cal_saved[prio] || cal_status == CompassCalibrator::Status::NOT_STARTED) {
return true;
} else if (cal_status == CompassCalibrator::Status::SUCCESS) {
_cal_complete_requires_reboot = true;
_cal_saved[i] = true;
_cal_saved[prio] = true;
Vector3f ofs, diag, offdiag;
float scale_factor;
@ -162,8 +170,8 @@ bool Compass::_accept_calibration(uint8_t i)
set_and_save_offdiagonals(i,offdiag);
set_and_save_scale_factor(i,scale_factor);
if (_state[i].external && _rotate_auto >= 2) {
_state[i].orientation.set_and_save_ifchanged(cal.get_orientation());
if (_get_state(prio).external && _rotate_auto >= 2) {
set_and_save_orientation(i, cal.get_orientation());
}
if (!is_calibrating()) {
@ -178,9 +186,9 @@ bool Compass::_accept_calibration(uint8_t i)
bool Compass::_accept_calibration_mask(uint8_t mask)
{
bool success = true;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if ((1<<i) & mask) {
if (!_accept_calibration(i)) {
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if ((1<<uint8_t(i)) & mask) {
if (!_accept_calibration(uint8_t(i))) {
success = false;
}
_calibrator[i].stop();
@ -194,7 +202,7 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
{
uint8_t cal_mask = _get_cal_mask();
for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
for (Priority compass_id(0); compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
// ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(link.get_chan(), MAG_CAL_PROGRESS)) {
// ideally we would only send one progress message per
@ -216,7 +224,7 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
mavlink_msg_mag_cal_progress_send(
link.get_chan(),
compass_id, cal_mask,
uint8_t(compass_id), cal_mask,
(uint8_t)cal_status, attempt, completion_pct, completion_mask,
direction.x, direction.y, direction.z
);
@ -230,7 +238,7 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
{
uint8_t cal_mask = _get_cal_mask();
for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
for (Priority compass_id(0); compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
// ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(link.get_chan(), MAG_CAL_REPORT)) {
// ideally we would only send one progress message per
@ -251,7 +259,7 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
mavlink_msg_mag_cal_report_send(
link.get_chan(),
compass_id, cal_mask,
uint8_t(compass_id), cal_mask,
(uint8_t)cal_status, autosaved,
fitness,
ofs.x, ofs.y, ofs.z,
@ -269,7 +277,7 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
bool Compass::is_calibrating() const
{
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
switch(_calibrator[i].get_status()) {
case CompassCalibrator::Status::NOT_STARTED:
case CompassCalibrator::Status::SUCCESS:
@ -286,9 +294,9 @@ bool Compass::is_calibrating() const
uint8_t Compass::_get_cal_mask() const
{
uint8_t cal_mask = 0;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (_calibrator[i].get_status() != CompassCalibrator::Status::NOT_STARTED) {
cal_mask |= 1 << i;
cal_mask |= 1 << uint8_t(i);
}
}
return cal_mask;
@ -305,7 +313,7 @@ MAV_RESULT Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
case MAV_CMD_DO_START_MAG_CAL: {
result = MAV_RESULT_ACCEPTED;
if (hal.util->get_soft_armed()) {
hal.console->printf("Disarm for compass calibration\n");
gcs().send_text(MAV_SEVERITY_NOTICE, "Disarm to allow compass calibration");
result = MAV_RESULT_FAILED;
break;
}

7
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -48,9 +48,12 @@ AP_Compass_Backend *AP_Compass_HIL::detect()
bool
AP_Compass_HIL::init(void)
{
// register two compass instances
// register compass instances
for (uint8_t i=0; i<HIL_NUM_COMPASSES; i++) {
_compass_instance[i] = register_compass();
uint32_t dev_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 0, DEVTYPE_SITL);
if (!register_compass(dev_id, _compass_instance[i])) {
return false;
}
}
return true;
}

2
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -2,7 +2,7 @@
#include "AP_Compass.h"
#define HIL_NUM_COMPASSES 2
#define HIL_NUM_COMPASSES 1
class AP_Compass_HIL : public AP_Compass_Backend
{

13
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -152,10 +152,11 @@ bool AP_Compass_HMC5843::init()
{
AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
if (!bus_sem || !bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (!bus_sem) {
hal.console->printf("HMC5843: Unable to get bus semaphore\n");
return false;
}
bus_sem->take_blocking();
// high retries for init
_bus->set_retries(10);
@ -193,13 +194,15 @@ bool AP_Compass_HMC5843::init()
// perform an initial read
read();
_compass_instance = register_compass();
//register compass instance
_bus->set_device_type(DEVTYPE_HMC5883);
if (!register_compass(_bus->get_bus_id(), _compass_instance)) {
return false;
}
set_dev_id(_compass_instance, _bus->get_bus_id());
set_rotation(_compass_instance, _rotation);
_bus->set_device_type(DEVTYPE_HMC5883);
set_dev_id(_compass_instance, _bus->get_bus_id());
if (_force_external) {
set_external(_compass_instance, true);
}

14
libraries/AP_Compass/AP_Compass_IST8308.cpp
View File

@ -109,9 +109,7 @@ bool AP_Compass_IST8308::init()
{
uint8_t reset_count = 0;
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
_dev->get_semaphore()->take_blocking();
// high retries for init
_dev->set_retries(10);
@ -160,16 +158,18 @@ bool AP_Compass_IST8308::init()
_dev->get_semaphore()->give();
_instance = register_compass();
//register compass instance
_dev->set_device_type(DEVTYPE_IST8308);
if (!register_compass(_dev->get_bus_id(), _instance)) {
return false;
}
set_dev_id(_instance, _dev->get_bus_id());
printf("%s found on bus %u id %u address 0x%02x\n", name,
_dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address());
set_rotation(_instance, _rotation);
_dev->set_device_type(DEVTYPE_IST8308);
set_dev_id(_instance, _dev->get_bus_id());
if (_force_external) {
set_external(_instance, true);
}

14
libraries/AP_Compass/AP_Compass_IST8310.cpp
View File

@ -105,9 +105,7 @@ bool AP_Compass_IST8310::init()
{
uint8_t reset_count = 0;
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
_dev->get_semaphore()->take_blocking();
// high retries for init
_dev->set_retries(10);
@ -153,16 +151,18 @@ bool AP_Compass_IST8310::init()
_dev->get_semaphore()->give();
_instance = register_compass();
// register compass instance
_dev->set_device_type(DEVTYPE_IST8310);
if (!register_compass(_dev->get_bus_id(), _instance)) {
return false;
}
set_dev_id(_instance, _dev->get_bus_id());
printf("%s found on bus %u id %u address 0x%02x\n", name,
_dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address());
set_rotation(_instance, _rotation);
_dev->set_device_type(DEVTYPE_IST8310);
set_dev_id(_instance, _dev->get_bus_id());
if (_force_external) {
set_external(_instance, true);
}

13
libraries/AP_Compass/AP_Compass_LIS3MDL.cpp
View File

@ -75,9 +75,7 @@ AP_Compass_LIS3MDL::AP_Compass_LIS3MDL(AP_HAL::OwnPtr<AP_HAL::Device> _dev,
bool AP_Compass_LIS3MDL::init()
{
if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
dev->get_semaphore()->take_blocking();
if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
dev->set_read_flag(0xC0);
@ -107,7 +105,11 @@ bool AP_Compass_LIS3MDL::init()
dev->get_semaphore()->give();
/* register the compass instance in the frontend */
compass_instance = register_compass();
dev->set_device_type(DEVTYPE_LIS3MDL);
if (!register_compass(dev->get_bus_id(), compass_instance)) {
return false;
}
set_dev_id(compass_instance, dev->get_bus_id());
printf("Found a LIS3MDL on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance);
@ -117,9 +119,6 @@ bool AP_Compass_LIS3MDL::init()
set_external(compass_instance, true);
}
dev->set_device_type(DEVTYPE_LIS3MDL);
set_dev_id(compass_instance, dev->get_bus_id());
// call timer() at 80Hz
dev->register_periodic_callback(1000000U/80U,
FUNCTOR_BIND_MEMBER(&AP_Compass_LIS3MDL::timer, void));

14
libraries/AP_Compass/AP_Compass_LSM303D.cpp
View File

@ -266,13 +266,14 @@ bool AP_Compass_LSM303D::init(enum Rotation rotation)
_initialised = true;
/* register the compass instance in the frontend */
_compass_instance = register_compass();
_dev->set_device_type(DEVTYPE_LSM303D);
if (!register_compass(_dev->get_bus_id(), _compass_instance)) {
return false;
}
set_dev_id(_compass_instance, _dev->get_bus_id());
set_rotation(_compass_instance, rotation);
_dev->set_device_type(DEVTYPE_LSM303D);
set_dev_id(_compass_instance, _dev->get_bus_id());
// read at 91Hz. We don't run at 100Hz as fetching data too fast can cause some very
// odd periodic changes in the output data
_dev->register_periodic_callback(11000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, void));
@ -282,9 +283,7 @@ bool AP_Compass_LSM303D::init(enum Rotation rotation)
bool AP_Compass_LSM303D::_hardware_init()
{
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
AP_HAL::panic("LSM303D: Unable to get semaphore");
}
_dev->get_semaphore()->take_blocking();
// initially run the bus at low speed
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
@ -292,7 +291,6 @@ bool AP_Compass_LSM303D::_hardware_init()
// Test WHOAMI
uint8_t whoami = _register_read(ADDR_WHO_AM_I);
if (whoami != WHO_I_AM) {
hal.console->printf("LSM303D: unexpected WHOAMI 0x%x\n", (unsigned)whoami);
goto fail_whoami;
}

12
libraries/AP_Compass/AP_Compass_LSM9DS1.cpp
View File

@ -83,10 +83,11 @@ bool AP_Compass_LSM9DS1::init()
{
AP_HAL::Semaphore *bus_sem = _dev->get_semaphore();
if (!bus_sem || !bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (!bus_sem) {
hal.console->printf("LSM9DS1: Unable to get bus semaphore\n");
return false;
}
bus_sem->take_blocking();
if (!_check_id()) {
hal.console->printf("LSM9DS1: Could not check id\n");
@ -103,12 +104,15 @@ bool AP_Compass_LSM9DS1::init()
goto errout;
}
_compass_instance = register_compass();
//register compass instance
_dev->set_device_type(DEVTYPE_LSM9DS1);
if (!register_compass(_dev->get_bus_id(), _compass_instance)) {
goto errout;
}
set_dev_id(_compass_instance, _dev->get_bus_id());
set_rotation(_compass_instance, _rotation);
_dev->set_device_type(DEVTYPE_LSM9DS1);
set_dev_id(_compass_instance, _dev->get_bus_id());
_dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM9DS1::_update, void));

13
libraries/AP_Compass/AP_Compass_MAG3110.cpp
View File

@ -116,13 +116,14 @@ bool AP_Compass_MAG3110::init(enum Rotation rotation)
read();
/* register the compass instance in the frontend */
_compass_instance = register_compass();
_dev->set_device_type(DEVTYPE_MAG3110);
if (!register_compass(_dev->get_bus_id(), _compass_instance)) {
return false;
}
set_dev_id(_compass_instance, _dev->get_bus_id());
set_rotation(_compass_instance, rotation);
_dev->set_device_type(DEVTYPE_MAG3110);
set_dev_id(_compass_instance, _dev->get_bus_id());
set_external(_compass_instance, true);
// read at 75Hz
@ -135,9 +136,7 @@ bool AP_Compass_MAG3110::_hardware_init()
{
AP_HAL::Semaphore *bus_sem = _dev->get_semaphore();
if (!bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
AP_HAL::panic("MAG3110: Unable to get semaphore");
}
bus_sem->take_blocking();
// initially run the bus at low speed
_dev->set_speed(AP_HAL::Device::SPEED_LOW);

23
libraries/AP_Compass/AP_Compass_MMC3416.cpp
View File

@ -68,9 +68,7 @@ AP_Compass_MMC3416::AP_Compass_MMC3416(AP_HAL::OwnPtr<AP_HAL::Device> _dev,
bool AP_Compass_MMC3416::init()
{
if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
dev->get_semaphore()->take_blocking();
dev->set_retries(10);
@ -92,7 +90,12 @@ bool AP_Compass_MMC3416::init()
dev->get_semaphore()->give();
/* register the compass instance in the frontend */
compass_instance = register_compass();
dev->set_device_type(DEVTYPE_MMC3416);
if (!register_compass(dev->get_bus_id(), compass_instance)) {
return false;
}
set_dev_id(compass_instance, dev->get_bus_id());
printf("Found a MMC3416 on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance);
@ -102,9 +105,6 @@ bool AP_Compass_MMC3416::init()
set_external(compass_instance, true);
}
dev->set_device_type(DEVTYPE_MMC3416);
set_dev_id(compass_instance, dev->get_bus_id());
dev->set_retries(1);
// call timer() at 100Hz
@ -224,6 +224,15 @@ void AP_Compass_MMC3416::timer()
}
#if 0
// @LoggerMessage: MMO
// @Description: MMC3416 compass data
// @Field: TimeUS: Time since system startup
// @Field: Nx: new measurement X axis
// @Field: Ny: new measurement Y axis
// @Field: Nz: new measurement Z axis
// @Field: Ox: new offset X axis
// @Field: Oy: new offset Y axis
// @Field: Oz: new offset Z axis
AP::logger().Write("MMO", "TimeUS,Nx,Ny,Nz,Ox,Oy,Oz", "Qffffff",
AP_HAL::micros64(),
(double)new_offset.x,

14
libraries/AP_Compass/AP_Compass_QMC5883L.cpp
View File

@ -85,9 +85,7 @@ AP_Compass_QMC5883L::AP_Compass_QMC5883L(AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool AP_Compass_QMC5883L::init()
{
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
_dev->get_semaphore()->take_blocking();
_dev->set_retries(10);
@ -115,16 +113,18 @@ bool AP_Compass_QMC5883L::init()
_dev->get_semaphore()->give();
_instance = register_compass();
//register compass instance
_dev->set_device_type(DEVTYPE_QMC5883L);
if (!register_compass(_dev->get_bus_id(), _instance)) {
return false;
}
set_dev_id(_instance, _dev->get_bus_id());
printf("%s found on bus %u id %u address 0x%02x\n", name,
_dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address());
set_rotation(_instance, _rotation);
_dev->set_device_type(DEVTYPE_QMC5883L);
set_dev_id(_instance, _dev->get_bus_id());
if (_force_external) {
set_external(_instance, true);
}

17
libraries/AP_Compass/AP_Compass_RM3100.cpp
View File

@ -93,9 +93,7 @@ AP_Compass_RM3100::AP_Compass_RM3100(AP_HAL::OwnPtr<AP_HAL::Device> _dev,
bool AP_Compass_RM3100::init()
{
if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
dev->get_semaphore()->take_blocking();
if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
// read has high bit set for SPI
@ -145,19 +143,20 @@ bool AP_Compass_RM3100::init()
dev->get_semaphore()->give();
/* register the compass instance in the frontend */
compass_instance = register_compass();
dev->set_device_type(DEVTYPE_RM3100);
if (!register_compass(dev->get_bus_id(), compass_instance)) {
return false;
}
set_dev_id(compass_instance, dev->get_bus_id());
hal.console->printf("RM3100: Found at address 0x%x as compass %u\n", dev->get_bus_address(), compass_instance);
set_rotation(compass_instance, rotation);
if (force_external) {
set_external(compass_instance, true);
}
dev->set_device_type(DEVTYPE_RM3100);
set_dev_id(compass_instance, dev->get_bus_id());
// call timer() at 80Hz
dev->register_periodic_callback(1000000U/80U,
FUNCTOR_BIND_MEMBER(&AP_Compass_RM3100::timer, void));

39
libraries/AP_Compass/AP_Compass_SITL.cpp
View File

@ -10,17 +10,29 @@ AP_Compass_SITL::AP_Compass_SITL()
{
if (_sitl != nullptr) {
_compass._setup_earth_field();
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
// default offsets to correct value
for (uint8_t i=0; i<MAX_CONNECTED_MAGS; i++) {
uint32_t dev_id = _sitl->mag_devid[i];
if (dev_id == 0) {
continue;
}
uint8_t instance;
if (!register_compass(dev_id, instance)) {
continue;
} else if (_num_compass<MAX_SITL_COMPASSES) {
_compass_instance[_num_compass] = instance;
set_dev_id(_compass_instance[_num_compass], dev_id);
// save so the compass always comes up configured in SITL
save_dev_id(_compass_instance[_num_compass]);
_num_compass++;
}
}
// Scroll through the registered compasses, and set the offsets
for (uint8_t i=0; i<_num_compass; i++) {
if (_compass.get_offsets(i).is_zero()) {
_compass.set_offsets(i, _sitl->mag_ofs);
}
_compass_instance[i] = register_compass();
set_dev_id(_compass_instance[i], AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 0, DEVTYPE_SITL));
// save so the compass always comes up configured in SITL
save_dev_id(_compass_instance[i]);
}
// make first compass external
@ -109,12 +121,17 @@ void AP_Compass_SITL::_timer()
new_mag_data = _eliptical_corr * new_mag_data;
new_mag_data -= _sitl->mag_ofs.get();
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
for (uint8_t i=0; i<_num_compass; i++) {
Vector3f f = new_mag_data;
if (i == 0) {
// rotate the first compass, allowing for testing of external compass rotation
f.rotate_inverse((enum Rotation)_sitl->mag_orient.get());
f.rotate(get_board_orientation());
// and add in AHRS_ORIENTATION setting if not an external compass
if (get_board_orientation() == ROTATION_CUSTOM) {
f = _sitl->ahrs_rotation * f;
} else {
f.rotate(get_board_orientation());
}
// scale the first compass to simulate sensor scale factor errors
f *= _sitl->mag_scaling;
@ -126,7 +143,7 @@ void AP_Compass_SITL::_timer()
void AP_Compass_SITL::read()
{
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
for (uint8_t i=0; i<_num_compass; i++) {
drain_accumulated_samples(_compass_instance[i], nullptr);
}
}

5
libraries/AP_Compass/AP_Compass_SITL.h
View File

@ -9,7 +9,7 @@
#include <AP_Math/AP_Math.h>
#include <AP_Declination/AP_Declination.h>
#define SITL_NUM_COMPASSES 3
#define MAX_SITL_COMPASSES 3
class AP_Compass_SITL : public AP_Compass_Backend {
public:
@ -18,7 +18,8 @@ public:
void read(void) override;
private:
uint8_t _compass_instance[SITL_NUM_COMPASSES];
uint8_t _compass_instance[MAX_SITL_COMPASSES];
uint8_t _num_compass;
SITL::SITL *_sitl;
// delay buffer variables

59
libraries/AP_Compass/AP_Compass_UAVCAN.cpp
View File

@ -69,44 +69,46 @@ void AP_Compass_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
}
}
AP_Compass_Backend* AP_Compass_UAVCAN::probe()
AP_Compass_Backend* AP_Compass_UAVCAN::probe(uint8_t index)
{
WITH_SEMAPHORE(_sem_registry);
AP_Compass_UAVCAN* driver = nullptr;
for (uint8_t i = 0; i < COMPASS_MAX_BACKEND; i++) {
if (!_detected_modules[i].driver && _detected_modules[i].ap_uavcan) {
// Register new Compass mode to a backend
driver = new AP_Compass_UAVCAN(_detected_modules[i].ap_uavcan, _detected_modules[i].node_id, _detected_modules[i].sensor_id);
if (driver) {
_detected_modules[i].driver = driver;
driver->init();
debug_mag_uavcan(2,
_detected_modules[i].ap_uavcan->get_driver_index(),
"Found Mag Node %d on Bus %d Sensor ID %d\n",
_detected_modules[i].node_id,
_detected_modules[i].ap_uavcan->get_driver_index(),
_detected_modules[i].sensor_id);
if (!_detected_modules[index].driver && _detected_modules[index].ap_uavcan) {
WITH_SEMAPHORE(_sem_registry);
// Register new Compass mode to a backend
driver = new AP_Compass_UAVCAN(_detected_modules[index].ap_uavcan, _detected_modules[index].node_id, _detected_modules[index].sensor_id);
if (driver) {
if (!driver->init()) {
delete driver;
return nullptr;
}
break;
_detected_modules[index].driver = driver;
debug_mag_uavcan(2,
_detected_modules[index].ap_uavcan->get_driver_index(),
"Found Mag Node %d on Bus %d Sensor ID %d\n",
_detected_modules[index].node_id,
_detected_modules[index].ap_uavcan->get_driver_index(),
_detected_modules[index].sensor_id);
}
}
return driver;
}
void AP_Compass_UAVCAN::init()
bool AP_Compass_UAVCAN::init()
{
_instance = register_compass();
uint32_t devid = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,
_ap_uavcan->get_driver_index(),
_node_id,
1); // the 1 is arbitrary
_sensor_id + 1); // we use sensor_id as devtype
// Adding 1 is necessary to allow backward compatibilty, where this field was set as 1 by default
if (!register_compass(devid, _instance)) {
return false;
}
set_dev_id(_instance, devid);
set_external(_instance, true);
debug_mag_uavcan(2, _ap_uavcan->get_driver_index(), "AP_Compass_UAVCAN loaded\n\r");
return true;
}
AP_Compass_UAVCAN* AP_Compass_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id)
@ -144,6 +146,21 @@ AP_Compass_UAVCAN* AP_Compass_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, u
}
}
}
struct DetectedModules tempslot;
// Sort based on the node_id, larger values first
// we do this, so that we have repeatable compass
// registration, especially in cases of extraneous
// CAN compass is connected.
for (uint8_t i = 1; i < COMPASS_MAX_BACKEND; i++) {
for (uint8_t j = i; j > 0; j--) {
if (_detected_modules[j].node_id > _detected_modules[j-1].node_id) {
tempslot = _detected_modules[j];
_detected_modules[j] = _detected_modules[j-1];
_detected_modules[j-1] = tempslot;
}
}
}
return nullptr;
}

4
libraries/AP_Compass/AP_Compass_UAVCAN.h
View File

@ -15,13 +15,13 @@ public:
void read(void) override;
static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
static AP_Compass_Backend* probe();
static AP_Compass_Backend* probe(uint8_t index);
static void handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MagCb &cb);
static void handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Mag2Cb &cb);
private:
void init();
bool init();
// callback for UAVCAN messages
void handle_mag_msg(const Vector3f &mag);

11
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -878,15 +878,16 @@ bool CompassCalibrator::calculate_orientation(void)
pass = _orientation_confidence > variance_threshold;
}
if (!pass) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u/%u %.1f", _compass_idx,
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u/%u %.1f", _compass_idx,
besti, besti2, (double)_orientation_confidence);
(void)besti2;
} else if (besti == _orientation) {
// no orientation change
gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence);
} else if (!_is_external || !_fix_orientation) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence);
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence);
} else {
gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f", _compass_idx, besti, _orientation, (double)_orientation_confidence);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f", _compass_idx, besti, _orientation, (double)_orientation_confidence);
}
if (!pass) {
@ -956,7 +957,7 @@ bool CompassCalibrator::fix_radius(void)
if (correction > COMPASS_MAX_SCALE_FACTOR || correction < COMPASS_MIN_SCALE_FACTOR) {
// don't allow more than 30% scale factor correction
gcs().send_text(MAV_SEVERITY_ERROR, "Mag(%u) bad radius %.0f expected %.0f",
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag(%u) bad radius %.0f expected %.0f",
_compass_idx,
_params.radius,
expected_radius);

39
libraries/AP_Compass/Compass_learn.cpp
View File

@ -9,6 +9,7 @@
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
#include <AP_Vehicle/AP_Vehicle.h>
#if COMPASS_LEARN_ENABLED
@ -19,11 +20,11 @@ CompassLearn::CompassLearn(Compass &_compass) :
compass(_compass)
{
gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Initialised");
for (uint8_t i=0; i<compass.get_count(); i++) {
if (compass._state[i].use_for_yaw) {
for (Compass::Priority i(0); i<compass.get_count(); i++) {
if (compass._use_for_yaw[Compass::Priority(i)]) {
// reset scale factors, we can't learn scale factors in
// flight
compass.set_and_save_scale_factor(i, 0.0);
compass.set_and_save_scale_factor(uint8_t(i), 0.0);
}
}
}
@ -48,9 +49,6 @@ void CompassLearn::update(void)
return;
}
// remember primary mag
primary_mag = compass.get_primary();
// setup the expected earth field in mGauss at this location
mag_ef = AP_Declination::get_earth_field_ga(loc) * 1000;
have_earth_field = true;
@ -58,8 +56,8 @@ void CompassLearn::update(void)
// form eliptical correction matrix and invert it. This is
// needed to remove the effects of the eliptical correction
// when calculating new offsets
const Vector3f &diagonals = compass.get_diagonals(primary_mag);
const Vector3f &offdiagonals = compass.get_offdiagonals(primary_mag);
const Vector3f &diagonals = compass.get_diagonals(0);
const Vector3f &offdiagonals = compass.get_offdiagonals(0);
mat = Matrix3f(
diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.x, diagonals.y, offdiagonals.z,
@ -87,7 +85,7 @@ void CompassLearn::update(void)
return;
}
Vector3f field = compass.get_field(primary_mag);
Vector3f field = compass.get_field(0);
Vector3f field_change = field - last_field;
if (field_change.length() < min_field_change) {
return;
@ -97,7 +95,7 @@ void CompassLearn::update(void)
WITH_SEMAPHORE(sem);
// give a sample to the backend to process
new_sample.field = field;
new_sample.offsets = compass.get_offsets(primary_mag);
new_sample.offsets = compass.get_offsets(0);
new_sample.attitude = Vector3f(ahrs.roll, ahrs.pitch, ahrs.yaw);
sample_available = true;
last_field = field;
@ -105,6 +103,16 @@ void CompassLearn::update(void)
}
if (sample_available) {
// @LoggerMessage: COFS
// @Description: Current compass learn offsets
// @Field: TimeUS: Time since system startup
// @Field: OfsX: best learnt offset, x-axis
// @Field: OfsY: best learnt offset, y-axis
// @Field: OfsZ: best learnt offset, z-axis
// @Field: Var: error of best offset vector
// @Field: Yaw: best learnt yaw
// @Field: WVar: error of best learn yaw
// @Field: N: number of samples used
AP::logger().Write("COFS", "TimeUS,OfsX,OfsY,OfsZ,Var,Yaw,WVar,N", "QffffffI",
AP_HAL::micros64(),
(double)best_offsets.x,
@ -120,12 +128,12 @@ void CompassLearn::update(void)
WITH_SEMAPHORE(sem);
// set offsets to current best guess
compass.set_offsets(primary_mag, best_offsets);
compass.set_offsets(0, best_offsets);
// set non-primary offsets to match primary
Vector3f field_primary = compass.get_field(primary_mag);
for (uint8_t i=0; i<compass.get_count(); i++) {
if (i == primary_mag || !compass._state[i].use_for_yaw) {
Vector3f field_primary = compass.get_field(0);
for (uint8_t i=1; i<compass.get_count(); i++) {
if (!compass._use_for_yaw[Compass::Priority(i)]) {
continue;
}
Vector3f field2 = compass.get_field(i);
@ -138,10 +146,9 @@ void CompassLearn::update(void)
// set the offsets and enable compass for EKF use. Let the
// EKF learn the remaining compass offset error
for (uint8_t i=0; i<compass.get_count(); i++) {
if (compass._state[i].use_for_yaw) {
if (compass._use_for_yaw[Compass::Priority(i)]) {
compass.save_offsets(i);
compass.set_and_save_scale_factor(i, 0.0);
compass.set_use_for_yaw(i, true);
}
}
compass.set_learn_type(Compass::LEARN_NONE, true);

1
libraries/AP_Compass/Compass_learn.h
View File

@ -51,7 +51,6 @@ private:
float best_yaw_deg;
float worst_error;
bool converged;
uint8_t primary_mag;
void io_timer(void);
void process_sample(const struct sample &s);