mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: use compass reference rather than repeatedly asking AHRS for it
This commit is contained in:
parent
903d81c263
commit
4c19eb4bab
|
@ -9,6 +9,7 @@
|
|||
#include <AP_RangeFinder/AP_RangeFinder_Backend.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_Compass/AP_Compass.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -246,15 +247,18 @@ void NavEKF3_core::readMagData()
|
|||
allMagSensorsFailed = true;
|
||||
return;
|
||||
}
|
||||
|
||||
const Compass &compass = AP::compass();
|
||||
|
||||
// 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();
|
||||
uint8_t maxCount = compass.get_count();
|
||||
if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
|
||||
allMagSensorsFailed = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if (_ahrs->get_compass()->learn_offsets_enabled()) {
|
||||
if (compass.learn_offsets_enabled()) {
|
||||
// while learning offsets keep all mag states reset
|
||||
InitialiseVariablesMag();
|
||||
wasLearningCompass_ms = imuSampleTime_ms;
|
||||
|
@ -267,7 +271,7 @@ void NavEKF3_core::readMagData()
|
|||
}
|
||||
|
||||
// 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)) {
|
||||
if (use_compass() && ((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
|
||||
|
@ -283,7 +287,7 @@ void NavEKF3_core::readMagData()
|
|||
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) {
|
||||
if (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
|
||||
|
@ -305,7 +309,7 @@ void NavEKF3_core::readMagData()
|
|||
}
|
||||
|
||||
// detect changes to magnetometer offset parameters and reset states
|
||||
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex);
|
||||
Vector3f nowMagOffsets = compass.get_offsets(magSelectIndex);
|
||||
bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
|
||||
if (changeDetected) {
|
||||
// zero the learned magnetometer bias states
|
||||
|
@ -317,7 +321,7 @@ void NavEKF3_core::readMagData()
|
|||
lastMagOffsetsValid = true;
|
||||
|
||||
// store time of last measurement update
|
||||
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex);
|
||||
lastMagUpdate_us = compass.last_update_usec(magSelectIndex);
|
||||
|
||||
// estimate of time magnetometer measurement was taken, allowing for delays
|
||||
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
|
||||
|
@ -326,10 +330,10 @@ void NavEKF3_core::readMagData()
|
|||
magDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
|
||||
// read compass data and scale to improve numerical conditioning
|
||||
magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f;
|
||||
magDataNew.mag = compass.get_field(magSelectIndex) * 0.001f;
|
||||
|
||||
// check for consistent data between magnetometers
|
||||
consistentMagData = _ahrs->get_compass()->consistent();
|
||||
consistentMagData = compass.consistent();
|
||||
|
||||
// save magnetometer measurement to buffer to be fused later
|
||||
storedMag.push(magDataNew);
|
||||
|
|
Loading…
Reference in New Issue