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