@ -3842,10 +3842,10 @@ bool NavEKF::getMagOffsets(Vector3f &magOffsets) const
@@ -3842,10 +3842,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
if ( secondMagYawInit & & ( _magCal ! = 2 ) & & _ahrs - > get_compass ( ) - > healthy ( ) ) {
magOffsets = _ahrs - > get_compass ( ) - > get_offsets_milligauss ( ) - state . body_magfield * 1000.0f ;
magOffsets = _ahrs - > get_compass ( ) - > get_offsets ( ) - state . body_magfield * 1000.0f ;
return true ;
} else {
magOffsets = _ahrs - > get_compass ( ) - > get_offsets_milligauss ( ) ;
magOffsets = _ahrs - > get_compass ( ) - > get_offsets ( ) ;
return false ;
}
}
@ -4402,7 +4402,7 @@ void NavEKF::readMagData()
@@ -4402,7 +4402,7 @@ void NavEKF::readMagData()
lastMagUpdate = _ahrs - > get_compass ( ) - > last_update_usec ( ) ;
// read compass data and scale to improve numerical conditioning
magData = _ahrs - > get_compass ( ) - > get_field_milligauss ( ) * 0.001f ;
magData = _ahrs - > get_compass ( ) - > get_field ( ) * 0.001f ;
// check for consistent data between magnetometers
consistentMagData = _ahrs - > get_compass ( ) - > consistent ( ) ;
@ -4415,7 +4415,7 @@ void NavEKF::readMagData()
@@ -4415,7 +4415,7 @@ void NavEKF::readMagData()
// check if compass offsets have ben changed and adjust EKF bias states to maintain consistent innovations
if ( _ahrs - > get_compass ( ) - > healthy ( ) ) {
Vector3f nowMagOffsets = _ahrs - > get_compass ( ) - > get_offsets_milligauss ( ) ;
Vector3f nowMagOffsets = _ahrs - > get_compass ( ) - > get_offsets ( ) ;
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
if ( changeDetected & & secondMagYawInit ) {