// produce a yaw error value. The returned value is proportional
// to sin() of the current heading error in earth frame
float
AP_AHRS_DCM::yaw_error_compass(void)
{
const Vector3f &mag = _compass->get_field();
// get the mag vector in the earth frame
Vector2f rb = _dcm_matrix.mulXY(mag);
if (rb.length() < FLT_EPSILON) {
return 0.0f;
}
rb.normalize();
if (rb.is_inf()) {
// not a valid vector
return 0.0f;
}
// update vector holding earths magnetic field (if required)
if( !is_equal(_last_declination,_compass->get_declination()) ) {
_last_declination = _compass->get_declination();
_mag_earth.x = cosf(_last_declination);
_mag_earth.y = sinf(_last_declination);
}
// calculate the error term in earth frame
// calculate the Z component of the cross product of rb and _mag_earth
return rb % _mag_earth;
}
// produce a yaw error value. The returned value is proportional
// to sin() of the current heading error in earth frame
float
AP_AHRS_DCM::yaw_error_compass(void)
{
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z);
// get the mag vector in the earth frame
Vector2f rb = _dcm_matrix.mulXY(mag);
rb.normalize();
if (rb.is_inf()) {
// not a valid vector
return 0.0;
}
// update vector holding earths magnetic field (if required)
if( _last_declination != _compass->get_declination() ) {
_last_declination = _compass->get_declination();
_mag_earth.x = cosf(_last_declination);
_mag_earth.y = sinf(_last_declination);
}
// calculate the error term in earth frame
// calculate the Z component of the cross product of rb and _mag_earth
return rb % _mag_earth;
}
