Application Report SLAA518A – February 2012 Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Erick Macias, Daniel Torres, Sourabh Ravindran ................................................................................... ABSTRACT This application report explains the implementation of an Attitude and Heading Reference System (AHRS), using the ultra-low-power MSP430F5xx microcontroller, a magnetometer, a gyroscope, and an accelerometer on all three axes. The calibration of the sensors is key to the accuracy of the algorithm, therefore, the sensors’ output must be calibrated before being input to the Direction Cosine Matrix (DCM) algorithm. The algorithm is applied to the calibrated sensor readings to calculate the Euler angles describing the orientation of a body; consisting of the yaw, roll, and pitch angles. Project collateral and source code discussed in this application report can be downloaded from the following URL: http://www.ti.com/lit/zip/slaa518. This application report uses the MPU-9150 MotionFit™ Wireless Developer Kit from InvenSense (http://www.invensense.com). Document License: This work is licensed under the Creative Commons Attribution-NonCommercialNoDerivs 3.0 Unported License (CC BY-NC-ND 3.0). To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/legalcode or send a letter to Creative Commons, 171 Second Street, Suite 300, San Francisco, California, 94105, USA. 1 2 3 4 5 6 7 Contents Introduction .................................................................................................................. 2 Direction Cosine Matrix Algorithm ........................................................................................ 3 MSP430F5xx Firmware .................................................................................................... 4 Sensors Calibration ......................................................................................................... 7 Conclusion ................................................................................................................... 9 Schematics ................................................................................................................. 11 References ................................................................................................................. 14 List of Figures .................................................................................................... 1 AHRS Circuit Overview 2 Direct Cosine Matrix Algorithm Overview ................................................................................ 4 3 MSP430F5xx AHRS Firmware Overview ................................................................................ 5 4 AHRS GUI 5 5 MSP430F5xx Calibration Firmware Overview 6 6 7 8 9 ................................................................................................................... .......................................................................... AHRS Calibration GUI ...................................................................................................... Magnetometer Hard Iron Calibration ..................................................................................... Rotations for Hard Iron Calibration of the Magnetometer ............................................................. MSP430 Voltage Measurement ......................................................................................... 3 7 8 9 10 List of Tables 1 Sensor Data Request Commands ........................................................................................ 6 2 System Current Consumption ........................................................................................... 10 All trademarks are the property of their respective owners. SLAA518A – February 2012 Submit Documentation Feedback Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated 1 Introduction www.ti.com Micro USB Connector Blue Low Energy (BLE) JTAG MSP430F5528 LEDs Barometric Pressure Sensor Magnetometer Gyroscope Accelerometer 1 Introduction Modeling the orientation of a rigid body, including airplanes, RC toys, sport watches, smart phones, humans, etc. can be implemented by using the DCM algorithm. When creating an AHRS, also known as Magnetic, Angular Rate, and Gravity sensor (MARG), a magnetometer, a gyroscope, and an accelerometer are required. The calibrated sensors readings are fed to the DCM algorithm, which provides a complete measurement of the orientation, relative to the earth’s magnetic field and the direction of gravity, expressed by the Euler (roll, yaw, and pitch) angles. In certain applications such as smart phones the ultra-low-power MSP430F5xx can handle all the communication with the motion sensors via I2C protocol. This leads to lower power consumption and higher CPU performance in the system, since they can request raw data or the orientation angles at any given time, meanwhile they can be in sleep mode, or they can perform other tasks that could have been delayed by the calculation of the orientation. This document covers the following key points: • Direction Cosine Matrix Algorithm (Section 2) • MSP430F5xx Firmware (Section 3) • Sensor Calibration (Section 4) Figure 1 contains the AHRS circuit overview. The MSP430F5xx can communicate via the USB Module CDC class with two GUIs running in the computer: • AHRS GUI (See Figure 4) • AHRS Calibration GUI (See Figure 6) The calibration GUI must be used when the AHRS system is used for the first time and in the case where the system’s calibration values get corrupted by the presence of a constant magnetic field (hard iron effects). The AHRS GUI displays the Euler angles, as well as the visual representation of such; a horizon and a digital compass. These two GUIs require separate firmware to be downloaded to the MSP430F5xx: • MSP430 AHRS Project AHRS Mode → AHRS GUI • MSP430 AHRS Project Calibration Mode → Calibration GUI There are other alternatives to send the Euler angles to the computer or other devices instead of USB; Bluetooth Low Energy (BLE) is one of them. The MSP430 communicates with BLE (BR-LE4.0) chip via UART. This application report will not cover the communication with the BR-LE4.0 chip (http://www.blueradios.com). To find more information about BLE, visit the http://www.ti.com website. 2 Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated SLAA518A – February 2012 Submit Documentation Feedback Direction Cosine Matrix Algorithm www.ti.com +3.3 V MSP430F5528 Accelerometer 3.3k W Magnetometer Computer 3.3k W SDA I2C Module Calibration GUI SCL Gyroscope Barometric Pressure AHRS GUI TX UART Module Bluetooth Low Energy RX HANDOVER EULER ANGLES AND SENSOR DATA USB Module USB Module REQUEST SENSOR DATA Legend: DCM Sensors Figure 1. AHRS Circuit Overview 2 Direction Cosine Matrix Algorithm The DCM algorithm calculates the orientation of a rigid body, in respect to the rotation of the earth by using rotation matrices. For a visual representation of the Direction Cosine Matrix Algorithm, see Figure 2. The rotation matrices are related to the Euler angles, which describe the three consecutive rotations needed to describe the orientation. The three sensors used in the algorithm are: • The accelerometer measures earth’s gravity field minus acceleration. • The magnetometer measures earth’s magnetic field. • The gyroscope sensor measures angular velocity. The gyroscope sensor is the primary sensor used to calculate the orientation of the system. Since the gyroscope is not affected by the gravitational or magnetic field, it requires the readings from the accelerometer and magnetometer to calculate a reference vector. Gyroscopes’ readings have different offsets depending on which direction the gyroscope is facing; when these readings are integrated over time it causes the integral result to drift. The accelerometer is not affected by drift, therefore, it can be used as an orientation reference in the X and Z axis of the rigid body to compensate the roll-pitch error (gyro’s offset error). The magnetometer’s readings are used to calculate the heading of the rigid body. The magnetometer must be three axes to be able to calculate the heading of the system in any position of the sensor platform; to compensate yaw error. The heading of the system used as the reference vector in the Y axis (yaw error), in addition to the roll-pitch error calculated by the accelerometer, it allows the system to calculate the rotation correction matrix. Afterwards, the algorithm uses a proportional plus integral feedback controller on the correction matrix to the remove the drift from the gyro’s readings. The compensated gyroscope readings denoted as ω (omega), are then fed to the “Normalization & Kinematics” block as it can be seen in Figure 2. The rotation matrix’s columns are unit vectors. Thus, before calculating the kinematics portion it must be normalized. (See Renormalization section in [2]). Once normalized, the gyroscope along with the previous rotation matrix are used to calculate the current rotation matrix (R Matrix) by using Equation 17 in the Computing Direction Cosines From Gyro Signals section in [2]. Finally, the Euler angles are calculated from the updated rotation matrix. SLAA518A – February 2012 Submit Documentation Feedback Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated 3 MSP430F5xx Firmware www.ti.com Accelerometer X*, Y*, Z* Accelerometer X, Y, Z Magnetometer X, Y, Z R Matrix R Matrix Heading Yaw Drift Detection Sensors Calibration (Section 4) Magnetometer X*, Y*, Z* Heading Error Roll-Pitch Error Gyroscope X*, Y*, Z* Gyroscope X, Y, Z Error Adjustment PI Controller + Drift Adjustment Euler Angles (roll, pitch, and yaw) R Matrix R Matrix Normalization and Kinematics Figure 2. Direct Cosine Matrix Algorithm Overview 3 MSP430F5xx Firmware This section covers the firmware’s architecture of the AHRS. There are two modes of firmware that can be downloaded to the MSP430F5xx family: AHRS mode (see Figure 3) or the calibration mode (see Figure 4). The mode must be defined in device.h, where you must enable either #define AHRS_MODE or #define CALIBRATION_MODE. Both modes initialize the MSP430F5xx by following these steps: 1. Set the main clock to 16 MHz. 2. Initialize I2C module (Master mode, Baud Rate ~ 400k Hz, 7-bit Addressing). 3. Initialize background timer (Rate ~20 mS, Disabled). 4. Initialize USB Module (CDC Class). Afterwards the motion sensors are initialized, and the background timer is enabled. The timer wakes the MCU from low-power-mode 0 (lowest power consumption mode allowed when using USB module) at a 50 Hz (20 mS) rate. The accelerometer and gyroscope are read and calibrated at a 50 Hz (20 mS) rate. The magnetometer is read at 10 Hz (100 mS), since the heading of the system does not fluctuate as much as the gravitational field or angular velocity. 3.1 AHRS Mode The magnetometer’s three axes readings are soft and hard iron compensated (see Section 4), and the sensor platform’s heading is calculated. The calibrated sensor readings are fed to the Direction Cosine Matrix algorithm (see Section 2) to calculate the Euler angles (roll, pitch, and yaw). The orientation angles are sent via USB to the AHRS GUI in the computer, at a 20 Hz (50 mS) rate. The GUI displays the Euler angles in a horizon (pitch & roll) and a digital compass (yaw) (see Figure 4). 4 Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated SLAA518A – February 2012 Submit Documentation Feedback MSP430F5xx Firmware www.ti.com Legend: Initialize MCU AHRS_ENABLED Initialize Sensors Read Accelerometer and Gyroscope 100 ms elapsed? Yes No Magnetometer Calibration DCM Algorithm 50 ms elapsed? Read Magnetometer Calculate Heading Yes Send Euler Angles to GUI No Blink LED Yes Enter Low Power Mode 0 20 ms elapsed? No Figure 3. MSP430F5xx AHRS Firmware Overview Figure 4. AHRS GUI SLAA518A – February 2012 Submit Documentation Feedback Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated 5 MSP430F5xx Firmware www.ti.com The background timer and the reading of the sensors can run at a faster frequency to increase the resolution of the algorithm’s integration; therefore, gaining better accuracy of the orientation angles. On the other hand, when running the algorithm at a higher frequency it causes the power consumption and CPU usage to increase. Running the background timer at 50 Hz is the sweet spot for low-power consumption and orientation accuracy. 3.2 Calibration Mode When the calibration mode (see Figure 5) is enabled instead of calculating the Euler angles, the firmware checks if a request for sensor raw data has been received or not. When the request (see Table 1) is received, the MSP430F5xx sends the GUI 500 samples of sensor data in all 3 axes via USB; this results in a 10 second calibration for the accelerometer and gyroscope, and a 50 second calibration for the magnetometer. When calibrating the magnetometer, it is very important to read the max and min value for each axis; sending more samples from the MSP430 to the GUI allows you to move the board in all the angles necessary for calibration. Table 1. Sensor Data Request Commands Command ID Sensor Data Requested 0x31 Accelerometer 0x32 Gyroscope 0x33 Magnetometer Legend: Initialize MCU CALIBRATION_ENABLED Initialize Sensors Read Accelerometer and Gyroscope 100 ms elapsed? Yes Read Magnetometer No Request received? Yes Send Raw Sensor Data No Yes Blink LED Enter Low Power Mode 0 20 ms elapsed? No Figure 5. MSP430F5xx Calibration Firmware Overview 6 Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated SLAA518A – February 2012 Submit Documentation Feedback Sensors Calibration www.ti.com 4 Sensors Calibration This section covers how to calibrate the DCM sensors using the 9-Axis Sensor Fusion Calibration GUI (see Figure 6). The X, Y, and Z offsets calculated for each sensor must be updated inside calibrationSensors() in AppRoutines.c. Figure 6. AHRS Calibration GUI Follow these steps to calibrate your sensor platform: 1. Download the firmware with CALIBRATION_MODE enabled to the MSP430F5xx. 2. Install the USB drivers in the PC. 3. Open the 9-Axis Sensor Fusion Calibration GUI. 4. Click “Auto Connect”. Now the Accelerometer, Gyroscope and Magnetometer buttons should be enabled. 4.1 Accelerometer When calibrating the accelerometer, the GUI requests for the AHRS board to be placed in the three different positions. When the board is placed on a flat surface (parallel to earth), the only axis that should be non zero is the perpendicular axis to the surface; therefore, the readings on the two other axes will be offsets (using Equation 1). with a three axes accelerometer, readings can be taken from the accelerometer having the axis perpendicular to the flat surface and pointing to the sky being X, Y, and Z. For example, when X is pointing upwards to the sky, the offsets are calculated for Y and Z. Then when Y is pointing upwards to the sky, the offsets are calculated for X and Z. Therefore, when having gone through all three positions there are 2 offsets values for each axis, which are averaged and displayed as the X, Y, and Z offsets. dataoffset = 4.2 datamin + datamax 2 (1) Gyroscope When calibrating the gyroscope, the AHRS board must be stationary on a flat surface where the angular velocity for all three axes should be 0. The offsets are calculated by using Equation 1 on all three axes. Once the GUI receives the requested raw gyroscope readings from the AHRS, it will display the offsets as the X, Y, and z offsets. SLAA518A – February 2012 Submit Documentation Feedback Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated 7 Sensors Calibration 4.3 www.ti.com Magnetometer There are two types of calibrations required for magnetometers: soft iron and hard iron calibration. Hard iron calibration is considered to remove constant magnetic field affecting the sensor platform. When graphing the output of a magnetometer in an ideal case, the output should be a perfect sphere in 3D centered at (0,0,0), but this is usually not the case. Instead, it is centered in another x,y,z location. For example, in Figure 7 the center lies in (34.5, -140.5, 46.5). These offsets were calculated by using Equation 1 on the readings of the magnetometer on all three axes after moving the board in all different angles. Figure 7. Magnetometer Hard Iron Calibration When calibrating the magnetometer, it is best to rotate the board in three different rotations as displayed in Figure 8. While calibrating the magnetometer, the three graphs displayed in Figure 7 should be graphing a circle, which allows for the minimum and maximum of each axis to be taken into account when calculating the offsets. Soft iron calibration is required to eliminate the effects of electromagnetic fields, which causes the ideal sphere to become an oval shape figure. Soft iron calibration is performed in the firmware after the magnetometer values have been read and hard iron calibration has been applied. 8 Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated SLAA518A – February 2012 Submit Documentation Feedback Conclusion www.ti.com Figure 8. Rotations for Hard Iron Calibration of the Magnetometer 5 Conclusion This section covers the power consumption of the sensor platform and the MSP430 requirements. The voltage across a shunt resistor (26.63 Ω) connected to the VCC of the MSP430F5xx can be seen in Figure 9. The voltage measurements were taken with the USB module disabled and low-power-mode 3 enabled when the MSP430F5xx was neither reading the sensors nor calculating the DCM algorithm. The MSP430F5xx consumes ~ 5 µA in low-power-mode 3 and ~ 5.6 mA in active mode. The AHRS board requires being battery powered when the transmission of the Euler angles is via BLE. The cycle area of the voltage is 685.3 µVs, where each cycle is 20 ms. Therefore, the average voltage is 685.3 µVs / 20ms = 34.265 mV. Using Equation 2, the average current consumption of the MSP430F5xx is 34.265 mV/ 26.63 Ω ~ 1.28 mA. D Vavg Vavg = Iavg * R ® Iavg = R SLAA518A – February 2012 Submit Documentation Feedback (2) Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated 9 Conclusion www.ti.com Figure 9. MSP430 Voltage Measurement Figure 9 shows the reading the motion sensors at different frequencies; the magnetometer (M) is read at a 10 Hz rate, while the accelerometer (A) and gyroscope (G) are read at 50 Hz. This figure shows the MSP430F5xx using ~30% of the CPU. The accuracy of the MSP430 AHRS board is TBD. Table 2 includes the current consumption measurements for the different ICs in the AHRS board. Table 2. System Current Consumption Integrated Circuits (ICs) Average Current Measurement MSP430F5xx 1.06 mA Accelerometer (1) (1) 0.14 mA Gyroscope 6.5 mA Barometric Pressure Sensor 0.65 mA (1) (1) Magnetometer 0.1 mA BLE Radio 30-mA worst case peak at 4 dBm The current measurement values were obtained from the sensors’ datasheet. MSP430 Requirements: • RAM ~ 0.75 kB • Flash ~ 11.7 kB • CPU Usage ~ 21.34% • SMCLK = 16 MHz, MCLK = 16 MHz, ACLK = REFO ~ 32.6 kHz NOTE: The memory requirements include the DCM algorithm and exclude the USB Stack. IAR Embedded Workbench 5.20.1 was the IDE used to benchmark. 10 Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated SLAA518A – February 2012 Submit Documentation Feedback Schematics www.ti.com 6 Schematics SLAA518A – February 2012 Submit Documentation Feedback Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated 11 Schematics 12 www.ti.com Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated SLAA518A – February 2012 Submit Documentation Feedback Schematics www.ti.com SLAA518A – February 2012 Submit Documentation Feedback Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated 13 References 7 References 1. 2. 3. 4. 5. 14 www.ti.com MSP430x5xx/MSP430x6xx Family User's Guide (SLAU208) Direction Cosine Matrix IMU: Theory (http://gentlenav.googlecode.com/files/DCMDraft2.pdf) CkDevices Open Source Firmware (http://www.ckdevices.com) CkDevices Open Source Mongoose Visualizer (http://www.ckdevices.com) Compensating for Tilt, Hard-Iron, and Soft-Iron Effects (http://www.sensorsmag.com/sensors/motionvelocity-displacement/compensating-tilt-hard-iron-and-soft-iron-effects-6475) Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family Copyright © 2012, Texas Instruments Incorporated SLAA518A – February 2012 Submit Documentation Feedback IMPORTANT NOTICE Texas Instruments Incorporated and its subsidiaries (TI) reserve the right to make corrections, modifications, enhancements, improvements, and other changes to its products and services at any time and to discontinue any product or service without notice. Customers should obtain the latest relevant information before placing orders and should verify that such information is current and complete. All products are sold subject to TI’s terms and conditions of sale supplied at the time of order acknowledgment. TI warrants performance of its hardware products to the specifications applicable at the time of sale in accordance with TI’s standard warranty. Testing and other quality control techniques are used to the extent TI deems necessary to support this warranty. Except where mandated by government requirements, testing of all parameters of each product is not necessarily performed. TI assumes no liability for applications assistance or customer product design. Customers are responsible for their products and applications using TI components. To minimize the risks associated with customer products and applications, customers should provide adequate design and operating safeguards. TI does not warrant or represent that any license, either express or implied, is granted under any TI patent right, copyright, mask work right, or other TI intellectual property right relating to any combination, machine, or process in which TI products or services are used. Information published by TI regarding third-party products or services does not constitute a license from TI to use such products or services or a warranty or endorsement thereof. Use of such information may require a license from a third party under the patents or other intellectual property of the third party, or a license from TI under the patents or other intellectual property of TI. Reproduction of TI information in TI data books or data sheets is permissible only if reproduction is without alteration and is accompanied by all associated warranties, conditions, limitations, and notices. Reproduction of this information with alteration is an unfair and deceptive business practice. TI is not responsible or liable for such altered documentation. Information of third parties may be subject to additional restrictions. Resale of TI products or services with statements different from or beyond the parameters stated by TI for that product or service voids all express and any implied warranties for the associated TI product or service and is an unfair and deceptive business practice. TI is not responsible or liable for any such statements. TI products are not authorized for use in safety-critical applications (such as life support) where a failure of the TI product would reasonably be expected to cause severe personal injury or death, unless officers of the parties have executed an agreement specifically governing such use. Buyers represent that they have all necessary expertise in the safety and regulatory ramifications of their applications, and acknowledge and agree that they are solely responsible for all legal, regulatory and safety-related requirements concerning their products and any use of TI products in such safety-critical applications, notwithstanding any applications-related information or support that may be provided by TI. Further, Buyers must fully indemnify TI and its representatives against any damages arising out of the use of TI products in such safety-critical applications. TI products are neither designed nor intended for use in military/aerospace applications or environments unless the TI products are specifically designated by TI as military-grade or "enhanced plastic." Only products designated by TI as military-grade meet military specifications. Buyers acknowledge and agree that any such use of TI products which TI has not designated as military-grade is solely at the Buyer's risk, and that they are solely responsible for compliance with all legal and regulatory requirements in connection with such use. TI products are neither designed nor intended for use in automotive applications or environments unless the specific TI products are designated by TI as compliant with ISO/TS 16949 requirements. Buyers acknowledge and agree that, if they use any non-designated products in automotive applications, TI will not be responsible for any failure to meet such requirements. Following are URLs where you can obtain information on other Texas Instruments products and application solutions: Products Applications Audio www.ti.com/audio Automotive and Transportation www.ti.com/automotive Amplifiers amplifier.ti.com Communications and Telecom www.ti.com/communications Data Converters dataconverter.ti.com Computers and Peripherals www.ti.com/computers DLP® Products www.dlp.com Consumer Electronics www.ti.com/consumer-apps DSP dsp.ti.com Energy and Lighting www.ti.com/energy Clocks and Timers www.ti.com/clocks Industrial www.ti.com/industrial Interface interface.ti.com Medical www.ti.com/medical Logic logic.ti.com Security www.ti.com/security Power Mgmt power.ti.com Space, Avionics and Defense www.ti.com/space-avionics-defense Microcontrollers microcontroller.ti.com Video and Imaging www.ti.com/video RFID www.ti-rfid.com OMAP Mobile Processors www.ti.com/omap Wireless Connectivity www.ti.com/wirelessconnectivity TI E2E Community Home Page e2e.ti.com Mailing Address: Texas Instruments, Post Office Box 655303, Dallas, Texas 75265 Copyright © 2012, Texas Instruments Incorporated