├── LICENSE.md
├── library.properties
├── .gitattributes
├── keywords.txt
├── README.md
├── .gitignore
├── examples
    ├── SparkFun_LSM9DS0_Simple
    │   └── SparkFun_LSM9DS0_Simple.ino
    ├── SparkFun_LSM9DS0_SerialMenus
    │   └── SparkFun_LSM9DS0_SerialMenus.ino
    └── SparkFun_LSM9DS0_AHRS
    │   └── SparkFun_LSM9DS0_AHRS.ino
└── src
    ├── SFE_LSM9DS0.h
    └── SFE_LSM9DS0.cpp


/LICENSE.md:
--------------------------------------------------------------------------------
 1 | 
 2 | License Information
 3 | -------------------
 4 | 
 5 | The hardware is released under [Creative Commons Share-alike 3.0](http://creativecommons.org/licenses/by-sa/3.0/). 
 6 | 
 7 | All other code is open source so please feel free to do anything you want with it; you buy me a beer if you use this and we meet someday ([Beerware license](http://en.wikipedia.org/wiki/Beerware)).
 8 | 
 9 | ->Additional Licenses and attributions to original authors as needed.<-
10 | 
11 | 


--------------------------------------------------------------------------------
/library.properties:
--------------------------------------------------------------------------------
 1 | name=SparkFun LSM9DS0 Breakout
 2 | version=1.0.1
 3 | author=SparkFun Electronics <techsupport@sparkfun.com>
 4 | maintainer=SparkFun Electronics <sparkfun.com>
 5 | sentence=Library for the 9 degree of freedom IC -ST Micro's LSM9DS0. 
 6 | paragraph=A <a href="https://www.sparkfun.com/products/12636">breakout board</a> for ST Micro's LSM9DS0 -- a 3D accelerometer, gyroscop, and magnetometer. This library gives the user 9DOF functionality. 
 7 | category=Sensors
 8 | url=https://github.com/sparkfun/SparkFun_LSM9DS0_Arduino_Library
 9 | architectures=*
10 | 


--------------------------------------------------------------------------------
/.gitattributes:
--------------------------------------------------------------------------------
 1 | # Auto detect text files and perform LF normalization
 2 | * text=auto
 3 | 
 4 | # Custom for Visual Studio
 5 | *.cs     diff=csharp
 6 | *.sln    merge=union
 7 | *.csproj merge=union
 8 | *.vbproj merge=union
 9 | *.fsproj merge=union
10 | *.dbproj merge=union
11 | 
12 | # Standard to msysgit
13 | *.doc	 diff=astextplain
14 | *.DOC	 diff=astextplain
15 | *.docx diff=astextplain
16 | *.DOCX diff=astextplain
17 | *.dot  diff=astextplain
18 | *.DOT  diff=astextplain
19 | *.pdf  diff=astextplain
20 | *.PDF	 diff=astextplain
21 | *.rtf	 diff=astextplain
22 | *.RTF	 diff=astextplain
23 | 


--------------------------------------------------------------------------------
/keywords.txt:
--------------------------------------------------------------------------------
 1 | ########################################################
 2 | # Syntax Coloring Map for SparkFun LSM9DS0 IMU Library #
 3 | ########################################################
 4 | # Class
 5 | ###################################################################
 6 | 
 7 | LSM9DS0	KEYWORD1
 8 | 
 9 | 
10 | ###################################################################
11 | # Methods and Functions
12 | ###################################################################
13 | 
14 | begin	KEYWORD2
15 | readGyro	KEYWORD2
16 | readAccel	KEYWORD2
17 | readMag	KEYWORD2
18 | calcGyro	KEYWORD2
19 | calcAccel	KEYWORD2
20 | calcMag	KEYWORD2
21 | setGyroScale	KEYWORD2
22 | setAccelScale	KEYWORD2
23 | setMagScale	KEYWORD2
24 | setGyroODR	KEYWORD2
25 | setAccelODR	KEYWORD2
26 | setAccelABW	KEYWORD2
27 | setMagODR	KEYWORD2
28 | calLSM9DS0	KEYWORD2
29 | gx	KEYWORD2
30 | gy	KEYWORD2
31 | gz	KEYWORD2
32 | ax	KEYWORD2
33 | ay	KEYWORD2
34 | az	KEYWORD2
35 | mx	KEYWORD2
36 | my	KEYWORD2
37 | mz	KEYWORD2
38 | temperature	KEYWORD2
39 | abias	KEYWORD2
40 | gbias	KEYWORD2
41 | 
42 | ###################################################################
43 | # Constants
44 | ###################################################################
45 | 
46 | MODE_SPI	LITERAL1
47 | MODE_I2C	LITERAL1
48 | G_SCALE_245DPS	LITERAL1
49 | G_SCALE_500DPS	LITERAL1
50 | G_SCALE_2000DPS	LITERAL1
51 | A_SCALE_2G	LITERAL1
52 | A_SCALE_4G	LITERAL1
53 | A_SCALE_6G	LITERAL1
54 | A_SCALE_8G	LITERAL1
55 | A_SCALE_16G	LITERAL1
56 | M_SCALE_2GS	LITERAL1
57 | M_SCALE_4GS	LITERAL1
58 | M_SCALE_8GS	LITERAL1
59 | M_SCALE_12GS	LITERAL1
60 | G_ODR_95_BW_125	LITERAL1
61 | G_ODR_95_BW_25	LITERAL1
62 | G_ODR_190_BW_125	LITERAL1
63 | G_ODR_190_BW_25	LITERAL1
64 | G_ODR_190_BW_50	LITERAL1
65 | G_ODR_190_BW_70	LITERAL1
66 | G_ODR_380_BW_20	LITERAL1
67 | G_ODR_380_BW_25	LITERAL1
68 | G_ODR_380_BW_50	LITERAL1
69 | G_ODR_380_BW_100	LITERAL1
70 | G_ODR_760_BW_30	LITERAL1
71 | G_ODR_760_BW_35	LITERAL1
72 | G_ODR_760_BW_50	LITERAL1
73 | G_ODR_760_BW_100	LITERAL1
74 | A_POWER_DOWN	LITERAL1
75 | A_ODR_3125	LITERAL1
76 | A_ODR_625	LITERAL1
77 | A_ODR_125	LITERAL1
78 | A_ODR_25	LITERAL1
79 | A_ODR_50	LITERAL1
80 | A_ODR_100	LITERAL1
81 | A_ODR_200	LITERAL1
82 | A_ODR_400	LITERAL1
83 | A_ODR_800	LITERAL1
84 | A_ODR_1600	LITERAL1
85 | A_ABW_773	LITERAL1
86 | A_ABW_194	LITERAL1
87 | A_ABW_362	LITERAL1
88 | A_ABW_50	LITERAL1
89 | M_ODR_3125	LITERAL1
90 | M_ODR_625	LITERAL1
91 | M_ODR_125	LITERAL1
92 | M_ODR_25	LITERAL1
93 | M_ODR_50	LITERAL1
94 | M_ODR_100	LITERAL1
95 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | SparkFun LSM9DS0 Arduino Library
 2 | ======================
 3 | 
 4 | [![Breakout Board ISO](https://cdn.sparkfun.com//assets/parts/9/3/1/9/12636-01.jpg)](https://www.sparkfun.com/products/12636)
 5 | 
 6 | _[LSM9DS0 Breakout Board (SEN-12636)](https://www.sparkfun.com/products/12636)_
 7 | 
 8 | This is a breakout board for [ST Micro's LSM9DS0](http://www.st.com/web/catalog/sense_power/FM89/SC1448/PF258556) -- a 3D accelerometer, gyroscope, and magnetometer.
 9 | 
10 | Repository Contents
11 | -------------------
12 | * **/examples** - Example sketches for the library (.ino). Run these from the Arduino IDE.
13 | * **/src** - Source files for the library (.cpp, .h).
14 | * **keywords.txt** - Keywords from this library that will be highlighted in the Arduino IDE.
15 | * **library.properties** - General library properties for the Arduino package manager.
16 | 
17 | Documentation
18 | --------------
19 | 
20 | * **[Installing an Arduino Library Guide](https://learn.sparkfun.com/tutorials/installing-an-arduino-library)** - Basic information on how to install an Arduino library.
21 | * **[Product Repository](https://github.com/sparkfun/LSM9DS0_Breakout)** - Main repository (including hardware files) for the LSM9DS0 Breakout.
22 | * **[Hookup Guide](https://learn.sparkfun.com/tutorials/lsm9ds0-hookup-guide)** - Basic hookup guide for the LSM9DS0 Breakout.
23 | 
24 | Products that use this Library
25 | ---------------------------------
26 | 
27 | * [LSM9DS0 Breakout Board (SEN-12636)](https://www.sparkfun.com/products/12636)- LSM9DS0 Breakout board.
28 | 
29 | Version History
30 | ---------------
31 | 
32 | * [V 1.0.2](https://github.com/sparkfun/SparkFun_LSM9DS0_Arduino_Library/releases/tag/V_1.0.2) -- Fixed bug in setAccelABW() function.
33 | * [V 1.0.1](https://github.com/sparkfun/SparkFun_LSM9DS0_Arduino_Library/releases/tag/V_1.0.1) -- Updated library.properties and standardized example names.
34 | * [V 1.0.0](https://github.com/sparkfun/SparkFun_LSM9DS0_Arduino_Library/releases/tag/V_1.0.0) -- Initial commit of Arduino 1.6-compatible library.
35 | 
36 | License Information
37 | -------------------
38 | 
39 | This product is _**open source**_!
40 | 
41 | The **code** is beerware; if you see me (or any other SparkFun employee) at the local, and you've found our code helpful, please buy us a round!
42 | 
43 | Please use, reuse, and modify these files as you see fit. Please maintain attribution to SparkFun Electronics and release anything derivative under the same license.
44 | 
45 | Distributed as-is; no warranty is given.
46 | 
47 | - Your friends at SparkFun.
48 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
  1 | #################
  2 | ## SparkFun Useful stuff
  3 | #################
  4 | 
  5 | ## AVR Development
  6 | *.eep
  7 | *.elf
  8 | *.lst
  9 | *.lss
 10 | *.sym
 11 | *.d
 12 | *.o
 13 | *.srec
 14 | *.map
 15 | 
 16 | ## Notepad++ backup files
 17 | *.bak
 18 | 
 19 | ## BOM files
 20 | *bom*
 21 | 
 22 | #################
 23 | ## Eclipse
 24 | #################
 25 | 
 26 | *.pydevproject
 27 | .project
 28 | .metadata
 29 | bin/
 30 | tmp/
 31 | *.tmp
 32 | *.bak
 33 | *.swp
 34 | *~.nib
 35 | local.properties
 36 | .classpath
 37 | .settings/
 38 | .loadpath
 39 | 
 40 | # External tool builders
 41 | .externalToolBuilders/
 42 | 
 43 | # Locally stored "Eclipse launch configurations"
 44 | *.launch
 45 | 
 46 | # CDT-specific
 47 | .cproject
 48 | 
 49 | # PDT-specific
 50 | .buildpath
 51 | 
 52 | 
 53 | #############
 54 | ## Eagle
 55 | #############
 56 | 
 57 | # Ignore the board and schematic backup files
 58 | *.b#?
 59 | *.s#?
 60 | 
 61 | 
 62 | #################
 63 | ## Visual Studio
 64 | #################
 65 | 
 66 | ## Ignore Visual Studio temporary files, build results, and
 67 | ## files generated by popular Visual Studio add-ons.
 68 | 
 69 | # User-specific files
 70 | *.suo
 71 | *.user
 72 | *.sln.docstates
 73 | 
 74 | # Build results
 75 | [Dd]ebug/
 76 | [Rr]elease/
 77 | *_i.c
 78 | *_p.c
 79 | *.ilk
 80 | *.meta
 81 | *.obj
 82 | *.pch
 83 | *.pdb
 84 | *.pgc
 85 | *.pgd
 86 | *.rsp
 87 | *.sbr
 88 | *.tlb
 89 | *.tli
 90 | *.tlh
 91 | *.tmp
 92 | *.vspscc
 93 | .builds
 94 | *.dotCover
 95 | 
 96 | ## TODO: If you have NuGet Package Restore enabled, uncomment this
 97 | #packages/
 98 | 
 99 | # Visual C++ cache files
100 | ipch/
101 | *.aps
102 | *.ncb
103 | *.opensdf
104 | *.sdf
105 | 
106 | # Visual Studio profiler
107 | *.psess
108 | *.vsp
109 | 
110 | # ReSharper is a .NET coding add-in
111 | _ReSharper*
112 | 
113 | # Installshield output folder
114 | [Ee]xpress
115 | 
116 | # DocProject is a documentation generator add-in
117 | DocProject/buildhelp/
118 | DocProject/Help/*.HxT
119 | DocProject/Help/*.HxC
120 | DocProject/Help/*.hhc
121 | DocProject/Help/*.hhk
122 | DocProject/Help/*.hhp
123 | DocProject/Help/Html2
124 | DocProject/Help/html
125 | 
126 | # Click-Once directory
127 | publish
128 | 
129 | # Others
130 | [Bb]in
131 | [Oo]bj
132 | sql
133 | TestResults
134 | *.Cache
135 | ClientBin
136 | stylecop.*
137 | ~$*
138 | *.dbmdl
139 | Generated_Code #added for RIA/Silverlight projects
140 | 
141 | # Backup & report files from converting an old project file to a newer
142 | # Visual Studio version. Backup files are not needed, because we have git ;-)
143 | _UpgradeReport_Files/
144 | Backup*/
145 | UpgradeLog*.XML
146 | 
147 | 
148 | ############
149 | ## Windows
150 | ############
151 | 
152 | # Windows image file caches
153 | Thumbs.db
154 | 
155 | # Folder config file
156 | Desktop.ini
157 | 
158 | 
159 | #############
160 | ## Python
161 | #############
162 | 
163 | *.py[co]
164 | 
165 | # Packages
166 | *.egg
167 | *.egg-info
168 | dist
169 | build
170 | eggs
171 | parts
172 | bin
173 | var
174 | sdist
175 | develop-eggs
176 | .installed.cfg
177 | 
178 | # Installer logs
179 | pip-log.txt
180 | 
181 | # Unit test / coverage reports
182 | .coverage
183 | .tox
184 | 
185 | #Translations
186 | *.mo
187 | 
188 | #Mr Developer
189 | .mr.developer.cfg
190 | 
191 | # Mac crap
192 | .DS_Store
193 | 


--------------------------------------------------------------------------------
/examples/SparkFun_LSM9DS0_Simple/SparkFun_LSM9DS0_Simple.ino:
--------------------------------------------------------------------------------
  1 | /*****************************************************************
  2 | LSM9DS0_Simple.ino
  3 | SFE_LSM9DS0 Library Simple Example Code
  4 | Jim Lindblom @ SparkFun Electronics
  5 | Original Creation Date: February 18, 2014
  6 | https://github.com/sparkfun/LSM9DS0_Breakout
  7 | 
  8 | The LSM9DS0 is a versatile 9DOF sensor. It has a built-in
  9 | accelerometer, gyroscope, and magnetometer. Very cool! Plus it
 10 | functions over either SPI or I2C.
 11 | 
 12 | This Arduino sketch is a demo of the simple side of the
 13 | SFE_LSM9DS0 library. It'll demo the following:
 14 | * How to create a LSM9DS0 object, using a constructor (global
 15 |   variables section).
 16 | * How to use the begin() function of the LSM9DS0 class.
 17 | * How to read the gyroscope, accelerometer, and magnetometer
 18 |   using the readGryo(), readAccel(), readMag() functions and the
 19 |   gx, gy, gz, ax, ay, az, mx, my, and mz variables.
 20 | * How to calculate actual acceleration, rotation speed, magnetic
 21 |   field strength using the calcAccel(), calcGyro() and calcMag()
 22 |   functions.
 23 | * How to use the data from the LSM9DS0 to calculate orientation
 24 |   and heading.
 25 | 
 26 | Hardware setup: This library supports communicating with the
 27 | LSM9DS0 over either I2C or SPI. If you're using I2C, these are
 28 | the only connections that need to be made:
 29 | 	LSM9DS0 --------- Arduino
 30 | 	 SCL ---------- SCL (A5 on older 'Duinos')
 31 | 	 SDA ---------- SDA (A4 on older 'Duinos')
 32 | 	 VDD ------------- 3.3V
 33 | 	 GND ------------- GND
 34 | (CSG, CSXM, SDOG, and SDOXM should all be pulled high jumpers on 
 35 |   the breakout board will do this for you.)
 36 |   
 37 | If you're using SPI, here is an example hardware setup:
 38 | 	LSM9DS0 --------- Arduino
 39 |           CSG -------------- 9
 40 |           CSXM ------------- 10
 41 |           SDOG ------------- 12
 42 |           SDOXM ------------ 12 (tied to SDOG)
 43 |           SCL -------------- 13
 44 |           SDA -------------- 11
 45 |           VDD -------------- 3.3V
 46 |           GND -------------- GND
 47 | 	
 48 | The LSM9DS0 has a maximum voltage of 3.6V. Make sure you power it
 49 | off the 3.3V rail! And either use level shifters between SCL
 50 | and SDA or just use a 3.3V Arduino Pro.	  
 51 | 
 52 | Development environment specifics:
 53 | 	IDE: Arduino 1.0.5
 54 | 	Hardware Platform: Arduino Pro 3.3V/8MHz
 55 | 	LSM9DS0 Breakout Version: 1.0
 56 | 
 57 | This code is beerware. If you see me (or any other SparkFun 
 58 | employee) at the local, and you've found our code helpful, please 
 59 | buy us a round!
 60 | 
 61 | Distributed as-is; no warranty is given.
 62 | *****************************************************************/
 63 | 
 64 | // The SFE_LSM9DS0 requires both the SPI and Wire libraries.
 65 | // Unfortunately, you'll need to include both in the Arduino
 66 | // sketch, before including the SFE_LSM9DS0 library.
 67 | #include <SPI.h> // Included for SFE_LSM9DS0 library
 68 | #include <Wire.h>
 69 | #include <SFE_LSM9DS0.h>
 70 | 
 71 | ///////////////////////
 72 | // Example I2C Setup //
 73 | ///////////////////////
 74 | // Comment out this section if you're using SPI
 75 | // SDO_XM and SDO_G are both grounded, so our addresses are:
 76 | #define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
 77 | #define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW
 78 | // Create an instance of the LSM9DS0 library called `dof` the
 79 | // parameters for this constructor are:
 80 | // [SPI or I2C Mode declaration],[gyro I2C address],[xm I2C add.]
 81 | LSM9DS0 dof(MODE_I2C, LSM9DS0_G, LSM9DS0_XM);
 82 | 
 83 | ///////////////////////
 84 | // Example SPI Setup //
 85 | ///////////////////////
 86 | /* // Uncomment this section if you're using SPI
 87 | #define LSM9DS0_CSG  9  // CSG connected to Arduino pin 9
 88 | #define LSM9DS0_CSXM 10 // CSXM connected to Arduino pin 10
 89 | LSM9DS0 dof(MODE_SPI, LSM9DS0_CSG, LSM9DS0_CSXM);
 90 | */
 91 | 
 92 | // Do you want to print calculated values or raw ADC ticks read
 93 | // from the sensor? Comment out ONE of the two #defines below
 94 | // to pick:
 95 | #define PRINT_CALCULATED
 96 | //#define PRINT_RAW
 97 | 
 98 | #define PRINT_SPEED 500 // 500 ms between prints
 99 | 
100 | void setup()
101 | {
102 |   Serial.begin(115200); // Start serial at 115200 bps
103 |   // Use the begin() function to initialize the LSM9DS0 library.
104 |   // You can either call it with no parameters (the easy way):
105 |   uint16_t status = dof.begin();
106 |   // Or call it with declarations for sensor scales and data rates:  
107 |   //uint16_t status = dof.begin(dof.G_SCALE_2000DPS, 
108 |   //                            dof.A_SCALE_6G, dof.M_SCALE_2GS);
109 |   
110 |   // begin() returns a 16-bit value which includes both the gyro 
111 |   // and accelerometers WHO_AM_I response. You can check this to
112 |   // make sure communication was successful.
113 |   Serial.print("LSM9DS0 WHO_AM_I's returned: 0x");
114 |   Serial.println(status, HEX);
115 |   Serial.println("Should be 0x49D4");
116 |   Serial.println();
117 | }
118 | 
119 | void loop()
120 | {
121 |   printGyro();  // Print "G: gx, gy, gz"
122 |   printAccel(); // Print "A: ax, ay, az"
123 |   printMag();   // Print "M: mx, my, mz"
124 |   
125 |   // Print the heading and orientation for fun!
126 |   printHeading((float) dof.mx, (float) dof.my);
127 |   printOrientation(dof.calcAccel(dof.ax), dof.calcAccel(dof.ay), 
128 |                    dof.calcAccel(dof.az));
129 |   Serial.println();
130 |   
131 |   delay(PRINT_SPEED);
132 | }
133 | 
134 | void printGyro()
135 | {
136 |   // To read from the gyroscope, you must first call the
137 |   // readGyro() function. When this exits, it'll update the
138 |   // gx, gy, and gz variables with the most current data.
139 |   dof.readGyro();
140 |   
141 |   // Now we can use the gx, gy, and gz variables as we please.
142 |   // Either print them as raw ADC values, or calculated in DPS.
143 |   Serial.print("G: ");
144 | #ifdef PRINT_CALCULATED
145 |   // If you want to print calculated values, you can use the
146 |   // calcGyro helper function to convert a raw ADC value to
147 |   // DPS. Give the function the value that you want to convert.
148 |   Serial.print(dof.calcGyro(dof.gx), 2);
149 |   Serial.print(", ");
150 |   Serial.print(dof.calcGyro(dof.gy), 2);
151 |   Serial.print(", ");
152 |   Serial.println(dof.calcGyro(dof.gz), 2);
153 | #elif defined PRINT_RAW
154 |   Serial.print(dof.gx);
155 |   Serial.print(", ");
156 |   Serial.print(dof.gy);
157 |   Serial.print(", ");
158 |   Serial.println(dof.gz);
159 | #endif
160 | }
161 | 
162 | void printAccel()
163 | {
164 |   // To read from the accelerometer, you must first call the
165 |   // readAccel() function. When this exits, it'll update the
166 |   // ax, ay, and az variables with the most current data.
167 |   dof.readAccel();
168 |   
169 |   // Now we can use the ax, ay, and az variables as we please.
170 |   // Either print them as raw ADC values, or calculated in g's.
171 |   Serial.print("A: ");
172 | #ifdef PRINT_CALCULATED
173 |   // If you want to print calculated values, you can use the
174 |   // calcAccel helper function to convert a raw ADC value to
175 |   // g's. Give the function the value that you want to convert.
176 |   Serial.print(dof.calcAccel(dof.ax), 2);
177 |   Serial.print(", ");
178 |   Serial.print(dof.calcAccel(dof.ay), 2);
179 |   Serial.print(", ");
180 |   Serial.println(dof.calcAccel(dof.az), 2);
181 | #elif defined PRINT_RAW 
182 |   Serial.print(dof.ax);
183 |   Serial.print(", ");
184 |   Serial.print(dof.ay);
185 |   Serial.print(", ");
186 |   Serial.println(dof.az);
187 | #endif
188 | 
189 | }
190 | 
191 | void printMag()
192 | {
193 |   // To read from the magnetometer, you must first call the
194 |   // readMag() function. When this exits, it'll update the
195 |   // mx, my, and mz variables with the most current data.
196 |   dof.readMag();
197 |   
198 |   // Now we can use the mx, my, and mz variables as we please.
199 |   // Either print them as raw ADC values, or calculated in Gauss.
200 |   Serial.print("M: ");
201 | #ifdef PRINT_CALCULATED
202 |   // If you want to print calculated values, you can use the
203 |   // calcMag helper function to convert a raw ADC value to
204 |   // Gauss. Give the function the value that you want to convert.
205 |   Serial.print(dof.calcMag(dof.mx), 2);
206 |   Serial.print(", ");
207 |   Serial.print(dof.calcMag(dof.my), 2);
208 |   Serial.print(", ");
209 |   Serial.println(dof.calcMag(dof.mz), 2);
210 | #elif defined PRINT_RAW
211 |   Serial.print(dof.mx);
212 |   Serial.print(", ");
213 |   Serial.print(dof.my);
214 |   Serial.print(", ");
215 |   Serial.println(dof.mz);
216 | #endif
217 | }
218 | 
219 | // Here's a fun function to calculate your heading, using Earth's
220 | // magnetic field.
221 | // It only works if the sensor is flat (z-axis normal to Earth).
222 | // Additionally, you may need to add or subtract a declination
223 | // angle to get the heading normalized to your location.
224 | // See: http://www.ngdc.noaa.gov/geomag/declination.shtml
225 | void printHeading(float hx, float hy)
226 | {
227 |   float heading;
228 |   
229 |   if (hy > 0)
230 |   {
231 |     heading = 90 - (atan(hx / hy) * (180 / PI));
232 |   }
233 |   else if (hy < 0)
234 |   {
235 |     heading = - (atan(hx / hy) * (180 / PI));
236 |   }
237 |   else // hy = 0
238 |   {
239 |     if (hx < 0) heading = 180;
240 |     else heading = 0;
241 |   }
242 |   
243 |   Serial.print("Heading: ");
244 |   Serial.println(heading, 2);
245 | }
246 | 
247 | // Another fun function that does calculations based on the
248 | // acclerometer data. This function will print your LSM9DS0's
249 | // orientation -- it's roll and pitch angles.
250 | void printOrientation(float x, float y, float z)
251 | {
252 |   float pitch, roll;
253 |   
254 |   pitch = atan2(x, sqrt(y * y) + (z * z));
255 |   roll = atan2(y, sqrt(x * x) + (z * z));
256 |   pitch *= 180.0 / PI;
257 |   roll *= 180.0 / PI;
258 |   
259 |   Serial.print("Pitch, Roll: ");
260 |   Serial.print(pitch, 2);
261 |   Serial.print(", ");
262 |   Serial.println(roll, 2);
263 | }


--------------------------------------------------------------------------------
/examples/SparkFun_LSM9DS0_SerialMenus/SparkFun_LSM9DS0_SerialMenus.ino:
--------------------------------------------------------------------------------
  1 | /*****************************************************************
  2 | LSM9DS0_SerialMenus.ino
  3 | SFE_LSM9DS0 Library Example Code: Interact With Serial Menus
  4 | Jim Lindblom @ SparkFun Electronics
  5 | Original Creation Date: February 14, 2014 (Happy Valentines Day!)
  6 | https://github.com/sparkfun/LSM9DS0_Breakout
  7 | 
  8 | The LSM9DS0 is a versatile motion-sensing system-in-a-chip. It
  9 | features 3-axis accelerometer/gyroscope/magnetometer, and can be
 10 | controlled over either an SPI or I2C interface.
 11 | 
 12 | This Arduino sketch is a demo of all things SEF_LSM9DS0 library.
 13 | Once you attach all hardware, and upload the sketch, open your
 14 | Serial monitor at 115200 BPS. Follow the menu prompts to either:
 15 | 	1) Stream readings from the accelerometer.
 16 | 	2) Stream readings from the gyroscope.
 17 | 	3) Stream readings from the magnetometer.
 18 | 	4) Set the scales of each sensor (e.g. +/-4g, 500DPS, 8Gs)
 19 | 	5) Switch to/from calculated or raw data (e.g. ADC ticks or
 20 | 		g's, DPS, and Gs)
 21 | 	6) Set the output data rate of each sensor.
 22 | 
 23 | Hardware setup: This library supports communicating with the
 24 | LSM9DS0 over either I2C or SPI. In addition to those wires, this
 25 | sketch demos how to use the interrupts. Here's what the I2C setup
 26 | looks like:
 27 | 	LSM9DS0 --------- Arduino
 28 | 	 CSG ------------- NONE (Pulled HIGH [indicates I2C mode])
 29 | 	 CSXM ------------ NONE (Pulled HIGH [indicates I2C mode])
 30 | 	 SDOG ------------ NONE (Pulled HIGH [sets I2C address])
 31 | 	 SDOXM ----------- NONE (Pulled HIGH [sets I2C address])
 32 | 	 SCL ---------- SCL (A5 on older 'Duinos')
 33 | 	 SDA ---------- SDA (A4 on older 'Duinos')
 34 | 	 VDD ------------- 3.3V
 35 | 	 GND ------------- GND
 36 | 	 DEN ------------- NONE (Not used in this example)
 37 | 	 INTG ------------ NONE (Not used in this example)
 38 | 	 DRDYG ------------ 4 (Could be any digital pin)
 39 | 	 INT1XM ----------- 3 (Could be any digital pin)
 40 | 	 INT2XM ----------- 2 (Could be any digital pin)
 41 | 	
 42 | The LSM9DS0 has a maximum voltage of 3.6V. Make sure you power it
 43 | off the 3.3V rail! And either use level shifters between SCL
 44 | and SDA or just use a 3.3V Arduino Pro.	  
 45 | 
 46 | Development environment specifics:
 47 | 	IDE: Arduino 1.0.5
 48 | 	Hardware Platform: Arduino Pro 3.3V/8MHz
 49 | 	LSM9DS0 Breakout Version: 1.0
 50 | 
 51 | This code is beerware; if you see me (or any other SparkFun 
 52 | employee) at the local, and you've found our code helpful, please 
 53 | buy us a round!
 54 | 
 55 | Distributed as-is; no warranty is given.
 56 | *****************************************************************/
 57 | 
 58 | // The SFE_LSM9DS0 requires both the SPI and Wire libraries.
 59 | // Unfortunately, you'll need to include both in the Arduino
 60 | // sketch, before including the SFE_LSM9DS0 library.
 61 | #include <SPI.h> // Included for SFE_LSM9DS0 library
 62 | #include <Wire.h>
 63 | #include <SFE_LSM9DS0.h>
 64 | 
 65 | ///////////////////////
 66 | // Example I2C Setup //
 67 | ///////////////////////
 68 | // SDO_XM and SDO_G are both grounded, therefore our addresses are:
 69 | #define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
 70 | #define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW
 71 | // Create an instance of the LSM9DS0 library called `dof` the
 72 | // parameters for this constructor are:
 73 | // [SPI or I2C Mode declaration], [gyro I2C address], [xm I2C address]
 74 | LSM9DS0 dof(MODE_I2C, LSM9DS0_G, LSM9DS0_XM);
 75 | 
 76 | ///////////////////////
 77 | // Example SPI Setup //
 78 | ///////////////////////
 79 | //#define LSM9DS0_CSG  9  // CSG connected to Arduino pin 9
 80 | //#define LSM9DS0_CSXM 10 // CSXM connected to Arduino pin 10
 81 | //LSM9DS0 dof(MODE_SPI, LSM9DS0_CSG, LSM9DS0_CSXM);
 82 | 
 83 | ///////////////////////////////
 84 | // Interrupt Pin Definitions //
 85 | ///////////////////////////////
 86 | const byte INT1XM = 2; // INT1XM tells us when accel data is ready
 87 | const byte INT2XM = 3; // INT2XM tells us when mag data is ready
 88 | const byte DRDYG = 4;  // DRDYG tells us when gyro data is ready
 89 | 
 90 | // A boolean to keep track of whether we're printing raw (ADC)
 91 | // or calculated (g's, DPS, Gs) sensor data:
 92 | boolean printRaw = true;
 93 | 
 94 | void setup()
 95 | {
 96 |   // Set up interrupt pins as inputs:
 97 |   pinMode(INT1XM, INPUT);
 98 |   pinMode(INT2XM, INPUT);
 99 |   pinMode(DRDYG, INPUT);
100 |   
101 |   Serial.begin(115200); // Start serial at 115200 bps
102 |   // Use the begin() function to initialize the LSM9DS0 library.
103 |   // You can either call it with no parameters (the easy way):
104 |   uint16_t status = dof.begin();
105 |   // Or call it with declarations for sensor scales and data rates:  
106 |   //uint16_t status = dof.begin(dof.G_SCALE_2000DPS, dof.A_SCALE_6G, dof.M_SCALE_2GS);
107 |   
108 |   // begin() returns a 16-bit value which includes both the gyro and
109 |   // accelerometers WHO_AM_I response. You can check this to make sure
110 |   // communication was successful.
111 |   Serial.println(status, HEX);
112 | }
113 | 
114 | void loop()
115 | {
116 |   // Print the control menu:
117 |   printMenu();
118 |   // Then wait for any serial data to come in:
119 |   while (!Serial.available())
120 |     ;
121 |   // Once serial data is received, call parseMenu to act on it:
122 |   parseMenu(Serial.read());
123 |   
124 | }
125 | 
126 | void printAccel()
127 | {
128 |   // Only read from the accelerometer if the accel interrupts,
129 |   // which means that new data is ready.
130 |   if (digitalRead(INT1XM))
131 |   {
132 |     // Use the readAccel() function to get new data from the accel.
133 | 	// After calling this function, new values will be stored in
134 | 	// the ax, ay, and az variables.
135 |     dof.readAccel();
136 | 	
137 |     Serial.print("A: ");
138 |     if (printRaw)
139 |     {
140 |       Serial.print(dof.ax);
141 |       Serial.print(", ");
142 |       Serial.print(dof.ay);
143 |       Serial.print(", ");
144 |       Serial.println(dof.az);
145 |     }
146 |     else
147 |     {
148 | 	  // Using the calcAccel helper function, we can get the
149 | 	  // accelerometer readings in g's.
150 |       Serial.print(dof.calcAccel(dof.ax));
151 |       Serial.print(", ");
152 |       Serial.print(dof.calcAccel(dof.ay));
153 |       Serial.print(", ");
154 |       Serial.println(dof.calcAccel(dof.az));
155 |     }
156 |   }
157 | }
158 | 
159 | void printGyro()
160 | {
161 |   // Only read from the gyro if the DRDY interrupts,
162 |   // which means that new data is ready.
163 |   if (digitalRead(DRDYG))
164 |   {
165 |     // Use the readGyro() function to get new data from the gyro.
166 | 	// After calling this function, new values will be stored in
167 | 	// the gx, gy, and gz variables.
168 |     dof.readGyro();
169 | 	
170 |     Serial.print("G: ");
171 |     if (printRaw)
172 |     {
173 |       Serial.print(dof.gx);
174 |       Serial.print(", ");
175 |       Serial.print(dof.gy);
176 |       Serial.print(", ");
177 |       Serial.println(dof.gz);
178 |     }
179 |     else
180 |     {
181 | 	  // Using the calcGyro helper function, we can get the
182 | 	  // gyroscope readings in degrees per second (DPS).
183 |       Serial.print(dof.calcGyro(dof.gx));
184 |       Serial.print(", ");
185 |       Serial.print(dof.calcGyro(dof.gy));
186 |       Serial.print(", ");
187 |       Serial.println(dof.calcGyro(dof.gz));
188 |     }
189 |   }
190 | }
191 | 
192 | void printMag()
193 | {
194 |   // Only read from the magnetometer if the INT2XM interrupts,
195 |   // which means that new data is ready.
196 |   if (digitalRead(INT2XM))
197 |   {
198 |     // Use the readMag() function to get new data from the mag.
199 | 	// After calling this function, new values will be stored in
200 | 	// the mx, my, and mz variables.
201 |     dof.readMag();
202 | 	
203 |     Serial.print("M: ");
204 |     if (printRaw)
205 |     {
206 |       Serial.print(dof.mx);
207 |       Serial.print(", ");
208 |       Serial.print(dof.my);
209 |       Serial.print(", ");
210 |       Serial.print(dof.mz);
211 |       Serial.print(", ");
212 |       Serial.println(calcHeading(dof.mx, dof.my));
213 |     }
214 |     else
215 |     {
216 | 	  // Using the calcMg helper function, we can get the
217 | 	  // magnetometer readings in gauss (Gs).
218 |       Serial.print(dof.calcMag(dof.mx), 4);
219 |       Serial.print(", ");
220 |       Serial.print(dof.calcMag(dof.my), 4);
221 |       Serial.print(", ");
222 |       Serial.print(dof.calcMag(dof.mz), 4);
223 |       Serial.print(", ");
224 |       Serial.println(calcHeading(dof.mx, dof.my));
225 |     }
226 |   }
227 | }
228 | 
229 | // Here's a simple example function to calculate heading based on
230 | // magnetometer readings. This only works when the 9DOF is flat
231 | // (x-axis normal to gravity).
232 | float calcHeading(float hx, float hy)
233 | {  
234 |   if (hy > 0)
235 |   {
236 |     return 90 - (atan(hx / hy) * 180 / PI);
237 |   }
238 |   else if (hy < 0)
239 |   {
240 |     return 270 - (atan(hx / hy) * 180 / PI);
241 |   }
242 |   else // hy = 0
243 |   {
244 |     if (hx < 0) return 180;
245 |     else return 0;
246 |   }
247 | }
248 | 
249 | // This function will print all data from all sensors at once.
250 | // It'll wait until every sensor interrupt triggers before
251 | // printing.
252 | void streamAll()
253 | {
254 |   if ((digitalRead(INT2XM)) && (digitalRead(INT1XM)) &&
255 |       (digitalRead(DRDYG)))
256 |   {
257 |     printAccel();
258 |     printGyro();
259 |     printMag();
260 |   }
261 | }
262 | 
263 | // setScale() provides an interface to switch the full-scale range
264 | // of each sensor. This function will block until three characters
265 | // (to select the three ranges) are received.
266 | void setScale()
267 | {
268 |   char c;
269 |   
270 |   // Print the accelerometer range options:
271 |   Serial.println(F("Set accelerometer scale:"));
272 |   Serial.println(F("\t1) +/- 2G"));
273 |   Serial.println(F("\t2) +/- 4G"));
274 |   Serial.println(F("\t3) +/- 6G"));
275 |   Serial.println(F("\t4) +/- 8G"));
276 |   Serial.println(F("\t5) +/- 16G"));
277 |   // Wait for a serial char to come in:
278 |   while (Serial.available() < 1)
279 |     ;
280 |   c = Serial.read();
281 |   // Use the setAccelScale function to set the accelerometer
282 |   // full-scale range to any of the possible ranges. These ranges
283 |   // are all defined in SFE_LSM9DS0.h.
284 |   switch (c)
285 |   {
286 |     case '1':
287 |       dof.setAccelScale(dof.A_SCALE_2G);
288 |       break;
289 |     case '2':
290 |       dof.setAccelScale(dof.A_SCALE_4G);
291 |       break;
292 |     case '3':
293 |       dof.setAccelScale(dof.A_SCALE_6G);
294 |       break;
295 |     case '4':
296 |       dof.setAccelScale(dof.A_SCALE_8G);
297 |       break;
298 |     case '5':
299 |       dof.setAccelScale(dof.A_SCALE_16G);
300 |       break;
301 |   }
302 |   // Print the gyro scale ranges:
303 |   Serial.println(F("Set gyroscope scale:"));
304 |   Serial.println(F("\t1) +/- 245 DPS"));
305 |   Serial.println(F("\t2) +/- 500 DPS"));
306 |   Serial.println(F("\t3) +/- 2000 DPS"));
307 |   // Wait for a character to come in:
308 |   while (Serial.available() < 1)
309 |     ;
310 |   c = Serial.read();
311 |   // Use the setGyroScale function to set the gyroscope
312 |   // full-scale range to any of the possible ranges. These ranges
313 |   // are all defined in SFE_LSM9DS0.h.
314 |   switch (c)
315 |   {
316 |     case '1':
317 |       dof.setGyroScale(dof.G_SCALE_245DPS);
318 |       break;
319 |     case '2':
320 |       dof.setGyroScale(dof.G_SCALE_500DPS);
321 |       break;
322 |     case '3':
323 |       dof.setGyroScale(dof.G_SCALE_2000DPS);
324 |       break;
325 |   }
326 |   // Print the magnetometer scale options:
327 |   Serial.println(F("Set magnetometer scale:"));
328 |   Serial.println(F("\t1) +/- 2GS"));
329 |   Serial.println(F("\t2) +/- 4GS"));
330 |   Serial.println(F("\t3) +/- 8GS"));
331 |   Serial.println(F("\t4) +/- 12GS"));
332 |   // Wait for a char:
333 |   while (Serial.available() < 1)
334 |     ;
335 |   c = Serial.read();
336 |   // Use the setMagScale function to set the magnetometer
337 |   // full-scale range to any of the possible ranges. These ranges
338 |   // are all defined in SFE_LSM9DS0.h.
339 |   switch (c)
340 |   {
341 |     case '1':
342 |       dof.setMagScale(dof.M_SCALE_2GS);
343 |       break;
344 |     case '2':
345 |       dof.setMagScale(dof.M_SCALE_4GS);
346 |       break;
347 |     case '3':
348 |       dof.setMagScale(dof.M_SCALE_8GS);
349 |       break;
350 |     case '4':
351 |       dof.setMagScale(dof.M_SCALE_12GS);
352 |       break;
353 |   }
354 | }
355 | 
356 | // setRaw simply switches the state of the global printRaw
357 | // variable. It'll print a message to say what it's switched to.
358 | void setRaw()
359 | {
360 |   if (printRaw)
361 |   {
362 |     printRaw = false;
363 |     Serial.println(F("Printing calculated readings"));
364 |   }
365 |   else
366 |   {
367 |     printRaw = true;
368 |     Serial.println(F("Printing raw readings"));
369 |   }
370 | }
371 | 
372 | // setODR() provides a serial interface to set the output data
373 | // rate (ODR) for each sensor. It will block until it receives
374 | // three characters to set the data rates.
375 | void setODR()
376 | {
377 |   char c;
378 |   
379 |   // Print the menu options for accel data rate:
380 |   Serial.println(F("Set Accelerometer ODR (Hz):"));
381 |   Serial.println(F("\t1) 3.125 \t 6) 100"));
382 |   Serial.println(F("\t2) 6.25  \t 7) 200"));
383 |   Serial.println(F("\t3) 12.5  \t 8) 400"));
384 |   Serial.println(F("\t4) 25    \t 9) 800"));
385 |   Serial.println(F("\t5) 50    \t A) 1600"));
386 |   // Wait for a character to be read in:
387 |   while (Serial.available() < 1)
388 |     ;
389 |   c = Serial.read();
390 |   // Use the setAccelODR function to set the accelerometer
391 |   // data rate to any of the possible ranges. These ranges
392 |   // are all defined in SFE_LSM9DS0.h.
393 |   switch (c)
394 |   {
395 |     case '1':
396 |       dof.setAccelODR(dof.A_ODR_3125);
397 |       break;
398 |     case '2':
399 |       dof.setAccelODR(dof.A_ODR_625);
400 |       break;
401 |     case '3':
402 |       dof.setAccelODR(dof.A_ODR_125);
403 |       break;
404 |     case '4':
405 |       dof.setAccelODR(dof.A_ODR_25);
406 |       break;
407 |     case '5':
408 |       dof.setAccelODR(dof.A_ODR_50);
409 |       break;
410 |     case '6':
411 |       dof.setAccelODR(dof.A_ODR_100);
412 |       break;
413 |     case '7':
414 |       dof.setAccelODR(dof.A_ODR_200);
415 |       break;
416 |     case '8':
417 |       dof.setAccelODR(dof.A_ODR_400);
418 |       break;
419 |     case '9':
420 |       dof.setAccelODR(dof.A_ODR_800);
421 |       break;
422 |     case 'A':
423 |     case 'a':
424 |       dof.setAccelODR(dof.A_ODR_1600);
425 |       break;
426 |   }
427 |   
428 |   // Print the menu options for the gyro ODR's
429 |   Serial.println(F("Set Gyro ODR/Cutoff (Hz):"));
430 |   Serial.println(F("\t1) 95/12.5 \t 8) 380/25"));
431 |   Serial.println(F("\t2) 95/25   \t 9) 380/50"));
432 |   Serial.println(F("\t3) 190/125 \t A) 380/100"));
433 |   Serial.println(F("\t4) 190/25  \t B) 760/30"));
434 |   Serial.println(F("\t5) 190/50  \t C) 760/35"));
435 |   Serial.println(F("\t6) 190/70  \t D) 760/50"));
436 |   Serial.println(F("\t7) 380/20  \t E) 760/100"));
437 |   // Wait for a character to arrive:
438 |   while (Serial.available() < 1)
439 |     ;
440 |   c = Serial.read();
441 |   // Use the setGyroODR function to set the gyroscope
442 |   // data rate to any of the possible ranges. These ranges
443 |   // are all defined in SFE_LSM9DS0.h.
444 |   switch (c)
445 |   {
446 |     case '1':
447 |       dof.setGyroODR(dof.G_ODR_95_BW_125);
448 |       break;
449 |     case '2':
450 |       dof.setGyroODR(dof.G_ODR_95_BW_25);
451 |       break;
452 |     case '3':
453 |       dof.setGyroODR(dof.G_ODR_190_BW_125);
454 |       break;
455 |     case '4':
456 |       dof.setGyroODR(dof.G_ODR_190_BW_25);
457 |       break;
458 |     case '5':
459 |       dof.setGyroODR(dof.G_ODR_190_BW_50);
460 |       break;
461 |     case '6':
462 |       dof.setGyroODR(dof.G_ODR_190_BW_70);
463 |       break;
464 |     case '7':
465 |       dof.setGyroODR(dof.G_ODR_380_BW_20);
466 |       break;
467 |     case '8':
468 |       dof.setGyroODR(dof.G_ODR_380_BW_25);
469 |       break;
470 |     case '9':
471 |       dof.setGyroODR(dof.G_ODR_380_BW_50);
472 |       break;
473 |     case 'A':
474 |     case 'a':
475 |       dof.setGyroODR(dof.G_ODR_380_BW_100);
476 |       break;
477 |     case 'B':
478 |     case 'b':
479 |       dof.setGyroODR(dof.G_ODR_760_BW_30);
480 |       break;
481 |     case 'C':
482 |     case 'c':
483 |       dof.setGyroODR(dof.G_ODR_760_BW_35);
484 |       break;
485 |     case 'D':
486 |     case 'd':
487 |       dof.setGyroODR(dof.G_ODR_760_BW_50);
488 |       break;
489 |     case 'E':
490 |     case 'e':
491 |       dof.setGyroODR(dof.G_ODR_760_BW_100);
492 |       break;
493 |   }
494 |   
495 |   // Print all possible range selections for the magnetometer:
496 |   Serial.println(F("Set Magnetometer ODR (Hz):"));
497 |   Serial.println(F("\t1) 3.125 \t 4) 25"));
498 |   Serial.println(F("\t2) 6.25  \t 5) 50"));
499 |   Serial.println(F("\t3) 12.5  \t 6) 100"));
500 |   // Wait for a character to come in:
501 |   while (Serial.available() < 1)
502 |     ;
503 |   c = Serial.read();
504 |   // Use the setMagODR function to set the magnetometer
505 |   // data rate to any of the possible ranges. These ranges
506 |   // are all defined in SFE_LSM9DS0.h.
507 |   switch (c)
508 |   {
509 |     case '1':
510 |       dof.setMagODR(dof.M_ODR_3125);
511 |       break;
512 |     case '2':
513 |       dof.setMagODR(dof.M_ODR_625);
514 |       break;
515 |     case '3':
516 |       dof.setMagODR(dof.M_ODR_125);
517 |       break;
518 |     case '4':
519 |       dof.setMagODR(dof.M_ODR_25);
520 |       break;
521 |     case '5':
522 |       dof.setMagODR(dof.M_ODR_50);
523 |       break;
524 |     case '6':
525 |       dof.setMagODR(dof.M_ODR_100);
526 |       break;
527 |   }
528 | }
529 | 
530 | void printMenu()
531 | {
532 |   Serial.println();
533 |   Serial.println(F("////////////////////////////////////////////"));
534 |   Serial.println(F("// LSM9DS0 Super Awesome Amazing Fun Time //"));
535 |   Serial.println(F("////////////////////////////////////////////"));
536 |   Serial.println();
537 |   Serial.println(F("1) Stream Accelerometer"));
538 |   Serial.println(F("2) Stream Gyroscope"));
539 |   Serial.println(F("3) Stream Magnetometer"));
540 |   Serial.println(F("4) Stream output from all sensors"));
541 |   Serial.println(F("5) Set Sensor Scales"));
542 |   Serial.println(F("6) Switch To/From Raw/Calculated Readings"));
543 |   Serial.println(F("7) Set Output Data Rates")); 
544 |   Serial.println(); 
545 | }
546 | 
547 | // parseMenu() takes a char parameter, which should map to one of
548 | // the defined menu options. A switch statement will control what
549 | // happens based on the given character input.
550 | void parseMenu(char c)
551 | {
552 |   switch (c)
553 |   {
554 |     case '1':
555 |       while(!Serial.available())
556 |         printAccel(); // Print accelerometer values
557 |       break;
558 |     case '2':
559 |       while(!Serial.available())
560 |         printGyro(); // Print gyroscope values
561 |       break;
562 |     case '3':
563 |       while(!Serial.available())
564 |         printMag(); // Print magnetometer values
565 |       break;
566 |     case '4':
567 |       while(!Serial.available())
568 |         streamAll(); // Print all sensor readings
569 |       break;
570 |     case '5':
571 |       setScale(); // Set the ranges of each sensor
572 |       break;
573 |     case '6':
574 |       setRaw(); // Switch between calculated and raw output
575 |       break;
576 |     case '7':
577 |       setODR(); // Set the data rates of each sensor
578 |       break;
579 |   }
580 | }
581 | 


--------------------------------------------------------------------------------
/src/SFE_LSM9DS0.h:
--------------------------------------------------------------------------------
  1 | /******************************************************************************
  2 | SFE_LSM9DS0.h
  3 | SFE_LSM9DS0 Library Header File
  4 | Jim Lindblom @ SparkFun Electronics
  5 | Original Creation Date: February 14, 2014 (Happy Valentines Day!)
  6 | https://github.com/sparkfun/LSM9DS0_Breakout
  7 | 
  8 | This file prototypes the LSM9DS0 class, implemented in SFE_LSM9DS0.cpp. In
  9 | addition, it defines every register in the LSM9DS0 (both the Gyro and Accel/
 10 | Magnetometer registers).
 11 | 
 12 | Development environment specifics:
 13 | 	IDE: Arduino 1.0.5
 14 | 	Hardware Platform: Arduino Pro 3.3V/8MHz
 15 | 	LSM9DS0 Breakout Version: 1.0
 16 | 
 17 | This code is beerware; if you see me (or any other SparkFun employee) at the
 18 | local, and you've found our code helpful, please buy us a round!
 19 | 
 20 | Distributed as-is; no warranty is given.
 21 | ******************************************************************************/
 22 | #ifndef __SFE_LSM9DS0_H__
 23 | #define __SFE_LSM9DS0_H__
 24 | 
 25 | #if defined(ARDUINO) && ARDUINO >= 100
 26 |   #include "Arduino.h"
 27 | #else
 28 |   #include "WProgram.h"
 29 |   #include "pins_arduino.h"
 30 | #endif
 31 | 
 32 | ////////////////////////////
 33 | // LSM9DS0 Gyro Registers //
 34 | ////////////////////////////
 35 | #define WHO_AM_I_G			0x0F
 36 | #define CTRL_REG1_G			0x20
 37 | #define CTRL_REG2_G			0x21
 38 | #define CTRL_REG3_G			0x22
 39 | #define CTRL_REG4_G			0x23
 40 | #define CTRL_REG5_G			0x24
 41 | #define REFERENCE_G			0x25
 42 | #define STATUS_REG_G		0x27
 43 | #define OUT_X_L_G			0x28
 44 | #define OUT_X_H_G			0x29
 45 | #define OUT_Y_L_G			0x2A
 46 | #define OUT_Y_H_G			0x2B
 47 | #define OUT_Z_L_G			0x2C
 48 | #define OUT_Z_H_G			0x2D
 49 | #define FIFO_CTRL_REG_G		0x2E
 50 | #define FIFO_SRC_REG_G		0x2F
 51 | #define INT1_CFG_G			0x30
 52 | #define INT1_SRC_G			0x31
 53 | #define INT1_THS_XH_G		0x32
 54 | #define INT1_THS_XL_G		0x33
 55 | #define INT1_THS_YH_G		0x34
 56 | #define INT1_THS_YL_G		0x35
 57 | #define INT1_THS_ZH_G		0x36
 58 | #define INT1_THS_ZL_G		0x37
 59 | #define INT1_DURATION_G		0x38
 60 | 
 61 | //////////////////////////////////////////
 62 | // LSM9DS0 Accel/Magneto (XM) Registers //
 63 | //////////////////////////////////////////
 64 | #define OUT_TEMP_L_XM		0x05
 65 | #define OUT_TEMP_H_XM		0x06
 66 | #define STATUS_REG_M		0x07
 67 | #define OUT_X_L_M			0x08
 68 | #define OUT_X_H_M			0x09
 69 | #define OUT_Y_L_M			0x0A
 70 | #define OUT_Y_H_M			0x0B
 71 | #define OUT_Z_L_M			0x0C
 72 | #define OUT_Z_H_M			0x0D
 73 | #define WHO_AM_I_XM			0x0F
 74 | #define INT_CTRL_REG_M		0x12
 75 | #define INT_SRC_REG_M		0x13
 76 | #define INT_THS_L_M			0x14
 77 | #define INT_THS_H_M			0x15
 78 | #define OFFSET_X_L_M		0x16
 79 | #define OFFSET_X_H_M		0x17
 80 | #define OFFSET_Y_L_M		0x18
 81 | #define OFFSET_Y_H_M		0x19
 82 | #define OFFSET_Z_L_M		0x1A
 83 | #define OFFSET_Z_H_M		0x1B
 84 | #define REFERENCE_X			0x1C
 85 | #define REFERENCE_Y			0x1D
 86 | #define REFERENCE_Z			0x1E
 87 | #define CTRL_REG0_XM		0x1F
 88 | #define CTRL_REG1_XM		0x20
 89 | #define CTRL_REG2_XM		0x21
 90 | #define CTRL_REG3_XM		0x22
 91 | #define CTRL_REG4_XM		0x23
 92 | #define CTRL_REG5_XM		0x24
 93 | #define CTRL_REG6_XM		0x25
 94 | #define CTRL_REG7_XM		0x26
 95 | #define STATUS_REG_A		0x27
 96 | #define OUT_X_L_A			0x28
 97 | #define OUT_X_H_A			0x29
 98 | #define OUT_Y_L_A			0x2A
 99 | #define OUT_Y_H_A			0x2B
100 | #define OUT_Z_L_A			0x2C
101 | #define OUT_Z_H_A			0x2D
102 | #define FIFO_CTRL_REG		0x2E
103 | #define FIFO_SRC_REG		0x2F
104 | #define INT_GEN_1_REG		0x30
105 | #define INT_GEN_1_SRC		0x31
106 | #define INT_GEN_1_THS		0x32
107 | #define INT_GEN_1_DURATION	0x33
108 | #define INT_GEN_2_REG		0x34
109 | #define INT_GEN_2_SRC		0x35
110 | #define INT_GEN_2_THS		0x36
111 | #define INT_GEN_2_DURATION	0x37
112 | #define CLICK_CFG			0x38
113 | #define CLICK_SRC			0x39
114 | #define CLICK_THS			0x3A
115 | #define TIME_LIMIT			0x3B
116 | #define TIME_LATENCY		0x3C
117 | #define TIME_WINDOW			0x3D
118 | #define ACT_THS				0x3E
119 | #define ACT_DUR				0x3F
120 | 
121 | // The LSM9DS0 functions over both I2C or SPI. This library supports both.
122 | // But the interface mode used must be sent to the LSM9DS0 constructor. Use
123 | // one of these two as the first parameter of the constructor.
124 | enum interface_mode
125 | {
126 | 	MODE_SPI,
127 | 	MODE_I2C,
128 | };
129 | 
130 | class LSM9DS0
131 | {
132 | public:
133 | 	// gyro_scale defines the possible full-scale ranges of the gyroscope:
134 | 	enum gyro_scale
135 | 	{
136 | 		G_SCALE_245DPS,		// 00:  245 degrees per second
137 | 		G_SCALE_500DPS,		// 01:  500 dps
138 | 		G_SCALE_2000DPS,	// 10:  2000 dps
139 | 	};
140 | 	// accel_scale defines all possible FSR's of the accelerometer:
141 | 	enum accel_scale
142 | 	{
143 | 		A_SCALE_2G,	// 000:  2g
144 | 		A_SCALE_4G,	// 001:  4g
145 | 		A_SCALE_6G,	// 010:  6g
146 | 		A_SCALE_8G,	// 011:  8g
147 | 		A_SCALE_16G	// 100:  16g
148 | 	};
149 | 	// mag_scale defines all possible FSR's of the magnetometer:
150 | 	enum mag_scale
151 | 	{
152 | 		M_SCALE_2GS,	// 00:  2Gs
153 | 		M_SCALE_4GS, 	// 01:  4Gs
154 | 		M_SCALE_8GS,	// 10:  8Gs
155 | 		M_SCALE_12GS,	// 11:  12Gs
156 | 	};
157 | 	// gyro_odr defines all possible data rate/bandwidth combos of the gyro:
158 | 	enum gyro_odr
159 | 	{							// ODR (Hz) --- Cutoff
160 | 		G_ODR_95_BW_125  = 0x0, //   95         12.5
161 | 		G_ODR_95_BW_25   = 0x1, //   95          25
162 | 		// 0x2 and 0x3 define the same data rate and bandwidth
163 | 		G_ODR_190_BW_125 = 0x4, //   190        12.5
164 | 		G_ODR_190_BW_25  = 0x5, //   190         25
165 | 		G_ODR_190_BW_50  = 0x6, //   190         50
166 | 		G_ODR_190_BW_70  = 0x7, //   190         70
167 | 		G_ODR_380_BW_20  = 0x8, //   380         20
168 | 		G_ODR_380_BW_25  = 0x9, //   380         25
169 | 		G_ODR_380_BW_50  = 0xA, //   380         50
170 | 		G_ODR_380_BW_100 = 0xB, //   380         100
171 | 		G_ODR_760_BW_30  = 0xC, //   760         30
172 | 		G_ODR_760_BW_35  = 0xD, //   760         35
173 | 		G_ODR_760_BW_50  = 0xE, //   760         50
174 | 		G_ODR_760_BW_100 = 0xF, //   760         100
175 | 	};
176 | 	// accel_oder defines all possible output data rates of the accelerometer:
177 | 	enum accel_odr
178 | 	{
179 | 		A_POWER_DOWN, 	// Power-down mode (0x0)
180 | 		A_ODR_3125,		// 3.125 Hz	(0x1)
181 | 		A_ODR_625,		// 6.25 Hz (0x2)
182 | 		A_ODR_125,		// 12.5 Hz (0x3)
183 | 		A_ODR_25,		// 25 Hz (0x4)
184 | 		A_ODR_50,		// 50 Hz (0x5)
185 | 		A_ODR_100,		// 100 Hz (0x6)
186 | 		A_ODR_200,		// 200 Hz (0x7)
187 | 		A_ODR_400,		// 400 Hz (0x8)
188 | 		A_ODR_800,		// 800 Hz (9)
189 | 		A_ODR_1600		// 1600 Hz (0xA)
190 | 	};
191 | 
192 |       // accel_abw defines all possible anti-aliasing filter rates of the accelerometer:
193 | 	enum accel_abw
194 | 	{
195 | 		A_ABW_773,		// 773 Hz (0x0)
196 | 		A_ABW_194,		// 194 Hz (0x1)
197 | 		A_ABW_362,		// 362 Hz (0x2)
198 | 		A_ABW_50,		//  50 Hz (0x3)
199 | 	};
200 | 
201 | 
202 | 	// mag_oder defines all possible output data rates of the magnetometer:
203 | 	enum mag_odr
204 | 	{
205 | 		M_ODR_3125,	// 3.125 Hz (0x00)
206 | 		M_ODR_625,	// 6.25 Hz (0x01)
207 | 		M_ODR_125,	// 12.5 Hz (0x02)
208 | 		M_ODR_25,	// 25 Hz (0x03)
209 | 		M_ODR_50,	// 50 (0x04)
210 | 		M_ODR_100,	// 100 Hz (0x05)
211 | 	};
212 | 
213 | 	// We'll store the gyro, accel, and magnetometer readings in a series of
214 | 	// public class variables. Each sensor gets three variables -- one for each
215 | 	// axis. Call readGyro(), readAccel(), and readMag() first, before using
216 | 	// these variables!
217 | 	// These values are the RAW signed 16-bit readings from the sensors.
218 | 	int16_t gx, gy, gz; // x, y, and z axis readings of the gyroscope
219 | 	int16_t ax, ay, az; // x, y, and z axis readings of the accelerometer
220 | 	int16_t mx, my, mz; // x, y, and z axis readings of the magnetometer
221 |         int16_t temperature;
222 | 	float abias[3];
223 |         float gbias[3];
224 | 
225 | 	// LSM9DS0 -- LSM9DS0 class constructor
226 | 	// The constructor will set up a handful of private variables, and set the
227 | 	// communication mode as well.
228 | 	// Input:
229 | 	//	- interface = Either MODE_SPI or MODE_I2C, whichever you're using
230 | 	//				to talk to the IC.
231 | 	//	- gAddr = If MODE_I2C, this is the I2C address of the gyroscope.
232 | 	// 				If MODE_SPI, this is the chip select pin of the gyro (CSG)
233 | 	//	- xmAddr = If MODE_I2C, this is the I2C address of the accel/mag.
234 | 	//				If MODE_SPI, this is the cs pin of the accel/mag (CSXM)
235 | 	LSM9DS0(interface_mode interface, uint8_t gAddr, uint8_t xmAddr);
236 | 	
237 | 	// begin() -- Initialize the gyro, accelerometer, and magnetometer.
238 | 	// This will set up the scale and output rate of each sensor. It'll also
239 | 	// "turn on" every sensor and every axis of every sensor.
240 | 	// Input:
241 | 	//	- gScl = The scale of the gyroscope. This should be a gyro_scale value.
242 | 	//	- aScl = The scale of the accelerometer. Should be a accel_scale value.
243 | 	//	- mScl = The scale of the magnetometer. Should be a mag_scale value.
244 | 	//	- gODR = Output data rate of the gyroscope. gyro_odr value.
245 | 	//	- aODR = Output data rate of the accelerometer. accel_odr value.
246 | 	//	- mODR = Output data rate of the magnetometer. mag_odr value.
247 | 	// Output: The function will return an unsigned 16-bit value. The most-sig
248 | 	//		bytes of the output are the WHO_AM_I reading of the accel. The
249 | 	//		least significant two bytes are the WHO_AM_I reading of the gyro.
250 | 	// All parameters have a defaulted value, so you can call just "begin()".
251 | 	// Default values are FSR's of:  245DPS, 2g, 2Gs; ODRs of 95 Hz for 
252 | 	// gyro, 100 Hz for accelerometer, 100 Hz for magnetometer.
253 | 	// Use the return value of this function to verify communication.
254 | 	uint16_t begin(gyro_scale gScl = G_SCALE_245DPS, 
255 | 				accel_scale aScl = A_SCALE_2G, mag_scale mScl = M_SCALE_2GS,
256 | 				gyro_odr gODR = G_ODR_95_BW_125, accel_odr aODR = A_ODR_50, 
257 | 				mag_odr mODR = M_ODR_50);
258 | 	
259 | 	// readGyro() -- Read the gyroscope output registers.
260 | 	// This function will read all six gyroscope output registers.
261 | 	// The readings are stored in the class' gx, gy, and gz variables. Read
262 | 	// those _after_ calling readGyro().
263 | 	void readGyro();
264 | 	
265 | 	// readAccel() -- Read the accelerometer output registers.
266 | 	// This function will read all six accelerometer output registers.
267 | 	// The readings are stored in the class' ax, ay, and az variables. Read
268 | 	// those _after_ calling readAccel().
269 | 	void readAccel();
270 | 	
271 | 	// readMag() -- Read the magnetometer output registers.
272 | 	// This function will read all six magnetometer output registers.
273 | 	// The readings are stored in the class' mx, my, and mz variables. Read
274 | 	// those _after_ calling readMag().
275 | 	void readMag();
276 | 
277 | 	// readTemp() -- Read the temperature output register.
278 | 	// This function will read two temperature output registers.
279 | 	// The combined readings are stored in the class' temperature variables. Read
280 | 	// those _after_ calling readTemp().
281 | 	void readTemp();
282 | 	
283 | 	// calcGyro() -- Convert from RAW signed 16-bit value to degrees per second
284 | 	// This function reads in a signed 16-bit value and returns the scaled
285 | 	// DPS. This function relies on gScale and gRes being correct.
286 | 	// Input:
287 | 	//	- gyro = A signed 16-bit raw reading from the gyroscope.
288 | 	float calcGyro(int16_t gyro);
289 | 	
290 | 	// calcAccel() -- Convert from RAW signed 16-bit value to gravity (g's).
291 | 	// This function reads in a signed 16-bit value and returns the scaled
292 | 	// g's. This function relies on aScale and aRes being correct.
293 | 	// Input:
294 | 	//	- accel = A signed 16-bit raw reading from the accelerometer.
295 | 	float calcAccel(int16_t accel);
296 | 	
297 | 	// calcMag() -- Convert from RAW signed 16-bit value to Gauss (Gs)
298 | 	// This function reads in a signed 16-bit value and returns the scaled
299 | 	// Gs. This function relies on mScale and mRes being correct.
300 | 	// Input:
301 | 	//	- mag = A signed 16-bit raw reading from the magnetometer.
302 | 	float calcMag(int16_t mag);
303 | 	
304 | 	// setGyroScale() -- Set the full-scale range of the gyroscope.
305 | 	// This function can be called to set the scale of the gyroscope to 
306 | 	// 245, 500, or 200 degrees per second.
307 | 	// Input:
308 | 	// 	- gScl = The desired gyroscope scale. Must be one of three possible
309 | 	//		values from the gyro_scale enum.
310 | 	void setGyroScale(gyro_scale gScl);
311 | 	
312 | 	// setAccelScale() -- Set the full-scale range of the accelerometer.
313 | 	// This function can be called to set the scale of the accelerometer to
314 | 	// 2, 4, 6, 8, or 16 g's.
315 | 	// Input:
316 | 	// 	- aScl = The desired accelerometer scale. Must be one of five possible
317 | 	//		values from the accel_scale enum.
318 | 	void setAccelScale(accel_scale aScl);
319 | 	
320 | 	// setMagScale() -- Set the full-scale range of the magnetometer.
321 | 	// This function can be called to set the scale of the magnetometer to
322 | 	// 2, 4, 8, or 12 Gs.
323 | 	// Input:
324 | 	// 	- mScl = The desired magnetometer scale. Must be one of four possible
325 | 	//		values from the mag_scale enum.
326 | 	void setMagScale(mag_scale mScl);
327 | 	
328 | 	// setGyroODR() -- Set the output data rate and bandwidth of the gyroscope
329 | 	// Input:
330 | 	//	- gRate = The desired output rate and cutoff frequency of the gyro.
331 | 	//		Must be a value from the gyro_odr enum (check above, there're 14).
332 | 	void setGyroODR(gyro_odr gRate);
333 | 	
334 | 	// setAccelODR() -- Set the output data rate of the accelerometer
335 | 	// Input:
336 | 	//	- aRate = The desired output rate of the accel.
337 | 	//		Must be a value from the accel_odr enum (check above, there're 11).
338 | 	void setAccelODR(accel_odr aRate); 	
339 | 
340 |         // setAccelABW() -- Set the anti-aliasing filter rate of the accelerometer
341 | 	// Input:
342 | 	//	- abwRate = The desired anti-aliasing filter rate of the accel.
343 | 	//		Must be a value from the accel_abw enum (check above, there're 4).
344 | 	void setAccelABW(accel_abw abwRate);
345 | 
346 | 
347 | 	
348 | 	// setMagODR() -- Set the output data rate of the magnetometer
349 | 	// Input:
350 | 	//	- mRate = The desired output rate of the mag.
351 | 	//		Must be a value from the mag_odr enum (check above, there're 6).
352 | 	void setMagODR(mag_odr mRate);
353 | 	
354 | 	// configGyroInt() -- Configure the gyro interrupt output.
355 | 	// Triggers can be set to either rising above or falling below a specified
356 | 	// threshold. This function helps setup the interrupt configuration and 
357 | 	// threshold values for all axes.
358 | 	// Input:
359 | 	//	- int1Cfg = A 8-bit value that is sent directly to the INT1_CFG_G
360 | 	//		register. This sets AND/OR and high/low interrupt gen for each axis
361 | 	//	- int1ThsX = 16-bit interrupt threshold value for x-axis
362 | 	//	- int1ThsY = 16-bit interrupt threshold value for y-axis
363 | 	//	- int1ThsZ = 16-bit interrupt threshold value for z-axis
364 | 	//	- duration = Duration an interrupt holds after triggered. This value
365 | 	// 		is copied directly into the INT1_DURATION_G register.
366 | 	// Before using this function, read about the INT1_CFG_G register and
367 | 	// the related INT1* registers in the LMS9DS0 datasheet.
368 | 	void configGyroInt(uint8_t int1Cfg, uint16_t int1ThsX = 0,
369 | 						  uint16_t int1ThsY = 0, uint16_t int1ThsZ = 0, 
370 | 						  uint8_t duration = 0);
371 | 
372 | 
373 |         void calLSM9DS0(float gbias[3], float abias[3]);
374 | 
375 | 
376 | private:	
377 | 	// xmAddress and gAddress store the I2C address or SPI chip select pin
378 | 	// for each sensor.
379 | 	uint8_t xmAddress, gAddress;
380 | 	// interfaceMode keeps track of whether we're using SPI or I2C to talk
381 | 	interface_mode interfaceMode;
382 | 	
383 | 	// gScale, aScale, and mScale store the current scale range for each 
384 | 	// sensor. Should be updated whenever that value changes.
385 | 	gyro_scale gScale;
386 | 	accel_scale aScale;
387 | 	mag_scale mScale;
388 | 	
389 | 	// gRes, aRes, and mRes store the current resolution for each sensor. 
390 | 	// Units of these values would be DPS (or g's or Gs's) per ADC tick.
391 | 	// This value is calculated as (sensor scale) / (2^15).
392 | 	float gRes, aRes, mRes;
393 | 	
394 | 	// initGyro() -- Sets up the gyroscope to begin reading.
395 | 	// This function steps through all five gyroscope control registers.
396 | 	// Upon exit, the following parameters will be set:
397 | 	//	- CTRL_REG1_G = 0x0F: Normal operation mode, all axes enabled. 
398 | 	//		95 Hz ODR, 12.5 Hz cutoff frequency.
399 | 	//	- CTRL_REG2_G = 0x00: HPF set to normal mode, cutoff frequency
400 | 	//		set to 7.2 Hz (depends on ODR).
401 | 	//	- CTRL_REG3_G = 0x88: Interrupt enabled on INT_G (set to push-pull and
402 | 	//		active high). Data-ready output enabled on DRDY_G.
403 | 	//	- CTRL_REG4_G = 0x00: Continuous update mode. Data LSB stored in lower
404 | 	//		address. Scale set to 245 DPS. SPI mode set to 4-wire.
405 | 	//	- CTRL_REG5_G = 0x00: FIFO disabled. HPF disabled.
406 | 	void initGyro();
407 | 	
408 | 	// initAccel() -- Sets up the accelerometer to begin reading.
409 | 	// This function steps through all accelerometer related control registers.
410 | 	// Upon exit these registers will be set as:
411 | 	//	- CTRL_REG0_XM = 0x00: FIFO disabled. HPF bypassed. Normal mode.
412 | 	//	- CTRL_REG1_XM = 0x57: 100 Hz data rate. Continuous update.
413 | 	//		all axes enabled.
414 | 	//	- CTRL_REG2_XM = 0x00:  2g scale. 773 Hz anti-alias filter BW.
415 | 	//	- CTRL_REG3_XM = 0x04: Accel data ready signal on INT1_XM pin.
416 | 	void initAccel();
417 | 	
418 | 	// initMag() -- Sets up the magnetometer to begin reading.
419 | 	// This function steps through all magnetometer-related control registers.
420 | 	// Upon exit these registers will be set as:
421 | 	//	- CTRL_REG4_XM = 0x04: Mag data ready signal on INT2_XM pin.
422 | 	//	- CTRL_REG5_XM = 0x14: 100 Hz update rate. Low resolution. Interrupt
423 | 	//		requests don't latch. Temperature sensor disabled.
424 | 	//	- CTRL_REG6_XM = 0x00:  2 Gs scale.
425 | 	//	- CTRL_REG7_XM = 0x00: Continuous conversion mode. Normal HPF mode.
426 | 	//	- INT_CTRL_REG_M = 0x09: Interrupt active-high. Enable interrupts.
427 | 	void initMag();
428 | 	
429 | 	// gReadByte() -- Reads a byte from a specified gyroscope register.
430 | 	// Input:
431 | 	// 	- subAddress = Register to be read from.
432 | 	// Output:
433 | 	// 	- An 8-bit value read from the requested address.
434 | 	uint8_t gReadByte(uint8_t subAddress);
435 | 	
436 | 	// gReadBytes() -- Reads a number of bytes -- beginning at an address
437 | 	// and incrementing from there -- from the gyroscope.
438 | 	// Input:
439 | 	// 	- subAddress = Register to be read from.
440 | 	// 	- * dest = A pointer to an array of uint8_t's. Values read will be
441 | 	//		stored in here on return.
442 | 	//	- count = The number of bytes to be read.
443 | 	// Output: No value is returned, but the `dest` array will store
444 | 	// 	the data read upon exit.
445 | 	void gReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count);
446 | 	
447 | 	// gWriteByte() -- Write a byte to a register in the gyroscope.
448 | 	// Input:
449 | 	//	- subAddress = Register to be written to.
450 | 	//	- data = data to be written to the register.
451 | 	void gWriteByte(uint8_t subAddress, uint8_t data);
452 | 	
453 | 	// xmReadByte() -- Read a byte from a register in the accel/mag sensor
454 | 	// Input:
455 | 	//	- subAddress = Register to be read from.
456 | 	// Output:
457 | 	//	- An 8-bit value read from the requested register.
458 | 	uint8_t xmReadByte(uint8_t subAddress);
459 | 	
460 | 	// xmReadBytes() -- Reads a number of bytes -- beginning at an address
461 | 	// and incrementing from there -- from the accelerometer/magnetometer.
462 | 	// Input:
463 | 	// 	- subAddress = Register to be read from.
464 | 	// 	- * dest = A pointer to an array of uint8_t's. Values read will be
465 | 	//		stored in here on return.
466 | 	//	- count = The number of bytes to be read.
467 | 	// Output: No value is returned, but the `dest` array will store
468 | 	// 	the data read upon exit.
469 | 	void xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count);
470 | 	
471 | 	// xmWriteByte() -- Write a byte to a register in the accel/mag sensor.
472 | 	// Input:
473 | 	//	- subAddress = Register to be written to.
474 | 	//	- data = data to be written to the register.
475 | 	void xmWriteByte(uint8_t subAddress, uint8_t data);
476 | 	
477 | 	// calcgRes() -- Calculate the resolution of the gyroscope.
478 | 	// This function will set the value of the gRes variable. gScale must
479 | 	// be set prior to calling this function.
480 | 	void calcgRes();
481 | 	
482 | 	// calcmRes() -- Calculate the resolution of the magnetometer.
483 | 	// This function will set the value of the mRes variable. mScale must
484 | 	// be set prior to calling this function.
485 | 	void calcmRes();
486 | 	
487 | 	// calcaRes() -- Calculate the resolution of the accelerometer.
488 | 	// This function will set the value of the aRes variable. aScale must
489 | 	// be set prior to calling this function.
490 | 	void calcaRes();
491 | 	
492 | 	///////////////////
493 | 	// SPI Functions //
494 | 	///////////////////
495 | 	// initSPI() -- Initialize the SPI hardware.
496 | 	// This function will setup all SPI pins and related hardware.
497 | 	void initSPI();
498 | 	
499 | 	// SPIwriteByte() -- Write a byte out of SPI to a register in the device
500 | 	// Input:
501 | 	//	- csPin = The chip select pin of the slave device.
502 | 	//	- subAddress = The register to be written to.
503 | 	//	- data = Byte to be written to the register.
504 | 	void SPIwriteByte(uint8_t csPin, uint8_t subAddress, uint8_t data);
505 | 	
506 | 	// SPIreadByte() -- Read a single byte from a register over SPI.
507 | 	// Input:
508 | 	//	- csPin = The chip select pin of the slave device.
509 | 	//	- subAddress = The register to be read from.
510 | 	// Output:
511 | 	//	- The byte read from the requested address.
512 | 	uint8_t SPIreadByte(uint8_t csPin, uint8_t subAddress);
513 | 	
514 | 	// SPIreadBytes() -- Read a series of bytes, starting at a register via SPI
515 | 	// Input:
516 | 	//	- csPin = The chip select pin of a slave device.
517 | 	//	- subAddress = The register to begin reading.
518 | 	// 	- * dest = Pointer to an array where we'll store the readings.
519 | 	//	- count = Number of registers to be read.
520 | 	// Output: No value is returned by the function, but the registers read are
521 | 	// 		all stored in the *dest array given.
522 | 	void SPIreadBytes(uint8_t csPin, uint8_t subAddress, 
523 | 							uint8_t * dest, uint8_t count);
524 | 	
525 | 	///////////////////
526 | 	// I2C Functions //
527 | 	///////////////////
528 | 	// initI2C() -- Initialize the I2C hardware.
529 | 	// This function will setup all I2C pins and related hardware.
530 | 	void initI2C();
531 | 	
532 | 	// I2CwriteByte() -- Write a byte out of I2C to a register in the device
533 | 	// Input:
534 | 	//	- address = The 7-bit I2C address of the slave device.
535 | 	//	- subAddress = The register to be written to.
536 | 	//	- data = Byte to be written to the register.
537 | 	void I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data);
538 | 	
539 | 	// I2CreadByte() -- Read a single byte from a register over I2C.
540 | 	// Input:
541 | 	//	- address = The 7-bit I2C address of the slave device.
542 | 	//	- subAddress = The register to be read from.
543 | 	// Output:
544 | 	//	- The byte read from the requested address.
545 | 	uint8_t I2CreadByte(uint8_t address, uint8_t subAddress);
546 | 	
547 | 	// I2CreadBytes() -- Read a series of bytes, starting at a register via SPI
548 | 	// Input:
549 | 	//	- address = The 7-bit I2C address of the slave device.
550 | 	//	- subAddress = The register to begin reading.
551 | 	// 	- * dest = Pointer to an array where we'll store the readings.
552 | 	//	- count = Number of registers to be read.
553 | 	// Output: No value is returned by the function, but the registers read are
554 | 	// 		all stored in the *dest array given.
555 | 	void I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count);
556 | };
557 | 
558 | #endif // SFE_LSM9DS0_H //
559 | 


--------------------------------------------------------------------------------
/src/SFE_LSM9DS0.cpp:
--------------------------------------------------------------------------------
  1 | /******************************************************************************
  2 | SFE_LSM9DS0.cpp
  3 | SFE_LSM9DS0 Library Source File
  4 | Jim Lindblom @ SparkFun Electronics
  5 | Original Creation Date: February 14, 2014 (Happy Valentines Day!)
  6 | https://github.com/sparkfun/LSM9DS0_Breakout
  7 | 
  8 | This file implements all functions of the LSM9DS0 class. Functions here range
  9 | from higher level stuff, like reading/writing LSM9DS0 registers to low-level,
 10 | hardware reads and writes. Both SPI and I2C handler functions can be found
 11 | towards the bottom of this file.
 12 | 
 13 | Development environment specifics:
 14 | 	IDE: Arduino 1.0.5
 15 | 	Hardware Platform: Arduino Pro 3.3V/8MHz
 16 | 	LSM9DS0 Breakout Version: 1.0
 17 | 
 18 | This code is beerware; if you see me (or any other SparkFun employee) at the
 19 | local, and you've found our code helpful, please buy us a round!
 20 | 
 21 | Distributed as-is; no warranty is given.
 22 | ******************************************************************************/
 23 | 
 24 | #include "SFE_LSM9DS0.h"
 25 | #include <Wire.h> // Wire library is used for I2C
 26 | #include <SPI.h>  // SPI library is used for...SPI.
 27 | 
 28 | #if defined(ARDUINO) && ARDUINO >= 100
 29 |   #include "Arduino.h"
 30 | #else
 31 |   #include "WProgram.h"
 32 | #endif
 33 | 
 34 | LSM9DS0::LSM9DS0(interface_mode interface, uint8_t gAddr, uint8_t xmAddr)
 35 | {
 36 | 	// interfaceMode will keep track of whether we're using SPI or I2C:
 37 | 	interfaceMode = interface;
 38 | 	
 39 | 	// xmAddress and gAddress will store the 7-bit I2C address, if using I2C.
 40 | 	// If we're using SPI, these variables store the chip-select pins.
 41 | 	xmAddress = xmAddr;
 42 | 	gAddress = gAddr;
 43 | }
 44 | 
 45 | uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl, 
 46 | 						gyro_odr gODR, accel_odr aODR, mag_odr mODR)
 47 | {
 48 | 	// Store the given scales in class variables. These scale variables
 49 | 	// are used throughout to calculate the actual g's, DPS,and Gs's.
 50 | 	gScale = gScl;
 51 | 	aScale = aScl;
 52 | 	mScale = mScl;
 53 | 	
 54 | 	// Once we have the scale values, we can calculate the resolution
 55 | 	// of each sensor. That's what these functions are for. One for each sensor
 56 | 	calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
 57 | 	calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
 58 | 	calcaRes(); // Calculate g / ADC tick, stored in aRes variable
 59 | 	
 60 | 	// Now, initialize our hardware interface.
 61 | 	if (interfaceMode == MODE_I2C)	// If we're using I2C
 62 | 		initI2C();					// Initialize I2C
 63 | 	else if (interfaceMode == MODE_SPI) 	// else, if we're using SPI
 64 | 		initSPI();							// Initialize SPI
 65 | 	
 66 | 	// To verify communication, we can read from the WHO_AM_I register of
 67 | 	// each device. Store those in a variable so we can return them.
 68 | 	uint8_t gTest = gReadByte(WHO_AM_I_G);		// Read the gyro WHO_AM_I
 69 | 	uint8_t xmTest = xmReadByte(WHO_AM_I_XM);	// Read the accel/mag WHO_AM_I
 70 | 	
 71 | 	// Gyro initialization stuff:
 72 | 	initGyro();	// This will "turn on" the gyro. Setting up interrupts, etc.
 73 | 	setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
 74 | 	setGyroScale(gScale); // Set the gyro range
 75 | 	
 76 | 	// Accelerometer initialization stuff:
 77 | 	initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
 78 | 	setAccelODR(aODR); // Set the accel data rate.
 79 | 	setAccelScale(aScale); // Set the accel range.
 80 | 	
 81 | 	// Magnetometer initialization stuff:
 82 | 	initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
 83 | 	setMagODR(mODR); // Set the magnetometer output data rate.
 84 | 	setMagScale(mScale); // Set the magnetometer's range.
 85 | 	
 86 | 	// Once everything is initialized, return the WHO_AM_I registers we read:
 87 | 	return (xmTest << 8) | gTest;
 88 | }
 89 | 
 90 | void LSM9DS0::initGyro()
 91 | {
 92 | 	/* CTRL_REG1_G sets output data rate, bandwidth, power-down and enables
 93 | 	Bits[7:0]: DR1 DR0 BW1 BW0 PD Zen Xen Yen
 94 | 	DR[1:0] - Output data rate selection
 95 | 		00=95Hz, 01=190Hz, 10=380Hz, 11=760Hz
 96 | 	BW[1:0] - Bandwidth selection (sets cutoff frequency)
 97 | 		 Value depends on ODR. See datasheet table 21.
 98 | 	PD - Power down enable (0=power down mode, 1=normal or sleep mode)
 99 | 	Zen, Xen, Yen - Axis enable (o=disabled, 1=enabled)	*/
100 | 	gWriteByte(CTRL_REG1_G, 0x0F); // Normal mode, enable all axes
101 | 	
102 | 	/* CTRL_REG2_G sets up the HPF
103 | 	Bits[7:0]: 0 0 HPM1 HPM0 HPCF3 HPCF2 HPCF1 HPCF0
104 | 	HPM[1:0] - High pass filter mode selection
105 | 		00=normal (reset reading HP_RESET_FILTER, 01=ref signal for filtering,
106 | 		10=normal, 11=autoreset on interrupt
107 | 	HPCF[3:0] - High pass filter cutoff frequency
108 | 		Value depends on data rate. See datasheet table 26.
109 | 	*/
110 | 	gWriteByte(CTRL_REG2_G, 0x00); // Normal mode, high cutoff frequency
111 | 	
112 | 	/* CTRL_REG3_G sets up interrupt and DRDY_G pins
113 | 	Bits[7:0]: I1_IINT1 I1_BOOT H_LACTIVE PP_OD I2_DRDY I2_WTM I2_ORUN I2_EMPTY
114 | 	I1_INT1 - Interrupt enable on INT_G pin (0=disable, 1=enable)
115 | 	I1_BOOT - Boot status available on INT_G (0=disable, 1=enable)
116 | 	H_LACTIVE - Interrupt active configuration on INT_G (0:high, 1:low)
117 | 	PP_OD - Push-pull/open-drain (0=push-pull, 1=open-drain)
118 | 	I2_DRDY - Data ready on DRDY_G (0=disable, 1=enable)
119 | 	I2_WTM - FIFO watermark interrupt on DRDY_G (0=disable 1=enable)
120 | 	I2_ORUN - FIFO overrun interrupt on DRDY_G (0=disable 1=enable)
121 | 	I2_EMPTY - FIFO empty interrupt on DRDY_G (0=disable 1=enable) */
122 | 	// Int1 enabled (pp, active low), data read on DRDY_G:
123 | 	gWriteByte(CTRL_REG3_G, 0x88); 
124 | 	
125 | 	/* CTRL_REG4_G sets the scale, update mode
126 | 	Bits[7:0] - BDU BLE FS1 FS0 - ST1 ST0 SIM
127 | 	BDU - Block data update (0=continuous, 1=output not updated until read
128 | 	BLE - Big/little endian (0=data LSB @ lower address, 1=LSB @ higher add)
129 | 	FS[1:0] - Full-scale selection
130 | 		00=245dps, 01=500dps, 10=2000dps, 11=2000dps
131 | 	ST[1:0] - Self-test enable
132 | 		00=disabled, 01=st 0 (x+, y-, z-), 10=undefined, 11=st 1 (x-, y+, z+)
133 | 	SIM - SPI serial interface mode select
134 | 		0=4 wire, 1=3 wire */
135 | 	gWriteByte(CTRL_REG4_G, 0x00); // Set scale to 245 dps
136 | 	
137 | 	/* CTRL_REG5_G sets up the FIFO, HPF, and INT1
138 | 	Bits[7:0] - BOOT FIFO_EN - HPen INT1_Sel1 INT1_Sel0 Out_Sel1 Out_Sel0
139 | 	BOOT - Reboot memory content (0=normal, 1=reboot)
140 | 	FIFO_EN - FIFO enable (0=disable, 1=enable)
141 | 	HPen - HPF enable (0=disable, 1=enable)
142 | 	INT1_Sel[1:0] - Int 1 selection configuration
143 | 	Out_Sel[1:0] - Out selection configuration */
144 | 	gWriteByte(CTRL_REG5_G, 0x00);
145 | 	
146 | 	// Temporary !!! For testing !!! Remove !!! Or make useful !!!
147 | 	configGyroInt(0x2A, 0, 0, 0, 0); // Trigger interrupt when above 0 DPS...
148 | }
149 | 
150 | void LSM9DS0::initAccel()
151 | {
152 | 	/* CTRL_REG0_XM (0x1F) (Default value: 0x00)
153 | 	Bits (7-0): BOOT FIFO_EN WTM_EN 0 0 HP_CLICK HPIS1 HPIS2
154 | 	BOOT - Reboot memory content (0: normal, 1: reboot)
155 | 	FIFO_EN - Fifo enable (0: disable, 1: enable)
156 | 	WTM_EN - FIFO watermark enable (0: disable, 1: enable)
157 | 	HP_CLICK - HPF enabled for click (0: filter bypassed, 1: enabled)
158 | 	HPIS1 - HPF enabled for interrupt generator 1 (0: bypassed, 1: enabled)
159 | 	HPIS2 - HPF enabled for interrupt generator 2 (0: bypassed, 1 enabled)   */
160 | 	xmWriteByte(CTRL_REG0_XM, 0x00);
161 | 	
162 | 	/* CTRL_REG1_XM (0x20) (Default value: 0x07)
163 | 	Bits (7-0): AODR3 AODR2 AODR1 AODR0 BDU AZEN AYEN AXEN
164 | 	AODR[3:0] - select the acceleration data rate:
165 | 		0000=power down, 0001=3.125Hz, 0010=6.25Hz, 0011=12.5Hz, 
166 | 		0100=25Hz, 0101=50Hz, 0110=100Hz, 0111=200Hz, 1000=400Hz,
167 | 		1001=800Hz, 1010=1600Hz, (remaining combinations undefined).
168 | 	BDU - block data update for accel AND mag
169 | 		0: Continuous update
170 | 		1: Output registers aren't updated until MSB and LSB have been read.
171 | 	AZEN, AYEN, and AXEN - Acceleration x/y/z-axis enabled.
172 | 		0: Axis disabled, 1: Axis enabled									 */	
173 | 	xmWriteByte(CTRL_REG1_XM, 0x57); // 100Hz data rate, x/y/z all enabled
174 | 	
175 | 	//Serial.println(xmReadByte(CTRL_REG1_XM));
176 | 	/* CTRL_REG2_XM (0x21) (Default value: 0x00)
177 | 	Bits (7-0): ABW1 ABW0 AFS2 AFS1 AFS0 AST1 AST0 SIM
178 | 	ABW[1:0] - Accelerometer anti-alias filter bandwidth
179 | 		00=773Hz, 01=194Hz, 10=362Hz, 11=50Hz
180 | 	AFS[2:0] - Accel full-scale selection
181 | 		000=+/-2g, 001=+/-4g, 010=+/-6g, 011=+/-8g, 100=+/-16g
182 | 	AST[1:0] - Accel self-test enable
183 | 		00=normal (no self-test), 01=positive st, 10=negative st, 11=not allowed
184 | 	SIM - SPI mode selection
185 | 		0=4-wire, 1=3-wire													 */
186 | 	xmWriteByte(CTRL_REG2_XM, 0x00); // Set scale to 2g
187 | 	
188 | 	/* CTRL_REG3_XM is used to set interrupt generators on INT1_XM
189 | 	Bits (7-0): P1_BOOT P1_TAP P1_INT1 P1_INT2 P1_INTM P1_DRDYA P1_DRDYM P1_EMPTY
190 | 	*/
191 | 	// Accelerometer data ready on INT1_XM (0x04)
192 | 	xmWriteByte(CTRL_REG3_XM, 0x04); 
193 | }
194 | 
195 | void LSM9DS0::initMag()
196 | {	
197 | 	/* CTRL_REG5_XM enables temp sensor, sets mag resolution and data rate
198 | 	Bits (7-0): TEMP_EN M_RES1 M_RES0 M_ODR2 M_ODR1 M_ODR0 LIR2 LIR1
199 | 	TEMP_EN - Enable temperature sensor (0=disabled, 1=enabled)
200 | 	M_RES[1:0] - Magnetometer resolution select (0=low, 3=high)
201 | 	M_ODR[2:0] - Magnetometer data rate select
202 | 		000=3.125Hz, 001=6.25Hz, 010=12.5Hz, 011=25Hz, 100=50Hz, 101=100Hz
203 | 	LIR2 - Latch interrupt request on INT2_SRC (cleared by reading INT2_SRC)
204 | 		0=interrupt request not latched, 1=interrupt request latched
205 | 	LIR1 - Latch interrupt request on INT1_SRC (cleared by readging INT1_SRC)
206 | 		0=irq not latched, 1=irq latched 									 */
207 | 	xmWriteByte(CTRL_REG5_XM, 0x94); // Mag data rate - 100 Hz, enable temperature sensor
208 | 	
209 | 	/* CTRL_REG6_XM sets the magnetometer full-scale
210 | 	Bits (7-0): 0 MFS1 MFS0 0 0 0 0 0
211 | 	MFS[1:0] - Magnetic full-scale selection
212 | 	00:+/-2Gauss, 01:+/-4Gs, 10:+/-8Gs, 11:+/-12Gs							 */
213 | 	xmWriteByte(CTRL_REG6_XM, 0x00); // Mag scale to +/- 2GS
214 | 	
215 | 	/* CTRL_REG7_XM sets magnetic sensor mode, low power mode, and filters
216 | 	AHPM1 AHPM0 AFDS 0 0 MLP MD1 MD0
217 | 	AHPM[1:0] - HPF mode selection
218 | 		00=normal (resets reference registers), 01=reference signal for filtering, 
219 | 		10=normal, 11=autoreset on interrupt event
220 | 	AFDS - Filtered acceleration data selection
221 | 		0=internal filter bypassed, 1=data from internal filter sent to FIFO
222 | 	MLP - Magnetic data low-power mode
223 | 		0=data rate is set by M_ODR bits in CTRL_REG5
224 | 		1=data rate is set to 3.125Hz
225 | 	MD[1:0] - Magnetic sensor mode selection (default 10)
226 | 		00=continuous-conversion, 01=single-conversion, 10 and 11=power-down */
227 | 	xmWriteByte(CTRL_REG7_XM, 0x00); // Continuous conversion mode
228 | 	
229 | 	/* CTRL_REG4_XM is used to set interrupt generators on INT2_XM
230 | 	Bits (7-0): P2_TAP P2_INT1 P2_INT2 P2_INTM P2_DRDYA P2_DRDYM P2_Overrun P2_WTM
231 | 	*/
232 | 	xmWriteByte(CTRL_REG4_XM, 0x04); // Magnetometer data ready on INT2_XM (0x08)
233 | 	
234 | 	/* INT_CTRL_REG_M to set push-pull/open drain, and active-low/high
235 | 	Bits[7:0] - XMIEN YMIEN ZMIEN PP_OD IEA IEL 4D MIEN
236 | 	XMIEN, YMIEN, ZMIEN - Enable interrupt recognition on axis for mag data
237 | 	PP_OD - Push-pull/open-drain interrupt configuration (0=push-pull, 1=od)
238 | 	IEA - Interrupt polarity for accel and magneto
239 | 		0=active-low, 1=active-high
240 | 	IEL - Latch interrupt request for accel and magneto
241 | 		0=irq not latched, 1=irq latched
242 | 	4D - 4D enable. 4D detection is enabled when 6D bit in INT_GEN1_REG is set
243 | 	MIEN - Enable interrupt generation for magnetic data
244 | 		0=disable, 1=enable) */
245 | 	xmWriteByte(INT_CTRL_REG_M, 0x09); // Enable interrupts for mag, active-low, push-pull
246 | }
247 | 
248 | // This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average
249 | // them, scales them to  gs and deg/s, respectively, and then passes the biases to the main sketch
250 | // for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store
251 | // the data as there are in the ADXL345, a precursor to the LSM9DS0, or the MPU-9150, so we have to
252 | // subtract the biases ourselves. This results in a more accurate measurement in general and can
253 | // remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner
254 | // is good practice.
255 | void LSM9DS0::calLSM9DS0(float * gbias, float * abias)
256 | {  
257 |   uint8_t data[6] = {0, 0, 0, 0, 0, 0};
258 |   int16_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
259 |   int samples, ii;
260 |   
261 |   // First get gyro bias
262 |   byte c = gReadByte(CTRL_REG5_G);
263 |   gWriteByte(CTRL_REG5_G, c | 0x40);         // Enable gyro FIFO  
264 |   delay(20);                                 // Wait for change to take effect
265 |   gWriteByte(FIFO_CTRL_REG_G, 0x20 | 0x1F);  // Enable gyro FIFO stream mode and set watermark at 32 samples
266 |   delay(1000);  // delay 1000 milliseconds to collect FIFO samples
267 |   
268 |   samples = (gReadByte(FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples
269 | 
270 |   for(ii = 0; ii < samples ; ii++) {            // Read the gyro data stored in the FIFO
271 |     gReadBytes(OUT_X_L_G,  &data[0], 6);
272 |     gyro_bias[0] += (((int16_t)data[1] << 8) | data[0]);
273 |     gyro_bias[1] += (((int16_t)data[3] << 8) | data[2]);
274 |     gyro_bias[2] += (((int16_t)data[5] << 8) | data[4]);
275 |   }  
276 | 
277 |   gyro_bias[0] /= samples; // average the data
278 |   gyro_bias[1] /= samples; 
279 |   gyro_bias[2] /= samples; 
280 |   
281 |   gbias[0] = (float)gyro_bias[0]*gRes;  // Properly scale the data to get deg/s
282 |   gbias[1] = (float)gyro_bias[1]*gRes;
283 |   gbias[2] = (float)gyro_bias[2]*gRes;
284 |   
285 |   c = gReadByte(CTRL_REG5_G);
286 |   gWriteByte(CTRL_REG5_G, c & ~0x40);  // Disable gyro FIFO  
287 |   delay(20);
288 |   gWriteByte(FIFO_CTRL_REG_G, 0x00);   // Enable gyro bypass mode
289 |   
290 | 
291 |   //  Now get the accelerometer biases
292 |   c = xmReadByte(CTRL_REG0_XM);
293 |   xmWriteByte(CTRL_REG0_XM, c | 0x40);      // Enable accelerometer FIFO  
294 |   delay(20);                                // Wait for change to take effect
295 |   xmWriteByte(FIFO_CTRL_REG, 0x20 | 0x1F);  // Enable accelerometer FIFO stream mode and set watermark at 32 samples
296 |   delay(1000);  // delay 1000 milliseconds to collect FIFO samples
297 | 
298 |   samples = (xmReadByte(FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples
299 | 
300 |    for(ii = 0; ii < samples ; ii++) {          // Read the accelerometer data stored in the FIFO
301 |     xmReadBytes(OUT_X_L_A, &data[0], 6);
302 |     accel_bias[0] += (((int16_t)data[1] << 8) | data[0]);
303 |     accel_bias[1] += (((int16_t)data[3] << 8) | data[2]);
304 |     accel_bias[2] += (((int16_t)data[5] << 8) | data[4]) - (int16_t)(1./aRes); // Assumes sensor facing up!
305 |   }  
306 | 
307 |   accel_bias[0] /= samples; // average the data
308 |   accel_bias[1] /= samples; 
309 |   accel_bias[2] /= samples; 
310 |   
311 |   abias[0] = (float)accel_bias[0]*aRes; // Properly scale data to get gs
312 |   abias[1] = (float)accel_bias[1]*aRes;
313 |   abias[2] = (float)accel_bias[2]*aRes;
314 | 
315 |   c = xmReadByte(CTRL_REG0_XM);
316 |   xmWriteByte(CTRL_REG0_XM, c & ~0x40);    // Disable accelerometer FIFO  
317 |   delay(20);
318 |   xmWriteByte(FIFO_CTRL_REG, 0x00);       // Enable accelerometer bypass mode
319 | }
320 | 
321 | void LSM9DS0::readAccel()
322 | {
323 | 	uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp	
324 | 	xmReadBytes(OUT_X_L_A, temp, 6); // Read 6 bytes, beginning at OUT_X_L_A
325 | 	ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax
326 | 	ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay
327 | 	az = (temp[5] << 8) | temp[4]; // Store z-axis values into az
328 | }
329 | 
330 | void LSM9DS0::readMag()
331 | {
332 | 	uint8_t temp[6]; // We'll read six bytes from the mag into temp	
333 | 	xmReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M
334 | 	mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx
335 | 	my = (temp[3] << 8) | temp[2]; // Store y-axis values into my
336 | 	mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz
337 | }
338 | 
339 | void LSM9DS0::readTemp()
340 | {
341 | 	uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp	
342 | 	xmReadBytes(OUT_TEMP_L_XM, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L_M
343 | 	temperature = (((int16_t) temp[1] << 12) | temp[0] << 4 ) >> 4; // Temperature is a 12-bit signed integer
344 | }
345 | 
346 | void LSM9DS0::readGyro()
347 | {
348 | 	uint8_t temp[6]; // We'll read six bytes from the gyro into temp
349 | 	gReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
350 | 	gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx
351 | 	gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
352 | 	gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
353 | }
354 | 
355 | float LSM9DS0::calcGyro(int16_t gyro)
356 | {
357 | 	// Return the gyro raw reading times our pre-calculated DPS / (ADC tick):
358 | 	return gRes * gyro; 
359 | }
360 | 
361 | float LSM9DS0::calcAccel(int16_t accel)
362 | {
363 | 	// Return the accel raw reading times our pre-calculated g's / (ADC tick):
364 | 	return aRes * accel;
365 | }
366 | 
367 | float LSM9DS0::calcMag(int16_t mag)
368 | {
369 | 	// Return the mag raw reading times our pre-calculated Gs / (ADC tick):
370 | 	return mRes * mag;
371 | }
372 | 
373 | void LSM9DS0::setGyroScale(gyro_scale gScl)
374 | {
375 | 	// We need to preserve the other bytes in CTRL_REG4_G. So, first read it:
376 | 	uint8_t temp = gReadByte(CTRL_REG4_G);
377 | 	// Then mask out the gyro scale bits:
378 | 	temp &= 0xFF^(0x3 << 4);
379 | 	// Then shift in our new scale bits:
380 | 	temp |= gScl << 4;
381 | 	// And write the new register value back into CTRL_REG4_G:
382 | 	gWriteByte(CTRL_REG4_G, temp);
383 | 	
384 | 	// We've updated the sensor, but we also need to update our class variables
385 | 	// First update gScale:
386 | 	gScale = gScl;
387 | 	// Then calculate a new gRes, which relies on gScale being set correctly:
388 | 	calcgRes();
389 | }
390 | 
391 | void LSM9DS0::setAccelScale(accel_scale aScl)
392 | {
393 | 	// We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
394 | 	uint8_t temp = xmReadByte(CTRL_REG2_XM);
395 | 	// Then mask out the accel scale bits:
396 | 	temp &= 0xFF^(0x3 << 3);
397 | 	// Then shift in our new scale bits:
398 | 	temp |= aScl << 3;
399 | 	// And write the new register value back into CTRL_REG2_XM:
400 | 	xmWriteByte(CTRL_REG2_XM, temp);
401 | 	
402 | 	// We've updated the sensor, but we also need to update our class variables
403 | 	// First update aScale:
404 | 	aScale = aScl;
405 | 	// Then calculate a new aRes, which relies on aScale being set correctly:
406 | 	calcaRes();
407 | }
408 | 
409 | void LSM9DS0::setMagScale(mag_scale mScl)
410 | {
411 | 	// We need to preserve the other bytes in CTRL_REG6_XM. So, first read it:
412 | 	uint8_t temp = xmReadByte(CTRL_REG6_XM);
413 | 	// Then mask out the mag scale bits:
414 | 	temp &= 0xFF^(0x3 << 5);
415 | 	// Then shift in our new scale bits:
416 | 	temp |= mScl << 5;
417 | 	// And write the new register value back into CTRL_REG6_XM:
418 | 	xmWriteByte(CTRL_REG6_XM, temp);
419 | 	
420 | 	// We've updated the sensor, but we also need to update our class variables
421 | 	// First update mScale:
422 | 	mScale = mScl;
423 | 	// Then calculate a new mRes, which relies on mScale being set correctly:
424 | 	calcmRes();
425 | }
426 | 
427 | void LSM9DS0::setGyroODR(gyro_odr gRate)
428 | {
429 | 	// We need to preserve the other bytes in CTRL_REG1_G. So, first read it:
430 | 	uint8_t temp = gReadByte(CTRL_REG1_G);
431 | 	// Then mask out the gyro ODR bits:
432 | 	temp &= 0xFF^(0xF << 4);
433 | 	// Then shift in our new ODR bits:
434 | 	temp |= (gRate << 4);
435 | 	// And write the new register value back into CTRL_REG1_G:
436 | 	gWriteByte(CTRL_REG1_G, temp);
437 | }
438 | void LSM9DS0::setAccelODR(accel_odr aRate)
439 | {
440 | 	// We need to preserve the other bytes in CTRL_REG1_XM. So, first read it:
441 | 	uint8_t temp = xmReadByte(CTRL_REG1_XM);
442 | 	// Then mask out the accel ODR bits:
443 | 	temp &= 0xFF^(0xF << 4);
444 | 	// Then shift in our new ODR bits:
445 | 	temp |= (aRate << 4);
446 | 	// And write the new register value back into CTRL_REG1_XM:
447 | 	xmWriteByte(CTRL_REG1_XM, temp);
448 | }
449 | void LSM9DS0::setAccelABW(accel_abw abwRate)
450 | {
451 | 	// We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
452 | 	uint8_t temp = xmReadByte(CTRL_REG2_XM);
453 | 	// Then mask out the accel ABW bits:
454 | 	temp &= 0xFF^(0x3 << 6);
455 | 	// Then shift in our new ODR bits:
456 | 	temp |= (abwRate << 6);
457 | 	// And write the new register value back into CTRL_REG2_XM:
458 | 	xmWriteByte(CTRL_REG2_XM, temp);
459 | }
460 | void LSM9DS0::setMagODR(mag_odr mRate)
461 | {
462 | 	// We need to preserve the other bytes in CTRL_REG5_XM. So, first read it:
463 | 	uint8_t temp = xmReadByte(CTRL_REG5_XM);
464 | 	// Then mask out the mag ODR bits:
465 | 	temp &= 0xFF^(0x7 << 2);
466 | 	// Then shift in our new ODR bits:
467 | 	temp |= (mRate << 2);
468 | 	// And write the new register value back into CTRL_REG5_XM:
469 | 	xmWriteByte(CTRL_REG5_XM, temp);
470 | }
471 | 
472 | void LSM9DS0::configGyroInt(uint8_t int1Cfg, uint16_t int1ThsX, uint16_t int1ThsY, uint16_t int1ThsZ, uint8_t duration)
473 | {
474 | 	gWriteByte(INT1_CFG_G, int1Cfg);
475 | 	gWriteByte(INT1_THS_XH_G, (int1ThsX & 0xFF00) >> 8);
476 | 	gWriteByte(INT1_THS_XL_G, (int1ThsX & 0xFF));
477 | 	gWriteByte(INT1_THS_YH_G, (int1ThsY & 0xFF00) >> 8);
478 | 	gWriteByte(INT1_THS_YL_G, (int1ThsY & 0xFF));
479 | 	gWriteByte(INT1_THS_ZH_G, (int1ThsZ & 0xFF00) >> 8);
480 | 	gWriteByte(INT1_THS_ZL_G, (int1ThsZ & 0xFF));
481 | 	if (duration)
482 | 		gWriteByte(INT1_DURATION_G, 0x80 | duration);
483 | 	else
484 | 		gWriteByte(INT1_DURATION_G, 0x00);
485 | }
486 | 
487 | void LSM9DS0::calcgRes()
488 | {
489 | 	// Possible gyro scales (and their register bit settings) are:
490 | 	// 245 DPS (00), 500 DPS (01), 2000 DPS (10). Here's a bit of an algorithm
491 | 	// to calculate DPS/(ADC tick) based on that 2-bit value:
492 | 	switch (gScale)
493 | 	{
494 | 	case G_SCALE_245DPS:
495 | 		gRes = 245.0 / 32768.0;
496 | 		break;
497 | 	case G_SCALE_500DPS:
498 | 		gRes = 500.0 / 32768.0;
499 | 		break;
500 | 	case G_SCALE_2000DPS:
501 | 		gRes = 2000.0 / 32768.0;
502 | 		break;
503 | 	}
504 | }
505 | 
506 | void LSM9DS0::calcaRes()
507 | {
508 | 	// Possible accelerometer scales (and their register bit settings) are:
509 | 	// 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an 
510 | 	// algorithm to calculate g/(ADC tick) based on that 3-bit value:
511 | 	aRes = aScale == A_SCALE_16G ? 16.0 / 32768.0 : 
512 | 		   (((float) aScale + 1.0) * 2.0) / 32768.0;
513 | }
514 | 
515 | void LSM9DS0::calcmRes()
516 | {
517 | 	// Possible magnetometer scales (and their register bit settings) are:
518 | 	// 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11). Here's a bit of an algorithm
519 | 	// to calculate Gs/(ADC tick) based on that 2-bit value:
520 | 	mRes = mScale == M_SCALE_2GS ? 2.0 / 32768.0 : 
521 | 	       (float) (mScale << 2) / 32768.0;
522 | }
523 | 	
524 | void LSM9DS0::gWriteByte(uint8_t subAddress, uint8_t data)
525 | {
526 | 	// Whether we're using I2C or SPI, write a byte using the
527 | 	// gyro-specific I2C address or SPI CS pin.
528 | 	if (interfaceMode == MODE_I2C)
529 | 		I2CwriteByte(gAddress, subAddress, data);
530 | 	else if (interfaceMode == MODE_SPI)
531 | 		SPIwriteByte(gAddress, subAddress, data);
532 | }
533 | 
534 | void LSM9DS0::xmWriteByte(uint8_t subAddress, uint8_t data)
535 | {
536 | 	// Whether we're using I2C or SPI, write a byte using the
537 | 	// accelerometer-specific I2C address or SPI CS pin.
538 | 	if (interfaceMode == MODE_I2C)
539 | 		return I2CwriteByte(xmAddress, subAddress, data);
540 | 	else if (interfaceMode == MODE_SPI)
541 | 		return SPIwriteByte(xmAddress, subAddress, data);
542 | }
543 | 
544 | uint8_t LSM9DS0::gReadByte(uint8_t subAddress)
545 | {
546 | 	// Whether we're using I2C or SPI, read a byte using the
547 | 	// gyro-specific I2C address or SPI CS pin.
548 | 	if (interfaceMode == MODE_I2C)
549 | 		return I2CreadByte(gAddress, subAddress);
550 | 	else if (interfaceMode == MODE_SPI)
551 | 		return SPIreadByte(gAddress, subAddress);
552 | }
553 | 
554 | void LSM9DS0::gReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
555 | {
556 | 	// Whether we're using I2C or SPI, read multiple bytes using the
557 | 	// gyro-specific I2C address or SPI CS pin.
558 | 	if (interfaceMode == MODE_I2C)
559 | 		I2CreadBytes(gAddress, subAddress, dest, count);
560 | 	else if (interfaceMode == MODE_SPI)
561 | 		SPIreadBytes(gAddress, subAddress, dest, count);
562 | }
563 | 
564 | uint8_t LSM9DS0::xmReadByte(uint8_t subAddress)
565 | {
566 | 	// Whether we're using I2C or SPI, read a byte using the
567 | 	// accelerometer-specific I2C address or SPI CS pin.
568 | 	if (interfaceMode == MODE_I2C)
569 | 		return I2CreadByte(xmAddress, subAddress);
570 | 	else if (interfaceMode == MODE_SPI)
571 | 		return SPIreadByte(xmAddress, subAddress);
572 | }
573 | 
574 | void LSM9DS0::xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
575 | {
576 | 	// Whether we're using I2C or SPI, read multiple bytes using the
577 | 	// accelerometer-specific I2C address or SPI CS pin.
578 | 	if (interfaceMode == MODE_I2C)
579 | 		I2CreadBytes(xmAddress, subAddress, dest, count);
580 | 	else if (interfaceMode == MODE_SPI)
581 | 		SPIreadBytes(xmAddress, subAddress, dest, count);
582 | }
583 | 
584 | void LSM9DS0::initSPI()
585 | {
586 | 	pinMode(gAddress, OUTPUT);
587 | 	digitalWrite(gAddress, HIGH);
588 | 	pinMode(xmAddress, OUTPUT);
589 | 	digitalWrite(xmAddress, HIGH);
590 | 	
591 | 	SPI.begin();
592 | 	// Maximum SPI frequency is 10MHz, could divide by 2 here:
593 | 	SPI.setClockDivider(SPI_CLOCK_DIV4);
594 | 	// Data is read and written MSb first.
595 | 	SPI.setBitOrder(MSBFIRST);
596 | 	// Data is captured on rising edge of clock (CPHA = 0)
597 | 	// Base value of the clock is HIGH (CPOL = 1)
598 | 	SPI.setDataMode(SPI_MODE1);
599 | }
600 | 
601 | void LSM9DS0::SPIwriteByte(uint8_t csPin, uint8_t subAddress, uint8_t data)
602 | {
603 | 	digitalWrite(csPin, LOW); // Initiate communication
604 | 	
605 | 	// If write, bit 0 (MSB) should be 0
606 | 	// If single write, bit 1 should be 0
607 | 	SPI.transfer(subAddress & 0x3F); // Send Address
608 | 	SPI.transfer(data); // Send data
609 | 	
610 | 	digitalWrite(csPin, HIGH); // Close communication
611 | }
612 | 
613 | uint8_t LSM9DS0::SPIreadByte(uint8_t csPin, uint8_t subAddress)
614 | {
615 | 	uint8_t temp;
616 | 	// Use the multiple read function to read 1 byte. 
617 | 	// Value is returned to `temp`.
618 | 	SPIreadBytes(csPin, subAddress, &temp, 1);
619 | 	return temp;
620 | }
621 | 
622 | void LSM9DS0::SPIreadBytes(uint8_t csPin, uint8_t subAddress,
623 | 							uint8_t * dest, uint8_t count)
624 | {
625 | 	digitalWrite(csPin, LOW); // Initiate communication
626 | 	// To indicate a read, set bit 0 (msb) to 1
627 | 	// If we're reading multiple bytes, set bit 1 to 1
628 | 	// The remaining six bytes are the address to be read
629 | 	if (count > 1)
630 | 		SPI.transfer(0xC0 | (subAddress & 0x3F));
631 | 	else
632 | 		SPI.transfer(0x80 | (subAddress & 0x3F));
633 | 	for (int i=0; i<count; i++)
634 | 	{
635 | 		dest[i] = SPI.transfer(0x00); // Read into destination array
636 | 	}
637 | 	digitalWrite(csPin, HIGH); // Close communication
638 | }
639 | 
640 | void LSM9DS0::initI2C()
641 | {
642 | 	Wire.begin();	// Initialize I2C library
643 | }
644 | 
645 | // Wire.h read and write protocols
646 | void LSM9DS0::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data)
647 | {
648 | 	Wire.beginTransmission(address);  // Initialize the Tx buffer
649 | 	Wire.write(subAddress);           // Put slave register address in Tx buffer
650 | 	Wire.write(data);                 // Put data in Tx buffer
651 | 	Wire.endTransmission();           // Send the Tx buffer
652 | }
653 | 
654 | uint8_t LSM9DS0::I2CreadByte(uint8_t address, uint8_t subAddress)
655 | {
656 | 	uint8_t data; // `data` will store the register data	 
657 | 	Wire.beginTransmission(address);         // Initialize the Tx buffer
658 | 	Wire.write(subAddress);	                 // Put slave register address in Tx buffer
659 | 	Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
660 | 	Wire.requestFrom(address, (uint8_t) 1);  // Read one byte from slave register address 
661 | 	data = Wire.read();                      // Fill Rx buffer with result
662 | 	return data;                             // Return data read from slave register
663 | }
664 | 
665 | void LSM9DS0::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count)
666 | {  
667 | 	Wire.beginTransmission(address);   // Initialize the Tx buffer
668 | 	// Next send the register to be read. OR with 0x80 to indicate multi-read.
669 | 	Wire.write(subAddress | 0x80);     // Put slave register address in Tx buffer
670 | 	Wire.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
671 | 	uint8_t i = 0;
672 | 	Wire.requestFrom(address, count);  // Read bytes from slave register address 
673 | 	while (Wire.available()) 
674 | 	{
675 | 		dest[i++] = Wire.read(); // Put read results in the Rx buffer
676 | 	}
677 | }
678 | 


--------------------------------------------------------------------------------
/examples/SparkFun_LSM9DS0_AHRS/SparkFun_LSM9DS0_AHRS.ino:
--------------------------------------------------------------------------------
  1 | /*****************************************************************
  2 | LSM9DS0_AHRS.ino
  3 | SFE_LSM9DS0 Library AHRS Data Fusion Example Code
  4 | Jim Lindblom @ SparkFun Electronics
  5 | Original Creation Date: February 18, 2014
  6 | https://github.com/sparkfun/LSM9DS0_Breakout
  7 | 
  8 | Modified by Kris Winer, April 4, 2014
  9 | 
 10 | The LSM9DS0 is a versatile 9DOF sensor. It has a built-in
 11 | accelerometer, gyroscope, and magnetometer. Very cool! Plus it
 12 | functions over either SPI or I2C.
 13 | 
 14 | This Arduino sketch utilizes Jim Lindblom's SFE_LSM9DS0 library to generate the basic sensor data
 15 | for use in two sensor fusion algorithms becoming increasingly popular with DIY quadcopter and robotics engineers. 
 16 | 
 17 | Like the original LSM9SD0_simple.ino sketch, it'll demo the following:
 18 | * How to create a LSM9DS0 object, using a constructor (global
 19 |   variables section).
 20 | * How to use the begin() function of the LSM9DS0 class.
 21 | * How to read the gyroscope, accelerometer, and magnetometer
 22 |   using the readGryo(), readAccel(), readMag() functions and the
 23 |   gx, gy, gz, ax, ay, az, mx, my, and mz variables.
 24 | * How to calculate actual acceleration, rotation speed, magnetic
 25 |   field strength using the calcAccel(), calcGyro() and calcMag()
 26 |   functions.
 27 |   
 28 | In addition, the sketch will demo:
 29 | * How to check for data updates using interrupts
 30 | * How to display output at a rate different from the sensor data update and fusion filter update rates
 31 | * How to specify the accelerometer anti-aliasing (low-pass) filter rate
 32 | * How to use the data from the LSM9DS0 to fuse the sensor data into a quaternion representation of the sensor frame
 33 |   orientation relative to a fixed Earth frame providing absolute orientation information for subsequent use.
 34 | * An example of how to use the quaternion data to generate standard aircraft orientation data in the form of
 35 |   Tait-Bryan angles representing the sensor yaw, pitch, and roll angles suitable for any vehicle stablization control application.
 36 | 
 37 | Hardware setup: This library supports communicating with the
 38 | LSM9DS0 over either I2C or SPI. If you're using I2C, these are
 39 | the only connections that need to be made:
 40 | 	LSM9DS0 --------- Arduino
 41 | 	 SCL ---------- SCL (A5 on older 'Duinos')
 42 | 	 SDA ---------- SDA (A4 on older 'Duinos')
 43 | 	 VDD ------------- 3.3V
 44 | 	 GND ------------- GND
 45 |          DRDYG-------------4   (gyro data ready interrupt, can be any digital pin)
 46 |          INTX1-------------3   (accelerometer data ready interrupt, can be any digital pin)
 47 |          INTX2-------------2   (magnetometer data ready interrupt, can be any digital pin)
 48 | (CSG, CSXM, SDOG, and SDOXM should all be pulled high jumpers on 
 49 |   the breakout board will do this for you.)
 50 |   
 51 |  Note: The LSM9DS0 in the I2C mode uses the Arduino Wire library. 
 52 |  Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
 53 |  We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
 54 |  We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L /twi.h utility file.
 55 |   
 56 | If you're using SPI, here is an example hardware setup:
 57 | 	LSM9DS0 --------- Arduino
 58 |           CSG -------------- 9
 59 |           CSXM ------------- 10
 60 |           SDOG ------------- 12
 61 |           SDOXM ------------ 12 (tied to SDOG)
 62 |           SCL -------------- 13
 63 |           SDA -------------- 11
 64 |           VDD -------------- 3.3V
 65 |           GND -------------- GND
 66 | 	
 67 | The LSM9DS0 has a maximum voltage of 3.6V. Make sure you power it
 68 | off the 3.3V rail! And either use level shifters between SCL
 69 | and SDA or just use a 3.3V Arduino Pro.	  
 70 | 
 71 | In addition, this sketch uses a Nokia 5110 48 x 84 pixel display which requires 
 72 | digital pins 5 - 9 described below. If using SPI you might need to press one of the A0 - A3 pins
 73 | into service as a digital input instead.
 74 | 
 75 | Development environment specifics:
 76 | 	IDE: Arduino 1.0.5
 77 | 	Hardware Platform: Arduino Pro 3.3V/8MHz
 78 | 	LSM9DS0 Breakout Version: 1.0
 79 | 
 80 | This code is beerware. If you see me (or any other SparkFun 
 81 | employee) at the local, and you've found our code helpful, please 
 82 | buy us a round!
 83 | 
 84 | Distributed as-is; no warranty is given.
 85 | *****************************************************************/
 86 | 
 87 | // The SFE_LSM9DS0 requires both the SPI and Wire libraries.
 88 | // Unfortunately, you'll need to include both in the Arduino
 89 | // sketch, before including the SFE_LSM9DS0 library.
 90 | #include <SPI.h> // Included for SFE_LSM9DS0 library
 91 | #include <Wire.h>
 92 | #include <SFE_LSM9DS0.h>
 93 | //#include "Arduino.h"
 94 | #include <Adafruit_GFX.h>
 95 | #include <Adafruit_PCD8544.h>
 96 | 
 97 | // Using NOKIA 5110 monochrome 84 x 48 pixel display
 98 | // pin 9 - Serial clock out (SCLK)
 99 | // pin 8 - Serial data out (DIN)
100 | // pin 7 - Data/Command select (D/C)
101 | // pin 5 - LCD chip select (CS)
102 | // pin 6 - LCD reset (RST)
103 | Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
104 | 
105 | ///////////////////////
106 | // Example I2C Setup //
107 | ///////////////////////
108 | // Comment out this section if you're using SPI
109 | // SDO_XM and SDO_G are both grounded, so our addresses are:
110 | #define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
111 | #define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW
112 | // Create an instance of the LSM9DS0 library called `dof` the
113 | // parameters for this constructor are:
114 | // [SPI or I2C Mode declaration],[gyro I2C address],[xm I2C add.]
115 | LSM9DS0 dof(MODE_I2C, LSM9DS0_G, LSM9DS0_XM);
116 | 
117 | ///////////////////////
118 | // Example SPI Setup //
119 | ///////////////////////
120 | /* // Uncomment this section if you're using SPI
121 | #define LSM9DS0_CSG  9  // CSG connected to Arduino pin 9
122 | #define LSM9DS0_CSXM 10 // CSXM connected to Arduino pin 10
123 | LSM9DS0 dof(MODE_SPI, LSM9DS0_CSG, LSM9DS0_CSXM);
124 | */
125 | 
126 | ///////////////////////////////
127 | // Interrupt Pin Definitions //
128 | ///////////////////////////////
129 | const byte INT1XM = 3; // INT1XM tells us when accel data is ready
130 | const byte INT2XM = 2; // INT2XM tells us when mag data is ready
131 | const byte DRDYG  = 4; // DRDYG  tells us when gyro data is ready
132 | 
133 | // global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
134 | #define GyroMeasError PI * (40.0f / 180.0f)       // gyroscope measurement error in rads/s (shown as 3 deg/s)
135 | #define GyroMeasDrift PI * (0.0f / 180.0f)      // gyroscope measurement drift in rad/s/s (shown as 0.0 deg/s/s)
136 | // There is a tradeoff in the beta parameter between accuracy and response speed.
137 | // In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
138 | // However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
139 | // Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
140 | // By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
141 | // I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; 
142 | // the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. 
143 | // In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
144 | #define beta sqrt(3.0f / 4.0f) * GyroMeasError   // compute beta
145 | #define zeta sqrt(3.0f / 4.0f) * GyroMeasDrift   // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
146 | #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
147 | #define Ki 0.0f
148 | 
149 | uint32_t count = 0;  // used to control display output rate
150 | uint32_t delt_t = 0; // used to control display output rate
151 | float pitch, yaw, roll, heading;
152 | float deltat = 0.0f;        // integration interval for both filter schemes
153 | uint32_t lastUpdate = 0;    // used to calculate integration interval
154 | uint32_t Now = 0;           // used to calculate integration interval
155 | 
156 | float abias[3] = {0, 0, 0}, gbias[3] = {0, 0, 0};
157 | float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 
158 | float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion
159 | float eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method
160 | float temperature;
161 | 
162 | void setup()
163 | {
164 |   Serial.begin(38400); // Start serial at 38400 bps
165 |  
166 |   // Set up interrupt pins as inputs:
167 |   pinMode(INT1XM, INPUT);
168 |   pinMode(INT2XM, INPUT);
169 |   pinMode(DRDYG,  INPUT);
170 | 
171 |   display.begin(); // Initialize the display
172 |   display.setContrast(58); // Set the contrast
173 |   display.setRotation(0); //  0 or 2) width = width, 1 or 3) width = height, swapped etc.
174 |   
175 | // Start device display with ID of sensor
176 |   display.clearDisplay();
177 |   display.setTextSize(2);
178 |   display.setCursor(0,0);  display.print("LSM9DS0");
179 |   display.setTextSize(1);
180 |   display.setCursor(0, 20); display.print("9 DOF sensor");
181 |   display.setCursor(5, 30); display.print("data fusion");
182 |   display.setCursor(20, 40); display.print("AHRS");
183 |   display.display();
184 |   delay(2000);
185 | 
186 | // Set up for data display
187 |   display.setTextSize(1); // Set text size to normal, 2 is twice normal etc.
188 |   display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen
189 |   display.clearDisplay();   // clears the screen and buffer
190 |   display.display();
191 |             
192 |   // begin() returns a 16-bit value which includes both the gyro 
193 |   // and accelerometers WHO_AM_I response. You can check this to
194 |   // make sure communication was successful.
195 |   uint32_t status = dof.begin();
196 |  
197 |   Serial.print("LSM9DS0 WHO_AM_I's returned: 0x");
198 |   Serial.println(status, HEX);
199 |   Serial.println("Should be 0x49D4");
200 |   Serial.println();
201 |   display.setCursor(0,0); display.print("I AM");
202 |   display.setCursor(0,10); display.print(status, HEX);
203 |   display.setCursor(0,30); display.print("I Should Be");
204 |   display.setCursor(0,40); display.print(0x49D4, HEX); 
205 |   display.display();
206 |   delay(2000); 
207 |   
208 |  // Set data output ranges; choose lowest ranges for maximum resolution
209 |  // Accelerometer scale can be: A_SCALE_2G, A_SCALE_4G, A_SCALE_6G, A_SCALE_8G, or A_SCALE_16G   
210 |     dof.setAccelScale(dof.A_SCALE_2G);
211 |  // Gyro scale can be:  G_SCALE__245, G_SCALE__500, or G_SCALE__2000DPS
212 |     dof.setGyroScale(dof.G_SCALE_245DPS);
213 |  // Magnetometer scale can be: M_SCALE_2GS, M_SCALE_4GS, M_SCALE_8GS, M_SCALE_12GS   
214 |     dof.setMagScale(dof.M_SCALE_2GS);
215 |   
216 |  // Set output data rates  
217 |  // Accelerometer output data rate (ODR) can be: A_ODR_3125 (3.225 Hz), A_ODR_625 (6.25 Hz), A_ODR_125 (12.5 Hz), A_ODR_25, A_ODR_50, 
218 |  //                                              A_ODR_100,  A_ODR_200, A_ODR_400, A_ODR_800, A_ODR_1600 (1600 Hz)
219 |     dof.setAccelODR(dof.A_ODR_200); // Set accelerometer update rate at 100 Hz
220 |  // Accelerometer anti-aliasing filter rate can be 50, 194, 362, or 763 Hz
221 |  // Anti-aliasing acts like a low-pass filter allowing oversampling of accelerometer and rejection of high-frequency spurious noise.
222 |  // Strategy here is to effectively oversample accelerometer at 100 Hz and use a 50 Hz anti-aliasing (low-pass) filter frequency
223 |  // to get a smooth ~150 Hz filter update rate
224 |     dof.setAccelABW(dof.A_ABW_50); // Choose lowest filter setting for low noise
225 |  
226 |  // Gyro output data rates can be: 95 Hz (bandwidth 12.5 or 25 Hz), 190 Hz (bandwidth 12.5, 25, 50, or 70 Hz)
227 |  //                                 380 Hz (bandwidth 20, 25, 50, 100 Hz), or 760 Hz (bandwidth 30, 35, 50, 100 Hz)
228 |     dof.setGyroODR(dof.G_ODR_190_BW_125);  // Set gyro update rate to 190 Hz with the smallest bandwidth for low noise
229 | 
230 |  // Magnetometer output data rate can be: 3.125 (ODR_3125), 6.25 (ODR_625), 12.5 (ODR_125), 25, 50, or 100 Hz
231 |     dof.setMagODR(dof.M_ODR_125); // Set magnetometer to update every 80 ms
232 |     
233 |  // Use the FIFO mode to average accelerometer and gyro readings to calculate the biases, which can then be removed from
234 |  // all subsequent measurements.
235 |     dof.calLSM9DS0(gbias, abias);
236 | }
237 | 
238 | void loop()
239 | {
240 |   if(digitalRead(DRDYG)) {  // When new gyro data is ready
241 |   dof.readGyro();           // Read raw gyro data
242 |     gx = dof.calcGyro(dof.gx) - gbias[0];   // Convert to degrees per seconds, remove gyro biases
243 |     gy = dof.calcGyro(dof.gy) - gbias[1];
244 |     gz = dof.calcGyro(dof.gz) - gbias[2];
245 |   }
246 |   
247 |   if(digitalRead(INT1XM)) {  // When new accelerometer data is ready
248 |     dof.readAccel();         // Read raw accelerometer data
249 |     ax = dof.calcAccel(dof.ax) - abias[0];   // Convert to g's, remove accelerometer biases
250 |     ay = dof.calcAccel(dof.ay) - abias[1];
251 |     az = dof.calcAccel(dof.az) - abias[2];
252 |   }
253 |   
254 |   if(digitalRead(INT2XM)) {  // When new magnetometer data is ready
255 |     dof.readMag();           // Read raw magnetometer data
256 |     mx = dof.calcMag(dof.mx);     // Convert to Gauss and correct for calibration
257 |     my = dof.calcMag(dof.my);
258 |     mz = dof.calcMag(dof.mz);
259 |     
260 |     dof.readTemp();
261 |     temperature = 21.0 + (float) dof.temperature/8.; // slope is 8 LSB per degree C, just guessing at the intercept
262 |   }
263 | 
264 |   Now = micros();
265 |   deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
266 |   lastUpdate = Now;
267 |   // Sensors x- and y-axes are aligned but magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro!
268 |   // This is ok by aircraft orientation standards!  
269 |   // Pass gyro rate as rad/s
270 |    MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
271 |  //MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
272 | 
273 |     // Serial print and/or display at 0.5 s rate independent of data rates
274 |     delt_t = millis() - count;
275 |     if (delt_t > 500) { // update LCD once per half-second independent of read rate
276 |  
277 |   // Print the heading and orientation for fun!
278 |     printHeading(mx, my);
279 |     printOrientation(ax, ay, az);
280 | 
281 |   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
282 |   // In this coordinate system, the positive z-axis is down toward Earth. 
283 |   // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination), 
284 |   // looking down on the sensor positive yaw is counterclockwise.
285 |   // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
286 |   // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
287 |   // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
288 |   // Tait-Bryan angles as well as Euler angles are non-commutative; that is, to get the correct orientation the rotations must be
289 |   // applied in the correct order which for this configuration is yaw, pitch, and then roll.
290 |   // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
291 |     yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
292 |     pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
293 |     roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
294 |     pitch *= 180.0f / PI;
295 |     yaw   *= 180.0f / PI; 
296 |     yaw   -= 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
297 |     roll  *= 180.0f / PI;
298 | 
299 |     Serial.print("ax = "); Serial.print((int)1000*ax);  
300 |     Serial.print(" ay = "); Serial.print((int)1000*ay); 
301 |     Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");
302 |     Serial.print("gx = "); Serial.print( gx, 2); 
303 |     Serial.print(" gy = "); Serial.print( gy, 2); 
304 |     Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
305 |     Serial.print("mx = "); Serial.print( (int)1000*mx); 
306 |     Serial.print(" my = "); Serial.print( (int)1000*my); 
307 |     Serial.print(" mz = "); Serial.print( (int)1000*mz); Serial.println(" mG");
308 |     
309 |     Serial.print("temperature = "); Serial.println(temperature, 2);
310 |     
311 |     Serial.print("Yaw, Pitch, Roll: ");
312 |     Serial.print(yaw, 2);
313 |     Serial.print(", ");
314 |     Serial.print(pitch, 2);
315 |     Serial.print(", ");
316 |     Serial.println(roll, 2);
317 |     
318 |     Serial.print("q0 = "); Serial.print(q[0]);
319 |     Serial.print(" qx = "); Serial.print(q[1]); 
320 |     Serial.print(" qy = "); Serial.print(q[2]); 
321 |     Serial.print(" qz = "); Serial.println(q[3]); 
322 |     
323 |     Serial.print("filter rate = "); Serial.println(1.0f/deltat, 1);
324 |     display.clearDisplay();
325 |      
326 |  
327 |     display.setCursor(0, 0); display.print(" x   y   z  ");
328 | 
329 |     display.setCursor(0,  8); display.print((int)(1000*ax)); 
330 |     display.setCursor(24, 8); display.print((int)(1000*ay)); 
331 |     display.setCursor(48, 8); display.print((int)(1000*az)); 
332 |     display.setCursor(72, 8); display.print("mg");
333 |     
334 |     display.setCursor(0,  16); display.print((int)(gx)); 
335 |     display.setCursor(24, 16); display.print((int)(gy)); 
336 |     display.setCursor(48, 16); display.print((int)(gz)); 
337 |     display.setCursor(66, 16); display.print("o/s");    
338 | 
339 |     display.setCursor(0,  24); display.print((int)(1000*mx)); 
340 |     display.setCursor(24, 24); display.print((int)(1000*my)); 
341 |     display.setCursor(48, 24); display.print((int)(1000*mz)); 
342 |     display.setCursor(72, 24); display.print("mG");    
343 |  
344 |     display.setCursor(0,  32); display.print((int)(yaw)); 
345 |     display.setCursor(24, 32); display.print((int)(pitch)); 
346 |     display.setCursor(48, 32); display.print((int)(roll)); 
347 |     display.setCursor(66, 32); display.print("ypr");  
348 |   
349 |     // With ODR settings of 400 Hz, 380 Hz, and 25 Hz for the accelerometer, gyro, and magnetometer, respectively,
350 |     // the filter is updating at a ~125 Hz rate using the Madgwick scheme and ~165 Hz using the Mahony scheme 
351 |     // even though the display refreshes at only 2 Hz.
352 |     // The filter update rate can be increased by reducing the rate of data reading. The optimal implementation is
353 |     // one which balances the competing rates so they are about the same; that is, the filter updates the sensor orientation
354 |     // at about the same rate the data is changing. Of course, other implementations are possible. One might consider
355 |     // updating the filter at twice the average new data rate to allow for finite filter convergence times.
356 |     // The filter update rate is determined mostly by the mathematical steps in the respective algorithms, 
357 |     // the processor speed (8 MHz for the 3.3V Pro Mini), and the sensor ODRs, especially the magnetometer ODR:
358 |     // smaller ODRs for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces
359 |     // filter update rates of ~110 and ~135 Hz for the Madgwick and Mahony schemes, respectively. 
360 |     // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads.
361 |     // With low ODR settings of 100 Hz, 95 Hz, and 6.25 Hz for the accelerometer, gyro, and magnetometer, respectively,
362 |     // the filter is updating at a ~150 Hz rate using the Madgwick scheme and ~200 Hz using the Mahony scheme.
363 |     // These filter update rates should be fast enough to maintain accurate platform orientation for 
364 |     // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz
365 |     // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors.
366 |     // The 3.3 V 8 MHz Pro Mini is doing pretty well!
367 |     display.setCursor(0, 40); display.print("rt: "); display.print((1/deltat)); display.print(" Hz"); 
368 | 
369 |     display.display();
370 |     count = millis();
371 |     }
372 | }
373 | 
374 | 
375 | // Here's a fun function to calculate your heading, using Earth's
376 | // magnetic field.
377 | // It only works if the sensor is flat (z-axis normal to Earth).
378 | // Additionally, you may need to add or subtract a declination
379 | // angle to get the heading normalized to your location.
380 | // See: http://www.ngdc.noaa.gov/geomag/declination.shtml
381 | void printHeading(float hx, float hy)
382 | {
383 |   if (hy > 0)
384 |   {
385 |     heading = 90 - (atan(hx / hy) * (180 / PI));
386 |   }
387 |   else if (hy < 0)
388 |   {
389 |     heading = - (atan(hx / hy) * (180 / PI));
390 |   }
391 |   else // hy = 0
392 |   {
393 |     if (hx < 0) heading = 180;
394 |     else heading = 0;
395 |   }
396 |   
397 |   Serial.print("Heading: ");
398 |   Serial.println(heading, 2);
399 | }
400 | 
401 | // Another fun function that does calculations based on the
402 | // acclerometer data. This function will print your LSM9DS0's
403 | // orientation -- it's roll and pitch angles.
404 | void printOrientation(float x, float y, float z)
405 | {
406 |  // float pitch, roll;
407 |   
408 |   pitch = atan2(x, sqrt(y * y) + (z * z));
409 |   roll = atan2(y, sqrt(x * x) + (z * z));
410 |   pitch *= 180.0 / PI;
411 |   roll *= 180.0 / PI;
412 |   
413 |   Serial.print("Pitch, Roll: ");
414 |   Serial.print(pitch, 2);
415 |   Serial.print(", ");
416 |   Serial.println(roll, 2);
417 | }
418 | 
419 | 
420 | // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
421 | // (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
422 | // which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
423 | // device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
424 | // The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
425 | // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
426 |         void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
427 |         {
428 |             float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
429 |             float norm;
430 |             float hx, hy, _2bx, _2bz;
431 |             float s1, s2, s3, s4;
432 |             float qDot1, qDot2, qDot3, qDot4;
433 | 
434 |             // Auxiliary variables to avoid repeated arithmetic
435 |             float _2q1mx;
436 |             float _2q1my;
437 |             float _2q1mz;
438 |             float _2q2mx;
439 |             float _4bx;
440 |             float _4bz;
441 |             float _2q1 = 2.0f * q1;
442 |             float _2q2 = 2.0f * q2;
443 |             float _2q3 = 2.0f * q3;
444 |             float _2q4 = 2.0f * q4;
445 |             float _2q1q3 = 2.0f * q1 * q3;
446 |             float _2q3q4 = 2.0f * q3 * q4;
447 |             float q1q1 = q1 * q1;
448 |             float q1q2 = q1 * q2;
449 |             float q1q3 = q1 * q3;
450 |             float q1q4 = q1 * q4;
451 |             float q2q2 = q2 * q2;
452 |             float q2q3 = q2 * q3;
453 |             float q2q4 = q2 * q4;
454 |             float q3q3 = q3 * q3;
455 |             float q3q4 = q3 * q4;
456 |             float q4q4 = q4 * q4;
457 | 
458 |             // Normalise accelerometer measurement
459 |             norm = sqrt(ax * ax + ay * ay + az * az);
460 |             if (norm == 0.0f) return; // handle NaN
461 |             norm = 1.0f/norm;
462 |             ax *= norm;
463 |             ay *= norm;
464 |             az *= norm;
465 | 
466 |             // Normalise magnetometer measurement
467 |             norm = sqrt(mx * mx + my * my + mz * mz);
468 |             if (norm == 0.0f) return; // handle NaN
469 |             norm = 1.0f/norm;
470 |             mx *= norm;
471 |             my *= norm;
472 |             mz *= norm;
473 | 
474 |             // Reference direction of Earth's magnetic field
475 |             _2q1mx = 2.0f * q1 * mx;
476 |             _2q1my = 2.0f * q1 * my;
477 |             _2q1mz = 2.0f * q1 * mz;
478 |             _2q2mx = 2.0f * q2 * mx;
479 |             hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
480 |             hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
481 |             _2bx = sqrt(hx * hx + hy * hy);
482 |             _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
483 |             _4bx = 2.0f * _2bx;
484 |             _4bz = 2.0f * _2bz;
485 | 
486 |             // Gradient decent algorithm corrective step
487 |             s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
488 |             s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
489 |             s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
490 |             s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
491 |             norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
492 |             norm = 1.0f/norm;
493 |             s1 *= norm;
494 |             s2 *= norm;
495 |             s3 *= norm;
496 |             s4 *= norm;
497 | 
498 |             // Compute rate of change of quaternion
499 |             qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
500 |             qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
501 |             qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
502 |             qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
503 | 
504 |             // Integrate to yield quaternion
505 |             q1 += qDot1 * deltat;
506 |             q2 += qDot2 * deltat;
507 |             q3 += qDot3 * deltat;
508 |             q4 += qDot4 * deltat;
509 |             norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
510 |             norm = 1.0f/norm;
511 |             q[0] = q1 * norm;
512 |             q[1] = q2 * norm;
513 |             q[2] = q3 * norm;
514 |             q[3] = q4 * norm;
515 | 
516 |         }
517 |   
518 |   
519 |   
520 |  // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
521 |  // measured ones. 
522 |             void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
523 |         {
524 |             float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
525 |             float norm;
526 |             float hx, hy, bx, bz;
527 |             float vx, vy, vz, wx, wy, wz;
528 |             float ex, ey, ez;
529 |             float pa, pb, pc;
530 | 
531 |             // Auxiliary variables to avoid repeated arithmetic
532 |             float q1q1 = q1 * q1;
533 |             float q1q2 = q1 * q2;
534 |             float q1q3 = q1 * q3;
535 |             float q1q4 = q1 * q4;
536 |             float q2q2 = q2 * q2;
537 |             float q2q3 = q2 * q3;
538 |             float q2q4 = q2 * q4;
539 |             float q3q3 = q3 * q3;
540 |             float q3q4 = q3 * q4;
541 |             float q4q4 = q4 * q4;   
542 | 
543 |             // Normalise accelerometer measurement
544 |             norm = sqrt(ax * ax + ay * ay + az * az);
545 |             if (norm == 0.0f) return; // handle NaN
546 |             norm = 1.0f / norm;        // use reciprocal for division
547 |             ax *= norm;
548 |             ay *= norm;
549 |             az *= norm;
550 | 
551 |             // Normalise magnetometer measurement
552 |             norm = sqrt(mx * mx + my * my + mz * mz);
553 |             if (norm == 0.0f) return; // handle NaN
554 |             norm = 1.0f / norm;        // use reciprocal for division
555 |             mx *= norm;
556 |             my *= norm;
557 |             mz *= norm;
558 | 
559 |             // Reference direction of Earth's magnetic field
560 |             hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
561 |             hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
562 |             bx = sqrt((hx * hx) + (hy * hy));
563 |             bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
564 | 
565 |             // Estimated direction of gravity and magnetic field
566 |             vx = 2.0f * (q2q4 - q1q3);
567 |             vy = 2.0f * (q1q2 + q3q4);
568 |             vz = q1q1 - q2q2 - q3q3 + q4q4;
569 |             wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
570 |             wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
571 |             wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);  
572 | 
573 |             // Error is cross product between estimated direction and measured direction of gravity
574 |             ex = (ay * vz - az * vy) + (my * wz - mz * wy);
575 |             ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
576 |             ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
577 |             if (Ki > 0.0f)
578 |             {
579 |                 eInt[0] += ex;      // accumulate integral error
580 |                 eInt[1] += ey;
581 |                 eInt[2] += ez;
582 |             }
583 |             else
584 |             {
585 |                 eInt[0] = 0.0f;     // prevent integral wind up
586 |                 eInt[1] = 0.0f;
587 |                 eInt[2] = 0.0f;
588 |             }
589 | 
590 |             // Apply feedback terms
591 |             gx = gx + Kp * ex + Ki * eInt[0];
592 |             gy = gy + Kp * ey + Ki * eInt[1];
593 |             gz = gz + Kp * ez + Ki * eInt[2];
594 | 
595 |             // Integrate rate of change of quaternion
596 |             pa = q2;
597 |             pb = q3;
598 |             pc = q4;
599 |             q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
600 |             q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
601 |             q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
602 |             q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
603 | 
604 |             // Normalise quaternion
605 |             norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
606 |             norm = 1.0f / norm;
607 |             q[0] = q1 * norm;
608 |             q[1] = q2 * norm;
609 |             q[2] = q3 * norm;
610 |             q[3] = q4 * norm;
611 |  
612 |         }
613 | 
614 | 


--------------------------------------------------------------------------------