Browse Source

AP_NavEKF: use compass get_{field,offsets}() functions

Both functions are equivalent, so we're going to simply use
get_{field,offsets}() instead of get_{field,offsets}_milligauss().
master
Gustavo Jose de Sousa 10 years ago committed by Randy Mackay
parent
commit
afccf615d5
  1. 8
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 2
      libraries/AP_NavEKF/AP_SmallEKF.cpp

8
libraries/AP_NavEKF/AP_NavEKF.cpp

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

2
libraries/AP_NavEKF/AP_SmallEKF.cpp

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

Loading…
Cancel
Save