diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index bf9a720c29ec1..913fa29aaabba 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -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 @@ -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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index c7c9387c491fe..2637dfadc32cf 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -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(); @@ -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) { + 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) { - 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); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index c88b6147fe4da..5b30ceabc00c7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -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(); @@ -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) { + 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) { - 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);