Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: use compass reference rather than repeatedly asking AHRS for it

This commit is contained in:
Peter Barker 2020-03-11 19:05:16 +11:00 committed by Andrew Tridgell
parent 903d81c263
commit 4c19eb4bab
1 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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);