Naoya Muramatsu / Mbed 2 deprecated LSM9DS0

publish

Dependencies:   mbed

Dependents:   RoboCup_2015

Fork of LSM9DS0 by Taylor Andrews

LSM9DS0.cpp@0:1b975a6ae539, 2014-10-21 (annotated)

Committer:
randrews33
Date:
Tue Oct 21 18:11:45 2014 +0000
Revision:
0:1b975a6ae539
Child:
5:bf8f4e7c9905
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
randrews33 0:1b975a6ae539 1 #include "LSM9DS0.h"
randrews33 0:1b975a6ae539 2
randrews33 0:1b975a6ae539 3 LSM9DS0::LSM9DS0(PinName sda, PinName scl, uint8_t gAddr, uint8_t xmAddr)
randrews33 0:1b975a6ae539 4 {
randrews33 0:1b975a6ae539 5
randrews33 0:1b975a6ae539 6 // xmAddress and gAddress will store the 7-bit I2C address, if using I2C.
randrews33 0:1b975a6ae539 7 // If we're using SPI, these variables store the chip-select pins.
randrews33 0:1b975a6ae539 8 xmAddress = xmAddr;
randrews33 0:1b975a6ae539 9 gAddress = gAddr;
randrews33 0:1b975a6ae539 10
randrews33 0:1b975a6ae539 11 i2c_ = new I2Cdev(sda, scl);
randrews33 0:1b975a6ae539 12 //100KHz, as specified by the datasheet.
randrews33 0:1b975a6ae539 13 //i2c_->frequency(100000);
randrews33 0:1b975a6ae539 14 }
randrews33 0:1b975a6ae539 15
randrews33 0:1b975a6ae539 16 uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl,
randrews33 0:1b975a6ae539 17 gyro_odr gODR, accel_odr aODR, mag_odr mODR)
randrews33 0:1b975a6ae539 18 {
randrews33 0:1b975a6ae539 19 // Store the given scales in class variables. These scale variables
randrews33 0:1b975a6ae539 20 // are used throughout to calculate the actual g's, DPS,and Gs's.
randrews33 0:1b975a6ae539 21 gScale = gScl;
randrews33 0:1b975a6ae539 22 aScale = aScl;
randrews33 0:1b975a6ae539 23 mScale = mScl;
randrews33 0:1b975a6ae539 24
randrews33 0:1b975a6ae539 25 // Once we have the scale values, we can calculate the resolution
randrews33 0:1b975a6ae539 26 // of each sensor. That's what these functions are for. One for each sensor
randrews33 0:1b975a6ae539 27 calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
randrews33 0:1b975a6ae539 28 calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
randrews33 0:1b975a6ae539 29 calcaRes(); // Calculate g / ADC tick, stored in aRes variable
randrews33 0:1b975a6ae539 30
randrews33 0:1b975a6ae539 31
randrews33 0:1b975a6ae539 32 // To verify communication, we can read from the WHO_AM_I register of
randrews33 0:1b975a6ae539 33 // each device. Store those in a variable so we can return them.
randrews33 0:1b975a6ae539 34 uint8_t gTest = gReadByte(WHO_AM_I_G); // Read the gyro WHO_AM_I
randrews33 0:1b975a6ae539 35 uint8_t xmTest = xmReadByte(WHO_AM_I_XM); // Read the accel/mag WHO_AM_I
randrews33 0:1b975a6ae539 36
randrews33 0:1b975a6ae539 37 // Gyro initialization stuff:
randrews33 0:1b975a6ae539 38 initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
randrews33 0:1b975a6ae539 39 setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
randrews33 0:1b975a6ae539 40 setGyroScale(gScale); // Set the gyro range
randrews33 0:1b975a6ae539 41
randrews33 0:1b975a6ae539 42 // Accelerometer initialization stuff:
randrews33 0:1b975a6ae539 43 initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
randrews33 0:1b975a6ae539 44 // setAccelODR(aODR); // Set the accel data rate.
randrews33 0:1b975a6ae539 45 //setAccelScale(aScale); // Set the accel range.
randrews33 0:1b975a6ae539 46
randrews33 0:1b975a6ae539 47 // Magnetometer initialization stuff:
randrews33 0:1b975a6ae539 48 initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
randrews33 0:1b975a6ae539 49 setMagODR(mODR); // Set the magnetometer output data rate.
randrews33 0:1b975a6ae539 50 setMagScale(mScale); // Set the magnetometer's range.
randrews33 0:1b975a6ae539 51
randrews33 0:1b975a6ae539 52 // Once everything is initialized, return the WHO_AM_I registers we read:
randrews33 0:1b975a6ae539 53 return (xmTest << 8) | gTest;
randrews33 0:1b975a6ae539 54 }
randrews33 0:1b975a6ae539 55
randrews33 0:1b975a6ae539 56 void LSM9DS0::initGyro()
randrews33 0:1b975a6ae539 57 {
randrews33 0:1b975a6ae539 58 /* CTRL_REG1_G sets output data rate, bandwidth, power-down and enables
randrews33 0:1b975a6ae539 59 Bits[7:0]: DR1 DR0 BW1 BW0 PD Zen Xen Yen
randrews33 0:1b975a6ae539 60 DR[1:0] - Output data rate selection
randrews33 0:1b975a6ae539 61 00=95Hz, 01=190Hz, 10=380Hz, 11=760Hz
randrews33 0:1b975a6ae539 62 BW[1:0] - Bandwidth selection (sets cutoff frequency)
randrews33 0:1b975a6ae539 63 Value depends on ODR. See datasheet table 21.
randrews33 0:1b975a6ae539 64 PD - Power down enable (0=power down mode, 1=normal or sleep mode)
randrews33 0:1b975a6ae539 65 Zen, Xen, Yen - Axis enable (o=disabled, 1=enabled) */
randrews33 0:1b975a6ae539 66 gWriteByte(CTRL_REG1_G, 0x0F); // Normal mode, enable all axes
randrews33 0:1b975a6ae539 67
randrews33 0:1b975a6ae539 68 /* CTRL_REG2_G sets up the HPF
randrews33 0:1b975a6ae539 69 Bits[7:0]: 0 0 HPM1 HPM0 HPCF3 HPCF2 HPCF1 HPCF0
randrews33 0:1b975a6ae539 70 HPM[1:0] - High pass filter mode selection
randrews33 0:1b975a6ae539 71 00=normal (reset reading HP_RESET_FILTER, 01=ref signal for filtering,
randrews33 0:1b975a6ae539 72 10=normal, 11=autoreset on interrupt
randrews33 0:1b975a6ae539 73 HPCF[3:0] - High pass filter cutoff frequency
randrews33 0:1b975a6ae539 74 Value depends on data rate. See datasheet table 26.
randrews33 0:1b975a6ae539 75 */
randrews33 0:1b975a6ae539 76 gWriteByte(CTRL_REG2_G, 0x00); // Normal mode, high cutoff frequency
randrews33 0:1b975a6ae539 77
randrews33 0:1b975a6ae539 78 /* CTRL_REG3_G sets up interrupt and DRDY_G pins
randrews33 0:1b975a6ae539 79 Bits[7:0]: I1_IINT1 I1_BOOT H_LACTIVE PP_OD I2_DRDY I2_WTM I2_ORUN I2_EMPTY
randrews33 0:1b975a6ae539 80 I1_INT1 - Interrupt enable on INT_G pin (0=disable, 1=enable)
randrews33 0:1b975a6ae539 81 I1_BOOT - Boot status available on INT_G (0=disable, 1=enable)
randrews33 0:1b975a6ae539 82 H_LACTIVE - Interrupt active configuration on INT_G (0:high, 1:low)
randrews33 0:1b975a6ae539 83 PP_OD - Push-pull/open-drain (0=push-pull, 1=open-drain)
randrews33 0:1b975a6ae539 84 I2_DRDY - Data ready on DRDY_G (0=disable, 1=enable)
randrews33 0:1b975a6ae539 85 I2_WTM - FIFO watermark interrupt on DRDY_G (0=disable 1=enable)
randrews33 0:1b975a6ae539 86 I2_ORUN - FIFO overrun interrupt on DRDY_G (0=disable 1=enable)
randrews33 0:1b975a6ae539 87 I2_EMPTY - FIFO empty interrupt on DRDY_G (0=disable 1=enable) */
randrews33 0:1b975a6ae539 88 // Int1 enabled (pp, active low), data read on DRDY_G:
randrews33 0:1b975a6ae539 89 //gWriteByte(CTRL_REG3_G, 0x88);
randrews33 0:1b975a6ae539 90
randrews33 0:1b975a6ae539 91 /* CTRL_REG4_G sets the scale, update mode
randrews33 0:1b975a6ae539 92 Bits[7:0] - BDU BLE FS1 FS0 - ST1 ST0 SIM
randrews33 0:1b975a6ae539 93 BDU - Block data update (0=continuous, 1=output not updated until read
randrews33 0:1b975a6ae539 94 BLE - Big/little endian (0=data LSB @ lower address, 1=LSB @ higher add)
randrews33 0:1b975a6ae539 95 FS[1:0] - Full-scale selection
randrews33 0:1b975a6ae539 96 00=245dps, 01=500dps, 10=2000dps, 11=2000dps
randrews33 0:1b975a6ae539 97 ST[1:0] - Self-test enable
randrews33 0:1b975a6ae539 98 00=disabled, 01=st 0 (x+, y-, z-), 10=undefined, 11=st 1 (x-, y+, z+)
randrews33 0:1b975a6ae539 99 SIM - SPI serial interface mode select
randrews33 0:1b975a6ae539 100 0=4 wire, 1=3 wire */
randrews33 0:1b975a6ae539 101 gWriteByte(CTRL_REG4_G, 0x00); // Set scale to 245 dps
randrews33 0:1b975a6ae539 102
randrews33 0:1b975a6ae539 103 /* CTRL_REG5_G sets up the FIFO, HPF, and INT1
randrews33 0:1b975a6ae539 104 Bits[7:0] - BOOT FIFO_EN - HPen INT1_Sel1 INT1_Sel0 Out_Sel1 Out_Sel0
randrews33 0:1b975a6ae539 105 BOOT - Reboot memory content (0=normal, 1=reboot)
randrews33 0:1b975a6ae539 106 FIFO_EN - FIFO enable (0=disable, 1=enable)
randrews33 0:1b975a6ae539 107 HPen - HPF enable (0=disable, 1=enable)
randrews33 0:1b975a6ae539 108 INT1_Sel[1:0] - Int 1 selection configuration
randrews33 0:1b975a6ae539 109 Out_Sel[1:0] - Out selection configuration */
randrews33 0:1b975a6ae539 110 gWriteByte(CTRL_REG5_G, 0x00);
randrews33 0:1b975a6ae539 111
randrews33 0:1b975a6ae539 112 // Temporary !!! For testing !!! Remove !!! Or make useful !!!
randrews33 0:1b975a6ae539 113 //configGyroInt(0x2A, 0, 0, 0, 0); // Trigger interrupt when above 0 DPS...
randrews33 0:1b975a6ae539 114 }
randrews33 0:1b975a6ae539 115
randrews33 0:1b975a6ae539 116 void LSM9DS0::initAccel()
randrews33 0:1b975a6ae539 117 {
randrews33 0:1b975a6ae539 118 /* CTRL_REG0_XM (0x1F) (Default value: 0x00)
randrews33 0:1b975a6ae539 119 Bits (7-0): BOOT FIFO_EN WTM_EN 0 0 HP_CLICK HPIS1 HPIS2
randrews33 0:1b975a6ae539 120 BOOT - Reboot memory content (0: normal, 1: reboot)
randrews33 0:1b975a6ae539 121 FIFO_EN - Fifo enable (0: disable, 1: enable)
randrews33 0:1b975a6ae539 122 WTM_EN - FIFO watermark enable (0: disable, 1: enable)
randrews33 0:1b975a6ae539 123 HP_CLICK - HPF enabled for click (0: filter bypassed, 1: enabled)
randrews33 0:1b975a6ae539 124 HPIS1 - HPF enabled for interrupt generator 1 (0: bypassed, 1: enabled)
randrews33 0:1b975a6ae539 125 HPIS2 - HPF enabled for interrupt generator 2 (0: bypassed, 1 enabled) */
randrews33 0:1b975a6ae539 126 xmWriteByte(CTRL_REG0_XM, 0x00);
randrews33 0:1b975a6ae539 127
randrews33 0:1b975a6ae539 128 /* CTRL_REG1_XM (0x20) (Default value: 0x07)
randrews33 0:1b975a6ae539 129 Bits (7-0): AODR3 AODR2 AODR1 AODR0 BDU AZEN AYEN AXEN
randrews33 0:1b975a6ae539 130 AODR[3:0] - select the acceleration data rate:
randrews33 0:1b975a6ae539 131 0000=power down, 0001=3.125Hz, 0010=6.25Hz, 0011=12.5Hz,
randrews33 0:1b975a6ae539 132 0100=25Hz, 0101=50Hz, 0110=100Hz, 0111=200Hz, 1000=400Hz,
randrews33 0:1b975a6ae539 133 1001=800Hz, 1010=1600Hz, (remaining combinations undefined).
randrews33 0:1b975a6ae539 134 BDU - block data update for accel AND mag
randrews33 0:1b975a6ae539 135 0: Continuous update
randrews33 0:1b975a6ae539 136 1: Output registers aren't updated until MSB and LSB have been read.
randrews33 0:1b975a6ae539 137 AZEN, AYEN, and AXEN - Acceleration x/y/z-axis enabled.
randrews33 0:1b975a6ae539 138 0: Axis disabled, 1: Axis enabled */
randrews33 0:1b975a6ae539 139 xmWriteByte(CTRL_REG1_XM, 0x57); // 50Hz data rate, x/y/z all enabled
randrews33 0:1b975a6ae539 140
randrews33 0:1b975a6ae539 141 //Serial.println(xmReadByte(CTRL_REG1_XM));
randrews33 0:1b975a6ae539 142 /* CTRL_REG2_XM (0x21) (Default value: 0x00)
randrews33 0:1b975a6ae539 143 Bits (7-0): ABW1 ABW0 AFS2 AFS1 AFS0 AST1 AST0 SIM
randrews33 0:1b975a6ae539 144 ABW[1:0] - Accelerometer anti-alias filter bandwidth
randrews33 0:1b975a6ae539 145 00=773Hz, 01=194Hz, 10=362Hz, 11=50Hz
randrews33 0:1b975a6ae539 146 AFS[2:0] - Accel full-scale selection
randrews33 0:1b975a6ae539 147 000=+/-2g, 001=+/-4g, 010=+/-6g, 011=+/-8g, 100=+/-16g
randrews33 0:1b975a6ae539 148 AST[1:0] - Accel self-test enable
randrews33 0:1b975a6ae539 149 00=normal (no self-test), 01=positive st, 10=negative st, 11=not allowed
randrews33 0:1b975a6ae539 150 SIM - SPI mode selection
randrews33 0:1b975a6ae539 151 0=4-wire, 1=3-wire */
randrews33 0:1b975a6ae539 152 xmWriteByte(CTRL_REG2_XM, 0x00); // Set scale to 2g
randrews33 0:1b975a6ae539 153
randrews33 0:1b975a6ae539 154 /* CTRL_REG3_XM is used to set interrupt generators on INT1_XM
randrews33 0:1b975a6ae539 155 Bits (7-0): P1_BOOT P1_TAP P1_INT1 P1_INT2 P1_INTM P1_DRDYA P1_DRDYM P1_EMPTY
randrews33 0:1b975a6ae539 156 */
randrews33 0:1b975a6ae539 157 // Accelerometer data ready on INT1_XM (0x04)
randrews33 0:1b975a6ae539 158 // xmWriteByte(CTRL_REG3_XM, 0x04);
randrews33 0:1b975a6ae539 159 }
randrews33 0:1b975a6ae539 160
randrews33 0:1b975a6ae539 161 void LSM9DS0::initMag()
randrews33 0:1b975a6ae539 162 {
randrews33 0:1b975a6ae539 163 /* CTRL_REG5_XM enables temp sensor, sets mag resolution and data rate
randrews33 0:1b975a6ae539 164 Bits (7-0): TEMP_EN M_RES1 M_RES0 M_ODR2 M_ODR1 M_ODR0 LIR2 LIR1
randrews33 0:1b975a6ae539 165 TEMP_EN - Enable temperature sensor (0=disabled, 1=enabled)
randrews33 0:1b975a6ae539 166 M_RES[1:0] - Magnetometer resolution select (0=low, 3=high)
randrews33 0:1b975a6ae539 167 M_ODR[2:0] - Magnetometer data rate select
randrews33 0:1b975a6ae539 168 000=3.125Hz, 001=6.25Hz, 010=12.5Hz, 011=25Hz, 100=50Hz, 101=100Hz
randrews33 0:1b975a6ae539 169 LIR2 - Latch interrupt request on INT2_SRC (cleared by reading INT2_SRC)
randrews33 0:1b975a6ae539 170 0=interrupt request not latched, 1=interrupt request latched
randrews33 0:1b975a6ae539 171 LIR1 - Latch interrupt request on INT1_SRC (cleared by readging INT1_SRC)
randrews33 0:1b975a6ae539 172 0=irq not latched, 1=irq latched */
randrews33 0:1b975a6ae539 173 xmWriteByte(CTRL_REG5_XM, 0x14); // Mag data rate - 100 Hz
randrews33 0:1b975a6ae539 174
randrews33 0:1b975a6ae539 175 /* CTRL_REG6_XM sets the magnetometer full-scale
randrews33 0:1b975a6ae539 176 Bits (7-0): 0 MFS1 MFS0 0 0 0 0 0
randrews33 0:1b975a6ae539 177 MFS[1:0] - Magnetic full-scale selection
randrews33 0:1b975a6ae539 178 00:+/-2Gauss, 01:+/-4Gs, 10:+/-8Gs, 11:+/-12Gs */
randrews33 0:1b975a6ae539 179 xmWriteByte(CTRL_REG6_XM, 0x00); // Mag scale to +/- 2GS
randrews33 0:1b975a6ae539 180
randrews33 0:1b975a6ae539 181 /* CTRL_REG7_XM sets magnetic sensor mode, low power mode, and filters
randrews33 0:1b975a6ae539 182 AHPM1 AHPM0 AFDS 0 0 MLP MD1 MD0
randrews33 0:1b975a6ae539 183 AHPM[1:0] - HPF mode selection
randrews33 0:1b975a6ae539 184 00=normal (resets reference registers), 01=reference signal for filtering,
randrews33 0:1b975a6ae539 185 10=normal, 11=autoreset on interrupt event
randrews33 0:1b975a6ae539 186 AFDS - Filtered acceleration data selection
randrews33 0:1b975a6ae539 187 0=internal filter bypassed, 1=data from internal filter sent to FIFO
randrews33 0:1b975a6ae539 188 MLP - Magnetic data low-power mode
randrews33 0:1b975a6ae539 189 0=data rate is set by M_ODR bits in CTRL_REG5
randrews33 0:1b975a6ae539 190 1=data rate is set to 3.125Hz
randrews33 0:1b975a6ae539 191 MD[1:0] - Magnetic sensor mode selection (default 10)
randrews33 0:1b975a6ae539 192 00=continuous-conversion, 01=single-conversion, 10 and 11=power-down */
randrews33 0:1b975a6ae539 193 xmWriteByte(CTRL_REG7_XM, 0x00); // Continuous conversion mode
randrews33 0:1b975a6ae539 194
randrews33 0:1b975a6ae539 195 /* CTRL_REG4_XM is used to set interrupt generators on INT2_XM
randrews33 0:1b975a6ae539 196 Bits (7-0): P2_TAP P2_INT1 P2_INT2 P2_INTM P2_DRDYA P2_DRDYM P2_Overrun P2_WTM
randrews33 0:1b975a6ae539 197 */
randrews33 0:1b975a6ae539 198 xmWriteByte(CTRL_REG4_XM, 0x04); // Magnetometer data ready on INT2_XM (0x08)
randrews33 0:1b975a6ae539 199
randrews33 0:1b975a6ae539 200 /* INT_CTRL_REG_M to set push-pull/open drain, and active-low/high
randrews33 0:1b975a6ae539 201 Bits[7:0] - XMIEN YMIEN ZMIEN PP_OD IEA IEL 4D MIEN
randrews33 0:1b975a6ae539 202 XMIEN, YMIEN, ZMIEN - Enable interrupt recognition on axis for mag data
randrews33 0:1b975a6ae539 203 PP_OD - Push-pull/open-drain interrupt configuration (0=push-pull, 1=od)
randrews33 0:1b975a6ae539 204 IEA - Interrupt polarity for accel and magneto
randrews33 0:1b975a6ae539 205 0=active-low, 1=active-high
randrews33 0:1b975a6ae539 206 IEL - Latch interrupt request for accel and magneto
randrews33 0:1b975a6ae539 207 0=irq not latched, 1=irq latched
randrews33 0:1b975a6ae539 208 4D - 4D enable. 4D detection is enabled when 6D bit in INT_GEN1_REG is set
randrews33 0:1b975a6ae539 209 MIEN - Enable interrupt generation for magnetic data
randrews33 0:1b975a6ae539 210 0=disable, 1=enable) */
randrews33 0:1b975a6ae539 211 xmWriteByte(INT_CTRL_REG_M, 0x09); // Enable interrupts for mag, active-low, push-pull
randrews33 0:1b975a6ae539 212 }
randrews33 0:1b975a6ae539 213
randrews33 0:1b975a6ae539 214 void LSM9DS0::readAccel()
randrews33 0:1b975a6ae539 215 {
randrews33 0:1b975a6ae539 216 /*uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp
randrews33 0:1b975a6ae539 217 //xmReadByte(OUT_X_L_A, temp, 6); // Read 6 bytes, beginning at OUT_X_L_A
randrews33 0:1b975a6ae539 218 ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax
randrews33 0:1b975a6ae539 219 ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay
randrews33 0:1b975a6ae539 220 az = (temp[5] << 8) | temp[4]; // Store z-axis values into az*/
randrews33 0:1b975a6ae539 221
randrews33 0:1b975a6ae539 222 uint16_t Temp = 0;
randrews33 0:1b975a6ae539 223 uint8_t INTStatus = 0;
randrews33 0:1b975a6ae539 224
randrews33 0:1b975a6ae539 225 while(INTStatus == 0)
randrews33 0:1b975a6ae539 226 {
randrews33 0:1b975a6ae539 227 INTStatus = xmReadByte(STATUS_REG_A) & 0x08;
randrews33 0:1b975a6ae539 228 }
randrews33 0:1b975a6ae539 229
randrews33 0:1b975a6ae539 230 //Get x
randrews33 0:1b975a6ae539 231 Temp = xmReadByte(OUT_X_H_A);
randrews33 0:1b975a6ae539 232 Temp = Temp<<8;
randrews33 0:1b975a6ae539 233 Temp |= xmReadByte(OUT_X_L_A);
randrews33 0:1b975a6ae539 234 ax = Temp;
randrews33 0:1b975a6ae539 235
randrews33 0:1b975a6ae539 236
randrews33 0:1b975a6ae539 237 //Get y
randrews33 0:1b975a6ae539 238 Temp=0;
randrews33 0:1b975a6ae539 239 Temp = xmReadByte(OUT_Y_H_A);
randrews33 0:1b975a6ae539 240 Temp = Temp<<8;
randrews33 0:1b975a6ae539 241 Temp |= xmReadByte(OUT_Y_L_A);
randrews33 0:1b975a6ae539 242 ay = Temp;
randrews33 0:1b975a6ae539 243
randrews33 0:1b975a6ae539 244 //Get z
randrews33 0:1b975a6ae539 245 Temp=0;
randrews33 0:1b975a6ae539 246 Temp = xmReadByte(OUT_Z_H_A);
randrews33 0:1b975a6ae539 247 Temp = Temp<<8;
randrews33 0:1b975a6ae539 248 Temp |= xmReadByte(OUT_Z_L_A);
randrews33 0:1b975a6ae539 249 az = Temp;
randrews33 0:1b975a6ae539 250
randrews33 0:1b975a6ae539 251 }
randrews33 0:1b975a6ae539 252
randrews33 0:1b975a6ae539 253 void LSM9DS0::readMag()
randrews33 0:1b975a6ae539 254 {
randrews33 0:1b975a6ae539 255 /*uint8_t temp[6]; // We'll read six bytes from the mag into temp
randrews33 0:1b975a6ae539 256 xmReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M
randrews33 0:1b975a6ae539 257 mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx
randrews33 0:1b975a6ae539 258 my = (temp[3] << 8) | temp[2]; // Store y-axis values into my
randrews33 0:1b975a6ae539 259 mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz*/
randrews33 0:1b975a6ae539 260
randrews33 0:1b975a6ae539 261 uint16_t Temp = 0;
randrews33 0:1b975a6ae539 262 uint8_t INTStatus = 0;
randrews33 0:1b975a6ae539 263
randrews33 0:1b975a6ae539 264 while(INTStatus == 0)
randrews33 0:1b975a6ae539 265 {
randrews33 0:1b975a6ae539 266 INTStatus = xmReadByte(STATUS_REG_M) & 0x08;
randrews33 0:1b975a6ae539 267 }
randrews33 0:1b975a6ae539 268
randrews33 0:1b975a6ae539 269 //Get x
randrews33 0:1b975a6ae539 270 Temp = xmReadByte(OUT_X_H_M);
randrews33 0:1b975a6ae539 271 Temp = Temp<<8;
randrews33 0:1b975a6ae539 272 Temp |= xmReadByte(OUT_X_L_M);
randrews33 0:1b975a6ae539 273 mx = Temp;
randrews33 0:1b975a6ae539 274
randrews33 0:1b975a6ae539 275
randrews33 0:1b975a6ae539 276 //Get y
randrews33 0:1b975a6ae539 277 Temp=0;
randrews33 0:1b975a6ae539 278 Temp = xmReadByte(OUT_Y_H_M);
randrews33 0:1b975a6ae539 279 Temp = Temp<<8;
randrews33 0:1b975a6ae539 280 Temp |= xmReadByte(OUT_Y_L_M);
randrews33 0:1b975a6ae539 281 my = Temp;
randrews33 0:1b975a6ae539 282
randrews33 0:1b975a6ae539 283 //Get z
randrews33 0:1b975a6ae539 284 Temp=0;
randrews33 0:1b975a6ae539 285 Temp = xmReadByte(OUT_Z_H_M);
randrews33 0:1b975a6ae539 286 Temp = Temp<<8;
randrews33 0:1b975a6ae539 287 Temp |= xmReadByte(OUT_Z_L_M);
randrews33 0:1b975a6ae539 288 mz = Temp;
randrews33 0:1b975a6ae539 289 }
randrews33 0:1b975a6ae539 290
randrews33 0:1b975a6ae539 291 void LSM9DS0::readGyro()
randrews33 0:1b975a6ae539 292 {
randrews33 0:1b975a6ae539 293 /*uint8_t temp[6]; // We'll read six bytes from the gyro into temp
randrews33 0:1b975a6ae539 294 gReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
randrews33 0:1b975a6ae539 295 gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx
randrews33 0:1b975a6ae539 296 gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
randrews33 0:1b975a6ae539 297 gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz*/
randrews33 0:1b975a6ae539 298
randrews33 0:1b975a6ae539 299 uint16_t Temp = 0;
randrews33 0:1b975a6ae539 300 uint8_t INTStatus = 0;
randrews33 0:1b975a6ae539 301
randrews33 0:1b975a6ae539 302 while(INTStatus == 0)
randrews33 0:1b975a6ae539 303 {
randrews33 0:1b975a6ae539 304 INTStatus = (xmReadByte(STATUS_REG_G)&0x08);
randrews33 0:1b975a6ae539 305 }
randrews33 0:1b975a6ae539 306
randrews33 0:1b975a6ae539 307 //Get x
randrews33 0:1b975a6ae539 308 Temp = xmReadByte(OUT_X_H_G);
randrews33 0:1b975a6ae539 309 Temp = Temp<<8;
randrews33 0:1b975a6ae539 310 Temp |= xmReadByte(OUT_X_L_G);
randrews33 0:1b975a6ae539 311 gx = Temp;
randrews33 0:1b975a6ae539 312
randrews33 0:1b975a6ae539 313
randrews33 0:1b975a6ae539 314 //Get y
randrews33 0:1b975a6ae539 315 Temp=0;
randrews33 0:1b975a6ae539 316 Temp = xmReadByte(OUT_Y_H_G);
randrews33 0:1b975a6ae539 317 Temp = Temp<<8;
randrews33 0:1b975a6ae539 318 Temp |= xmReadByte(OUT_Y_L_G);
randrews33 0:1b975a6ae539 319 gy = Temp;
randrews33 0:1b975a6ae539 320
randrews33 0:1b975a6ae539 321 //Get z
randrews33 0:1b975a6ae539 322 Temp=0;
randrews33 0:1b975a6ae539 323 Temp = xmReadByte(OUT_Z_H_G);
randrews33 0:1b975a6ae539 324 Temp = Temp<<8;
randrews33 0:1b975a6ae539 325 Temp |= xmReadByte(OUT_Z_L_G);
randrews33 0:1b975a6ae539 326 gz = Temp;
randrews33 0:1b975a6ae539 327 }
randrews33 0:1b975a6ae539 328
randrews33 0:1b975a6ae539 329 float LSM9DS0::calcGyro(int16_t gyro)
randrews33 0:1b975a6ae539 330 {
randrews33 0:1b975a6ae539 331 // Return the gyro raw reading times our pre-calculated DPS / (ADC tick):
randrews33 0:1b975a6ae539 332 return gRes * gyro;
randrews33 0:1b975a6ae539 333 }
randrews33 0:1b975a6ae539 334
randrews33 0:1b975a6ae539 335 float LSM9DS0::calcAccel(int16_t accel)
randrews33 0:1b975a6ae539 336 {
randrews33 0:1b975a6ae539 337 // Return the accel raw reading times our pre-calculated g's / (ADC tick):
randrews33 0:1b975a6ae539 338 return aRes * accel;
randrews33 0:1b975a6ae539 339 //return accel * (2/32768) - 2;
randrews33 0:1b975a6ae539 340 }
randrews33 0:1b975a6ae539 341
randrews33 0:1b975a6ae539 342 float LSM9DS0::calcMag(int16_t mag)
randrews33 0:1b975a6ae539 343 {
randrews33 0:1b975a6ae539 344 // Return the mag raw reading times our pre-calculated Gs / (ADC tick):
randrews33 0:1b975a6ae539 345 return mRes * mag;
randrews33 0:1b975a6ae539 346 }
randrews33 0:1b975a6ae539 347
randrews33 0:1b975a6ae539 348 void LSM9DS0::setGyroScale(gyro_scale gScl)
randrews33 0:1b975a6ae539 349 {
randrews33 0:1b975a6ae539 350 // We need to preserve the other bytes in CTRL_REG4_G. So, first read it:
randrews33 0:1b975a6ae539 351 uint8_t temp = gReadByte(CTRL_REG4_G);
randrews33 0:1b975a6ae539 352 // Then mask out the gyro scale bits:
randrews33 0:1b975a6ae539 353 temp &= 0xFF^(0x3 << 4);
randrews33 0:1b975a6ae539 354 // Then shift in our new scale bits:
randrews33 0:1b975a6ae539 355 temp |= gScl << 4;
randrews33 0:1b975a6ae539 356 // And write the new register value back into CTRL_REG4_G:
randrews33 0:1b975a6ae539 357 gWriteByte(CTRL_REG4_G, temp);
randrews33 0:1b975a6ae539 358
randrews33 0:1b975a6ae539 359 // We've updated the sensor, but we also need to update our class variables
randrews33 0:1b975a6ae539 360 // First update gScale:
randrews33 0:1b975a6ae539 361 gScale = gScl;
randrews33 0:1b975a6ae539 362 // Then calculate a new gRes, which relies on gScale being set correctly:
randrews33 0:1b975a6ae539 363 calcgRes();
randrews33 0:1b975a6ae539 364 }
randrews33 0:1b975a6ae539 365
randrews33 0:1b975a6ae539 366 void LSM9DS0::setAccelScale(accel_scale aScl)
randrews33 0:1b975a6ae539 367 {
randrews33 0:1b975a6ae539 368 // We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
randrews33 0:1b975a6ae539 369 uint8_t temp = xmReadByte(CTRL_REG2_XM);
randrews33 0:1b975a6ae539 370 // Then mask out the accel scale bits:
randrews33 0:1b975a6ae539 371 temp &= 0xFF^(0x3 << 3);
randrews33 0:1b975a6ae539 372 // Then shift in our new scale bits:
randrews33 0:1b975a6ae539 373 temp |= aScl << 3;
randrews33 0:1b975a6ae539 374 // And write the new register value back into CTRL_REG2_XM:
randrews33 0:1b975a6ae539 375 xmWriteByte(CTRL_REG2_XM, temp);
randrews33 0:1b975a6ae539 376
randrews33 0:1b975a6ae539 377 // We've updated the sensor, but we also need to update our class variables
randrews33 0:1b975a6ae539 378 // First update aScale:
randrews33 0:1b975a6ae539 379 aScale = aScl;
randrews33 0:1b975a6ae539 380 // Then calculate a new aRes, which relies on aScale being set correctly:
randrews33 0:1b975a6ae539 381 calcaRes();
randrews33 0:1b975a6ae539 382 }
randrews33 0:1b975a6ae539 383
randrews33 0:1b975a6ae539 384 void LSM9DS0::setMagScale(mag_scale mScl)
randrews33 0:1b975a6ae539 385 {
randrews33 0:1b975a6ae539 386 // We need to preserve the other bytes in CTRL_REG6_XM. So, first read it:
randrews33 0:1b975a6ae539 387 uint8_t temp = xmReadByte(CTRL_REG6_XM);
randrews33 0:1b975a6ae539 388 // Then mask out the mag scale bits:
randrews33 0:1b975a6ae539 389 temp &= 0xFF^(0x3 << 5);
randrews33 0:1b975a6ae539 390 // Then shift in our new scale bits:
randrews33 0:1b975a6ae539 391 temp |= mScl << 5;
randrews33 0:1b975a6ae539 392 // And write the new register value back into CTRL_REG6_XM:
randrews33 0:1b975a6ae539 393 xmWriteByte(CTRL_REG6_XM, temp);
randrews33 0:1b975a6ae539 394
randrews33 0:1b975a6ae539 395 // We've updated the sensor, but we also need to update our class variables
randrews33 0:1b975a6ae539 396 // First update mScale:
randrews33 0:1b975a6ae539 397 mScale = mScl;
randrews33 0:1b975a6ae539 398 // Then calculate a new mRes, which relies on mScale being set correctly:
randrews33 0:1b975a6ae539 399 calcmRes();
randrews33 0:1b975a6ae539 400 }
randrews33 0:1b975a6ae539 401
randrews33 0:1b975a6ae539 402 void LSM9DS0::setGyroODR(gyro_odr gRate)
randrews33 0:1b975a6ae539 403 {
randrews33 0:1b975a6ae539 404 // We need to preserve the other bytes in CTRL_REG1_G. So, first read it:
randrews33 0:1b975a6ae539 405 uint8_t temp = gReadByte(CTRL_REG1_G);
randrews33 0:1b975a6ae539 406 // Then mask out the gyro ODR bits:
randrews33 0:1b975a6ae539 407 temp &= 0xFF^(0xF << 4);
randrews33 0:1b975a6ae539 408 // Then shift in our new ODR bits:
randrews33 0:1b975a6ae539 409 temp |= (gRate << 4);
randrews33 0:1b975a6ae539 410 // And write the new register value back into CTRL_REG1_G:
randrews33 0:1b975a6ae539 411 gWriteByte(CTRL_REG1_G, temp);
randrews33 0:1b975a6ae539 412 }
randrews33 0:1b975a6ae539 413 void LSM9DS0::setAccelODR(accel_odr aRate)
randrews33 0:1b975a6ae539 414 {
randrews33 0:1b975a6ae539 415 // We need to preserve the other bytes in CTRL_REG1_XM. So, first read it:
randrews33 0:1b975a6ae539 416 uint8_t temp = xmReadByte(CTRL_REG1_XM);
randrews33 0:1b975a6ae539 417 // Then mask out the accel ODR bits:
randrews33 0:1b975a6ae539 418 temp &= 0xFF^(0xF << 4);
randrews33 0:1b975a6ae539 419 // Then shift in our new ODR bits:
randrews33 0:1b975a6ae539 420 temp |= (aRate << 4);
randrews33 0:1b975a6ae539 421 // And write the new register value back into CTRL_REG1_XM:
randrews33 0:1b975a6ae539 422 xmWriteByte(CTRL_REG1_XM, temp);
randrews33 0:1b975a6ae539 423 }
randrews33 0:1b975a6ae539 424 void LSM9DS0::setMagODR(mag_odr mRate)
randrews33 0:1b975a6ae539 425 {
randrews33 0:1b975a6ae539 426 // We need to preserve the other bytes in CTRL_REG5_XM. So, first read it:
randrews33 0:1b975a6ae539 427 uint8_t temp = xmReadByte(CTRL_REG5_XM);
randrews33 0:1b975a6ae539 428 // Then mask out the mag ODR bits:
randrews33 0:1b975a6ae539 429 temp &= 0xFF^(0x7 << 2);
randrews33 0:1b975a6ae539 430 // Then shift in our new ODR bits:
randrews33 0:1b975a6ae539 431 temp |= (mRate << 2);
randrews33 0:1b975a6ae539 432 // And write the new register value back into CTRL_REG5_XM:
randrews33 0:1b975a6ae539 433 xmWriteByte(CTRL_REG5_XM, temp);
randrews33 0:1b975a6ae539 434 }
randrews33 0:1b975a6ae539 435
randrews33 0:1b975a6ae539 436 void LSM9DS0::configGyroInt(uint8_t int1Cfg, uint16_t int1ThsX, uint16_t int1ThsY, uint16_t int1ThsZ, uint8_t duration)
randrews33 0:1b975a6ae539 437 {
randrews33 0:1b975a6ae539 438 gWriteByte(INT1_CFG_G, int1Cfg);
randrews33 0:1b975a6ae539 439 gWriteByte(INT1_THS_XH_G, (int1ThsX & 0xFF00) >> 8);
randrews33 0:1b975a6ae539 440 gWriteByte(INT1_THS_XL_G, (int1ThsX & 0xFF));
randrews33 0:1b975a6ae539 441 gWriteByte(INT1_THS_YH_G, (int1ThsY & 0xFF00) >> 8);
randrews33 0:1b975a6ae539 442 gWriteByte(INT1_THS_YL_G, (int1ThsY & 0xFF));
randrews33 0:1b975a6ae539 443 gWriteByte(INT1_THS_ZH_G, (int1ThsZ & 0xFF00) >> 8);
randrews33 0:1b975a6ae539 444 gWriteByte(INT1_THS_ZL_G, (int1ThsZ & 0xFF));
randrews33 0:1b975a6ae539 445 if (duration)
randrews33 0:1b975a6ae539 446 gWriteByte(INT1_DURATION_G, 0x80 | duration);
randrews33 0:1b975a6ae539 447 else
randrews33 0:1b975a6ae539 448 gWriteByte(INT1_DURATION_G, 0x00);
randrews33 0:1b975a6ae539 449 }
randrews33 0:1b975a6ae539 450
randrews33 0:1b975a6ae539 451 void LSM9DS0::calcgRes()
randrews33 0:1b975a6ae539 452 {
randrews33 0:1b975a6ae539 453 // Possible gyro scales (and their register bit settings) are:
randrews33 0:1b975a6ae539 454 // 245 DPS (00), 500 DPS (01), 2000 DPS (10). Here's a bit of an algorithm
randrews33 0:1b975a6ae539 455 // to calculate DPS/(ADC tick) based on that 2-bit value:
randrews33 0:1b975a6ae539 456 switch (gScale)
randrews33 0:1b975a6ae539 457 {
randrews33 0:1b975a6ae539 458 case G_SCALE_245DPS:
randrews33 0:1b975a6ae539 459 gRes = 245.0 / 32768.0;
randrews33 0:1b975a6ae539 460 break;
randrews33 0:1b975a6ae539 461 case G_SCALE_500DPS:
randrews33 0:1b975a6ae539 462 gRes = 500.0 / 32768.0;
randrews33 0:1b975a6ae539 463 break;
randrews33 0:1b975a6ae539 464 case G_SCALE_2000DPS:
randrews33 0:1b975a6ae539 465 gRes = 2000.0 / 32768.0;
randrews33 0:1b975a6ae539 466 break;
randrews33 0:1b975a6ae539 467 }
randrews33 0:1b975a6ae539 468 }
randrews33 0:1b975a6ae539 469
randrews33 0:1b975a6ae539 470 void LSM9DS0::calcaRes()
randrews33 0:1b975a6ae539 471 {
randrews33 0:1b975a6ae539 472 // Possible accelerometer scales (and their register bit settings) are:
randrews33 0:1b975a6ae539 473 // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an
randrews33 0:1b975a6ae539 474 // algorithm to calculate g/(ADC tick) based on that 3-bit value:
randrews33 0:1b975a6ae539 475 aRes = aScale == A_SCALE_16G ? 16.0 / 32768.0 :
randrews33 0:1b975a6ae539 476 (((float) aScale + 1.0) * 2.0) / 32768.0;
randrews33 0:1b975a6ae539 477 }
randrews33 0:1b975a6ae539 478
randrews33 0:1b975a6ae539 479 void LSM9DS0::calcmRes()
randrews33 0:1b975a6ae539 480 {
randrews33 0:1b975a6ae539 481 // Possible magnetometer scales (and their register bit settings) are:
randrews33 0:1b975a6ae539 482 // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11). Here's a bit of an algorithm
randrews33 0:1b975a6ae539 483 // to calculate Gs/(ADC tick) based on that 2-bit value:
randrews33 0:1b975a6ae539 484 mRes = mScale == M_SCALE_2GS ? 2.0 / 32768.0 :
randrews33 0:1b975a6ae539 485 (float) (mScale << 2) / 32768.0;
randrews33 0:1b975a6ae539 486 }
randrews33 0:1b975a6ae539 487
randrews33 0:1b975a6ae539 488 void LSM9DS0::gWriteByte(uint8_t subAddress, uint8_t data)
randrews33 0:1b975a6ae539 489 {
randrews33 0:1b975a6ae539 490 // Whether we're using I2C or SPI, write a byte using the
randrews33 0:1b975a6ae539 491 // gyro-specific I2C address or SPI CS pin.
randrews33 0:1b975a6ae539 492 I2CwriteByte(gAddress, subAddress, data);
randrews33 0:1b975a6ae539 493 }
randrews33 0:1b975a6ae539 494
randrews33 0:1b975a6ae539 495 void LSM9DS0::xmWriteByte(uint8_t subAddress, uint8_t data)
randrews33 0:1b975a6ae539 496 {
randrews33 0:1b975a6ae539 497 // Whether we're using I2C or SPI, write a byte using the
randrews33 0:1b975a6ae539 498 // accelerometer-specific I2C address or SPI CS pin.
randrews33 0:1b975a6ae539 499 return I2CwriteByte(xmAddress, subAddress, data);
randrews33 0:1b975a6ae539 500 }
randrews33 0:1b975a6ae539 501
randrews33 0:1b975a6ae539 502 uint8_t LSM9DS0::gReadByte(uint8_t subAddress)
randrews33 0:1b975a6ae539 503 {
randrews33 0:1b975a6ae539 504 return I2CreadByte(gAddress, subAddress);
randrews33 0:1b975a6ae539 505 }
randrews33 0:1b975a6ae539 506
randrews33 0:1b975a6ae539 507 void LSM9DS0::gReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
randrews33 0:1b975a6ae539 508 {
randrews33 0:1b975a6ae539 509 // Whether we're using I2C or SPI, read multiple bytes using the
randrews33 0:1b975a6ae539 510 // gyro-specific I2C address or SPI CS pin.
randrews33 0:1b975a6ae539 511 I2CreadBytes(gAddress, subAddress, dest, count);
randrews33 0:1b975a6ae539 512 }
randrews33 0:1b975a6ae539 513
randrews33 0:1b975a6ae539 514 uint8_t LSM9DS0::xmReadByte(uint8_t subAddress)
randrews33 0:1b975a6ae539 515 {
randrews33 0:1b975a6ae539 516 // Whether we're using I2C or SPI, read a byte using the
randrews33 0:1b975a6ae539 517 // accelerometer-specific I2C address or SPI CS pin.
randrews33 0:1b975a6ae539 518 return I2CreadByte(xmAddress, subAddress);
randrews33 0:1b975a6ae539 519 }
randrews33 0:1b975a6ae539 520
randrews33 0:1b975a6ae539 521 void LSM9DS0::xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
randrews33 0:1b975a6ae539 522 {
randrews33 0:1b975a6ae539 523 // Whether we're using I2C or SPI, read multiple bytes using the
randrews33 0:1b975a6ae539 524 // accelerometer-specific I2C address or SPI CS pin.
randrews33 0:1b975a6ae539 525 I2CreadBytes(xmAddress, subAddress, dest, count);
randrews33 0:1b975a6ae539 526 }
randrews33 0:1b975a6ae539 527
randrews33 0:1b975a6ae539 528
randrews33 0:1b975a6ae539 529 void LSM9DS0::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data)
randrews33 0:1b975a6ae539 530 {
randrews33 0:1b975a6ae539 531 /* i2c_->start();
randrews33 0:1b975a6ae539 532 wait_ms(1);
randrews33 0:1b975a6ae539 533 i2c_->write(address);
randrews33 0:1b975a6ae539 534 wait_ms(1);
randrews33 0:1b975a6ae539 535 i2c_->write(subAddress);
randrews33 0:1b975a6ae539 536 wait_ms(1);
randrews33 0:1b975a6ae539 537
randrews33 0:1b975a6ae539 538 i2c_->write(data);
randrews33 0:1b975a6ae539 539 wait_ms(1);
randrews33 0:1b975a6ae539 540 i2c_->stop();*/
randrews33 0:1b975a6ae539 541
randrews33 0:1b975a6ae539 542 i2c_->writeByte(address,subAddress,data);
randrews33 0:1b975a6ae539 543 }
randrews33 0:1b975a6ae539 544
randrews33 0:1b975a6ae539 545 uint8_t LSM9DS0::I2CreadByte(uint8_t address, uint8_t subAddress)
randrews33 0:1b975a6ae539 546 {
randrews33 0:1b975a6ae539 547 char data[1]; // `data` will store the register data
randrews33 0:1b975a6ae539 548
randrews33 0:1b975a6ae539 549 /* data[0] = subAddress;
randrews33 0:1b975a6ae539 550
randrews33 0:1b975a6ae539 551 i2c_->write(address, data, 1, true);
randrews33 0:1b975a6ae539 552 i2c_->read(address, data, 1, true);
randrews33 0:1b975a6ae539 553
randrews33 0:1b975a6ae539 554 i2c_->stop();
randrews33 0:1b975a6ae539 555 return (uint8_t)data[0]; // Return data from register*/
randrews33 0:1b975a6ae539 556
randrews33 0:1b975a6ae539 557 I2CreadBytes(address, subAddress,(uint8_t*)data, 1);
randrews33 0:1b975a6ae539 558 return (uint8_t)data[0];
randrews33 0:1b975a6ae539 559
randrews33 0:1b975a6ae539 560 }
randrews33 0:1b975a6ae539 561
randrews33 0:1b975a6ae539 562 void LSM9DS0::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest,
randrews33 0:1b975a6ae539 563 uint8_t count)
randrews33 0:1b975a6ae539 564 {
randrews33 0:1b975a6ae539 565 /*char data[1]; // `data` will store the register data
randrews33 0:1b975a6ae539 566 data[0] = subAddress;
randrews33 0:1b975a6ae539 567
randrews33 0:1b975a6ae539 568
randrews33 0:1b975a6ae539 569 i2c_->write(address, data, 1, true);
randrews33 0:1b975a6ae539 570 i2c_->read(address, data, 1, true);
randrews33 0:1b975a6ae539 571
randrews33 0:1b975a6ae539 572 dest[0] = data[0];
randrews33 0:1b975a6ae539 573 for (int i=1; i<count ;i++)
randrews33 0:1b975a6ae539 574 {
randrews33 0:1b975a6ae539 575 if(i == (count -1))
randrews33 0:1b975a6ae539 576 dest[i] = i2c_->read(0);
randrews33 0:1b975a6ae539 577 else
randrews33 0:1b975a6ae539 578 dest[i] = i2c_->read(1);
randrews33 0:1b975a6ae539 579 }
randrews33 0:1b975a6ae539 580 // End I2C Transmission
randrews33 0:1b975a6ae539 581 i2c_->stop();*/
randrews33 0:1b975a6ae539 582 /*char command[1];
randrews33 0:1b975a6ae539 583 command[0] = subAddress;
randrews33 0:1b975a6ae539 584 char *redData = (char*)malloc(count);
randrews33 0:1b975a6ae539 585 i2c_->write(address, command, 1, true);
randrews33 0:1b975a6ae539 586
randrews33 0:1b975a6ae539 587 i2c_->read(address, redData, count);
randrews33 0:1b975a6ae539 588 for(int i =0; i < count; i++) {
randrews33 0:1b975a6ae539 589 dest[i] = redData[i];
randrews33 0:1b975a6ae539 590 }
randrews33 0:1b975a6ae539 591
randrews33 0:1b975a6ae539 592 free(redData);*/
randrews33 0:1b975a6ae539 593
randrews33 0:1b975a6ae539 594 i2c_->readBytes(address, subAddress, count, dest);
randrews33 0:1b975a6ae539 595 }