Freescale Semiconductor Application Note Document Number: AN4248
Rev. 0, 03/2011
Contents
1Introduction
This technical note provides the mathematics, reference source code and guidance for engineers implementing a tilt-compensated electronic compass (eCompass).
The eCompass us a three axis accelerometer and three axis magnetometer. The accelerometer measures the components of the earth's gravity and the magnetometer measures the components of earth's magnetic field (the geomagnetic field). Since both the accelerometer and magnetometer are fixed on the Printed Circuit Board (PCB), their readings change according to the orientation of the PCB.
If the PCB remains flat, then the compass heading could be computed from the arctangent of the ratio
of the two horizontal magnetic field components. Since, in general, the PCB will have an arbitrary orientation, the compass heading is a function of all three accelerometer readings and all three magnetometer readings.1Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.1Key Words. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2Summary. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 2Coordinate System and Package Alignment. . . . . . . . . . 3 3Accelerometer and Magnetometer Outputs as a Function of Phone Orientation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 4Tilt-Compensation Algorithm. . . . . . . . . . . . . . . . . . . . . . 6 5Estimation of the Hard Iron Offt V . . . . . . . . . . . . . . . . 8 6Software Implementation. . . . . . . . . . . . . . . . . . . . . . . . . 8
6.1eCompass C# Source Code . . . . . . . . . . . . . . . . . . 9
6.2Modulo Arithmetic Low Pass Filter for Angles C#
Source Code. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
6.3Sine and Cosine Calculation C# Source Code . . . 11
6.4ATAN2 Calculation C# Source Code. . . . . . . . . . . 13
6.5ATAN Calculation C# Source Code. . . . . . . . . . . . 14
个人总结学生6.6Integer Division C# Source Code . . . . . . . . . . . . . 15
Implementing a Tilt-Compensated eCompass using Accelerometer and Magnetometer Sensors
扁的沟by: Talat Ozyagcilar
Applications Engineer
Introduction
The tilt-compensated eCompass algorithm actually calculates all three angles (pitch, roll, and yaw or compass heading) that define the PCB orientation. The eCompass algorithms can therefore also be ud to create a 3-D Pointer with the pointing direction defined by the yaw and pitch angles.
The accuracy of an eCompass is highly dependent on the calculation and subtraction in software of stray magnetic fields both within, and in the vicinity of, the magnetometer on the PCB. By convention, the fields are divided into tho that are fixed (termed Hard Iron effects) and tho that are induced by the geomagnetic field (termed Soft Iron effects). Any zero field offt in the magn
etometer is normally included with the PCB’s Hard Iron effects and is calibrated at the same time.
This document describes a simple three-element model to compensate for Hard Iron effects. This three- element model should suffice for many situations. Plea contact your Freescale sales reprentative for details of a full nine-element model which compensates for both Hard and Soft Iron effects.
The C# language source code listed within this document contains cross-references to the equations ud. The listings contain all the code needed to return the yaw, pitch and roll angles from the magnetometer and accelerometer nsor readings.
For convenience, the remainder of the this document assumes that the eCompass will be implemented within a mobile phone.
1.1Key Words
Accelerometer, Magnetometer, Tilt angles, eCompass, 3-D Pointer, Tilt Compensation, Tilt Correction, Hard Iron, Soft Iron, Geomagnetism
1.2Summary
1. A tilt-compensated electronic compass (eCompass) is implemented using the combination of a
three-axis accelerometer and a three-axis magnetometer.
2.The accelerometer readings provide pitch and roll angle information which is ud to correct the
magnetometer data. This allows for accurate calculation of the yaw or compass heading when the eCompass is not held flat.
3.The pitch and roll angles are computed on the assumption that the accelerometer readings result
entirely from the eCompass orientation in the earth's gravitational field. The tilt-compensated
eCompass will not operate under freefall or low g conditions at one extreme nor high-g
accelerations at the other.
4. A 3-D Pointer can be implemented using the yaw (compass heading) and pitch angles from the
eCompass algorithms.
5.The magnetometer readings must be corrected for Hard Iron and Soft Iron effects.
6. A simple three parameter Hard Iron correction algorithm is described. Plea contact your
Freescale sales reprentative for details of Freescale's complete nine parameter Hard and Soft Iron correction algorithms.
7.Reference C# code is provided at the end of this document for the full tilt-compensated e-Compass
with Hard Iron compensation.
姨妈血怎么洗8.Demonstration eCompass platforms are available that show Freescale's latest nsors. Plea
contact your Freescale sales reprentative for details.
Coordinate System and Package Alignment 2Coordinate System and Package Alignment
This application note us the industry standard “NED” (North, East, Down) coordinate system to label axes on the mobile phone. The x-axis of the phone is the eCompass pointing direction, the y-axis points to the right and the z-axis points downward. (e Figure1).
Figure1. Coordinate System
柳公权字帖A positive Yaw angle ψ is defined to be a clockwi rotation about the positive z-axis. Similarly, a positive pitch angle θ and positive roll angle φ are defined as clockwi rotations about the positive y-
and positive x-axes respectively.
It is crucial that the accelerometer and magnetometer outputs are aligned with the phone coordinate system. Different PCB layouts may have different orientations of the accelerometer and magnetometer packages and even the same PCB may be mounted in different orientations within the final product.
For example, in Figure1, the accelerometer y-axis output Gy, is correctly aligned, but the x-axis Gx and z-axis Gz signals are inverted in sign. Also in Figure1, the magnetometer output Bz is correct, but the
y-axis signal should be t to Bx and the x-axis signal should be t to -By.
Once the package rotations and reflections are applied in software, a final check should be made while watching the raw accelerometer and magnetometer data from the PCB:
1.Place the PCB flat on the table. The z-axis accelerometer should read +1g and the x and y axes
negligible values. Invert the PCB so that the z-axis points upwards and verify that the z-axis
梦到婚礼accelerometer now indicates -1g. Repeat with the y-axis pointing downwards and then upwards to check that the y-axis reports 1g and then reports -1g. Repeat once more with the x-axis pointing downwards and then upwards to check that the x-axis reports 1g and then -1g.
Accelerometer and Magnetometer Outputs as a Function of Phone Orientation
2.The horizontal component of the geomagnetic field always points to the magnetic north pole. In
the northern hemisphere, the vertical component also points downward with the preci angle,
山东省行政区划
being dependent on location. When the PCB x-axis is pointed northward and downward, it should be possible to find a maximum value of the measured x component of the magnetic field. It should also be possible to find a minimum value when the PCB is aligned in the rever direction. Repeat the measurements with the PCB y- and z-axes aligned first with, and then against, the geomagnetic field which should result in maximum and minimum readings in the y- and then z-axes .
经典歌曲大全Figure 2. Gravitational and Magnetic Field Vectors
3Accelerometer and Magnetometer Outputs as a Function of Phone Orientation
Any orientation of the phone can be modeled as resulting from rotations in yaw, pitch and the roll applied to a starting position with the phone flat and pointing northwards. The accelerometer, G r , and
magnetometer, B r , readings in this starting reference position are (e Figure 2):
形胜之地
Eqn.1Eqn.
2
G r 00g ⎝⎠
⎜⎟⎜⎟⎛⎞
=B r B δcos 0δsin ⎝⎠⎜⎟⎜⎟⎛⎞
=
Accelerometer and Magnetometer Outputs as a Function of The acceleration due to gravity is g = 9.81 ms -2. B is the geomagnetic field strength which varies over the earth's surface from zero at the magnetic poles to a maximum of approximately 60 μT. δ is the angle of inclination of the geomagnetic field measured downwards from horizontal and varies over the earth's surface from -90° at the south magnetic pole, through zero near the equator to +90° at the north magnetic pole. For more information and geomagnetic field maps, plea e v/charts/.
There is no requirement to know the details of the geomagnetic field strength nor inclination angle in
order for the eCompass software to function. The magnetic field strength B and the inclination angle δ, cancel in the angle calculations (e Equations 20, 21 and 22).
The phone accelerometer, G p , and magnetometer, B p , readings measured after the three rotations R z (ψ) then R y (θ) and finally R x (φ) are described by the equations:
Eqn.3Eqn.4
The three rotation matrices referred to in Equations 3 and 4 are:
Eqn.5Eqn.6Eqn.7
Equation 3 assumes that the phone is not undergoing any linear acceleration and that the accelerometer signal G p is a function of gravity and the phone orientation only. A tilt-compensated eCompass will give erroneous readings if it is subjected to any linear acceleration.
Equation 4 ignores any stray magnetic fields from Hard and Soft Iron effects. The standard way of modeling the Hard Iron effect is as an additive magnetic vector, V , which rotates with the phone PCB and is therefore independent of phone orientation. Since any magnetometer nsor zero flux offt is also independent of phone orientation, it simply adds to the PCB Hard Iron component and i
s calibrated and removed at the same time.
G p R =x φ()R y θ()R z ψ()G r R x φ()R y θ()R z ψ()00g ⎝⎠
⎜⎟⎜⎟⎛⎞
=B p R =x φ()R y θ()R z ψ()B r R x φ()R y θ()R z ψ()B δcos 0δsin ⎝⎠⎜⎟⎜⎟⎛⎞=R x φ()1000φcos φsin 0φsin –φ
cos ⎝⎠⎜⎟⎜⎟⎜⎟⎛⎞=R y θ()θcos 0θsin –010θsin 0θ
cos ⎝⎠⎜⎟⎜⎟⎜⎟⎛⎞=R z Ψ()ψcos ψsin 0ψsin –ψcos 0001⎝⎠⎜⎟⎜⎟⎜⎟⎛⎞=
