|
|
@ -244,7 +244,7 @@ AP_AHRS_DCM::normalize(void) |
|
|
|
float |
|
|
|
float |
|
|
|
AP_AHRS_DCM::yaw_error_compass(void) |
|
|
|
AP_AHRS_DCM::yaw_error_compass(void) |
|
|
|
{ |
|
|
|
{ |
|
|
|
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z); |
|
|
|
const Vector3f &mag = _compass->get_field(); |
|
|
|
// get the mag vector in the earth frame
|
|
|
|
// get the mag vector in the earth frame
|
|
|
|
Vector2f rb = _dcm_matrix.mulXY(mag); |
|
|
|
Vector2f rb = _dcm_matrix.mulXY(mag); |
|
|
|
|
|
|
|
|
|
|
|