Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Compass Ordering Improvement #12629

Merged
merged 11 commits into from Feb 19, 2020
Merged

Compass Ordering Improvement #12629

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
4eba9f7
Arducopter: Primary Compass is always serial# 0
bugobliterator Nov 19, 2019
c03bf68
AP_Arming: Primary Compass is always at serial# 0
bugobliterator Nov 19, 2019
28a09d1
ArduCopter: add arming message for compass not detected but assigned
bugobliterator Nov 20, 2019
3f1a7be
AP_Arming: add arming message for compass not detected but assigned
bugobliterator Nov 20, 2019
1ea4d9e
AP_NavEKF2: Primary compass is always at serial number 0
bugobliterator Nov 20, 2019
4106b51
AP_NavEKF3: Primary compass is always at serial number 0
bugobliterator Nov 20, 2019
7d97ed8
AP_Compass: modify compass driver to support consistent ordering and …
bugobliterator Nov 20, 2019
4f439b3
SITL: add parameter MAG_DEVID in SITL
bugobliterator Dec 23, 2019
8aba8a5
AP_Param: add method to find key of top level group variable by pointer
bugobliterator Dec 24, 2019
54f2535
AP_Common: add method that can help to put contraint on accessor type
bugobliterator Dec 31, 2019
d7fda5d
AP_Compass: run style script on Compass.cpp
bugobliterator Feb 15, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
500 changes: 374 additions & 126 deletions libraries/AP_Compass/AP_Compass.cpp
View file
Open in desktop

Large diffs are not rendered by default.

131 changes: 84 additions & 47 deletions libraries/AP_Compass/AP_Compass.h
View file
Open in desktop
Expand Up @@ -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
Expand Down Expand Up @@ -45,8 +46,17 @@
maximum number of compass instances available on this platform. If more
than 1 then redundant sensors may be available
*/
#ifndef HAL_BUILD_AP_PERIPH
#define COMPASS_MAX_INSTANCES 3
#define COMPASS_MAX_BACKEND 3
#define COMPASS_MAX_UNREG_DEV 5
#else
#define COMPASS_MAX_INSTANCES 1
#define COMPASS_MAX_BACKEND 1
#define COMPASS_MAX_UNREG_DEV 0
#endif

#define MAX_CONNECTED_MAGS (COMPASS_MAX_UNREG_DEV+COMPASS_MAX_INSTANCES)

class CompassLearn;

Expand Down Expand Up @@ -87,7 +97,7 @@ friend class AP_Compass_Backend;
/// @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;

Expand All @@ -107,6 +117,7 @@ friend class AP_Compass_Backend;
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
///
Expand All @@ -122,8 +133,8 @@ friend class AP_Compass_Backend;
uint8_t get_count(void) const { return _compass_count; }

/// 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;
Expand Down Expand Up @@ -166,22 +177,22 @@ friend class AP_Compass_Backend;
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; }
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we should make it more clear in all the modified accessors what the argument is. So leaving it as "i" makes it appear that it is still an instance number that is being provided but in fact it's a priority number. Perhaps the argument should be renamed to "priority" or "pri" or something that makes it more clear that we are expecting a priority number. Another way to do it would be to add a new typedef so the argument could then be something like, "get_offsets(priority_num p) const ... ".

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ah, nevermind, I guess this "i" really is an instance.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Its not an instance, it is priority. As far as consumers of frontend are concerned all operations are via Priority, there is no concept of instance and priority like before. It is not possible for consumers to fetch details by instance number, that is reserved only for backends. "Priority()" call is a constructor which creates Priority type instance. This method was introduced, to make it compile time illegal to fetch anything by Instance or plain int by mistake. This ensures the coder understands that a priority number is expected.

const Vector3f &get_offsets(void) const { return get_offsets(0); }
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry if I'm misunderstanding but previously I think get_offsets() (with no argument field) would return the highest priority compass. I think now though it returns the 0th compass. Maybe I'm just confused about what "Priority()" does. Does it take an instance and convert it to a priority? or does it take a Priority and convert it to an instance? Maybe as mentioned in another comment we just need to renamed "Priority" to make it more clear what it's converting to what.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

