Developers_of_anti_slip_compensator / LSM9DS0

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of LSM9DS0 by LDSC_Robotics_TAs

LSM9DS0.cpp@1:2c34ccab5256, 2016-02-27 (annotated)

Committer:
YCTung
Date:
Sat Feb 27 09:03:06 2016 +0000
Revision:
1:2c34ccab5256
Parent:
0:0dbf7ee73651
Child:
2:48eb33afd0fa
Added some functions which can return measured data directly

Who changed what in which revision?

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