AdvRobot_Team / LSM9DS1_ADVRO

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.

Dependents:   LSM9DS1_SPI

LSM9DS1.cpp@1:c2804ae02c80, 2017-06-21 (annotated)

Committer:
ChangYuHsuan
Date:
Wed Jun 21 09:06:09 2017 +0000
Revision:
1:c2804ae02c80
Parent:
0:50166e25abb5 
LSM9DS1 SPI ver 1.01;

Who changed what in which revision?

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