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.

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

Compass Ordering Improvement #12629

Open
wants to merge 7 commits into
base: master
from

Conversation

@bugobliterator
Copy link
Member

bugobliterator commented Oct 23, 2019

This PR is targeted towards making compasses show up in a consistent order.

AP_Compass:

  • Enables registration to frontend based on device id, instead of auto incrementing compass_count. This ensure whatever order of compass was reached in first boot is retained in following boots.
  • Registration process involves, first looking if device id was already detected and recorded in previous boot, if so, return the instance id.
  • If above fails, we look for free instance, i.e. the one that has never been linked with in previous boots.
  • If all fails, we replace the first unregistered compass. This is most likely going to happen in case the compass was replaced by user with a different one.
  • Compass_UAVCAN driver is modified to reorder the detected devices, so that the compass with larger node id is sent for registration first. This ensures two things incase we are overflowing number of CAN Instances: We give older compasses priority, they can replace the newer one if connected again alongside, also makes it consistent that same uavcan compass appears as one of the instances.

AP_Logger, GCS_MAVLink, ArduCopter

  • We use new method instance_available to select whether to record/send/use the respective values for compass instance.

AP_NavEKF2, AP_NavEKF3

  • We scroll through list of available instances when needed to select different Compass, the changes are made to accommodate behaviour change in Compass library, where devices may appear in an order with missing instance in between.
@bugobliterator bugobliterator requested a review from tridge Oct 23, 2019
@bugobliterator bugobliterator self-assigned this Oct 23, 2019
@bugobliterator bugobliterator force-pushed the bugobliterator:pr-compass-ordering branch 3 times, most recently from 2425cac to 2afe64c Oct 23, 2019
@bugobliterator bugobliterator requested a review from rmackay9 Oct 23, 2019
Copy link
Member

OXINARF left a comment

This is a feature we've wanted for some time, but I have to say I'm a bit afraid with this change to non-sequential instance numbers - we have it in other libraries and we have had issues with it.

In my opinion, the correct way to do it would be to separate the parameters from the _state struct (this state should actually be in the AP_Compass_Backend class). Put the parameters in a struct array and then just give a reference to the backend when it registers a compass.

A last comment: I think we should have a pre-arm check (maybe not failing) that warns the user if not all previous compasses are present. Previously you would (most of times) get an uncalibrated compass and couldn't arm, but now the user will be oblivious to the problem.
Also this kind of breaks the expected dev ids feature because if it isn't there, it simply won't exist, but there is no warning.

