Joseph Boettcher
/
LSM9DS1_Library
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
LSM9DS1_Library
by 
jim hamblen
LSM9DS1.cpp@0:e8167f37725c, 2015-10-26 (annotated)
- Committer:
- jmar7
- Date:
- Mon Oct 26 14:55:43 2015 +0000
- Revision:
- 0:e8167f37725c
- Child:
- 1:87d535bf8c53
First Library Commit;
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| jmar7 | 0:e8167f37725c | 1 | /****************************************************************************** |
| jmar7 | 0:e8167f37725c | 2 | SFE_LSM9DS1.cpp |
| jmar7 | 0:e8167f37725c | 3 | SFE_LSM9DS1 Library Source File |
| jmar7 | 0:e8167f37725c | 4 | Jim Lindblom @ SparkFun Electronics |
| jmar7 | 0:e8167f37725c | 5 | Original Creation Date: February 27, 2015 |
| jmar7 | 0:e8167f37725c | 6 | https://github.com/sparkfun/LSM9DS1_Breakout |
| jmar7 | 0:e8167f37725c | 7 | |
| jmar7 | 0:e8167f37725c | 8 | This file implements all functions of the LSM9DS1 class. Functions here range |
| jmar7 | 0:e8167f37725c | 9 | from higher level stuff, like reading/writing LSM9DS1 registers to low-level, |
| jmar7 | 0:e8167f37725c | 10 | hardware reads and writes. Both SPI and I2C handler functions can be found |
| jmar7 | 0:e8167f37725c | 11 | towards the bottom of this file. |
| jmar7 | 0:e8167f37725c | 12 | |
| jmar7 | 0:e8167f37725c | 13 | Development environment specifics: |
| jmar7 | 0:e8167f37725c | 14 | IDE: Arduino 1.6 |
| jmar7 | 0:e8167f37725c | 15 | Hardware Platform: Arduino Uno |
| jmar7 | 0:e8167f37725c | 16 | LSM9DS1 Breakout Version: 1.0 |
| jmar7 | 0:e8167f37725c | 17 | |
| jmar7 | 0:e8167f37725c | 18 | This code is beerware; if you see me (or any other SparkFun employee) at the |
| jmar7 | 0:e8167f37725c | 19 | local, and you've found our code helpful, please buy us a round! |
| jmar7 | 0:e8167f37725c | 20 | |
| jmar7 | 0:e8167f37725c | 21 | Distributed as-is; no warranty is given. |
| jmar7 | 0:e8167f37725c | 22 | ******************************************************************************/ |
| jmar7 | 0:e8167f37725c | 23 | |
| jmar7 | 0:e8167f37725c | 24 | #include "LSM9DS1.h" |
| jmar7 | 0:e8167f37725c | 25 | #include "LSM9DS1_Registers.h" |
| jmar7 | 0:e8167f37725c | 26 | #include "LSM9DS1_Types.h" |
| jmar7 | 0:e8167f37725c | 27 | //#include <Wire.h> // Wire library is used for I2C |
| jmar7 | 0:e8167f37725c | 28 | //#include <SPI.h> // SPI library is used for...SPI. |
| jmar7 | 0:e8167f37725c | 29 | |
| jmar7 | 0:e8167f37725c | 30 | //#if defined(ARDUINO) && ARDUINO >= 100 |
| jmar7 | 0:e8167f37725c | 31 | // #include "Arduino.h" |
| jmar7 | 0:e8167f37725c | 32 | //#else |
| jmar7 | 0:e8167f37725c | 33 | // #include "WProgram.h" |
| jmar7 | 0:e8167f37725c | 34 | //#endif |
| jmar7 | 0:e8167f37725c | 35 | |
| jmar7 | 0:e8167f37725c | 36 | #define LSM9DS1_COMMUNICATION_TIMEOUT 1000 |
| jmar7 | 0:e8167f37725c | 37 | |
| jmar7 | 0:e8167f37725c | 38 | float magSensitivity[4] = {0.00014, 0.00029, 0.00043, 0.00058}; |
| jmar7 | 0:e8167f37725c | 39 | extern Serial pc; |
| jmar7 | 0:e8167f37725c | 40 | |
| jmar7 | 0:e8167f37725c | 41 | LSM9DS1::LSM9DS1(PinName sda, PinName scl, uint8_t xgAddr, uint8_t mAddr) |
| jmar7 | 0:e8167f37725c | 42 | :i2c(sda, scl) |
| jmar7 | 0:e8167f37725c | 43 | { |
| jmar7 | 0:e8167f37725c | 44 | init(IMU_MODE_I2C, xgAddr, mAddr); // dont know about 0xD6 or 0x3B |
| jmar7 | 0:e8167f37725c | 45 | } |
| jmar7 | 0:e8167f37725c | 46 | /* cw |
| jmar7 | 0:e8167f37725c | 47 | LSM9DS1::LSM9DS1() |
| jmar7 | 0:e8167f37725c | 48 | { |
| jmar7 | 0:e8167f37725c | 49 | init(IMU_MODE_I2C, LSM9DS1_AG_ADDR(1), LSM9DS1_M_ADDR(1)); |
| jmar7 | 0:e8167f37725c | 50 | } |
| jmar7 | 0:e8167f37725c | 51 | |
| jmar7 | 0:e8167f37725c | 52 | LSM9DS1::LSM9DS1(interface_mode interface, uint8_t xgAddr, uint8_t mAddr) |
| jmar7 | 0:e8167f37725c | 53 | { |
| jmar7 | 0:e8167f37725c | 54 | init(interface, xgAddr, mAddr); |
| jmar7 | 0:e8167f37725c | 55 | } |
| jmar7 | 0:e8167f37725c | 56 | */ |
| jmar7 | 0:e8167f37725c | 57 | |
| jmar7 | 0:e8167f37725c | 58 | void LSM9DS1::init(interface_mode interface, uint8_t xgAddr, uint8_t mAddr) |
| jmar7 | 0:e8167f37725c | 59 | { |
| jmar7 | 0:e8167f37725c | 60 | settings.device.commInterface = interface; |
| jmar7 | 0:e8167f37725c | 61 | settings.device.agAddress = xgAddr; |
| jmar7 | 0:e8167f37725c | 62 | settings.device.mAddress = mAddr; |
| jmar7 | 0:e8167f37725c | 63 | |
| jmar7 | 0:e8167f37725c | 64 | settings.gyro.enabled = true; |
| jmar7 | 0:e8167f37725c | 65 | settings.gyro.enableX = true; |
| jmar7 | 0:e8167f37725c | 66 | settings.gyro.enableY = true; |
| jmar7 | 0:e8167f37725c | 67 | settings.gyro.enableZ = true; |
| jmar7 | 0:e8167f37725c | 68 | // gyro scale can be 245, 500, or 2000 |
| jmar7 | 0:e8167f37725c | 69 | settings.gyro.scale = 245; |
| jmar7 | 0:e8167f37725c | 70 | // gyro sample rate: value between 1-6 |
| jmar7 | 0:e8167f37725c | 71 | // 1 = 14.9 4 = 238 |
| jmar7 | 0:e8167f37725c | 72 | // 2 = 59.5 5 = 476 |
| jmar7 | 0:e8167f37725c | 73 | // 3 = 119 6 = 952 |
| jmar7 | 0:e8167f37725c | 74 | settings.gyro.sampleRate = 6; |
| jmar7 | 0:e8167f37725c | 75 | // gyro cutoff frequency: value between 0-3 |
| jmar7 | 0:e8167f37725c | 76 | // Actual value of cutoff frequency depends |
| jmar7 | 0:e8167f37725c | 77 | // on sample rate. |
| jmar7 | 0:e8167f37725c | 78 | settings.gyro.bandwidth = 0; |
| jmar7 | 0:e8167f37725c | 79 | settings.gyro.lowPowerEnable = false; |
| jmar7 | 0:e8167f37725c | 80 | settings.gyro.HPFEnable = false; |
| jmar7 | 0:e8167f37725c | 81 | // Gyro HPF cutoff frequency: value between 0-9 |
| jmar7 | 0:e8167f37725c | 82 | // Actual value depends on sample rate. Only applies |
| jmar7 | 0:e8167f37725c | 83 | // if gyroHPFEnable is true. |
| jmar7 | 0:e8167f37725c | 84 | settings.gyro.HPFCutoff = 0; |
| jmar7 | 0:e8167f37725c | 85 | settings.gyro.flipX = false; |
| jmar7 | 0:e8167f37725c | 86 | settings.gyro.flipY = false; |
| jmar7 | 0:e8167f37725c | 87 | settings.gyro.flipZ = false; |
| jmar7 | 0:e8167f37725c | 88 | settings.gyro.orientation = 0; |
| jmar7 | 0:e8167f37725c | 89 | settings.gyro.latchInterrupt = true; |
| jmar7 | 0:e8167f37725c | 90 | |
| jmar7 | 0:e8167f37725c | 91 | settings.accel.enabled = true; |
| jmar7 | 0:e8167f37725c | 92 | settings.accel.enableX = true; |
| jmar7 | 0:e8167f37725c | 93 | settings.accel.enableY = true; |
| jmar7 | 0:e8167f37725c | 94 | settings.accel.enableZ = true; |
| jmar7 | 0:e8167f37725c | 95 | // accel scale can be 2, 4, 8, or 16 |
| jmar7 | 0:e8167f37725c | 96 | settings.accel.scale = 2; |
| jmar7 | 0:e8167f37725c | 97 | // accel sample rate can be 1-6 |
| jmar7 | 0:e8167f37725c | 98 | // 1 = 10 Hz 4 = 238 Hz |
| jmar7 | 0:e8167f37725c | 99 | // 2 = 50 Hz 5 = 476 Hz |
| jmar7 | 0:e8167f37725c | 100 | // 3 = 119 Hz 6 = 952 Hz |
| jmar7 | 0:e8167f37725c | 101 | settings.accel.sampleRate = 6; |
| jmar7 | 0:e8167f37725c | 102 | // Accel cutoff freqeuncy can be any value between -1 - 3. |
| jmar7 | 0:e8167f37725c | 103 | // -1 = bandwidth determined by sample rate |
| jmar7 | 0:e8167f37725c | 104 | // 0 = 408 Hz 2 = 105 Hz |
| jmar7 | 0:e8167f37725c | 105 | // 1 = 211 Hz 3 = 50 Hz |
| jmar7 | 0:e8167f37725c | 106 | settings.accel.bandwidth = -1; |
| jmar7 | 0:e8167f37725c | 107 | settings.accel.highResEnable = false; |
| jmar7 | 0:e8167f37725c | 108 | // accelHighResBandwidth can be any value between 0-3 |
| jmar7 | 0:e8167f37725c | 109 | // LP cutoff is set to a factor of sample rate |
| jmar7 | 0:e8167f37725c | 110 | // 0 = ODR/50 2 = ODR/9 |
| jmar7 | 0:e8167f37725c | 111 | // 1 = ODR/100 3 = ODR/400 |
| jmar7 | 0:e8167f37725c | 112 | settings.accel.highResBandwidth = 0; |
| jmar7 | 0:e8167f37725c | 113 | |
| jmar7 | 0:e8167f37725c | 114 | settings.mag.enabled = true; |
| jmar7 | 0:e8167f37725c | 115 | // mag scale can be 4, 8, 12, or 16 |
| jmar7 | 0:e8167f37725c | 116 | settings.mag.scale = 4; |
| jmar7 | 0:e8167f37725c | 117 | // mag data rate can be 0-7 |
| jmar7 | 0:e8167f37725c | 118 | // 0 = 0.625 Hz 4 = 10 Hz |
| jmar7 | 0:e8167f37725c | 119 | // 1 = 1.25 Hz 5 = 20 Hz |
| jmar7 | 0:e8167f37725c | 120 | // 2 = 2.5 Hz 6 = 40 Hz |
| jmar7 | 0:e8167f37725c | 121 | // 3 = 5 Hz 7 = 80 Hz |
| jmar7 | 0:e8167f37725c | 122 | settings.mag.sampleRate = 7; |
| jmar7 | 0:e8167f37725c | 123 | settings.mag.tempCompensationEnable = false; |
| jmar7 | 0:e8167f37725c | 124 | // magPerformance can be any value between 0-3 |
| jmar7 | 0:e8167f37725c | 125 | // 0 = Low power mode 2 = high performance |
| jmar7 | 0:e8167f37725c | 126 | // 1 = medium performance 3 = ultra-high performance |
| jmar7 | 0:e8167f37725c | 127 | settings.mag.XYPerformance = 3; |
| jmar7 | 0:e8167f37725c | 128 | settings.mag.ZPerformance = 3; |
| jmar7 | 0:e8167f37725c | 129 | settings.mag.lowPowerEnable = false; |
| jmar7 | 0:e8167f37725c | 130 | // magOperatingMode can be 0-2 |
| jmar7 | 0:e8167f37725c | 131 | // 0 = continuous conversion |
| jmar7 | 0:e8167f37725c | 132 | // 1 = single-conversion |
| jmar7 | 0:e8167f37725c | 133 | // 2 = power down |
| jmar7 | 0:e8167f37725c | 134 | settings.mag.operatingMode = 0; |
| jmar7 | 0:e8167f37725c | 135 | |
| jmar7 | 0:e8167f37725c | 136 | settings.temp.enabled = true; |
| jmar7 | 0:e8167f37725c | 137 | for (int i=0; i<3; i++) |
| jmar7 | 0:e8167f37725c | 138 | { |
| jmar7 | 0:e8167f37725c | 139 | gBias[i] = 0; |
| jmar7 | 0:e8167f37725c | 140 | aBias[i] = 0; |
| jmar7 | 0:e8167f37725c | 141 | mBias[i] = 0; |
| jmar7 | 0:e8167f37725c | 142 | gBiasRaw[i] = 0; |
| jmar7 | 0:e8167f37725c | 143 | aBiasRaw[i] = 0; |
| jmar7 | 0:e8167f37725c | 144 | mBiasRaw[i] = 0; |
| jmar7 | 0:e8167f37725c | 145 | } |
| jmar7 | 0:e8167f37725c | 146 | _autoCalc = false; |
| jmar7 | 0:e8167f37725c | 147 | } |
| jmar7 | 0:e8167f37725c | 148 | |
| jmar7 | 0:e8167f37725c | 149 | |
| jmar7 | 0:e8167f37725c | 150 | uint16_t LSM9DS1::begin() |
| jmar7 | 0:e8167f37725c | 151 | { |
| jmar7 | 0:e8167f37725c | 152 | //! Todo: don't use _xgAddress or _mAddress, duplicating memory |
| jmar7 | 0:e8167f37725c | 153 | _xgAddress = settings.device.agAddress; |
| jmar7 | 0:e8167f37725c | 154 | _mAddress = settings.device.mAddress; |
| jmar7 | 0:e8167f37725c | 155 | |
| jmar7 | 0:e8167f37725c | 156 | constrainScales(); |
| jmar7 | 0:e8167f37725c | 157 | // Once we have the scale values, we can calculate the resolution |
| jmar7 | 0:e8167f37725c | 158 | // of each sensor. That's what these functions are for. One for each sensor |
| jmar7 | 0:e8167f37725c | 159 | calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable |
| jmar7 | 0:e8167f37725c | 160 | calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable |
| jmar7 | 0:e8167f37725c | 161 | calcaRes(); // Calculate g / ADC tick, stored in aRes variable |
| jmar7 | 0:e8167f37725c | 162 | |
| jmar7 | 0:e8167f37725c | 163 | // Now, initialize our hardware interface. |
| jmar7 | 0:e8167f37725c | 164 | if (settings.device.commInterface == IMU_MODE_I2C) // If we're using I2C |
| jmar7 | 0:e8167f37725c | 165 | initI2C(); // Initialize I2C |
| jmar7 | 0:e8167f37725c | 166 | else if (settings.device.commInterface == IMU_MODE_SPI) // else, if we're using SPI |
| jmar7 | 0:e8167f37725c | 167 | initSPI(); // Initialize SPI |
| jmar7 | 0:e8167f37725c | 168 | |
| jmar7 | 0:e8167f37725c | 169 | // To verify communication, we can read from the WHO_AM_I register of |
| jmar7 | 0:e8167f37725c | 170 | // each device. Store those in a variable so we can return them. |
| jmar7 | 0:e8167f37725c | 171 | uint8_t mTest = mReadByte(WHO_AM_I_M); // Read the gyro WHO_AM_I |
| jmar7 | 0:e8167f37725c | 172 | uint8_t xgTest = xgReadByte(WHO_AM_I_XG); // Read the accel/mag WHO_AM_I |
| jmar7 | 0:e8167f37725c | 173 | pc.printf("%x, %x, %x, %x\n\r", mTest, xgTest, _xgAddress, _mAddress); |
| jmar7 | 0:e8167f37725c | 174 | uint16_t whoAmICombined = (xgTest << 8) | mTest; |
| jmar7 | 0:e8167f37725c | 175 | |
| jmar7 | 0:e8167f37725c | 176 | if (whoAmICombined != ((WHO_AM_I_AG_RSP << 8) | WHO_AM_I_M_RSP)) |
| jmar7 | 0:e8167f37725c | 177 | return 0; |
| jmar7 | 0:e8167f37725c | 178 | |
| jmar7 | 0:e8167f37725c | 179 | // Gyro initialization stuff: |
| jmar7 | 0:e8167f37725c | 180 | initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc. |
| jmar7 | 0:e8167f37725c | 181 | |
| jmar7 | 0:e8167f37725c | 182 | // Accelerometer initialization stuff: |
| jmar7 | 0:e8167f37725c | 183 | initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc. |
| jmar7 | 0:e8167f37725c | 184 | |
| jmar7 | 0:e8167f37725c | 185 | // Magnetometer initialization stuff: |
| jmar7 | 0:e8167f37725c | 186 | initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc. |
| jmar7 | 0:e8167f37725c | 187 | |
| jmar7 | 0:e8167f37725c | 188 | // Once everything is initialized, return the WHO_AM_I registers we read: |
| jmar7 | 0:e8167f37725c | 189 | return whoAmICombined; |
| jmar7 | 0:e8167f37725c | 190 | } |
| jmar7 | 0:e8167f37725c | 191 | |
| jmar7 | 0:e8167f37725c | 192 | void LSM9DS1::initGyro() |
| jmar7 | 0:e8167f37725c | 193 | { |
| jmar7 | 0:e8167f37725c | 194 | uint8_t tempRegValue = 0; |
| jmar7 | 0:e8167f37725c | 195 | |
| jmar7 | 0:e8167f37725c | 196 | // CTRL_REG1_G (Default value: 0x00) |
| jmar7 | 0:e8167f37725c | 197 | // [ODR_G2][ODR_G1][ODR_G0][FS_G1][FS_G0][0][BW_G1][BW_G0] |
| jmar7 | 0:e8167f37725c | 198 | // ODR_G[2:0] - Output data rate selection |
| jmar7 | 0:e8167f37725c | 199 | // FS_G[1:0] - Gyroscope full-scale selection |
| jmar7 | 0:e8167f37725c | 200 | // BW_G[1:0] - Gyroscope bandwidth selection |
| jmar7 | 0:e8167f37725c | 201 | |
| jmar7 | 0:e8167f37725c | 202 | // To disable gyro, set sample rate bits to 0. We'll only set sample |
| jmar7 | 0:e8167f37725c | 203 | // rate if the gyro is enabled. |
| jmar7 | 0:e8167f37725c | 204 | if (settings.gyro.enabled) |
| jmar7 | 0:e8167f37725c | 205 | { |
| jmar7 | 0:e8167f37725c | 206 | tempRegValue = (settings.gyro.sampleRate & 0x07) << 5; |
| jmar7 | 0:e8167f37725c | 207 | } |
| jmar7 | 0:e8167f37725c | 208 | switch (settings.gyro.scale) |
| jmar7 | 0:e8167f37725c | 209 | { |
| jmar7 | 0:e8167f37725c | 210 | case 500: |
| jmar7 | 0:e8167f37725c | 211 | tempRegValue |= (0x1 << 3); |
| jmar7 | 0:e8167f37725c | 212 | break; |
| jmar7 | 0:e8167f37725c | 213 | case 2000: |
| jmar7 | 0:e8167f37725c | 214 | tempRegValue |= (0x3 << 3); |
| jmar7 | 0:e8167f37725c | 215 | break; |
| jmar7 | 0:e8167f37725c | 216 | // Otherwise we'll set it to 245 dps (0x0 << 4) |
| jmar7 | 0:e8167f37725c | 217 | } |
| jmar7 | 0:e8167f37725c | 218 | tempRegValue |= (settings.gyro.bandwidth & 0x3); |
| jmar7 | 0:e8167f37725c | 219 | xgWriteByte(CTRL_REG1_G, tempRegValue); |
| jmar7 | 0:e8167f37725c | 220 | |
| jmar7 | 0:e8167f37725c | 221 | // CTRL_REG2_G (Default value: 0x00) |
| jmar7 | 0:e8167f37725c | 222 | // [0][0][0][0][INT_SEL1][INT_SEL0][OUT_SEL1][OUT_SEL0] |
| jmar7 | 0:e8167f37725c | 223 | // INT_SEL[1:0] - INT selection configuration |
| jmar7 | 0:e8167f37725c | 224 | // OUT_SEL[1:0] - Out selection configuration |
| jmar7 | 0:e8167f37725c | 225 | xgWriteByte(CTRL_REG2_G, 0x00); |
| jmar7 | 0:e8167f37725c | 226 | |
| jmar7 | 0:e8167f37725c | 227 | // CTRL_REG3_G (Default value: 0x00) |
| jmar7 | 0:e8167f37725c | 228 | // [LP_mode][HP_EN][0][0][HPCF3_G][HPCF2_G][HPCF1_G][HPCF0_G] |
| jmar7 | 0:e8167f37725c | 229 | // LP_mode - Low-power mode enable (0: disabled, 1: enabled) |
| jmar7 | 0:e8167f37725c | 230 | // HP_EN - HPF enable (0:disabled, 1: enabled) |
| jmar7 | 0:e8167f37725c | 231 | // HPCF_G[3:0] - HPF cutoff frequency |
| jmar7 | 0:e8167f37725c | 232 | tempRegValue = settings.gyro.lowPowerEnable ? (1<<7) : 0; |
| jmar7 | 0:e8167f37725c | 233 | if (settings.gyro.HPFEnable) |
| jmar7 | 0:e8167f37725c | 234 | { |
| jmar7 | 0:e8167f37725c | 235 | tempRegValue |= (1<<6) | (settings.gyro.HPFCutoff & 0x0F); |
| jmar7 | 0:e8167f37725c | 236 | } |
| jmar7 | 0:e8167f37725c | 237 | xgWriteByte(CTRL_REG3_G, tempRegValue); |
| jmar7 | 0:e8167f37725c | 238 | |
| jmar7 | 0:e8167f37725c | 239 | // CTRL_REG4 (Default value: 0x38) |
| jmar7 | 0:e8167f37725c | 240 | // [0][0][Zen_G][Yen_G][Xen_G][0][LIR_XL1][4D_XL1] |
| jmar7 | 0:e8167f37725c | 241 | // Zen_G - Z-axis output enable (0:disable, 1:enable) |
| jmar7 | 0:e8167f37725c | 242 | // Yen_G - Y-axis output enable (0:disable, 1:enable) |
| jmar7 | 0:e8167f37725c | 243 | // Xen_G - X-axis output enable (0:disable, 1:enable) |
| jmar7 | 0:e8167f37725c | 244 | // LIR_XL1 - Latched interrupt (0:not latched, 1:latched) |
| jmar7 | 0:e8167f37725c | 245 | // 4D_XL1 - 4D option on interrupt (0:6D used, 1:4D used) |
| jmar7 | 0:e8167f37725c | 246 | tempRegValue = 0; |
| jmar7 | 0:e8167f37725c | 247 | if (settings.gyro.enableZ) tempRegValue |= (1<<5); |
| jmar7 | 0:e8167f37725c | 248 | if (settings.gyro.enableY) tempRegValue |= (1<<4); |
| jmar7 | 0:e8167f37725c | 249 | if (settings.gyro.enableX) tempRegValue |= (1<<3); |
| jmar7 | 0:e8167f37725c | 250 | if (settings.gyro.latchInterrupt) tempRegValue |= (1<<1); |
| jmar7 | 0:e8167f37725c | 251 | xgWriteByte(CTRL_REG4, tempRegValue); |
| jmar7 | 0:e8167f37725c | 252 | |
| jmar7 | 0:e8167f37725c | 253 | // ORIENT_CFG_G (Default value: 0x00) |
| jmar7 | 0:e8167f37725c | 254 | // [0][0][SignX_G][SignY_G][SignZ_G][Orient_2][Orient_1][Orient_0] |
| jmar7 | 0:e8167f37725c | 255 | // SignX_G - Pitch axis (X) angular rate sign (0: positive, 1: negative) |
| jmar7 | 0:e8167f37725c | 256 | // Orient [2:0] - Directional user orientation selection |
| jmar7 | 0:e8167f37725c | 257 | tempRegValue = 0; |
| jmar7 | 0:e8167f37725c | 258 | if (settings.gyro.flipX) tempRegValue |= (1<<5); |
| jmar7 | 0:e8167f37725c | 259 | if (settings.gyro.flipY) tempRegValue |= (1<<4); |
| jmar7 | 0:e8167f37725c | 260 | if (settings.gyro.flipZ) tempRegValue |= (1<<3); |
| jmar7 | 0:e8167f37725c | 261 | xgWriteByte(ORIENT_CFG_G, tempRegValue); |
| jmar7 | 0:e8167f37725c | 262 | } |
| jmar7 | 0:e8167f37725c | 263 | |
| jmar7 | 0:e8167f37725c | 264 | void LSM9DS1::initAccel() |
| jmar7 | 0:e8167f37725c | 265 | { |
| jmar7 | 0:e8167f37725c | 266 | uint8_t tempRegValue = 0; |
| jmar7 | 0:e8167f37725c | 267 | |
| jmar7 | 0:e8167f37725c | 268 | // CTRL_REG5_XL (0x1F) (Default value: 0x38) |
| jmar7 | 0:e8167f37725c | 269 | // [DEC_1][DEC_0][Zen_XL][Yen_XL][Zen_XL][0][0][0] |
| jmar7 | 0:e8167f37725c | 270 | // DEC[0:1] - Decimation of accel data on OUT REG and FIFO. |
| jmar7 | 0:e8167f37725c | 271 | // 00: None, 01: 2 samples, 10: 4 samples 11: 8 samples |
| jmar7 | 0:e8167f37725c | 272 | // Zen_XL - Z-axis output enabled |
| jmar7 | 0:e8167f37725c | 273 | // Yen_XL - Y-axis output enabled |
| jmar7 | 0:e8167f37725c | 274 | // Xen_XL - X-axis output enabled |
| jmar7 | 0:e8167f37725c | 275 | if (settings.accel.enableZ) tempRegValue |= (1<<5); |
| jmar7 | 0:e8167f37725c | 276 | if (settings.accel.enableY) tempRegValue |= (1<<4); |
| jmar7 | 0:e8167f37725c | 277 | if (settings.accel.enableX) tempRegValue |= (1<<3); |
| jmar7 | 0:e8167f37725c | 278 | |
| jmar7 | 0:e8167f37725c | 279 | xgWriteByte(CTRL_REG5_XL, tempRegValue); |
| jmar7 | 0:e8167f37725c | 280 | |
| jmar7 | 0:e8167f37725c | 281 | // CTRL_REG6_XL (0x20) (Default value: 0x00) |
| jmar7 | 0:e8167f37725c | 282 | // [ODR_XL2][ODR_XL1][ODR_XL0][FS1_XL][FS0_XL][BW_SCAL_ODR][BW_XL1][BW_XL0] |
| jmar7 | 0:e8167f37725c | 283 | // ODR_XL[2:0] - Output data rate & power mode selection |
| jmar7 | 0:e8167f37725c | 284 | // FS_XL[1:0] - Full-scale selection |
| jmar7 | 0:e8167f37725c | 285 | // BW_SCAL_ODR - Bandwidth selection |
| jmar7 | 0:e8167f37725c | 286 | // BW_XL[1:0] - Anti-aliasing filter bandwidth selection |
| jmar7 | 0:e8167f37725c | 287 | tempRegValue = 0; |
| jmar7 | 0:e8167f37725c | 288 | // To disable the accel, set the sampleRate bits to 0. |
| jmar7 | 0:e8167f37725c | 289 | if (settings.accel.enabled) |
| jmar7 | 0:e8167f37725c | 290 | { |
| jmar7 | 0:e8167f37725c | 291 | tempRegValue |= (settings.accel.sampleRate & 0x07) << 5; |
| jmar7 | 0:e8167f37725c | 292 | } |
| jmar7 | 0:e8167f37725c | 293 | switch (settings.accel.scale) |
| jmar7 | 0:e8167f37725c | 294 | { |
| jmar7 | 0:e8167f37725c | 295 | case 4: |
| jmar7 | 0:e8167f37725c | 296 | tempRegValue |= (0x2 << 3); |
| jmar7 | 0:e8167f37725c | 297 | break; |
| jmar7 | 0:e8167f37725c | 298 | case 8: |
| jmar7 | 0:e8167f37725c | 299 | tempRegValue |= (0x3 << 3); |
| jmar7 | 0:e8167f37725c | 300 | break; |
| jmar7 | 0:e8167f37725c | 301 | case 16: |
| jmar7 | 0:e8167f37725c | 302 | tempRegValue |= (0x1 << 3); |
| jmar7 | 0:e8167f37725c | 303 | break; |
| jmar7 | 0:e8167f37725c | 304 | // Otherwise it'll be set to 2g (0x0 << 3) |
| jmar7 | 0:e8167f37725c | 305 | } |
| jmar7 | 0:e8167f37725c | 306 | if (settings.accel.bandwidth >= 0) |
| jmar7 | 0:e8167f37725c | 307 | { |
| jmar7 | 0:e8167f37725c | 308 | tempRegValue |= (1<<2); // Set BW_SCAL_ODR |
| jmar7 | 0:e8167f37725c | 309 | tempRegValue |= (settings.accel.bandwidth & 0x03); |
| jmar7 | 0:e8167f37725c | 310 | } |
| jmar7 | 0:e8167f37725c | 311 | xgWriteByte(CTRL_REG6_XL, tempRegValue); |
| jmar7 | 0:e8167f37725c | 312 | |
| jmar7 | 0:e8167f37725c | 313 | // CTRL_REG7_XL (0x21) (Default value: 0x00) |
| jmar7 | 0:e8167f37725c | 314 | // [HR][DCF1][DCF0][0][0][FDS][0][HPIS1] |
| jmar7 | 0:e8167f37725c | 315 | // HR - High resolution mode (0: disable, 1: enable) |
| jmar7 | 0:e8167f37725c | 316 | // DCF[1:0] - Digital filter cutoff frequency |
| jmar7 | 0:e8167f37725c | 317 | // FDS - Filtered data selection |
| jmar7 | 0:e8167f37725c | 318 | // HPIS1 - HPF enabled for interrupt function |
| jmar7 | 0:e8167f37725c | 319 | tempRegValue = 0; |
| jmar7 | 0:e8167f37725c | 320 | if (settings.accel.highResEnable) |
| jmar7 | 0:e8167f37725c | 321 | { |
| jmar7 | 0:e8167f37725c | 322 | tempRegValue |= (1<<7); // Set HR bit |
| jmar7 | 0:e8167f37725c | 323 | tempRegValue |= (settings.accel.highResBandwidth & 0x3) << 5; |
| jmar7 | 0:e8167f37725c | 324 | } |
| jmar7 | 0:e8167f37725c | 325 | xgWriteByte(CTRL_REG7_XL, tempRegValue); |
| jmar7 | 0:e8167f37725c | 326 | } |
| jmar7 | 0:e8167f37725c | 327 | |
| jmar7 | 0:e8167f37725c | 328 | // This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average |
| jmar7 | 0:e8167f37725c | 329 | // them, scales them to gs and deg/s, respectively, and then passes the biases to the main sketch |
| jmar7 | 0:e8167f37725c | 330 | // for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store |
| jmar7 | 0:e8167f37725c | 331 | // the data as there are in the ADXL345, a precursor to the LSM9DS0, or the MPU-9150, so we have to |
| jmar7 | 0:e8167f37725c | 332 | // subtract the biases ourselves. This results in a more accurate measurement in general and can |
| jmar7 | 0:e8167f37725c | 333 | // remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner |
| jmar7 | 0:e8167f37725c | 334 | // is good practice. |
| jmar7 | 0:e8167f37725c | 335 | void LSM9DS1::calibrate(bool autoCalc) |
| jmar7 | 0:e8167f37725c | 336 | { |
| jmar7 | 0:e8167f37725c | 337 | uint8_t data[6] = {0, 0, 0, 0, 0, 0}; |
| jmar7 | 0:e8167f37725c | 338 | uint8_t samples = 0; |
| jmar7 | 0:e8167f37725c | 339 | int ii; |
| jmar7 | 0:e8167f37725c | 340 | int32_t aBiasRawTemp[3] = {0, 0, 0}; |
| jmar7 | 0:e8167f37725c | 341 | int32_t gBiasRawTemp[3] = {0, 0, 0}; |
| jmar7 | 0:e8167f37725c | 342 | |
| jmar7 | 0:e8167f37725c | 343 | // Turn on FIFO and set threshold to 32 samples |
| jmar7 | 0:e8167f37725c | 344 | enableFIFO(true); |
| jmar7 | 0:e8167f37725c | 345 | setFIFO(FIFO_THS, 0x1F); |
| jmar7 | 0:e8167f37725c | 346 | while (samples < 0x1F) |
| jmar7 | 0:e8167f37725c | 347 | { |
| jmar7 | 0:e8167f37725c | 348 | samples = (xgReadByte(FIFO_SRC) & 0x3F); // Read number of stored samples |
| jmar7 | 0:e8167f37725c | 349 | } |
| jmar7 | 0:e8167f37725c | 350 | for(ii = 0; ii < samples ; ii++) |
| jmar7 | 0:e8167f37725c | 351 | { // Read the gyro data stored in the FIFO |
| jmar7 | 0:e8167f37725c | 352 | readGyro(); |
| jmar7 | 0:e8167f37725c | 353 | gBiasRawTemp[0] += gx; |
| jmar7 | 0:e8167f37725c | 354 | gBiasRawTemp[1] += gy; |
| jmar7 | 0:e8167f37725c | 355 | gBiasRawTemp[2] += gz; |
| jmar7 | 0:e8167f37725c | 356 | readAccel(); |
| jmar7 | 0:e8167f37725c | 357 | aBiasRawTemp[0] += ax; |
| jmar7 | 0:e8167f37725c | 358 | aBiasRawTemp[1] += ay; |
| jmar7 | 0:e8167f37725c | 359 | aBiasRawTemp[2] += az - (int16_t)(1./aRes); // Assumes sensor facing up! |
| jmar7 | 0:e8167f37725c | 360 | } |
| jmar7 | 0:e8167f37725c | 361 | for (ii = 0; ii < 3; ii++) |
| jmar7 | 0:e8167f37725c | 362 | { |
| jmar7 | 0:e8167f37725c | 363 | gBiasRaw[ii] = gBiasRawTemp[ii] / samples; |
| jmar7 | 0:e8167f37725c | 364 | gBias[ii] = calcGyro(gBiasRaw[ii]); |
| jmar7 | 0:e8167f37725c | 365 | aBiasRaw[ii] = aBiasRawTemp[ii] / samples; |
| jmar7 | 0:e8167f37725c | 366 | aBias[ii] = calcAccel(aBiasRaw[ii]); |
| jmar7 | 0:e8167f37725c | 367 | } |
| jmar7 | 0:e8167f37725c | 368 | |
| jmar7 | 0:e8167f37725c | 369 | enableFIFO(false); |
| jmar7 | 0:e8167f37725c | 370 | setFIFO(FIFO_OFF, 0x00); |
| jmar7 | 0:e8167f37725c | 371 | |
| jmar7 | 0:e8167f37725c | 372 | if (autoCalc) _autoCalc = true; |
| jmar7 | 0:e8167f37725c | 373 | } |
| jmar7 | 0:e8167f37725c | 374 | |
| jmar7 | 0:e8167f37725c | 375 | void LSM9DS1::calibrateMag(bool loadIn) |
| jmar7 | 0:e8167f37725c | 376 | { |
| jmar7 | 0:e8167f37725c | 377 | int i, j; |
| jmar7 | 0:e8167f37725c | 378 | int16_t magMin[3] = {0, 0, 0}; |
| jmar7 | 0:e8167f37725c | 379 | int16_t magMax[3] = {0, 0, 0}; // The road warrior |
| jmar7 | 0:e8167f37725c | 380 | |
| jmar7 | 0:e8167f37725c | 381 | for (i=0; i<128; i++) |
| jmar7 | 0:e8167f37725c | 382 | { |
| jmar7 | 0:e8167f37725c | 383 | while (!magAvailable()) |
| jmar7 | 0:e8167f37725c | 384 | ; |
| jmar7 | 0:e8167f37725c | 385 | readMag(); |
| jmar7 | 0:e8167f37725c | 386 | int16_t magTemp[3] = {0, 0, 0}; |
| jmar7 | 0:e8167f37725c | 387 | magTemp[0] = mx; |
| jmar7 | 0:e8167f37725c | 388 | magTemp[1] = my; |
| jmar7 | 0:e8167f37725c | 389 | magTemp[2] = mz; |
| jmar7 | 0:e8167f37725c | 390 | for (j = 0; j < 3; j++) |
| jmar7 | 0:e8167f37725c | 391 | { |
| jmar7 | 0:e8167f37725c | 392 | if (magTemp[j] > magMax[j]) magMax[j] = magTemp[j]; |
| jmar7 | 0:e8167f37725c | 393 | if (magTemp[j] < magMin[j]) magMin[j] = magTemp[j]; |
| jmar7 | 0:e8167f37725c | 394 | } |
| jmar7 | 0:e8167f37725c | 395 | } |
| jmar7 | 0:e8167f37725c | 396 | for (j = 0; j < 3; j++) |
| jmar7 | 0:e8167f37725c | 397 | { |
| jmar7 | 0:e8167f37725c | 398 | mBiasRaw[j] = (magMax[j] + magMin[j]) / 2; |
| jmar7 | 0:e8167f37725c | 399 | mBias[j] = calcMag(mBiasRaw[j]); |
| jmar7 | 0:e8167f37725c | 400 | if (loadIn) |
| jmar7 | 0:e8167f37725c | 401 | magOffset(j, mBiasRaw[j]); |
| jmar7 | 0:e8167f37725c | 402 | } |
| jmar7 | 0:e8167f37725c | 403 | |
| jmar7 | 0:e8167f37725c | 404 | } |
| jmar7 | 0:e8167f37725c | 405 | void LSM9DS1::magOffset(uint8_t axis, int16_t offset) |
| jmar7 | 0:e8167f37725c | 406 | { |
| jmar7 | 0:e8167f37725c | 407 | if (axis > 2) |
| jmar7 | 0:e8167f37725c | 408 | return; |
| jmar7 | 0:e8167f37725c | 409 | uint8_t msb, lsb; |
| jmar7 | 0:e8167f37725c | 410 | msb = (offset & 0xFF00) >> 8; |
| jmar7 | 0:e8167f37725c | 411 | lsb = offset & 0x00FF; |
| jmar7 | 0:e8167f37725c | 412 | mWriteByte(OFFSET_X_REG_L_M + (2 * axis), lsb); |
| jmar7 | 0:e8167f37725c | 413 | mWriteByte(OFFSET_X_REG_H_M + (2 * axis), msb); |
| jmar7 | 0:e8167f37725c | 414 | } |
| jmar7 | 0:e8167f37725c | 415 | |
| jmar7 | 0:e8167f37725c | 416 | void LSM9DS1::initMag() |
| jmar7 | 0:e8167f37725c | 417 | { |
| jmar7 | 0:e8167f37725c | 418 | uint8_t tempRegValue = 0; |
| jmar7 | 0:e8167f37725c | 419 | |
| jmar7 | 0:e8167f37725c | 420 | // CTRL_REG1_M (Default value: 0x10) |
| jmar7 | 0:e8167f37725c | 421 | // [TEMP_COMP][OM1][OM0][DO2][DO1][DO0][0][ST] |
| jmar7 | 0:e8167f37725c | 422 | // TEMP_COMP - Temperature compensation |
| jmar7 | 0:e8167f37725c | 423 | // OM[1:0] - X & Y axes op mode selection |
| jmar7 | 0:e8167f37725c | 424 | // 00:low-power, 01:medium performance |
| jmar7 | 0:e8167f37725c | 425 | // 10: high performance, 11:ultra-high performance |
| jmar7 | 0:e8167f37725c | 426 | // DO[2:0] - Output data rate selection |
| jmar7 | 0:e8167f37725c | 427 | // ST - Self-test enable |
| jmar7 | 0:e8167f37725c | 428 | if (settings.mag.tempCompensationEnable) tempRegValue |= (1<<7); |
| jmar7 | 0:e8167f37725c | 429 | tempRegValue |= (settings.mag.XYPerformance & 0x3) << 5; |
| jmar7 | 0:e8167f37725c | 430 | tempRegValue |= (settings.mag.sampleRate & 0x7) << 2; |
| jmar7 | 0:e8167f37725c | 431 | mWriteByte(CTRL_REG1_M, tempRegValue); |
| jmar7 | 0:e8167f37725c | 432 | |
| jmar7 | 0:e8167f37725c | 433 | // CTRL_REG2_M (Default value 0x00) |
| jmar7 | 0:e8167f37725c | 434 | // [0][FS1][FS0][0][REBOOT][SOFT_RST][0][0] |
| jmar7 | 0:e8167f37725c | 435 | // FS[1:0] - Full-scale configuration |
| jmar7 | 0:e8167f37725c | 436 | // REBOOT - Reboot memory content (0:normal, 1:reboot) |
| jmar7 | 0:e8167f37725c | 437 | // SOFT_RST - Reset config and user registers (0:default, 1:reset) |
| jmar7 | 0:e8167f37725c | 438 | tempRegValue = 0; |
| jmar7 | 0:e8167f37725c | 439 | switch (settings.mag.scale) |
| jmar7 | 0:e8167f37725c | 440 | { |
| jmar7 | 0:e8167f37725c | 441 | case 8: |
| jmar7 | 0:e8167f37725c | 442 | tempRegValue |= (0x1 << 5); |
| jmar7 | 0:e8167f37725c | 443 | break; |
| jmar7 | 0:e8167f37725c | 444 | case 12: |
| jmar7 | 0:e8167f37725c | 445 | tempRegValue |= (0x2 << 5); |
| jmar7 | 0:e8167f37725c | 446 | break; |
| jmar7 | 0:e8167f37725c | 447 | case 16: |
| jmar7 | 0:e8167f37725c | 448 | tempRegValue |= (0x3 << 5); |
| jmar7 | 0:e8167f37725c | 449 | break; |
| jmar7 | 0:e8167f37725c | 450 | // Otherwise we'll default to 4 gauss (00) |
| jmar7 | 0:e8167f37725c | 451 | } |
| jmar7 | 0:e8167f37725c | 452 | mWriteByte(CTRL_REG2_M, tempRegValue); // +/-4Gauss |
| jmar7 | 0:e8167f37725c | 453 | |
| jmar7 | 0:e8167f37725c | 454 | // CTRL_REG3_M (Default value: 0x03) |
| jmar7 | 0:e8167f37725c | 455 | // [I2C_DISABLE][0][LP][0][0][SIM][MD1][MD0] |
| jmar7 | 0:e8167f37725c | 456 | // I2C_DISABLE - Disable I2C interace (0:enable, 1:disable) |
| jmar7 | 0:e8167f37725c | 457 | // LP - Low-power mode cofiguration (1:enable) |
| jmar7 | 0:e8167f37725c | 458 | // SIM - SPI mode selection (0:write-only, 1:read/write enable) |
| jmar7 | 0:e8167f37725c | 459 | // MD[1:0] - Operating mode |
| jmar7 | 0:e8167f37725c | 460 | // 00:continuous conversion, 01:single-conversion, |
| jmar7 | 0:e8167f37725c | 461 | // 10,11: Power-down |
| jmar7 | 0:e8167f37725c | 462 | tempRegValue = 0; |
| jmar7 | 0:e8167f37725c | 463 | if (settings.mag.lowPowerEnable) tempRegValue |= (1<<5); |
| jmar7 | 0:e8167f37725c | 464 | tempRegValue |= (settings.mag.operatingMode & 0x3); |
| jmar7 | 0:e8167f37725c | 465 | mWriteByte(CTRL_REG3_M, tempRegValue); // Continuous conversion mode |
| jmar7 | 0:e8167f37725c | 466 | |
| jmar7 | 0:e8167f37725c | 467 | // CTRL_REG4_M (Default value: 0x00) |
| jmar7 | 0:e8167f37725c | 468 | // [0][0][0][0][OMZ1][OMZ0][BLE][0] |
| jmar7 | 0:e8167f37725c | 469 | // OMZ[1:0] - Z-axis operative mode selection |
| jmar7 | 0:e8167f37725c | 470 | // 00:low-power mode, 01:medium performance |
| jmar7 | 0:e8167f37725c | 471 | // 10:high performance, 10:ultra-high performance |
| jmar7 | 0:e8167f37725c | 472 | // BLE - Big/little endian data |
| jmar7 | 0:e8167f37725c | 473 | tempRegValue = 0; |
| jmar7 | 0:e8167f37725c | 474 | tempRegValue = (settings.mag.ZPerformance & 0x3) << 2; |
| jmar7 | 0:e8167f37725c | 475 | mWriteByte(CTRL_REG4_M, tempRegValue); |
| jmar7 | 0:e8167f37725c | 476 | |
| jmar7 | 0:e8167f37725c | 477 | // CTRL_REG5_M (Default value: 0x00) |
| jmar7 | 0:e8167f37725c | 478 | // [0][BDU][0][0][0][0][0][0] |
| jmar7 | 0:e8167f37725c | 479 | // BDU - Block data update for magnetic data |
| jmar7 | 0:e8167f37725c | 480 | // 0:continuous, 1:not updated until MSB/LSB are read |
| jmar7 | 0:e8167f37725c | 481 | tempRegValue = 0; |
| jmar7 | 0:e8167f37725c | 482 | mWriteByte(CTRL_REG5_M, tempRegValue); |
| jmar7 | 0:e8167f37725c | 483 | } |
| jmar7 | 0:e8167f37725c | 484 | |
| jmar7 | 0:e8167f37725c | 485 | uint8_t LSM9DS1::accelAvailable() |
| jmar7 | 0:e8167f37725c | 486 | { |
| jmar7 | 0:e8167f37725c | 487 | uint8_t status = xgReadByte(STATUS_REG_1); |
| jmar7 | 0:e8167f37725c | 488 | |
| jmar7 | 0:e8167f37725c | 489 | return (status & (1<<0)); |
| jmar7 | 0:e8167f37725c | 490 | } |
| jmar7 | 0:e8167f37725c | 491 | |
| jmar7 | 0:e8167f37725c | 492 | uint8_t LSM9DS1::gyroAvailable() |
| jmar7 | 0:e8167f37725c | 493 | { |
| jmar7 | 0:e8167f37725c | 494 | uint8_t status = xgReadByte(STATUS_REG_1); |
| jmar7 | 0:e8167f37725c | 495 | |
| jmar7 | 0:e8167f37725c | 496 | return ((status & (1<<1)) >> 1); |
| jmar7 | 0:e8167f37725c | 497 | } |
| jmar7 | 0:e8167f37725c | 498 | |
| jmar7 | 0:e8167f37725c | 499 | uint8_t LSM9DS1::tempAvailable() |
| jmar7 | 0:e8167f37725c | 500 | { |
| jmar7 | 0:e8167f37725c | 501 | uint8_t status = xgReadByte(STATUS_REG_1); |
| jmar7 | 0:e8167f37725c | 502 | |
| jmar7 | 0:e8167f37725c | 503 | return ((status & (1<<2)) >> 2); |
| jmar7 | 0:e8167f37725c | 504 | } |
| jmar7 | 0:e8167f37725c | 505 | |
| jmar7 | 0:e8167f37725c | 506 | uint8_t LSM9DS1::magAvailable(lsm9ds1_axis axis) |
| jmar7 | 0:e8167f37725c | 507 | { |
| jmar7 | 0:e8167f37725c | 508 | uint8_t status; |
| jmar7 | 0:e8167f37725c | 509 | status = mReadByte(STATUS_REG_M); |
| jmar7 | 0:e8167f37725c | 510 | |
| jmar7 | 0:e8167f37725c | 511 | return ((status & (1<<axis)) >> axis); |
| jmar7 | 0:e8167f37725c | 512 | } |
| jmar7 | 0:e8167f37725c | 513 | |
| jmar7 | 0:e8167f37725c | 514 | void LSM9DS1::readAccel() |
| jmar7 | 0:e8167f37725c | 515 | { |
| jmar7 | 0:e8167f37725c | 516 | uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp |
| jmar7 | 0:e8167f37725c | 517 | xgReadBytes(OUT_X_L_XL, temp, 6); // Read 6 bytes, beginning at OUT_X_L_XL |
| jmar7 | 0:e8167f37725c | 518 | ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax |
| jmar7 | 0:e8167f37725c | 519 | ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay |
| jmar7 | 0:e8167f37725c | 520 | az = (temp[5] << 8) | temp[4]; // Store z-axis values into az |
| jmar7 | 0:e8167f37725c | 521 | if (_autoCalc) |
| jmar7 | 0:e8167f37725c | 522 | { |
| jmar7 | 0:e8167f37725c | 523 | ax -= aBiasRaw[X_AXIS]; |
| jmar7 | 0:e8167f37725c | 524 | ay -= aBiasRaw[Y_AXIS]; |
| jmar7 | 0:e8167f37725c | 525 | az -= aBiasRaw[Z_AXIS]; |
| jmar7 | 0:e8167f37725c | 526 | } |
| jmar7 | 0:e8167f37725c | 527 | } |
| jmar7 | 0:e8167f37725c | 528 | |
| jmar7 | 0:e8167f37725c | 529 | int16_t LSM9DS1::readAccel(lsm9ds1_axis axis) |
| jmar7 | 0:e8167f37725c | 530 | { |
| jmar7 | 0:e8167f37725c | 531 | uint8_t temp[2]; |
| jmar7 | 0:e8167f37725c | 532 | int16_t value; |
| jmar7 | 0:e8167f37725c | 533 | xgReadBytes(OUT_X_L_XL + (2 * axis), temp, 2); |
| jmar7 | 0:e8167f37725c | 534 | value = (temp[1] << 8) | temp[0]; |
| jmar7 | 0:e8167f37725c | 535 | |
| jmar7 | 0:e8167f37725c | 536 | if (_autoCalc) |
| jmar7 | 0:e8167f37725c | 537 | value -= aBiasRaw[axis]; |
| jmar7 | 0:e8167f37725c | 538 | |
| jmar7 | 0:e8167f37725c | 539 | return value; |
| jmar7 | 0:e8167f37725c | 540 | } |
| jmar7 | 0:e8167f37725c | 541 | |
| jmar7 | 0:e8167f37725c | 542 | void LSM9DS1::readMag() |
| jmar7 | 0:e8167f37725c | 543 | { |
| jmar7 | 0:e8167f37725c | 544 | uint8_t temp[6]; // We'll read six bytes from the mag into temp |
| jmar7 | 0:e8167f37725c | 545 | mReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M |
| jmar7 | 0:e8167f37725c | 546 | mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx |
| jmar7 | 0:e8167f37725c | 547 | my = (temp[3] << 8) | temp[2]; // Store y-axis values into my |
| jmar7 | 0:e8167f37725c | 548 | mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz |
| jmar7 | 0:e8167f37725c | 549 | } |
| jmar7 | 0:e8167f37725c | 550 | |
| jmar7 | 0:e8167f37725c | 551 | int16_t LSM9DS1::readMag(lsm9ds1_axis axis) |
| jmar7 | 0:e8167f37725c | 552 | { |
| jmar7 | 0:e8167f37725c | 553 | uint8_t temp[2]; |
| jmar7 | 0:e8167f37725c | 554 | mReadBytes(OUT_X_L_M + (2 * axis), temp, 2); |
| jmar7 | 0:e8167f37725c | 555 | return (temp[1] << 8) | temp[0]; |
| jmar7 | 0:e8167f37725c | 556 | } |
| jmar7 | 0:e8167f37725c | 557 | |
| jmar7 | 0:e8167f37725c | 558 | void LSM9DS1::readTemp() |
| jmar7 | 0:e8167f37725c | 559 | { |
| jmar7 | 0:e8167f37725c | 560 | uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp |
| jmar7 | 0:e8167f37725c | 561 | xgReadBytes(OUT_TEMP_L, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L |
| jmar7 | 0:e8167f37725c | 562 | temperature = ((int16_t)temp[1] << 8) | temp[0]; |
| jmar7 | 0:e8167f37725c | 563 | } |
| jmar7 | 0:e8167f37725c | 564 | |
| jmar7 | 0:e8167f37725c | 565 | void LSM9DS1::readGyro() |
| jmar7 | 0:e8167f37725c | 566 | { |
| jmar7 | 0:e8167f37725c | 567 | uint8_t temp[6]; // We'll read six bytes from the gyro into temp |
| jmar7 | 0:e8167f37725c | 568 | xgReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G |
| jmar7 | 0:e8167f37725c | 569 | gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx |
| jmar7 | 0:e8167f37725c | 570 | gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy |
| jmar7 | 0:e8167f37725c | 571 | gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz |
| jmar7 | 0:e8167f37725c | 572 | if (_autoCalc) |
| jmar7 | 0:e8167f37725c | 573 | { |
| jmar7 | 0:e8167f37725c | 574 | gx -= gBiasRaw[X_AXIS]; |
| jmar7 | 0:e8167f37725c | 575 | gy -= gBiasRaw[Y_AXIS]; |
| jmar7 | 0:e8167f37725c | 576 | gz -= gBiasRaw[Z_AXIS]; |
| jmar7 | 0:e8167f37725c | 577 | } |
| jmar7 | 0:e8167f37725c | 578 | } |
| jmar7 | 0:e8167f37725c | 579 | |
| jmar7 | 0:e8167f37725c | 580 | int16_t LSM9DS1::readGyro(lsm9ds1_axis axis) |
| jmar7 | 0:e8167f37725c | 581 | { |
| jmar7 | 0:e8167f37725c | 582 | uint8_t temp[2]; |
| jmar7 | 0:e8167f37725c | 583 | int16_t value; |
| jmar7 | 0:e8167f37725c | 584 | |
| jmar7 | 0:e8167f37725c | 585 | xgReadBytes(OUT_X_L_G + (2 * axis), temp, 2); |
| jmar7 | 0:e8167f37725c | 586 | |
| jmar7 | 0:e8167f37725c | 587 | value = (temp[1] << 8) | temp[0]; |
| jmar7 | 0:e8167f37725c | 588 | |
| jmar7 | 0:e8167f37725c | 589 | if (_autoCalc) |
| jmar7 | 0:e8167f37725c | 590 | value -= gBiasRaw[axis]; |
| jmar7 | 0:e8167f37725c | 591 | |
| jmar7 | 0:e8167f37725c | 592 | return value; |
| jmar7 | 0:e8167f37725c | 593 | } |
| jmar7 | 0:e8167f37725c | 594 | |
| jmar7 | 0:e8167f37725c | 595 | float LSM9DS1::calcGyro(int16_t gyro) |
| jmar7 | 0:e8167f37725c | 596 | { |
| jmar7 | 0:e8167f37725c | 597 | // Return the gyro raw reading times our pre-calculated DPS / (ADC tick): |
| jmar7 | 0:e8167f37725c | 598 | return gRes * gyro; |
| jmar7 | 0:e8167f37725c | 599 | } |
| jmar7 | 0:e8167f37725c | 600 | |
| jmar7 | 0:e8167f37725c | 601 | float LSM9DS1::calcAccel(int16_t accel) |
| jmar7 | 0:e8167f37725c | 602 | { |
| jmar7 | 0:e8167f37725c | 603 | // Return the accel raw reading times our pre-calculated g's / (ADC tick): |
| jmar7 | 0:e8167f37725c | 604 | return aRes * accel; |
| jmar7 | 0:e8167f37725c | 605 | } |
| jmar7 | 0:e8167f37725c | 606 | |
| jmar7 | 0:e8167f37725c | 607 | float LSM9DS1::calcMag(int16_t mag) |
| jmar7 | 0:e8167f37725c | 608 | { |
| jmar7 | 0:e8167f37725c | 609 | // Return the mag raw reading times our pre-calculated Gs / (ADC tick): |
| jmar7 | 0:e8167f37725c | 610 | return mRes * mag; |
| jmar7 | 0:e8167f37725c | 611 | } |
| jmar7 | 0:e8167f37725c | 612 | |
| jmar7 | 0:e8167f37725c | 613 | void LSM9DS1::setGyroScale(uint16_t gScl) |
| jmar7 | 0:e8167f37725c | 614 | { |
| jmar7 | 0:e8167f37725c | 615 | // Read current value of CTRL_REG1_G: |
| jmar7 | 0:e8167f37725c | 616 | uint8_t ctrl1RegValue = xgReadByte(CTRL_REG1_G); |
| jmar7 | 0:e8167f37725c | 617 | // Mask out scale bits (3 & 4): |
| jmar7 | 0:e8167f37725c | 618 | ctrl1RegValue &= 0xE7; |
| jmar7 | 0:e8167f37725c | 619 | switch (gScl) |
| jmar7 | 0:e8167f37725c | 620 | { |
| jmar7 | 0:e8167f37725c | 621 | case 500: |
| jmar7 | 0:e8167f37725c | 622 | ctrl1RegValue |= (0x1 << 3); |
| jmar7 | 0:e8167f37725c | 623 | settings.gyro.scale = 500; |
| jmar7 | 0:e8167f37725c | 624 | break; |
| jmar7 | 0:e8167f37725c | 625 | case 2000: |
| jmar7 | 0:e8167f37725c | 626 | ctrl1RegValue |= (0x3 << 3); |
| jmar7 | 0:e8167f37725c | 627 | settings.gyro.scale = 2000; |
| jmar7 | 0:e8167f37725c | 628 | break; |
| jmar7 | 0:e8167f37725c | 629 | default: // Otherwise we'll set it to 245 dps (0x0 << 4) |
| jmar7 | 0:e8167f37725c | 630 | settings.gyro.scale = 245; |
| jmar7 | 0:e8167f37725c | 631 | break; |
| jmar7 | 0:e8167f37725c | 632 | } |
| jmar7 | 0:e8167f37725c | 633 | xgWriteByte(CTRL_REG1_G, ctrl1RegValue); |
| jmar7 | 0:e8167f37725c | 634 | |
| jmar7 | 0:e8167f37725c | 635 | calcgRes(); |
| jmar7 | 0:e8167f37725c | 636 | } |
| jmar7 | 0:e8167f37725c | 637 | |
| jmar7 | 0:e8167f37725c | 638 | void LSM9DS1::setAccelScale(uint8_t aScl) |
| jmar7 | 0:e8167f37725c | 639 | { |
| jmar7 | 0:e8167f37725c | 640 | // We need to preserve the other bytes in CTRL_REG6_XL. So, first read it: |
| jmar7 | 0:e8167f37725c | 641 | uint8_t tempRegValue = xgReadByte(CTRL_REG6_XL); |
| jmar7 | 0:e8167f37725c | 642 | // Mask out accel scale bits: |
| jmar7 | 0:e8167f37725c | 643 | tempRegValue &= 0xE7; |
| jmar7 | 0:e8167f37725c | 644 | |
| jmar7 | 0:e8167f37725c | 645 | switch (aScl) |
| jmar7 | 0:e8167f37725c | 646 | { |
| jmar7 | 0:e8167f37725c | 647 | case 4: |
| jmar7 | 0:e8167f37725c | 648 | tempRegValue |= (0x2 << 3); |
| jmar7 | 0:e8167f37725c | 649 | settings.accel.scale = 4; |
| jmar7 | 0:e8167f37725c | 650 | break; |
| jmar7 | 0:e8167f37725c | 651 | case 8: |
| jmar7 | 0:e8167f37725c | 652 | tempRegValue |= (0x3 << 3); |
| jmar7 | 0:e8167f37725c | 653 | settings.accel.scale = 8; |
| jmar7 | 0:e8167f37725c | 654 | break; |
| jmar7 | 0:e8167f37725c | 655 | case 16: |
| jmar7 | 0:e8167f37725c | 656 | tempRegValue |= (0x1 << 3); |
| jmar7 | 0:e8167f37725c | 657 | settings.accel.scale = 16; |
| jmar7 | 0:e8167f37725c | 658 | break; |
| jmar7 | 0:e8167f37725c | 659 | default: // Otherwise it'll be set to 2g (0x0 << 3) |
| jmar7 | 0:e8167f37725c | 660 | settings.accel.scale = 2; |
| jmar7 | 0:e8167f37725c | 661 | break; |
| jmar7 | 0:e8167f37725c | 662 | } |
| jmar7 | 0:e8167f37725c | 663 | xgWriteByte(CTRL_REG6_XL, tempRegValue); |
| jmar7 | 0:e8167f37725c | 664 | |
| jmar7 | 0:e8167f37725c | 665 | // Then calculate a new aRes, which relies on aScale being set correctly: |
| jmar7 | 0:e8167f37725c | 666 | calcaRes(); |
| jmar7 | 0:e8167f37725c | 667 | } |
| jmar7 | 0:e8167f37725c | 668 | |
| jmar7 | 0:e8167f37725c | 669 | void LSM9DS1::setMagScale(uint8_t mScl) |
| jmar7 | 0:e8167f37725c | 670 | { |
| jmar7 | 0:e8167f37725c | 671 | // We need to preserve the other bytes in CTRL_REG6_XM. So, first read it: |
| jmar7 | 0:e8167f37725c | 672 | uint8_t temp = mReadByte(CTRL_REG2_M); |
| jmar7 | 0:e8167f37725c | 673 | // Then mask out the mag scale bits: |
| jmar7 | 0:e8167f37725c | 674 | temp &= 0xFF^(0x3 << 5); |
| jmar7 | 0:e8167f37725c | 675 | |
| jmar7 | 0:e8167f37725c | 676 | switch (mScl) |
| jmar7 | 0:e8167f37725c | 677 | { |
| jmar7 | 0:e8167f37725c | 678 | case 8: |
| jmar7 | 0:e8167f37725c | 679 | temp |= (0x1 << 5); |
| jmar7 | 0:e8167f37725c | 680 | settings.mag.scale = 8; |
| jmar7 | 0:e8167f37725c | 681 | break; |
| jmar7 | 0:e8167f37725c | 682 | case 12: |
| jmar7 | 0:e8167f37725c | 683 | temp |= (0x2 << 5); |
| jmar7 | 0:e8167f37725c | 684 | settings.mag.scale = 12; |
| jmar7 | 0:e8167f37725c | 685 | break; |
| jmar7 | 0:e8167f37725c | 686 | case 16: |
| jmar7 | 0:e8167f37725c | 687 | temp |= (0x3 << 5); |
| jmar7 | 0:e8167f37725c | 688 | settings.mag.scale = 16; |
| jmar7 | 0:e8167f37725c | 689 | break; |
| jmar7 | 0:e8167f37725c | 690 | default: // Otherwise we'll default to 4 gauss (00) |
| jmar7 | 0:e8167f37725c | 691 | settings.mag.scale = 4; |
| jmar7 | 0:e8167f37725c | 692 | break; |
| jmar7 | 0:e8167f37725c | 693 | } |
| jmar7 | 0:e8167f37725c | 694 | |
| jmar7 | 0:e8167f37725c | 695 | // And write the new register value back into CTRL_REG6_XM: |
| jmar7 | 0:e8167f37725c | 696 | mWriteByte(CTRL_REG2_M, temp); |
| jmar7 | 0:e8167f37725c | 697 | |
| jmar7 | 0:e8167f37725c | 698 | // We've updated the sensor, but we also need to update our class variables |
| jmar7 | 0:e8167f37725c | 699 | // First update mScale: |
| jmar7 | 0:e8167f37725c | 700 | //mScale = mScl; |
| jmar7 | 0:e8167f37725c | 701 | // Then calculate a new mRes, which relies on mScale being set correctly: |
| jmar7 | 0:e8167f37725c | 702 | calcmRes(); |
| jmar7 | 0:e8167f37725c | 703 | } |
| jmar7 | 0:e8167f37725c | 704 | |
| jmar7 | 0:e8167f37725c | 705 | void LSM9DS1::setGyroODR(uint8_t gRate) |
| jmar7 | 0:e8167f37725c | 706 | { |
| jmar7 | 0:e8167f37725c | 707 | // Only do this if gRate is not 0 (which would disable the gyro) |
| jmar7 | 0:e8167f37725c | 708 | if ((gRate & 0x07) != 0) |
| jmar7 | 0:e8167f37725c | 709 | { |
| jmar7 | 0:e8167f37725c | 710 | // We need to preserve the other bytes in CTRL_REG1_G. So, first read it: |
| jmar7 | 0:e8167f37725c | 711 | uint8_t temp = xgReadByte(CTRL_REG1_G); |
| jmar7 | 0:e8167f37725c | 712 | // Then mask out the gyro ODR bits: |
| jmar7 | 0:e8167f37725c | 713 | temp &= 0xFF^(0x7 << 5); |
| jmar7 | 0:e8167f37725c | 714 | temp |= (gRate & 0x07) << 5; |
| jmar7 | 0:e8167f37725c | 715 | // Update our settings struct |
| jmar7 | 0:e8167f37725c | 716 | settings.gyro.sampleRate = gRate & 0x07; |
| jmar7 | 0:e8167f37725c | 717 | // And write the new register value back into CTRL_REG1_G: |
| jmar7 | 0:e8167f37725c | 718 | xgWriteByte(CTRL_REG1_G, temp); |
| jmar7 | 0:e8167f37725c | 719 | } |
| jmar7 | 0:e8167f37725c | 720 | } |
| jmar7 | 0:e8167f37725c | 721 | |
| jmar7 | 0:e8167f37725c | 722 | void LSM9DS1::setAccelODR(uint8_t aRate) |
| jmar7 | 0:e8167f37725c | 723 | { |
| jmar7 | 0:e8167f37725c | 724 | // Only do this if aRate is not 0 (which would disable the accel) |
| jmar7 | 0:e8167f37725c | 725 | if ((aRate & 0x07) != 0) |
| jmar7 | 0:e8167f37725c | 726 | { |
| jmar7 | 0:e8167f37725c | 727 | // We need to preserve the other bytes in CTRL_REG1_XM. So, first read it: |
| jmar7 | 0:e8167f37725c | 728 | uint8_t temp = xgReadByte(CTRL_REG6_XL); |
| jmar7 | 0:e8167f37725c | 729 | // Then mask out the accel ODR bits: |
| jmar7 | 0:e8167f37725c | 730 | temp &= 0x1F; |
| jmar7 | 0:e8167f37725c | 731 | // Then shift in our new ODR bits: |
| jmar7 | 0:e8167f37725c | 732 | temp |= ((aRate & 0x07) << 5); |
| jmar7 | 0:e8167f37725c | 733 | settings.accel.sampleRate = aRate & 0x07; |
| jmar7 | 0:e8167f37725c | 734 | // And write the new register value back into CTRL_REG1_XM: |
| jmar7 | 0:e8167f37725c | 735 | xgWriteByte(CTRL_REG6_XL, temp); |
| jmar7 | 0:e8167f37725c | 736 | } |
| jmar7 | 0:e8167f37725c | 737 | } |
| jmar7 | 0:e8167f37725c | 738 | |
| jmar7 | 0:e8167f37725c | 739 | void LSM9DS1::setMagODR(uint8_t mRate) |
| jmar7 | 0:e8167f37725c | 740 | { |
| jmar7 | 0:e8167f37725c | 741 | // We need to preserve the other bytes in CTRL_REG5_XM. So, first read it: |
| jmar7 | 0:e8167f37725c | 742 | uint8_t temp = mReadByte(CTRL_REG1_M); |
| jmar7 | 0:e8167f37725c | 743 | // Then mask out the mag ODR bits: |
| jmar7 | 0:e8167f37725c | 744 | temp &= 0xFF^(0x7 << 2); |
| jmar7 | 0:e8167f37725c | 745 | // Then shift in our new ODR bits: |
| jmar7 | 0:e8167f37725c | 746 | temp |= ((mRate & 0x07) << 2); |
| jmar7 | 0:e8167f37725c | 747 | settings.mag.sampleRate = mRate & 0x07; |
| jmar7 | 0:e8167f37725c | 748 | // And write the new register value back into CTRL_REG5_XM: |
| jmar7 | 0:e8167f37725c | 749 | mWriteByte(CTRL_REG1_M, temp); |
| jmar7 | 0:e8167f37725c | 750 | } |
| jmar7 | 0:e8167f37725c | 751 | |
| jmar7 | 0:e8167f37725c | 752 | void LSM9DS1::calcgRes() |
| jmar7 | 0:e8167f37725c | 753 | { |
| jmar7 | 0:e8167f37725c | 754 | gRes = ((float) settings.gyro.scale) / 32768.0; |
| jmar7 | 0:e8167f37725c | 755 | } |
| jmar7 | 0:e8167f37725c | 756 | |
| jmar7 | 0:e8167f37725c | 757 | void LSM9DS1::calcaRes() |
| jmar7 | 0:e8167f37725c | 758 | { |
| jmar7 | 0:e8167f37725c | 759 | aRes = ((float) settings.accel.scale) / 32768.0; |
| jmar7 | 0:e8167f37725c | 760 | } |
| jmar7 | 0:e8167f37725c | 761 | |
| jmar7 | 0:e8167f37725c | 762 | void LSM9DS1::calcmRes() |
| jmar7 | 0:e8167f37725c | 763 | { |
| jmar7 | 0:e8167f37725c | 764 | //mRes = ((float) settings.mag.scale) / 32768.0; |
| jmar7 | 0:e8167f37725c | 765 | switch (settings.mag.scale) |
| jmar7 | 0:e8167f37725c | 766 | { |
| jmar7 | 0:e8167f37725c | 767 | case 4: |
| jmar7 | 0:e8167f37725c | 768 | mRes = magSensitivity[0]; |
| jmar7 | 0:e8167f37725c | 769 | break; |
| jmar7 | 0:e8167f37725c | 770 | case 8: |
| jmar7 | 0:e8167f37725c | 771 | mRes = magSensitivity[1]; |
| jmar7 | 0:e8167f37725c | 772 | break; |
| jmar7 | 0:e8167f37725c | 773 | case 12: |
| jmar7 | 0:e8167f37725c | 774 | mRes = magSensitivity[2]; |
| jmar7 | 0:e8167f37725c | 775 | break; |
| jmar7 | 0:e8167f37725c | 776 | case 16: |
| jmar7 | 0:e8167f37725c | 777 | mRes = magSensitivity[3]; |
| jmar7 | 0:e8167f37725c | 778 | break; |
| jmar7 | 0:e8167f37725c | 779 | } |
| jmar7 | 0:e8167f37725c | 780 | |
| jmar7 | 0:e8167f37725c | 781 | } |
| jmar7 | 0:e8167f37725c | 782 | |
| jmar7 | 0:e8167f37725c | 783 | void LSM9DS1::configInt(interrupt_select interrupt, uint8_t generator, |
| jmar7 | 0:e8167f37725c | 784 | h_lactive activeLow, pp_od pushPull) |
| jmar7 | 0:e8167f37725c | 785 | { |
| jmar7 | 0:e8167f37725c | 786 | // Write to INT1_CTRL or INT2_CTRL. [interupt] should already be one of |
| jmar7 | 0:e8167f37725c | 787 | // those two values. |
| jmar7 | 0:e8167f37725c | 788 | // [generator] should be an OR'd list of values from the interrupt_generators enum |
| jmar7 | 0:e8167f37725c | 789 | xgWriteByte(interrupt, generator); |
| jmar7 | 0:e8167f37725c | 790 | |
| jmar7 | 0:e8167f37725c | 791 | // Configure CTRL_REG8 |
| jmar7 | 0:e8167f37725c | 792 | uint8_t temp; |
| jmar7 | 0:e8167f37725c | 793 | temp = xgReadByte(CTRL_REG8); |
| jmar7 | 0:e8167f37725c | 794 | |
| jmar7 | 0:e8167f37725c | 795 | if (activeLow) temp |= (1<<5); |
| jmar7 | 0:e8167f37725c | 796 | else temp &= ~(1<<5); |
| jmar7 | 0:e8167f37725c | 797 | |
| jmar7 | 0:e8167f37725c | 798 | if (pushPull) temp &= ~(1<<4); |
| jmar7 | 0:e8167f37725c | 799 | else temp |= (1<<4); |
| jmar7 | 0:e8167f37725c | 800 | |
| jmar7 | 0:e8167f37725c | 801 | xgWriteByte(CTRL_REG8, temp); |
| jmar7 | 0:e8167f37725c | 802 | } |
| jmar7 | 0:e8167f37725c | 803 | |
| jmar7 | 0:e8167f37725c | 804 | void LSM9DS1::configInactivity(uint8_t duration, uint8_t threshold, bool sleepOn) |
| jmar7 | 0:e8167f37725c | 805 | { |
| jmar7 | 0:e8167f37725c | 806 | uint8_t temp = 0; |
| jmar7 | 0:e8167f37725c | 807 | |
| jmar7 | 0:e8167f37725c | 808 | temp = threshold & 0x7F; |
| jmar7 | 0:e8167f37725c | 809 | if (sleepOn) temp |= (1<<7); |
| jmar7 | 0:e8167f37725c | 810 | xgWriteByte(ACT_THS, temp); |
| jmar7 | 0:e8167f37725c | 811 | |
| jmar7 | 0:e8167f37725c | 812 | xgWriteByte(ACT_DUR, duration); |
| jmar7 | 0:e8167f37725c | 813 | } |
| jmar7 | 0:e8167f37725c | 814 | |
| jmar7 | 0:e8167f37725c | 815 | uint8_t LSM9DS1::getInactivity() |
| jmar7 | 0:e8167f37725c | 816 | { |
| jmar7 | 0:e8167f37725c | 817 | uint8_t temp = xgReadByte(STATUS_REG_0); |
| jmar7 | 0:e8167f37725c | 818 | temp &= (0x10); |
| jmar7 | 0:e8167f37725c | 819 | return temp; |
| jmar7 | 0:e8167f37725c | 820 | } |
| jmar7 | 0:e8167f37725c | 821 | |
| jmar7 | 0:e8167f37725c | 822 | void LSM9DS1::configAccelInt(uint8_t generator, bool andInterrupts) |
| jmar7 | 0:e8167f37725c | 823 | { |
| jmar7 | 0:e8167f37725c | 824 | // Use variables from accel_interrupt_generator, OR'd together to create |
| jmar7 | 0:e8167f37725c | 825 | // the [generator]value. |
| jmar7 | 0:e8167f37725c | 826 | uint8_t temp = generator; |
| jmar7 | 0:e8167f37725c | 827 | if (andInterrupts) temp |= 0x80; |
| jmar7 | 0:e8167f37725c | 828 | xgWriteByte(INT_GEN_CFG_XL, temp); |
| jmar7 | 0:e8167f37725c | 829 | } |
| jmar7 | 0:e8167f37725c | 830 | |
| jmar7 | 0:e8167f37725c | 831 | void LSM9DS1::configAccelThs(uint8_t threshold, lsm9ds1_axis axis, uint8_t duration, bool wait) |
| jmar7 | 0:e8167f37725c | 832 | { |
| jmar7 | 0:e8167f37725c | 833 | // Write threshold value to INT_GEN_THS_?_XL. |
| jmar7 | 0:e8167f37725c | 834 | // axis will be 0, 1, or 2 (x, y, z respectively) |
| jmar7 | 0:e8167f37725c | 835 | xgWriteByte(INT_GEN_THS_X_XL + axis, threshold); |
| jmar7 | 0:e8167f37725c | 836 | |
| jmar7 | 0:e8167f37725c | 837 | // Write duration and wait to INT_GEN_DUR_XL |
| jmar7 | 0:e8167f37725c | 838 | uint8_t temp; |
| jmar7 | 0:e8167f37725c | 839 | temp = (duration & 0x7F); |
| jmar7 | 0:e8167f37725c | 840 | if (wait) temp |= 0x80; |
| jmar7 | 0:e8167f37725c | 841 | xgWriteByte(INT_GEN_DUR_XL, temp); |
| jmar7 | 0:e8167f37725c | 842 | } |
| jmar7 | 0:e8167f37725c | 843 | |
| jmar7 | 0:e8167f37725c | 844 | uint8_t LSM9DS1::getAccelIntSrc() |
| jmar7 | 0:e8167f37725c | 845 | { |
| jmar7 | 0:e8167f37725c | 846 | uint8_t intSrc = xgReadByte(INT_GEN_SRC_XL); |
| jmar7 | 0:e8167f37725c | 847 | |
| jmar7 | 0:e8167f37725c | 848 | // Check if the IA_XL (interrupt active) bit is set |
| jmar7 | 0:e8167f37725c | 849 | if (intSrc & (1<<6)) |
| jmar7 | 0:e8167f37725c | 850 | { |
| jmar7 | 0:e8167f37725c | 851 | return (intSrc & 0x3F); |
| jmar7 | 0:e8167f37725c | 852 | } |
| jmar7 | 0:e8167f37725c | 853 | |
| jmar7 | 0:e8167f37725c | 854 | return 0; |
| jmar7 | 0:e8167f37725c | 855 | } |
| jmar7 | 0:e8167f37725c | 856 | |
| jmar7 | 0:e8167f37725c | 857 | void LSM9DS1::configGyroInt(uint8_t generator, bool aoi, bool latch) |
| jmar7 | 0:e8167f37725c | 858 | { |
| jmar7 | 0:e8167f37725c | 859 | // Use variables from accel_interrupt_generator, OR'd together to create |
| jmar7 | 0:e8167f37725c | 860 | // the [generator]value. |
| jmar7 | 0:e8167f37725c | 861 | uint8_t temp = generator; |
| jmar7 | 0:e8167f37725c | 862 | if (aoi) temp |= 0x80; |
| jmar7 | 0:e8167f37725c | 863 | if (latch) temp |= 0x40; |
| jmar7 | 0:e8167f37725c | 864 | xgWriteByte(INT_GEN_CFG_G, temp); |
| jmar7 | 0:e8167f37725c | 865 | } |
| jmar7 | 0:e8167f37725c | 866 | |
| jmar7 | 0:e8167f37725c | 867 | void LSM9DS1::configGyroThs(int16_t threshold, lsm9ds1_axis axis, uint8_t duration, bool wait) |
| jmar7 | 0:e8167f37725c | 868 | { |
| jmar7 | 0:e8167f37725c | 869 | uint8_t buffer[2]; |
| jmar7 | 0:e8167f37725c | 870 | buffer[0] = (threshold & 0x7F00) >> 8; |
| jmar7 | 0:e8167f37725c | 871 | buffer[1] = (threshold & 0x00FF); |
| jmar7 | 0:e8167f37725c | 872 | // Write threshold value to INT_GEN_THS_?H_G and INT_GEN_THS_?L_G. |
| jmar7 | 0:e8167f37725c | 873 | // axis will be 0, 1, or 2 (x, y, z respectively) |
| jmar7 | 0:e8167f37725c | 874 | xgWriteByte(INT_GEN_THS_XH_G + (axis * 2), buffer[0]); |
| jmar7 | 0:e8167f37725c | 875 | xgWriteByte(INT_GEN_THS_XH_G + 1 + (axis * 2), buffer[1]); |
| jmar7 | 0:e8167f37725c | 876 | |
| jmar7 | 0:e8167f37725c | 877 | // Write duration and wait to INT_GEN_DUR_XL |
| jmar7 | 0:e8167f37725c | 878 | uint8_t temp; |
| jmar7 | 0:e8167f37725c | 879 | temp = (duration & 0x7F); |
| jmar7 | 0:e8167f37725c | 880 | if (wait) temp |= 0x80; |
| jmar7 | 0:e8167f37725c | 881 | xgWriteByte(INT_GEN_DUR_G, temp); |
| jmar7 | 0:e8167f37725c | 882 | } |
| jmar7 | 0:e8167f37725c | 883 | |
| jmar7 | 0:e8167f37725c | 884 | uint8_t LSM9DS1::getGyroIntSrc() |
| jmar7 | 0:e8167f37725c | 885 | { |
| jmar7 | 0:e8167f37725c | 886 | uint8_t intSrc = xgReadByte(INT_GEN_SRC_G); |
| jmar7 | 0:e8167f37725c | 887 | |
| jmar7 | 0:e8167f37725c | 888 | // Check if the IA_G (interrupt active) bit is set |
| jmar7 | 0:e8167f37725c | 889 | if (intSrc & (1<<6)) |
| jmar7 | 0:e8167f37725c | 890 | { |
| jmar7 | 0:e8167f37725c | 891 | return (intSrc & 0x3F); |
| jmar7 | 0:e8167f37725c | 892 | } |
| jmar7 | 0:e8167f37725c | 893 | |
| jmar7 | 0:e8167f37725c | 894 | return 0; |
| jmar7 | 0:e8167f37725c | 895 | } |
| jmar7 | 0:e8167f37725c | 896 | |
| jmar7 | 0:e8167f37725c | 897 | void LSM9DS1::configMagInt(uint8_t generator, h_lactive activeLow, bool latch) |
| jmar7 | 0:e8167f37725c | 898 | { |
| jmar7 | 0:e8167f37725c | 899 | // Mask out non-generator bits (0-4) |
| jmar7 | 0:e8167f37725c | 900 | uint8_t config = (generator & 0xE0); |
| jmar7 | 0:e8167f37725c | 901 | // IEA bit is 0 for active-low, 1 for active-high. |
| jmar7 | 0:e8167f37725c | 902 | if (activeLow == INT_ACTIVE_HIGH) config |= (1<<2); |
| jmar7 | 0:e8167f37725c | 903 | // IEL bit is 0 for latched, 1 for not-latched |
| jmar7 | 0:e8167f37725c | 904 | if (!latch) config |= (1<<1); |
| jmar7 | 0:e8167f37725c | 905 | // As long as we have at least 1 generator, enable the interrupt |
| jmar7 | 0:e8167f37725c | 906 | if (generator != 0) config |= (1<<0); |
| jmar7 | 0:e8167f37725c | 907 | |
| jmar7 | 0:e8167f37725c | 908 | mWriteByte(INT_CFG_M, config); |
| jmar7 | 0:e8167f37725c | 909 | } |
| jmar7 | 0:e8167f37725c | 910 | |
| jmar7 | 0:e8167f37725c | 911 | void LSM9DS1::configMagThs(uint16_t threshold) |
| jmar7 | 0:e8167f37725c | 912 | { |
| jmar7 | 0:e8167f37725c | 913 | // Write high eight bits of [threshold] to INT_THS_H_M |
| jmar7 | 0:e8167f37725c | 914 | mWriteByte(INT_THS_H_M, uint8_t((threshold & 0x7F00) >> 8)); |
| jmar7 | 0:e8167f37725c | 915 | // Write low eight bits of [threshold] to INT_THS_L_M |
| jmar7 | 0:e8167f37725c | 916 | mWriteByte(INT_THS_L_M, uint8_t(threshold & 0x00FF)); |
| jmar7 | 0:e8167f37725c | 917 | } |
| jmar7 | 0:e8167f37725c | 918 | |
| jmar7 | 0:e8167f37725c | 919 | uint8_t LSM9DS1::getMagIntSrc() |
| jmar7 | 0:e8167f37725c | 920 | { |
| jmar7 | 0:e8167f37725c | 921 | uint8_t intSrc = mReadByte(INT_SRC_M); |
| jmar7 | 0:e8167f37725c | 922 | |
| jmar7 | 0:e8167f37725c | 923 | // Check if the INT (interrupt active) bit is set |
| jmar7 | 0:e8167f37725c | 924 | if (intSrc & (1<<0)) |
| jmar7 | 0:e8167f37725c | 925 | { |
| jmar7 | 0:e8167f37725c | 926 | return (intSrc & 0xFE); |
| jmar7 | 0:e8167f37725c | 927 | } |
| jmar7 | 0:e8167f37725c | 928 | |
| jmar7 | 0:e8167f37725c | 929 | return 0; |
| jmar7 | 0:e8167f37725c | 930 | } |
| jmar7 | 0:e8167f37725c | 931 | |
| jmar7 | 0:e8167f37725c | 932 | void LSM9DS1::sleepGyro(bool enable) |
| jmar7 | 0:e8167f37725c | 933 | { |
| jmar7 | 0:e8167f37725c | 934 | uint8_t temp = xgReadByte(CTRL_REG9); |
| jmar7 | 0:e8167f37725c | 935 | if (enable) temp |= (1<<6); |
| jmar7 | 0:e8167f37725c | 936 | else temp &= ~(1<<6); |
| jmar7 | 0:e8167f37725c | 937 | xgWriteByte(CTRL_REG9, temp); |
| jmar7 | 0:e8167f37725c | 938 | } |
| jmar7 | 0:e8167f37725c | 939 | |
| jmar7 | 0:e8167f37725c | 940 | void LSM9DS1::enableFIFO(bool enable) |
| jmar7 | 0:e8167f37725c | 941 | { |
| jmar7 | 0:e8167f37725c | 942 | uint8_t temp = xgReadByte(CTRL_REG9); |
| jmar7 | 0:e8167f37725c | 943 | if (enable) temp |= (1<<1); |
| jmar7 | 0:e8167f37725c | 944 | else temp &= ~(1<<1); |
| jmar7 | 0:e8167f37725c | 945 | xgWriteByte(CTRL_REG9, temp); |
| jmar7 | 0:e8167f37725c | 946 | } |
| jmar7 | 0:e8167f37725c | 947 | |
| jmar7 | 0:e8167f37725c | 948 | void LSM9DS1::setFIFO(fifoMode_type fifoMode, uint8_t fifoThs) |
| jmar7 | 0:e8167f37725c | 949 | { |
| jmar7 | 0:e8167f37725c | 950 | // Limit threshold - 0x1F (31) is the maximum. If more than that was asked |
| jmar7 | 0:e8167f37725c | 951 | // limit it to the maximum. |
| jmar7 | 0:e8167f37725c | 952 | uint8_t threshold = fifoThs <= 0x1F ? fifoThs : 0x1F; |
| jmar7 | 0:e8167f37725c | 953 | xgWriteByte(FIFO_CTRL, ((fifoMode & 0x7) << 5) | (threshold & 0x1F)); |
| jmar7 | 0:e8167f37725c | 954 | } |
| jmar7 | 0:e8167f37725c | 955 | |
| jmar7 | 0:e8167f37725c | 956 | uint8_t LSM9DS1::getFIFOSamples() |
| jmar7 | 0:e8167f37725c | 957 | { |
| jmar7 | 0:e8167f37725c | 958 | return (xgReadByte(FIFO_SRC) & 0x3F); |
| jmar7 | 0:e8167f37725c | 959 | } |
| jmar7 | 0:e8167f37725c | 960 | |
| jmar7 | 0:e8167f37725c | 961 | void LSM9DS1::constrainScales() |
| jmar7 | 0:e8167f37725c | 962 | { |
| jmar7 | 0:e8167f37725c | 963 | if ((settings.gyro.scale != 245) && (settings.gyro.scale != 500) && |
| jmar7 | 0:e8167f37725c | 964 | (settings.gyro.scale != 2000)) |
| jmar7 | 0:e8167f37725c | 965 | { |
| jmar7 | 0:e8167f37725c | 966 | settings.gyro.scale = 245; |
| jmar7 | 0:e8167f37725c | 967 | } |
| jmar7 | 0:e8167f37725c | 968 | |
| jmar7 | 0:e8167f37725c | 969 | if ((settings.accel.scale != 2) && (settings.accel.scale != 4) && |
| jmar7 | 0:e8167f37725c | 970 | (settings.accel.scale != 8) && (settings.accel.scale != 16)) |
| jmar7 | 0:e8167f37725c | 971 | { |
| jmar7 | 0:e8167f37725c | 972 | settings.accel.scale = 2; |
| jmar7 | 0:e8167f37725c | 973 | } |
| jmar7 | 0:e8167f37725c | 974 | |
| jmar7 | 0:e8167f37725c | 975 | if ((settings.mag.scale != 4) && (settings.mag.scale != 8) && |
| jmar7 | 0:e8167f37725c | 976 | (settings.mag.scale != 12) && (settings.mag.scale != 16)) |
| jmar7 | 0:e8167f37725c | 977 | { |
| jmar7 | 0:e8167f37725c | 978 | settings.mag.scale = 4; |
| jmar7 | 0:e8167f37725c | 979 | } |
| jmar7 | 0:e8167f37725c | 980 | } |
| jmar7 | 0:e8167f37725c | 981 | |
| jmar7 | 0:e8167f37725c | 982 | void LSM9DS1::xgWriteByte(uint8_t subAddress, uint8_t data) |
| jmar7 | 0:e8167f37725c | 983 | { |
| jmar7 | 0:e8167f37725c | 984 | // Whether we're using I2C or SPI, write a byte using the |
| jmar7 | 0:e8167f37725c | 985 | // gyro-specific I2C address or SPI CS pin. |
| jmar7 | 0:e8167f37725c | 986 | if (settings.device.commInterface == IMU_MODE_I2C) { |
| jmar7 | 0:e8167f37725c | 987 | printf("yo"); |
| jmar7 | 0:e8167f37725c | 988 | I2CwriteByte(_xgAddress, subAddress, data); |
| jmar7 | 0:e8167f37725c | 989 | } else if (settings.device.commInterface == IMU_MODE_SPI) { |
| jmar7 | 0:e8167f37725c | 990 | SPIwriteByte(_xgAddress, subAddress, data); |
| jmar7 | 0:e8167f37725c | 991 | } |
| jmar7 | 0:e8167f37725c | 992 | } |
| jmar7 | 0:e8167f37725c | 993 | |
| jmar7 | 0:e8167f37725c | 994 | void LSM9DS1::mWriteByte(uint8_t subAddress, uint8_t data) |
| jmar7 | 0:e8167f37725c | 995 | { |
| jmar7 | 0:e8167f37725c | 996 | // Whether we're using I2C or SPI, write a byte using the |
| jmar7 | 0:e8167f37725c | 997 | // accelerometer-specific I2C address or SPI CS pin. |
| jmar7 | 0:e8167f37725c | 998 | if (settings.device.commInterface == IMU_MODE_I2C) |
| jmar7 | 0:e8167f37725c | 999 | return I2CwriteByte(_mAddress, subAddress, data); |
| jmar7 | 0:e8167f37725c | 1000 | else if (settings.device.commInterface == IMU_MODE_SPI) |
| jmar7 | 0:e8167f37725c | 1001 | return SPIwriteByte(_mAddress, subAddress, data); |
| jmar7 | 0:e8167f37725c | 1002 | } |
| jmar7 | 0:e8167f37725c | 1003 | |
| jmar7 | 0:e8167f37725c | 1004 | uint8_t LSM9DS1::xgReadByte(uint8_t subAddress) |
| jmar7 | 0:e8167f37725c | 1005 | { |
| jmar7 | 0:e8167f37725c | 1006 | // Whether we're using I2C or SPI, read a byte using the |
| jmar7 | 0:e8167f37725c | 1007 | // gyro-specific I2C address or SPI CS pin. |
| jmar7 | 0:e8167f37725c | 1008 | if (settings.device.commInterface == IMU_MODE_I2C) |
| jmar7 | 0:e8167f37725c | 1009 | return I2CreadByte(_xgAddress, subAddress); |
| jmar7 | 0:e8167f37725c | 1010 | else if (settings.device.commInterface == IMU_MODE_SPI) |
| jmar7 | 0:e8167f37725c | 1011 | return SPIreadByte(_xgAddress, subAddress); |
| jmar7 | 0:e8167f37725c | 1012 | } |
| jmar7 | 0:e8167f37725c | 1013 | |
| jmar7 | 0:e8167f37725c | 1014 | void LSM9DS1::xgReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count) |
| jmar7 | 0:e8167f37725c | 1015 | { |
| jmar7 | 0:e8167f37725c | 1016 | // Whether we're using I2C or SPI, read multiple bytes using the |
| jmar7 | 0:e8167f37725c | 1017 | // gyro-specific I2C address or SPI CS pin. |
| jmar7 | 0:e8167f37725c | 1018 | if (settings.device.commInterface == IMU_MODE_I2C) { |
| jmar7 | 0:e8167f37725c | 1019 | I2CreadBytes(_xgAddress, subAddress, dest, count); |
| jmar7 | 0:e8167f37725c | 1020 | } else if (settings.device.commInterface == IMU_MODE_SPI) { |
| jmar7 | 0:e8167f37725c | 1021 | SPIreadBytes(_xgAddress, subAddress, dest, count); |
| jmar7 | 0:e8167f37725c | 1022 | } |
| jmar7 | 0:e8167f37725c | 1023 | } |
| jmar7 | 0:e8167f37725c | 1024 | |
| jmar7 | 0:e8167f37725c | 1025 | uint8_t LSM9DS1::mReadByte(uint8_t subAddress) |
| jmar7 | 0:e8167f37725c | 1026 | { |
| jmar7 | 0:e8167f37725c | 1027 | // Whether we're using I2C or SPI, read a byte using the |
| jmar7 | 0:e8167f37725c | 1028 | // accelerometer-specific I2C address or SPI CS pin. |
| jmar7 | 0:e8167f37725c | 1029 | if (settings.device.commInterface == IMU_MODE_I2C) |
| jmar7 | 0:e8167f37725c | 1030 | return I2CreadByte(_mAddress, subAddress); |
| jmar7 | 0:e8167f37725c | 1031 | else if (settings.device.commInterface == IMU_MODE_SPI) |
| jmar7 | 0:e8167f37725c | 1032 | return SPIreadByte(_mAddress, subAddress); |
| jmar7 | 0:e8167f37725c | 1033 | } |
| jmar7 | 0:e8167f37725c | 1034 | |
| jmar7 | 0:e8167f37725c | 1035 | void LSM9DS1::mReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count) |
| jmar7 | 0:e8167f37725c | 1036 | { |
| jmar7 | 0:e8167f37725c | 1037 | // Whether we're using I2C or SPI, read multiple bytes using the |
| jmar7 | 0:e8167f37725c | 1038 | // accelerometer-specific I2C address or SPI CS pin. |
| jmar7 | 0:e8167f37725c | 1039 | if (settings.device.commInterface == IMU_MODE_I2C) |
| jmar7 | 0:e8167f37725c | 1040 | I2CreadBytes(_mAddress, subAddress, dest, count); |
| jmar7 | 0:e8167f37725c | 1041 | else if (settings.device.commInterface == IMU_MODE_SPI) |
| jmar7 | 0:e8167f37725c | 1042 | SPIreadBytes(_mAddress, subAddress, dest, count); |
| jmar7 | 0:e8167f37725c | 1043 | } |
| jmar7 | 0:e8167f37725c | 1044 | |
| jmar7 | 0:e8167f37725c | 1045 | void LSM9DS1::initSPI() |
| jmar7 | 0:e8167f37725c | 1046 | { |
| jmar7 | 0:e8167f37725c | 1047 | /* cw |
| jmar7 | 0:e8167f37725c | 1048 | pinMode(_xgAddress, OUTPUT); |
| jmar7 | 0:e8167f37725c | 1049 | digitalWrite(_xgAddress, HIGH); |
| jmar7 | 0:e8167f37725c | 1050 | pinMode(_mAddress, OUTPUT); |
| jmar7 | 0:e8167f37725c | 1051 | digitalWrite(_mAddress, HIGH); |
| jmar7 | 0:e8167f37725c | 1052 | |
| jmar7 | 0:e8167f37725c | 1053 | SPI.begin(); |
| jmar7 | 0:e8167f37725c | 1054 | // Maximum SPI frequency is 10MHz, could divide by 2 here: |
| jmar7 | 0:e8167f37725c | 1055 | SPI.setClockDivider(SPI_CLOCK_DIV2); |
| jmar7 | 0:e8167f37725c | 1056 | // Data is read and written MSb first. |
| jmar7 | 0:e8167f37725c | 1057 | SPI.setBitOrder(MSBFIRST); |
| jmar7 | 0:e8167f37725c | 1058 | // Data is captured on rising edge of clock (CPHA = 0) |
| jmar7 | 0:e8167f37725c | 1059 | // Base value of the clock is HIGH (CPOL = 1) |
| jmar7 | 0:e8167f37725c | 1060 | SPI.setDataMode(SPI_MODE0); |
| jmar7 | 0:e8167f37725c | 1061 | */ |
| jmar7 | 0:e8167f37725c | 1062 | } |
| jmar7 | 0:e8167f37725c | 1063 | |
| jmar7 | 0:e8167f37725c | 1064 | void LSM9DS1::SPIwriteByte(uint8_t csPin, uint8_t subAddress, uint8_t data) |
| jmar7 | 0:e8167f37725c | 1065 | { |
| jmar7 | 0:e8167f37725c | 1066 | /*cw |
| jmar7 | 0:e8167f37725c | 1067 | digitalWrite(csPin, LOW); // Initiate communication |
| jmar7 | 0:e8167f37725c | 1068 | |
| jmar7 | 0:e8167f37725c | 1069 | // If write, bit 0 (MSB) should be 0 |
| jmar7 | 0:e8167f37725c | 1070 | // If single write, bit 1 should be 0 |
| jmar7 | 0:e8167f37725c | 1071 | SPI.transfer(subAddress & 0x3F); // Send Address |
| jmar7 | 0:e8167f37725c | 1072 | SPI.transfer(data); // Send data |
| jmar7 | 0:e8167f37725c | 1073 | |
| jmar7 | 0:e8167f37725c | 1074 | digitalWrite(csPin, HIGH); // Close communication |
| jmar7 | 0:e8167f37725c | 1075 | */ |
| jmar7 | 0:e8167f37725c | 1076 | } |
| jmar7 | 0:e8167f37725c | 1077 | |
| jmar7 | 0:e8167f37725c | 1078 | uint8_t LSM9DS1::SPIreadByte(uint8_t csPin, uint8_t subAddress) |
| jmar7 | 0:e8167f37725c | 1079 | { |
| jmar7 | 0:e8167f37725c | 1080 | uint8_t temp; |
| jmar7 | 0:e8167f37725c | 1081 | // Use the multiple read function to read 1 byte. |
| jmar7 | 0:e8167f37725c | 1082 | // Value is returned to `temp`. |
| jmar7 | 0:e8167f37725c | 1083 | SPIreadBytes(csPin, subAddress, &temp, 1); |
| jmar7 | 0:e8167f37725c | 1084 | return temp; |
| jmar7 | 0:e8167f37725c | 1085 | } |
| jmar7 | 0:e8167f37725c | 1086 | |
| jmar7 | 0:e8167f37725c | 1087 | void LSM9DS1::SPIreadBytes(uint8_t csPin, uint8_t subAddress, |
| jmar7 | 0:e8167f37725c | 1088 | uint8_t * dest, uint8_t count) |
| jmar7 | 0:e8167f37725c | 1089 | { |
| jmar7 | 0:e8167f37725c | 1090 | // To indicate a read, set bit 0 (msb) of first byte to 1 |
| jmar7 | 0:e8167f37725c | 1091 | uint8_t rAddress = 0x80 | (subAddress & 0x3F); |
| jmar7 | 0:e8167f37725c | 1092 | // Mag SPI port is different. If we're reading multiple bytes, |
| jmar7 | 0:e8167f37725c | 1093 | // set bit 1 to 1. The remaining six bytes are the address to be read |
| jmar7 | 0:e8167f37725c | 1094 | if ((csPin == _mAddress) && count > 1) |
| jmar7 | 0:e8167f37725c | 1095 | rAddress |= 0x40; |
| jmar7 | 0:e8167f37725c | 1096 | |
| jmar7 | 0:e8167f37725c | 1097 | /* cw |
| jmar7 | 0:e8167f37725c | 1098 | digitalWrite(csPin, LOW); // Initiate communication |
| jmar7 | 0:e8167f37725c | 1099 | SPI.transfer(rAddress); |
| jmar7 | 0:e8167f37725c | 1100 | for (int i=0; i<count; i++) |
| jmar7 | 0:e8167f37725c | 1101 | { |
| jmar7 | 0:e8167f37725c | 1102 | dest[i] = SPI.transfer(0x00); // Read into destination array |
| jmar7 | 0:e8167f37725c | 1103 | } |
| jmar7 | 0:e8167f37725c | 1104 | digitalWrite(csPin, HIGH); // Close communication |
| jmar7 | 0:e8167f37725c | 1105 | */ |
| jmar7 | 0:e8167f37725c | 1106 | } |
| jmar7 | 0:e8167f37725c | 1107 | |
| jmar7 | 0:e8167f37725c | 1108 | void LSM9DS1::initI2C() |
| jmar7 | 0:e8167f37725c | 1109 | { |
| jmar7 | 0:e8167f37725c | 1110 | /* cw |
| jmar7 | 0:e8167f37725c | 1111 | Wire.begin(); // Initialize I2C library |
| jmar7 | 0:e8167f37725c | 1112 | */ |
| jmar7 | 0:e8167f37725c | 1113 | |
| jmar7 | 0:e8167f37725c | 1114 | //already initialized in constructor! |
| jmar7 | 0:e8167f37725c | 1115 | } |
| jmar7 | 0:e8167f37725c | 1116 | |
| jmar7 | 0:e8167f37725c | 1117 | // Wire.h read and write protocols |
| jmar7 | 0:e8167f37725c | 1118 | void LSM9DS1::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data) |
| jmar7 | 0:e8167f37725c | 1119 | { |
| jmar7 | 0:e8167f37725c | 1120 | /* cw |
| jmar7 | 0:e8167f37725c | 1121 | Wire.beginTransmission(address); // Initialize the Tx buffer |
| jmar7 | 0:e8167f37725c | 1122 | Wire.write(subAddress); // Put slave register address in Tx buffer |
| jmar7 | 0:e8167f37725c | 1123 | Wire.write(data); // Put data in Tx buffer |
| jmar7 | 0:e8167f37725c | 1124 | Wire.endTransmission(); // Send the Tx buffer |
| jmar7 | 0:e8167f37725c | 1125 | */ |
| jmar7 | 0:e8167f37725c | 1126 | char temp_data[2] = {subAddress, data}; |
| jmar7 | 0:e8167f37725c | 1127 | i2c.write(address, temp_data, 2); |
| jmar7 | 0:e8167f37725c | 1128 | } |
| jmar7 | 0:e8167f37725c | 1129 | |
| jmar7 | 0:e8167f37725c | 1130 | uint8_t LSM9DS1::I2CreadByte(uint8_t address, uint8_t subAddress) |
| jmar7 | 0:e8167f37725c | 1131 | { |
| jmar7 | 0:e8167f37725c | 1132 | /* cw |
| jmar7 | 0:e8167f37725c | 1133 | int timeout = LSM9DS1_COMMUNICATION_TIMEOUT; |
| jmar7 | 0:e8167f37725c | 1134 | uint8_t data; // `data` will store the register data |
| jmar7 | 0:e8167f37725c | 1135 | |
| jmar7 | 0:e8167f37725c | 1136 | Wire.beginTransmission(address); // Initialize the Tx buffer |
| jmar7 | 0:e8167f37725c | 1137 | Wire.write(subAddress); // Put slave register address in Tx buffer |
| jmar7 | 0:e8167f37725c | 1138 | Wire.endTransmission(true); // Send the Tx buffer, but send a restart to keep connection alive |
| jmar7 | 0:e8167f37725c | 1139 | Wire.requestFrom(address, (uint8_t) 1); // Read one byte from slave register address |
| jmar7 | 0:e8167f37725c | 1140 | while ((Wire.available() < 1) && (timeout-- > 0)) |
| jmar7 | 0:e8167f37725c | 1141 | delay(1); |
| jmar7 | 0:e8167f37725c | 1142 | |
| jmar7 | 0:e8167f37725c | 1143 | if (timeout <= 0) |
| jmar7 | 0:e8167f37725c | 1144 | return 255; //! Bad! 255 will be misinterpreted as a good value. |
| jmar7 | 0:e8167f37725c | 1145 | |
| jmar7 | 0:e8167f37725c | 1146 | data = Wire.read(); // Fill Rx buffer with result |
| jmar7 | 0:e8167f37725c | 1147 | return data; // Return data read from slave register |
| jmar7 | 0:e8167f37725c | 1148 | */ |
| jmar7 | 0:e8167f37725c | 1149 | char data; |
| jmar7 | 0:e8167f37725c | 1150 | char temp[1] = {subAddress}; |
| jmar7 | 0:e8167f37725c | 1151 | |
| jmar7 | 0:e8167f37725c | 1152 | i2c.write(address, temp, 1); |
| jmar7 | 0:e8167f37725c | 1153 | //i2c.write(address & 0xFE); |
| jmar7 | 0:e8167f37725c | 1154 | temp[1] = 0x00; |
| jmar7 | 0:e8167f37725c | 1155 | i2c.write(address, temp, 1); |
| jmar7 | 0:e8167f37725c | 1156 | //i2c.write( address | 0x01); |
| jmar7 | 0:e8167f37725c | 1157 | int a = i2c.read(address, &data, 1); |
| jmar7 | 0:e8167f37725c | 1158 | return data; |
| jmar7 | 0:e8167f37725c | 1159 | } |
| jmar7 | 0:e8167f37725c | 1160 | |
| jmar7 | 0:e8167f37725c | 1161 | uint8_t LSM9DS1::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count) |
| jmar7 | 0:e8167f37725c | 1162 | { |
| jmar7 | 0:e8167f37725c | 1163 | /* cw |
| jmar7 | 0:e8167f37725c | 1164 | int timeout = LSM9DS1_COMMUNICATION_TIMEOUT; |
| jmar7 | 0:e8167f37725c | 1165 | Wire.beginTransmission(address); // Initialize the Tx buffer |
| jmar7 | 0:e8167f37725c | 1166 | // Next send the register to be read. OR with 0x80 to indicate multi-read. |
| jmar7 | 0:e8167f37725c | 1167 | Wire.write(subAddress | 0x80); // Put slave register address in Tx buffer |
| jmar7 | 0:e8167f37725c | 1168 | |
| jmar7 | 0:e8167f37725c | 1169 | Wire.endTransmission(true); // Send the Tx buffer, but send a restart to keep connection alive |
| jmar7 | 0:e8167f37725c | 1170 | uint8_t i = 0; |
| jmar7 | 0:e8167f37725c | 1171 | Wire.requestFrom(address, count); // Read bytes from slave register address |
| jmar7 | 0:e8167f37725c | 1172 | while ((Wire.available() < count) && (timeout-- > 0)) |
| jmar7 | 0:e8167f37725c | 1173 | delay(1); |
| jmar7 | 0:e8167f37725c | 1174 | if (timeout <= 0) |
| jmar7 | 0:e8167f37725c | 1175 | return -1; |
| jmar7 | 0:e8167f37725c | 1176 | |
| jmar7 | 0:e8167f37725c | 1177 | for (int i=0; i<count;) |
| jmar7 | 0:e8167f37725c | 1178 | { |
| jmar7 | 0:e8167f37725c | 1179 | if (Wire.available()) |
| jmar7 | 0:e8167f37725c | 1180 | { |
| jmar7 | 0:e8167f37725c | 1181 | dest[i++] = Wire.read(); |
| jmar7 | 0:e8167f37725c | 1182 | } |
| jmar7 | 0:e8167f37725c | 1183 | } |
| jmar7 | 0:e8167f37725c | 1184 | return count; |
| jmar7 | 0:e8167f37725c | 1185 | */ |
| jmar7 | 0:e8167f37725c | 1186 | int i; |
| jmar7 | 0:e8167f37725c | 1187 | char temp_dest[count]; |
| jmar7 | 0:e8167f37725c | 1188 | char temp[1] = {subAddress}; |
| jmar7 | 0:e8167f37725c | 1189 | i2c.write(address, temp, 1); |
| jmar7 | 0:e8167f37725c | 1190 | i2c.read(address, temp_dest, count); |
| jmar7 | 0:e8167f37725c | 1191 | |
| jmar7 | 0:e8167f37725c | 1192 | //i2c doesn't take uint8_ts, but rather chars so do this nasty af conversion |
| jmar7 | 0:e8167f37725c | 1193 | for (i=0; i < count; i++) { |
| jmar7 | 0:e8167f37725c | 1194 | dest[i] = temp_dest[i]; |
| jmar7 | 0:e8167f37725c | 1195 | } |
| jmar7 | 0:e8167f37725c | 1196 | return count; |
| jmar7 | 0:e8167f37725c | 1197 | } |