To Consumers of Compass frontend now 0 == Primary. There's no distinction between primary and 0th instance and secondary and 1st instance and so forth. We take care of that in the frontend.


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; }
Expand All @@ -190,9 +201,7 @@ friend class AP_Compass_Backend;
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.
///
Expand Down Expand Up @@ -227,8 +236,8 @@ friend class AP_Compass_Backend;
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.
///
Expand All @@ -240,8 +249,8 @@ friend class AP_Compass_Backend;
///
/// @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
Expand All @@ -263,13 +272,7 @@ friend class AP_Compass_Backend;
/// @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);
Expand All @@ -281,11 +284,11 @@ friend class AP_Compass_Backend;
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[];

Expand Down Expand Up @@ -333,10 +336,20 @@ friend class AP_Compass_Backend;

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
bugobliterator marked this conversation as resolved.
Show resolved Hide resolved
bool register_compass(int32_t dev_id, uint8_t& instance);
Copy link
Contributor

@rmackay9 rmackay9 Feb 15, 2020 •
edited

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

is the register_compass's first argument really a device id or a bus_id? should it be a uint32_t instead of an int32_t?

I think this may be an existing issue with a poorly name accessor in AP_HAL::Device so I've created an issue for it here: #13575

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We store this parameter as an int32_t, so its right to be consistent across the board.


// load backend drivers
bool _add_backend(AP_Compass_Backend *backend);
Expand Down Expand Up @@ -364,7 +377,7 @@ friend class AP_Compass_Backend;

#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

Expand Down Expand Up @@ -405,16 +418,16 @@ friend class AP_Compass_Backend;
// 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;
Matrix3f* _custom_rotation;

// primary instance
AP_Int8 _primary;

// declination in radians
AP_Float _declination;

Expand All @@ -427,9 +440,6 @@ friend class AP_Compass_Backend;
// 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;
Expand All @@ -443,6 +453,8 @@ friend class AP_Compass_Backend;
struct mag_state {
AP_Int8 external;
bool healthy;
bool registered;
Compass::Priority priority;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's perhaps error prone to have a variable called "priority" and a TSIndex object called "Priority"

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

"Priority" is not a TSIndex object, but a typedef that is created using template class TSIndex. its a type that is used to keep distinction between different types of indexes. And make it compile time strict.

AP_Int8 orientation;
AP_Vector3f offset;
AP_Vector3f diagonals;
Expand All @@ -453,13 +465,8 @@ friend class AP_Compass_Backend;
// 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;
Expand All @@ -480,7 +487,32 @@ friend class AP_Compass_Backend;
// 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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can we get some comments added around these new members and methods to clarify their purpose?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

will do

//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;
RestrictIDTypeArray<AP_Int32, COMPASS_MAX_INSTANCES, Priority> _priority_did_stored_list;
RestrictIDTypeArray<int32_t, COMPASS_MAX_INSTANCES, Priority> _priority_did_list;

AP_Int16 _offset_max;

Expand All @@ -491,7 +523,7 @@ friend class AP_Compass_Backend;
AP_Int16 _options;

#if COMPASS_CAL_ENABLED
CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
RestrictIDTypeArray<CompassCalibrator, COMPASS_MAX_INSTANCES, Priority> _calibrator;
#endif

#if COMPASS_MOT_ENABLED
Expand All @@ -506,7 +538,12 @@ friend class AP_Compass_Backend;

// 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;
Expand Down
9 changes: 5 additions & 4 deletions libraries/AP_Compass/AP_Compass_AK09916.cpp
View file
Open in desktop
Expand Up @@ -222,17 +222,18 @@ bool AP_Compass_AK09916::init()
_initialized = true;
bugobliterator marked this conversation as resolved.
Show resolved Hide resolved

/* 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);
}

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));
Expand Down
8 changes: 4 additions & 4 deletions libraries/AP_Compass/AP_Compass_AK8963.cpp
View file
Open in desktop
Expand Up @@ -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));
Expand Down
8 changes: 5 additions & 3 deletions libraries/AP_Compass/AP_Compass_BMM150.cpp
View file
Open in desktop
Expand Up @@ -211,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");

Expand Down