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

AP_Compass: Ensure that a compass is healthy before permitting it's use for yaw #6912

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
+72 −71
Open

AP_Compass: Ensure that a compass is healthy before permitting it's use for yaw #6912

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a4993e8
AP_NavEKF2: Allow mag selection change if use_for_yaw returns false
priseborough Sep 11, 2017
de38547
AP_NavEKF3: Allow mag selection change if use_for_yaw returns false
priseborough Sep 11, 2017
4026c98
AP_Compass: Ensure that a compass is healthy before permitting it's u…
WickedShell Sep 7, 2017
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
5 changes: 2 additions & 3 deletions libraries/AP_Compass/AP_Compass.cpp
View file
Open in desktop
Expand Up @@ -1056,8 +1056,7 @@ Compass::set_initial_location(int32_t latitude, int32_t longitude)
bool
Compass::use_for_yaw(void) const
{
uint8_t prim = get_primary();
return healthy(prim) && use_for_yaw(prim);
return use_for_yaw(get_primary());
}

/// return true if the specified compass can be used for yaw calculations
Expand All @@ -1067,7 +1066,7 @@ Compass::use_for_yaw(uint8_t i) const
// when we are doing in-flight compass learning the state
// estimator must not use the compass. The learning code turns off
// inflight learning when it has converged
return _state[i].use_for_yaw && _learn.get() != LEARN_INFLIGHT;
return healthy(i) && _state[i].use_for_yaw && _learn.get() != LEARN_INFLIGHT;
}

void
Expand Down
69 changes: 35 additions & 34 deletions libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View file
Open in desktop
Expand Up @@ -191,6 +191,7 @@ void NavEKF2_core::readMagData()
allMagSensorsFailed = true;
return;
}

// If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
// magnetometer, then declare the magnetometers as failed for this flight
uint8_t maxCount = _ahrs->get_compass()->get_count();
Expand All @@ -212,45 +213,45 @@ 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) {

// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
uint8_t tempIndex = magSelectIndex + i;
// loop back to the start index if we have exceeded the bounds
if (tempIndex >= maxCount) {
tempIndex -= maxCount;
}
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
magSelectIndex = tempIndex;
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
// clear the data waiting flag so that we do not use any data pending from the previous sensor
magDataToFuse = false;
// request a reset of the magnetic field states
magStateResetRequest = true;
// declare the field unlearned so that the reset request will be obeyed
magFieldLearned = false;
break;
}
}
}

// do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
// because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) {
frontend->logging.log_compass = true;

// 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) {

// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
uint8_t tempIndex = magSelectIndex + i;
// loop back to the start index if we have exceeded the bounds
if (tempIndex >= maxCount) {
tempIndex -= maxCount;
}
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
magSelectIndex = tempIndex;
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
// clear the data waiting flag so that we do not use any data pending from the previous sensor
magDataToFuse = false;
// request a reset of the magnetic field states
magStateResetRequest = true;
// declare the field unlearned so that the reset request will be obeyed
magFieldLearned = false;
break;
}
}
}

// detect changes to magnetometer offset parameters and reset states
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex);
bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
Expand Down
69 changes: 35 additions & 34 deletions libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View file
Open in desktop
Expand Up @@ -238,6 +238,7 @@ void NavEKF3_core::readMagData()
allMagSensorsFailed = true;
return;
}

// If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
// magnetometer, then declare the magnetometers as failed for this flight
uint8_t maxCount = _ahrs->get_compass()->get_count();
Expand All @@ -258,44 +259,44 @@ void NavEKF3_core::readMagData()
InitialiseVariablesMag();
}

// 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) {

// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
uint8_t tempIndex = magSelectIndex + i;
// loop back to the start index if we have exceeded the bounds
if (tempIndex >= maxCount) {
tempIndex -= maxCount;
}
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
magSelectIndex = tempIndex;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
// clear the data waiting flag so that we do not use any data pending from the previous sensor
magDataToFuse = false;
// request a reset of the magnetic field states
magStateResetRequest = true;
// declare the field unlearned so that the reset request will be obeyed
magFieldLearned = false;
break;
}
}
}

// limit compass update rate to prevent high processor loading because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
if (use_compass() && ((_ahrs->get_compass()->last_update_usec() - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) {
frontend->logging.log_compass = true;

// 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) {

// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
uint8_t tempIndex = magSelectIndex + i;
// loop back to the start index if we have exceeded the bounds
if (tempIndex >= maxCount) {
tempIndex -= maxCount;
}
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
magSelectIndex = tempIndex;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
// clear the data waiting flag so that we do not use any data pending from the previous sensor
magDataToFuse = false;
// request a reset of the magnetic field states
magStateResetRequest = true;
// declare the field unlearned so that the reset request will be obeyed
magFieldLearned = false;
break;
}
}
}

// detect changes to magnetometer offset parameters and reset states
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex);
bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
Expand Down