LDSC_Robotics_TAs / LSM9DS0

Nucleo-64 version

Dependents:   particle_filter_test read_sensor_data Bike_Sensor_Fusion Encoder ... more

LSM9DS0.cpp@9:60a176bd72b3, 2016-12-28 (annotated)

Committer:
benson516
Date:
Wed Dec 28 16:44:09 2016 +0000
Revision:
9:60a176bd72b3
Parent:
8:08932ac08cb2 
Add a set of functions for vector-output capability.

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