Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: make EKF use milligauss

Browse Source
This commit is contained in:
Staroselskii Georgii 2015-08-24 17:54:23 +03:00 committed by Andrew Tridgell
parent e93ff44a97
commit b5b6d767bd
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3827,10 +3827,10 @@ bool NavEKF::getMagOffsets(Vector3f &magOffsets) const
{ {
// compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid // compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid
if (secondMagYawInit && (_magCal != 2) && _ahrs->get_compass()->healthy()) { if (secondMagYawInit && (_magCal != 2) && _ahrs->get_compass()->healthy()) {
magOffsets = _ahrs->get_compass()->get_offsets() - state.body_magfield*1000.0f; magOffsets = _ahrs->get_compass()->get_offsets_milligauss() - state.body_magfield*1000.0f;
return true; return true;
} else { } else {
magOffsets = _ahrs->get_compass()->get_offsets(); magOffsets = _ahrs->get_compass()->get_offsets_milligauss();
return false; return false;
} }
} }
@ -4374,7 +4374,7 @@ void NavEKF::readMagData()
lastMagUpdate = _ahrs->get_compass()->last_update_usec(); lastMagUpdate = _ahrs->get_compass()->last_update_usec();
// read compass data and scale to improve numerical conditioning // read compass data and scale to improve numerical conditioning
magData = _ahrs->get_compass()->get_field() * 0.001f; magData = _ahrs->get_compass()->get_field_milligauss() * 0.001f;
// get states stored at time closest to measurement time after allowance for measurement delay // get states stored at time closest to measurement time after allowance for measurement delay
RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - msecMagDelay)); RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - msecMagDelay));
@ -4384,7 +4384,7 @@ void NavEKF::readMagData()
// check if compass offsets have ben changed and adjust EKF bias states to maintain consistent innovations // check if compass offsets have ben changed and adjust EKF bias states to maintain consistent innovations
if (_ahrs->get_compass()->healthy()) { if (_ahrs->get_compass()->healthy()) {
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(); Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets_milligauss();
bool changeDetected = (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(nowMagOffsets.z,lastMagOffsets.z)); bool changeDetected = (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(nowMagOffsets.z,lastMagOffsets.z));
// Ignore bias changes before final mag field and yaw initialisation, as there may have been a compass calibration // Ignore bias changes before final mag field and yaw initialisation, as there may have been a compass calibration
if (changeDetected && secondMagYawInit) { if (changeDetected && secondMagYawInit) {

2
libraries/AP_NavEKF/AP_SmallEKF.cpp
View File

@ -640,7 +640,7 @@ void SmallEKF::readMagData()
lastMagUpdate = _ahrs.get_compass()->last_update_usec(); lastMagUpdate = _ahrs.get_compass()->last_update_usec();
// read compass data and scale to improve numerical conditioning // read compass data and scale to improve numerical conditioning
magData = _ahrs.get_compass()->get_field(); magData = _ahrs.get_compass()->get_field_milligauss();
// let other processes know that new compass data has arrived // let other processes know that new compass data has arrived
newDataMag = true; newDataMag = true;