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
@@ -91,8 +91,9 @@ bool AP_Arming_Copter::compass_checks(bool display_failure)
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_COMPASS)) {
// check compass offsets have been set. AP_Arming only checks
// this if learning is off; Copter *always* checks.
if (!AP::compass().configured()) {
check_failed(ARMING_CHECK_COMPASS, display_failure, "Compass not calibrated");
char failure_msg[50] = {};
if (!AP::compass().configured(failure_msg, ARRAY_SIZE(failure_msg))) {
check_failed(ARMING_CHECK_COMPASS, display_failure, "%s", failure_msg);
ret = false;
}
}
@@ -221,10 +221,10 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
mavlink_msg_compassmot_status_send(gcs_chan.get_chan(),
channel_throttle->get_control_in(),
current,
interference_pct[compass.get_primary()],
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?

motor_compensation[0].x,
motor_compensation[0].y,
motor_compensation[0].z);
}
}

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

// compass use is disabled
return true;
}
@@ -394,9 +394,12 @@ bool AP_Arming::compass_checks(bool report)
return false;
}
// check compass learning is on or offsets have been set
if (!_compass.learn_offsets_enabled() && !_compass.configured()) {
check_failed(ARMING_CHECK_COMPASS, report, "Compass not calibrated");
return false;
if (!_compass.learn_offsets_enabled()) {
char failure_msg[50] = {};
if (!_compass.configured(failure_msg, ARRAY_SIZE(failure_msg))) {
check_failed(ARMING_CHECK_COMPASS, report, "%s", failure_msg);
return false;
}
}

// check for unreasonable compass offsets
ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.