AP_Compass: merged in compass device ID changes from master

libraries/AP_Compass/AP_Compass.h

@@ -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
+#ifndef HAL_BUILD_AP_PERIPH
-#define COMPASS_MAX_BACKEND   3
+#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(uint8_t i) const { return _get_state(Priority(i)).field; }
-    const Vector3f &get_field(void) const { return get_field(get_primary()); }
+    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(uint8_t i) const { return _get_state(Priority(i)).healthy; }
-    bool healthy(void) const { return healthy(get_primary()); }
+    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(uint8_t i) const { return _get_state(Priority(i)).offset; }
-    const Vector3f &get_offsets(void) const { return get_offsets(get_primary()); }
+    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(uint8_t i) const { return _get_state(Priority(i)).diagonals; }
-    const Vector3f &get_diagonals(void) const { return get_diagonals(get_primary()); }
+    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(uint8_t i) const { return _get_state(Priority(i)).offdiagonals; }
-    const Vector3f &get_offdiagonals(void) const { return get_offdiagonals(get_primary()); }
+    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) {
+    void set_use_for_yaw(uint8_t i, bool use);
-        _state[i].use_for_yaw.set(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(uint8_t i) const { return _get_state(Priority(i)).motor_compensation; }
-    const Vector3f& get_motor_compensation(void) const { return get_motor_compensation(get_primary()); }
+    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(uint8_t i) const { return _get_state(Priority(i)).motor_offset; }
-    const Vector3f &get_motor_offsets(void) const { return get_motor_offsets(get_primary()); }
+    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);
+    bool configured(char *failure_msg, uint8_t failure_msg_len);
-
-    /// Returns the instance of the primary compass
-    ///
-    /// @returns                    the instance number of the primary compass
-    ///
-    uint8_t get_primary(void) const { return _primary; }
 
     // 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(void) const { return last_update_usec(0); }
-    uint32_t last_update_usec(uint8_t i) const { return _state[i].last_update_usec; }
+    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(void) const { return last_update_ms(0); }
-    uint32_t last_update_ms(uint8_t i) const { return _state[i].last_update_ms; }
+    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
+    /// @param  dev_id                   Dev ID of compass to register against
-    uint8_t register_compass(void);
+    ///
+    /// @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,9 +451,6 @@ 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;
@@ -439,12 +458,19 @@ private:
     // 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;
 
     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;
-
+        int32_t expected_dev_id;
-        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;
@@ -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
@@ -509,6 +557,11 @@ 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;

libraries/AP_Compass/AP_Compass_AK09916.cpp

@@ -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)) {
+    dev->get_semaphore()->take_blocking();
-        return nullptr;
-    }
 
     /* 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));

libraries/AP_Compass/AP_Compass_AK8963.cpp

@@ -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)) {
+    if (!bus_sem) {
-        hal.console->printf("AK8963: Unable to get bus semaphore\n");
         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));

libraries/AP_Compass/AP_Compass_BMM150.cpp

@@ -142,10 +142,7 @@ bool AP_Compass_BMM150::init()
     uint8_t val = 0;
     bool ret;
 
-    if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    _dev->get_semaphore()->take_blocking();
-        hal.console->printf("BMM150: Unable to get bus semaphore\n");
-        return false;
-    }
 
     // 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");
 

libraries/AP_Compass/AP_Compass_Backend.cpp

@@ -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];
+    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[Compass::StateIndex(instance)].dev_id.set_and_notify(dev_id);
-    _compass._state[instance].detected_dev_id = 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) {
+    if (_compass._state[Compass::StateIndex(instance)].external != 2) {
-        _compass._state[instance].external.set_and_notify(external);
+        _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;

libraries/AP_Compass/AP_Compass_Backend.h

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

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -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) {
+    if (i == 0 && _get_state(prio).external != 0) {
-        _calibrator[i].set_tolerance(_calibration_threshold);
+        _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;
+    _cal_saved[prio] = false;
-    _calibrator[i].start(retry, delay, get_offsets_max(), i);
+    _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;
+    _cal_saved[prio] = false;
-    _calibrator[i].stop();
+    _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) {
+        if (_get_state(prio).external && _rotate_auto >= 2) {
-            _state[i].orientation.set_and_save_ifchanged(cal.get_orientation());
+            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++) {
+    for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
-        if ((1<<i) & mask) {
+        if ((1<<uint8_t(i)) & mask) {
-            if (!_accept_calibration(i)) {
+            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;
         }

libraries/AP_Compass/AP_Compass_HIL.cpp

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

libraries/AP_Compass/AP_Compass_HIL.h

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

libraries/AP_Compass/AP_Compass_HMC5843.cpp

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

libraries/AP_Compass/AP_Compass_IST8308.cpp

@@ -109,9 +109,7 @@ bool AP_Compass_IST8308::init()
 {
     uint8_t reset_count = 0;
 
-    if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    _dev->get_semaphore()->take_blocking();
-        return false;
-    }
 
     // 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);
     }

libraries/AP_Compass/AP_Compass_IST8310.cpp

@@ -105,9 +105,7 @@ bool AP_Compass_IST8310::init()
 {
     uint8_t reset_count = 0;
 
-    if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    _dev->get_semaphore()->take_blocking();
-        return false;
-    }
 
     // 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);
     }

libraries/AP_Compass/AP_Compass_LIS3MDL.cpp

@@ -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)) {
+    dev->get_semaphore()->take_blocking();
-        return false;
-    }
 
     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));

libraries/AP_Compass/AP_Compass_LSM303D.cpp

@@ -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)) {
+    _dev->get_semaphore()->take_blocking();
-        AP_HAL::panic("LSM303D: Unable to get semaphore");
-    }
 
     // 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;
     }
 

libraries/AP_Compass/AP_Compass_LSM9DS1.cpp

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

libraries/AP_Compass/AP_Compass_MAG3110.cpp

@@ -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)) {
+    bus_sem->take_blocking();
-        AP_HAL::panic("MAG3110: Unable to get semaphore");
-    }
 
     // initially run the bus at low speed
     _dev->set_speed(AP_HAL::Device::SPEED_LOW);

libraries/AP_Compass/AP_Compass_MMC3416.cpp

@@ -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)) {
+    dev->get_semaphore()->take_blocking();
-        return false;
-    }
 
     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,

libraries/AP_Compass/AP_Compass_QMC5883L.cpp

@@ -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)) {
+    _dev->get_semaphore()->take_blocking();
-        return false;
-    }
 
     _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);
     }

libraries/AP_Compass/AP_Compass_RM3100.cpp

@@ -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)) {
+    dev->get_semaphore()->take_blocking();
-        return false;
-    }
 
     if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
         // read has high bit set for SPI
@@ -145,7 +143,11 @@ 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);
     
@@ -155,9 +157,6 @@ bool AP_Compass_RM3100::init()
         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));

libraries/AP_Compass/AP_Compass_SITL.cpp

@@ -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++) {
+        for (uint8_t i=0; i<MAX_CONNECTED_MAGS; i++) {
-            // default offsets to correct value
+            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());
+            // 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);
     }
 }

libraries/AP_Compass/AP_Compass_SITL.h

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

libraries/AP_Compass/AP_Compass_UAVCAN.cpp

@@ -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[index].driver && _detected_modules[index].ap_uavcan) {
-        if (!_detected_modules[i].driver && _detected_modules[i].ap_uavcan) {
+        WITH_SEMAPHORE(_sem_registry);
         // 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);
+        driver = new AP_Compass_UAVCAN(_detected_modules[index].ap_uavcan, _detected_modules[index].node_id, _detected_modules[index].sensor_id);
         if (driver) {
-                _detected_modules[i].driver = driver;
+            if (!driver->init()) {
-                driver->init();
+                delete driver;
-                debug_mag_uavcan(2,
+                return nullptr;
-                                 _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);
             }
-            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;
 }
 

libraries/AP_Compass/AP_Compass_UAVCAN.h

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

libraries/AP_Compass/CompassCalibrator.cpp

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

libraries/AP_Compass/Compass_learn.cpp

@@ -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++) {
+    for (Compass::Priority i(0); i<compass.get_count(); i++) {
-        if (compass._state[i].use_for_yaw) {
+        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 &diagonals = compass.get_diagonals(0);
-        const Vector3f &offdiagonals = compass.get_offdiagonals(primary_mag);
+        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);
+        Vector3f field_primary = compass.get_field(0);
-        for (uint8_t i=0; i<compass.get_count(); i++) {
+        for (uint8_t i=1; i<compass.get_count(); i++) {
-            if (i == primary_mag || !compass._state[i].use_for_yaw) {
+            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);

libraries/AP_Compass/Compass_learn.h

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