libraries/AP_Compass/AP_Compass.cpp Outdated Show resolved Hide resolved
libraries/AP_Compass/AP_Compass.cpp Outdated Show resolved Hide resolved
libraries/AP_Compass/AP_Compass.cpp Outdated Show resolved Hide resolved
libraries/AP_Compass/AP_Compass.cpp Outdated Show resolved Hide resolved
libraries/AP_Compass/AP_Compass.h Show resolved Hide resolved
uint8_t tempIndex = magSelectIndex + i;
// loop back to the start index if we have exceeded the bounds
if (tempIndex >= maxCount) {
tempIndex -= maxCount;
if (tempIndex >= COMPASS_MAX_INSTANCES) {
tempIndex -= COMPASS_MAX_INSTANCES;
}
Comment on lines 230 to 234

This comment has been minimized.

Copy link
@OXINARF

OXINARF Oct 23, 2019

Member

I know this is pre-existing, but since we are changing it... This could just be: uint8_t tempIndex = (magSelectIndex + i) % COMPASS_MAX_INSTANCES;

This comment has been minimized.

Copy link
@bugobliterator

bugobliterator Oct 24, 2019

Author Member

done

@@ -222,14 +223,17 @@ void NavEKF2_core::readMagData()
// If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
// Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
// before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
if (magTimeout && _ahrs->get_compass()->any_detected() && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {

This comment has been minimized.

Copy link
@OXINARF

OXINARF Oct 23, 2019

Member

Again, not the same. The current code checks if there is more than one compass, this code just checks for any compass.
It isn't as bad because it just won't find any other compass in the loop below, but it will be doing an unnecessary loop.

This comment has been minimized.

Copy link
@bugobliterator

bugobliterator Oct 24, 2019

Author Member

fixed

@@ -1584,7 +1584,7 @@ void GCS_MAVLINK::send_raw_imu()
const Vector3f &accel = ins.get_accel(0);
const Vector3f &gyro = ins.get_gyro(0);
Vector3f mag;
if (compass.get_count() >= 1) {
if (compass.instance_available(0)) {

This comment has been minimized.

Copy link
@OXINARF

OXINARF Oct 23, 2019

Member

This means that if we don't have compass 0, but have compass 1, then it won't be sent. In my opinion, that's a bad regression.

This comment has been minimized.

Copy link
@bugobliterator

bugobliterator Oct 24, 2019

Author Member

we send compass 1 and more on SCALED_IMU message, we need to keep the order, otherwise it will lead to confusion while things like log-analysis

for (uint8_t i=0; i<compass.get_count(); i++) {
if (!compass.healthy(i)) {
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if (!compass.healthy(i) || !compass.instance_available(i)) {

This comment has been minimized.

Copy link
@OXINARF

OXINARF Oct 23, 2019

Member

Why is the second check needed? How could it be healthy if it's not there?

This comment has been minimized.

Copy link
@bugobliterator

bugobliterator Oct 24, 2019

Author Member

It's just part of the replacement strategy for compass.get_count. Its a bit paranoid here, I will remove this.

@@ -578,7 +578,7 @@ void Compass::init()

// Register a new compass instance
//
int8_t Compass::register_compass(uint32_t dev_id)
int8_t Compass::register_compass(int32_t dev_id)

This comment has been minimized.

Copy link
@OXINARF

OXINARF Oct 23, 2019

Member

This change is in the wrong commit (AP_Periph: use any_detected method for compass instances available or not)

This comment has been minimized.

Copy link
@bugobliterator

bugobliterator Oct 24, 2019

Author Member

Got it!

@bugobliterator bugobliterator force-pushed the bugobliterator:pr-compass-ordering branch 3 times, most recently from 1ffcb57 to 876567a Oct 24, 2019
@tridge

This comment has been minimized.

Copy link
Contributor

tridge commented Oct 24, 2019

I think this is a big improvement over what we have now. One thing that worries me a bit is that if a user permanently removes a compass and then does a recal they will still end up with a gap. Ideally a gap should only happen if a compass has changed since the last calibration.
To fix that we'd need to re-order drivers when we do a cal. That would imply we'd need to change the instance variables in the drivers. Alternatively we could move the instance number into the backend class so it can be manipulated by common code.
The tricky part is that users currently can use COMPASS_USE, USE2 and USE3 to disable an individual compass, and that is often done before calibration as they don't want that compass for cal.

@@ -42,7 +42,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)

// check compass health
compass.read();
for (uint8_t i=0; i<compass.get_count(); i++) {
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {

This comment has been minimized.

Copy link
@tridge

tridge Oct 26, 2019

Contributor

this will cause compassmot to always fail if we don't have 3 compasses (I tested this by changing SITL to have 2 compasses, and found compassmot did indeed fail). We need a check for whether there is a driver for this instance in this loop

This comment has been minimized.

Copy link
@tridge

tridge Oct 26, 2019

Contributor

I'd actually prefer a FORALL_COMPASS_AVAILABLE(i) macro, much like we have FOREACH_I2C_EXTERNAL(i), it would make for easier to read code, and quite a bit smaller patch (as no indent changes)

@@ -978,7 +978,7 @@ void AP_Periph_FW::can_mag_update(void)
#ifdef HAL_PERIPH_ENABLE_MAG
compass.read();
#if 1
if (compass.get_count() == 0) {
if (compass.num_detected() > 0) {

This comment has been minimized.

Copy link
@tridge

tridge Oct 26, 2019

Contributor

this has reversed the test - it is meant to run the init() if no sensors have been found

libraries/AP_Compass/AP_Compass.cpp Show resolved Hide resolved
libraries/AP_Compass/AP_Compass.cpp Show resolved Hide resolved
///
/// @returns true if compass backend successfully registered
///
bool instance_available(uint8_t i) const { return _state[i].registered; }

This comment has been minimized.

Copy link
@tridge

tridge Oct 26, 2019

Contributor

given how this is used I'd like it to be a cpp function and it also check if i >= COMPASS_MAX_INSTANCES, to prevent index beyond end of array when we limit the number of compasses below 3 on some builds

@@ -50,7 +50,7 @@ AP_Compass_HIL::init(void)
{
// register two compass instances
for (uint8_t i=0; i<HIL_NUM_COMPASSES; i++) {
_compass_instance[i] = register_compass();
_compass_instance[i] = register_compass(i + 1);

This comment has been minimized.

Copy link
@tridge

tridge Oct 26, 2019

Contributor

should use make_bus_id() with type SITL here

_bus->set_device_type(DEVTYPE_HMC5883);
_compass_instance = register_compass(_bus->get_bus_id());

This comment has been minimized.

Copy link
@tridge

tridge Oct 26, 2019

Contributor

can we roll the set_dev_id() into the register_compass() call so we have less duplication?

@@ -266,7 +266,9 @@ 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);

This comment has been minimized.

Copy link
@tridge

tridge Oct 26, 2019

Contributor

need to remove extra set_devid_type() below

uint8_t maxCount = _ahrs->get_compass()->get_count();
if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() &&
(magNumTimesChanged >= (_ahrs->get_compass()->num_detected() - 1)) &&

This comment has been minimized.

Copy link
@tridge

tridge Oct 26, 2019

Contributor

I don't understand the equivalence of this code. magNumTimesChanged is always increasing, and could quickly get beyond num_detected. I think this will kill the EKF after a few compass changes

uint8_t maxCount = _ahrs->get_compass()->get_count();
if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() &&
(magNumTimesChanged >= (_ahrs->get_compass()->num_detected() - 1)) &&

This comment has been minimized.

Copy link
@tridge

tridge Oct 26, 2019

Contributor

see EKF2 comment above

@tridge tridge added the DevCallTopic label Oct 28, 2019
@tridge

This comment has been minimized.

Copy link
Contributor

tridge commented Oct 28, 2019

discussed on dev call, we think it should pack the compasses to be contiguous on a re-cal at least

@bugobliterator bugobliterator force-pushed the bugobliterator:pr-compass-ordering branch 7 times, most recently from 6415e02 to bee426f Nov 20, 2019
@bugobliterator bugobliterator force-pushed the bugobliterator:pr-compass-ordering branch from bee426f to 31962c8 Nov 22, 2019
Copy link
Contributor

tridge left a comment

There are a lot of subtleties with this. The part I find hardest to review is dual indexing scheme. We now have indexing by instance ID and indexing by priority. The PR changes get_field() to use the priority indexing, but most other interfaces still use instance indexing. I'm pretty sure this breaks assumptions in the compass learn code which assumes that get_field(), get_offsets(), get_diagonals() etc all refer to a corresponding compass. I think it may also break things in the calibration code, especially the auto-orientation.
It also breaks logging of MAG messages, which assume that get_field(), get_offsets() and others all refer to the same indexing scheme. That would in turn break external log based analysis (eg. it would break the new magfit_WMM that I wrote recently).
To make progress on this we need to work out what we think the 'correct' API is for each exposed interface, and ensure that it is difficult to mis-use that API. We also need to work out if the pain of the dual indexing is worth the gains.
Then we need to work out how to test this. I think we'd need new SIM_ parameters to set the bus IDs of SITL compasses, so that we can setup scenarios in SITL where compasses are replaced or change probe order and then work out what the correct behaviour is).
For example, I think with what is proposed now that if a user has one UAVCAN compass and one internal SPI compass, then if they replaced the UAVCAN compass then it would change from having the UAVCAN compass as highest priority to it beving lowest priority. I don't think that is what the user would want, but it is so hard to know.

@@ -207,12 +208,6 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @User: Advanced
AP_GROUPINFO("MOT2", 11, Compass, _state[1].motor_compensation, 0),

// @Param: PRIMARY

This comment has been minimized.

Copy link
@tridge

tridge Dec 2, 2019

Contributor

this breaks existing ordering for users who have set COMPASS_PRIMARY already. Also, when removing a parameter we should leave a comment to reserve the slot to prevent it being re-used

motor_compensation[compass.get_primary()].x,
motor_compensation[compass.get_primary()].y,
motor_compensation[compass.get_primary()].z);
interference_pct[0],

This comment has been minimized.

Copy link
@tridge

tridge Dec 2, 2019

Contributor

this seems to assume the first instance is the primary? Don't we need to lookup via a mapping between the PRI var and instance?

@@ -384,7 +384,7 @@ bool AP_Arming::compass_checks(bool report)

// avoid Compass::use_for_yaw(void) as it implicitly calls healthy() which can
// incorrectly skip the remaining checks, pass the primary instance directly
if (!_compass.use_for_yaw(_compass.get_primary())) {
if (!_compass.use_for_yaw(0)) {

This comment has been minimized.

Copy link
@tridge

tridge Dec 2, 2019

Contributor

the API becomes rather unclear. Is the argument to use_for_yaw() the instance number or the order number?

@tridge

This comment has been minimized.

Copy link
Contributor

tridge commented Dec 2, 2019

minor thing on naming. I think I suggested the PRI name, but I now think 1ST, 2ND and 3RD would be clearer, especially for people who don't have english as first language

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
4 participants
You can’t perform that action at this time.