This is a port from the library for Arduino provided by Sparkfun with their breakout board of the LSM9DS0. The original library can be found here: https://github.com/sparkfun/SparkFun_LSM9DS0_Arduino_Library/tree/V_1.0.1 It is also provided an AHRS example based on Madgwick, also a port from an Arduino example. All of this was tested on a Nucleo F411RE and a Sparkfun breakout board.
LSM9DS0_mbed.h
00001 /* 00002 Code by @OlimexSmart - Luca Olivieri 00003 This is a port from the Sparkfun library provided 00004 with the breakout board of the LSM9DS0. 00005 Visit their github for full comments: 00006 https://github.com/sparkfun/SparkFun_LSM9DS0_Arduino_Library/tree/V_1.0.1 00007 */ 00008 00009 #ifndef _LSM9DS0_H__ 00010 #define _LSM9DS0_H__ 00011 00012 #include "mbed.h" 00013 00014 //////////////////////////// 00015 // LSM9DS0 Gyro Registers // 00016 //////////////////////////// 00017 #define WHO_AM_I_G 0x0F 00018 #define CTRL_REG1_G 0x20 00019 #define CTRL_REG2_G 0x21 00020 #define CTRL_REG3_G 0x22 00021 #define CTRL_REG4_G 0x23 00022 #define CTRL_REG5_G 0x24 00023 #define REFERENCE_G 0x25 00024 #define STATUS_REG_G 0x27 00025 #define OUT_X_L_G 0x28 00026 #define OUT_X_H_G 0x29 00027 #define OUT_Y_L_G 0x2A 00028 #define OUT_Y_H_G 0x2B 00029 #define OUT_Z_L_G 0x2C 00030 #define OUT_Z_H_G 0x2D 00031 #define FIFO_CTRL_REG_G 0x2E 00032 #define FIFO_SRC_REG_G 0x2F 00033 #define INT1_CFG_G 0x30 00034 #define INT1_SRC_G 0x31 00035 #define INT1_THS_XH_G 0x32 00036 #define INT1_THS_XL_G 0x33 00037 #define INT1_THS_YH_G 0x34 00038 #define INT1_THS_YL_G 0x35 00039 #define INT1_THS_ZH_G 0x36 00040 #define INT1_THS_ZL_G 0x37 00041 #define INT1_DURATION_G 0x38 00042 00043 ////////////////////////////////////////// 00044 // LSM9DS0 Accel/Magneto (XM) Registers // 00045 ////////////////////////////////////////// 00046 #define OUT_TEMP_L_XM 0x05 00047 #define OUT_TEMP_H_XM 0x06 00048 #define STATUS_REG_M 0x07 00049 #define OUT_X_L_M 0x08 00050 #define OUT_X_H_M 0x09 00051 #define OUT_Y_L_M 0x0A 00052 #define OUT_Y_H_M 0x0B 00053 #define OUT_Z_L_M 0x0C 00054 #define OUT_Z_H_M 0x0D 00055 #define WHO_AM_I_XM 0x0F 00056 #define INT_CTRL_REG_M 0x12 00057 #define INT_SRC_REG_M 0x13 00058 #define INT_THS_L_M 0x14 00059 #define INT_THS_H_M 0x15 00060 #define OFFSET_X_L_M 0x16 00061 #define OFFSET_X_H_M 0x17 00062 #define OFFSET_Y_L_M 0x18 00063 #define OFFSET_Y_H_M 0x19 00064 #define OFFSET_Z_L_M 0x1A 00065 #define OFFSET_Z_H_M 0x1B 00066 #define REFERENCE_X 0x1C 00067 #define REFERENCE_Y 0x1D 00068 #define REFERENCE_Z 0x1E 00069 #define CTRL_REG0_XM 0x1F 00070 #define CTRL_REG1_XM 0x20 00071 #define CTRL_REG2_XM 0x21 00072 #define CTRL_REG3_XM 0x22 00073 #define CTRL_REG4_XM 0x23 00074 #define CTRL_REG5_XM 0x24 00075 #define CTRL_REG6_XM 0x25 00076 #define CTRL_REG7_XM 0x26 00077 #define STATUS_REG_A 0x27 00078 #define OUT_X_L_A 0x28 00079 #define OUT_X_H_A 0x29 00080 #define OUT_Y_L_A 0x2A 00081 #define OUT_Y_H_A 0x2B 00082 #define OUT_Z_L_A 0x2C 00083 #define OUT_Z_H_A 0x2D 00084 #define FIFO_CTRL_REG 0x2E 00085 #define FIFO_SRC_REG 0x2F 00086 #define INT_GEN_1_REG 0x30 00087 #define INT_GEN_1_SRC 0x31 00088 #define INT_GEN_1_THS 0x32 00089 #define INT_GEN_1_DURATION 0x33 00090 #define INT_GEN_2_REG 0x34 00091 #define INT_GEN_2_SRC 0x35 00092 #define INT_GEN_2_THS 0x36 00093 #define INT_GEN_2_DURATION 0x37 00094 #define CLICK_CFG 0x38 00095 #define CLICK_SRC 0x39 00096 #define CLICK_THS 0x3A 00097 #define TIME_LIMIT 0x3B 00098 #define TIME_LATENCY 0x3C 00099 #define TIME_WINDOW 0x3D 00100 #define ACT_THS 0x3E 00101 #define ACT_DUR 0x3F 00102 00103 00104 class LSM9DS0 00105 { 00106 public: 00107 // gyro_scale defines the possible full-scale ranges of the gyroscope: 00108 enum gyro_scale { 00109 G_SCALE_245DPS, // 00: +/- 245 degrees per second 00110 G_SCALE_500DPS, // 01: +/- 500 dps 00111 G_SCALE_2000DPS, // 10: +/- 2000 dps 00112 }; 00113 00114 // accel_scale defines all possible FSR's of the accelerometer: 00115 enum accel_scale { 00116 A_SCALE_2G, // 000: +/- 2g 00117 A_SCALE_4G, // 001: +/- 4g 00118 A_SCALE_6G, // 010: +/- 6g 00119 A_SCALE_8G, // 011: +/- 8g 00120 A_SCALE_16G // 100: +/- 16g 00121 }; 00122 00123 // mag_scale defines all possible FSR's of the magnetometer: 00124 enum mag_scale { 00125 M_SCALE_2GS, // 00: +/- 2Gs 00126 M_SCALE_4GS, // 01: +/- 4Gs 00127 M_SCALE_8GS, // 10: +/- 8Gs 00128 M_SCALE_12GS, // 11: +/- 12Gs 00129 }; 00130 00131 // gyro_odr defines all possible data rate/bandwidth combos of the gyro: 00132 enum gyro_odr { 00133 // ODR (Hz) --- Cutoff 00134 G_ODR_95_BW_125 = 0x0, // 95 12.5 00135 G_ODR_95_BW_25 = 0x1, // 95 25 00136 // 0x2 and 0x3 define the same data rate and bandwidth 00137 G_ODR_190_BW_125 = 0x4, // 190 12.5 00138 G_ODR_190_BW_25 = 0x5, // 190 25 00139 G_ODR_190_BW_50 = 0x6, // 190 50 00140 G_ODR_190_BW_70 = 0x7, // 190 70 00141 G_ODR_380_BW_20 = 0x8, // 380 20 00142 G_ODR_380_BW_25 = 0x9, // 380 25 00143 G_ODR_380_BW_50 = 0xA, // 380 50 00144 G_ODR_380_BW_100 = 0xB, // 380 100 00145 G_ODR_760_BW_30 = 0xC, // 760 30 00146 G_ODR_760_BW_35 = 0xD, // 760 35 00147 G_ODR_760_BW_50 = 0xE, // 760 50 00148 G_ODR_760_BW_100 = 0xF, // 760 100 00149 }; 00150 00151 // accel_oder defines all possible output data rates of the accelerometer: 00152 enum accel_odr { 00153 A_POWER_DOWN, // Power-down mode (0x0) 00154 A_ODR_3125, // 3.125 Hz (0x1) 00155 A_ODR_625, // 6.25 Hz (0x2) 00156 A_ODR_125, // 12.5 Hz (0x3) 00157 A_ODR_25, // 25 Hz (0x4) 00158 A_ODR_50, // 50 Hz (0x5) 00159 A_ODR_100, // 100 Hz (0x6) 00160 A_ODR_200, // 200 Hz (0x7) 00161 A_ODR_400, // 400 Hz (0x8) 00162 A_ODR_800, // 800 Hz (9) 00163 A_ODR_1600 // 1600 Hz (0xA) 00164 }; 00165 00166 // accel_abw defines all possible anti-aliasing filter rates of the accelerometer: 00167 enum accel_abw { 00168 A_ABW_773, // 773 Hz (0x0) 00169 A_ABW_194, // 194 Hz (0x1) 00170 A_ABW_362, // 362 Hz (0x2) 00171 A_ABW_50, // 50 Hz (0x3) 00172 }; 00173 00174 // accel_oder defines all possible output data rates of the magnetometer: 00175 enum mag_odr { 00176 M_ODR_3125, // 3.125 Hz (0x00) 00177 M_ODR_625, // 6.25 Hz (0x01) 00178 M_ODR_125, // 12.5 Hz (0x02) 00179 M_ODR_25, // 25 Hz (0x03) 00180 M_ODR_50, // 50 (0x04) 00181 M_ODR_100, // 100 Hz (0x05) 00182 }; 00183 00184 // We'll store the gyro, accel, and magnetometer readings in a series of 00185 // public class variables. Each sensor gets three variables -- one for each 00186 // axis. Call readGyro(), readAccel(), and readMag() first, before using 00187 // these variables! 00188 // These values are the RAW signed 16-bit readings from the sensors. 00189 int16_t gx, gy, gz; // x, y, and z axis readings of the gyroscope 00190 int16_t ax, ay, az; // x, y, and z axis readings of the accelerometer 00191 int16_t mx, my, mz; // x, y, and z axis readings of the magnetometer 00192 int16_t temperature; 00193 float abias[3]; 00194 float gbias[3]; 00195 00196 00197 // LSM9DS0 -- LSM9DS0 class constructor 00198 // The constructor will set up a handful of private variables, and set the 00199 // communication mode as well. 00200 // Input: 00201 // - interface = Either MODE_SPI or MODE_I2C, whichever you're using 00202 // to talk to the IC. 00203 // - gAddr = If MODE_I2C, this is the I2C address of the gyroscope. 00204 // If MODE_SPI, this is the chip select pin of the gyro (CSG) 00205 // - xmAddr = If MODE_I2C, this is the I2C address of the accel/mag. 00206 // If MODE_SPI, this is the cs pin of the accel/mag (CSXM) 00207 LSM9DS0(PinName sda, PinName scl, uint8_t gAddr, uint8_t xmAddr); 00208 00209 // begin() -- Initialize the gyro, accelerometer, and magnetometer. 00210 // This will set up the scale and output rate of each sensor. It'll also 00211 // "turn on" every sensor and every axis of every sensor. 00212 // Input: 00213 // - gScl = The scale of the gyroscope. This should be a gyro_scale value. 00214 // - aScl = The scale of the accelerometer. Should be a accel_scale value. 00215 // - mScl = The scale of the magnetometer. Should be a mag_scale value. 00216 // - gODR = Output data rate of the gyroscope. gyro_odr value. 00217 // - aODR = Output data rate of the accelerometer. accel_odr value. 00218 // - mODR = Output data rate of the magnetometer. mag_odr value. 00219 // Output: The function will return an unsigned 16-bit value. The most-sig 00220 // bytes of the output are the WHO_AM_I reading of the accel. The 00221 // least significant two bytes are the WHO_AM_I reading of the gyro. 00222 // All parameters have a defaulted value, so you can call just "begin()". 00223 // Default values are FSR's of: +/- 245DPS, 2g, 2Gs; ODRs of 95 Hz for 00224 // gyro, 100 Hz for accelerometer, 100 Hz for magnetometer. 00225 // Use the return value of this function to verify communication. 00226 uint16_t begin(gyro_scale gScl = G_SCALE_245DPS, 00227 accel_scale aScl = A_SCALE_2G, mag_scale mScl = M_SCALE_2GS, 00228 gyro_odr gODR = G_ODR_95_BW_125, accel_odr aODR = A_ODR_50, 00229 mag_odr mODR = M_ODR_50); 00230 00231 // readGyro() -- Read the gyroscope output registers. 00232 // This function will read all six gyroscope output registers. 00233 // The readings are stored in the class' gx, gy, and gz variables. Read 00234 // those _after_ calling readGyro(). 00235 void readGyro(); 00236 00237 // readAccel() -- Read the accelerometer output registers. 00238 // This function will read all six accelerometer output registers. 00239 // The readings are stored in the class' ax, ay, and az variables. Read 00240 // those _after_ calling readAccel(). 00241 void readAccel(); 00242 00243 // readMag() -- Read the magnetometer output registers. 00244 // This function will read all six magnetometer output registers. 00245 // The readings are stored in the class' mx, my, and mz variables. Read 00246 // those _after_ calling readMag(). 00247 void readMag(); 00248 00249 // readTemp() -- Read the temperature output register. 00250 // This function will read two temperature output registers. 00251 // The combined readings are stored in the class' temperature variables. Read 00252 // those _after_ calling readTemp(). 00253 void readTemp(); 00254 00255 // calcGyro() -- Convert from RAW signed 16-bit value to degrees per second 00256 // This function reads in a signed 16-bit value and returns the scaled 00257 // DPS. This function relies on gScale and gRes being correct. 00258 // Input: 00259 // - gyro = A signed 16-bit raw reading from the gyroscope. 00260 float calcGyro(int16_t gyro); 00261 00262 // calcAccel() -- Convert from RAW signed 16-bit value to gravity (g's). 00263 // This function reads in a signed 16-bit value and returns the scaled 00264 // g's. This function relies on aScale and aRes being correct. 00265 // Input: 00266 // - accel = A signed 16-bit raw reading from the accelerometer. 00267 float calcAccel(int16_t accel); 00268 00269 // calcMag() -- Convert from RAW signed 16-bit value to Gauss (Gs) 00270 // This function reads in a signed 16-bit value and returns the scaled 00271 // Gs. This function relies on mScale and mRes being correct. 00272 // Input: 00273 // - mag = A signed 16-bit raw reading from the magnetometer. 00274 float calcMag(int16_t mag); 00275 00276 // setGyroScale() -- Set the full-scale range of the gyroscope. 00277 // This function can be called to set the scale of the gyroscope to 00278 // 245, 500, or 200 degrees per second. 00279 // Input: 00280 // - gScl = The desired gyroscope scale. Must be one of three possible 00281 // values from the gyro_scale enum. 00282 void setGyroScale(gyro_scale gScl); 00283 00284 // setAccelScale() -- Set the full-scale range of the accelerometer. 00285 // This function can be called to set the scale of the accelerometer to 00286 // 2, 4, 6, 8, or 16 g's. 00287 // Input: 00288 // - aScl = The desired accelerometer scale. Must be one of five possible 00289 // values from the accel_scale enum. 00290 void setAccelScale(accel_scale aScl); 00291 00292 // setMagScale() -- Set the full-scale range of the magnetometer. 00293 // This function can be called to set the scale of the magnetometer to 00294 // 2, 4, 8, or 12 Gs. 00295 // Input: 00296 // - mScl = The desired magnetometer scale. Must be one of four possible 00297 // values from the mag_scale enum. 00298 void setMagScale(mag_scale mScl); 00299 00300 // setGyroODR() -- Set the output data rate and bandwidth of the gyroscope 00301 // Input: 00302 // - gRate = The desired output rate and cutoff frequency of the gyro. 00303 // Must be a value from the gyro_odr enum (check above, there're 14). 00304 void setGyroODR(gyro_odr gRate); 00305 00306 // setAccelODR() -- Set the output data rate of the accelerometer 00307 // Input: 00308 // - aRate = The desired output rate of the accel. 00309 // Must be a value from the accel_odr enum (check above, there're 11). 00310 void setAccelODR(accel_odr aRate); 00311 00312 // setMagODR() -- Set the output data rate of the magnetometer 00313 // Input: 00314 // - mRate = The desired output rate of the mag. 00315 // Must be a value from the mag_odr enum (check above, there're 6). 00316 void setMagODR(mag_odr mRate); 00317 00318 // setAccelABW() -- Set the anti-aliasing filter rate of the accelerometer 00319 // Input: 00320 // - abwRate = The desired anti-aliasing filter rate of the accel. 00321 // Must be a value from the accel_abw enum (check above, there're 4). 00322 void setAccelABW(accel_abw abwRate); 00323 00324 00325 // configGyroInt() -- Configure the gyro interrupt output. 00326 // Triggers can be set to either rising above or falling below a specified 00327 // threshold. This function helps setup the interrupt configuration and 00328 // threshold values for all axes. 00329 // Input: 00330 // - int1Cfg = A 8-bit value that is sent directly to the INT1_CFG_G 00331 // register. This sets AND/OR and high/low interrupt gen for each axis 00332 // - int1ThsX = 16-bit interrupt threshold value for x-axis 00333 // - int1ThsY = 16-bit interrupt threshold value for y-axis 00334 // - int1ThsZ = 16-bit interrupt threshold value for z-axis 00335 // - duration = Duration an interrupt holds after triggered. This value 00336 // is copied directly into the INT1_DURATION_G register. 00337 // Before using this function, read about the INT1_CFG_G register and 00338 // the related INT1* registers in the LMS9DS0 datasheet. 00339 void configGyroInt(uint8_t int1Cfg, uint16_t int1ThsX = 0, 00340 uint16_t int1ThsY = 0, uint16_t int1ThsZ = 0, 00341 uint8_t duration = 0); 00342 00343 void calLSM9DS0(float gbias[3], float abias[3]); 00344 00345 00346 private: 00347 // xmAddress and gAddress store the I2C address 00348 // for each sensor. 00349 uint8_t xmAddress, gAddress; 00350 00351 // gScale, aScale, and mScale store the current scale range for each 00352 // sensor. Should be updated whenever that value changes. 00353 gyro_scale gScale; 00354 accel_scale aScale; 00355 mag_scale mScale; 00356 00357 // gRes, aRes, and mRes store the current resolution for each sensor. 00358 // Units of these values would be DPS (or g's or Gs's) per ADC tick. 00359 // This value is calculated as (sensor scale) / (2^15). 00360 float gRes, aRes, mRes; 00361 00362 // initGyro() -- Sets up the gyroscope to begin reading. 00363 // This function steps through all five gyroscope control registers. 00364 // Upon exit, the following parameters will be set: 00365 // - CTRL_REG1_G = 0x0F: Normal operation mode, all axes enabled. 00366 // 95 Hz ODR, 12.5 Hz cutoff frequency. 00367 // - CTRL_REG2_G = 0x00: HPF set to normal mode, cutoff frequency 00368 // set to 7.2 Hz (depends on ODR). 00369 // - CTRL_REG3_G = 0x88: Interrupt enabled on INT_G (set to push-pull and 00370 // active high). Data-ready output enabled on DRDY_G. 00371 // - CTRL_REG4_G = 0x00: Continuous update mode. Data LSB stored in lower 00372 // address. Scale set to 245 DPS. SPI mode set to 4-wire. 00373 // - CTRL_REG5_G = 0x00: FIFO disabled. HPF disabled. 00374 void initGyro(); 00375 00376 // initAccel() -- Sets up the accelerometer to begin reading. 00377 // This function steps through all accelerometer related control registers. 00378 // Upon exit these registers will be set as: 00379 // - CTRL_REG0_XM = 0x00: FIFO disabled. HPF bypassed. Normal mode. 00380 // - CTRL_REG1_XM = 0x57: 100 Hz data rate. Continuous update. 00381 // all axes enabled. 00382 // - CTRL_REG2_XM = 0x00: +/- 2g scale. 773 Hz anti-alias filter BW. 00383 // - CTRL_REG3_XM = 0x04: Accel data ready signal on INT1_XM pin. 00384 void initAccel(); 00385 00386 // initMag() -- Sets up the magnetometer to begin reading. 00387 // This function steps through all magnetometer-related control registers. 00388 // Upon exit these registers will be set as: 00389 // - CTRL_REG4_XM = 0x04: Mag data ready signal on INT2_XM pin. 00390 // - CTRL_REG5_XM = 0x14: 100 Hz update rate. Low resolution. Interrupt 00391 // requests don't latch. Temperature sensor disabled. 00392 // - CTRL_REG6_XM = 0x00: +/- 2 Gs scale. 00393 // - CTRL_REG7_XM = 0x00: Continuous conversion mode. Normal HPF mode. 00394 // - INT_CTRL_REG_M = 0x09: Interrupt active-high. Enable interrupts. 00395 void initMag(); 00396 00397 // gReadByte() -- Reads a byte from a specified gyroscope register. 00398 // Input: 00399 // - subAddress = Register to be read from. 00400 // Output: 00401 // - An 8-bit value read from the requested address. 00402 uint8_t gReadByte(uint8_t subAddress); 00403 00404 // gReadBytes() -- Reads a number of bytes -- beginning at an address 00405 // and incrementing from there -- from the gyroscope. 00406 // Input: 00407 // - subAddress = Register to be read from. 00408 // - * dest = A pointer to an array of uint8_t's. Values read will be 00409 // stored in here on return. 00410 // - count = The number of bytes to be read. 00411 // Output: No value is returned, but the `dest` array will store 00412 // the data read upon exit. 00413 void gReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count); 00414 00415 // gWriteByte() -- Write a byte to a register in the gyroscope. 00416 // Input: 00417 // - subAddress = Register to be written to. 00418 // - data = data to be written to the register. 00419 void gWriteByte(uint8_t subAddress, uint8_t data); 00420 00421 // xmReadByte() -- Read a byte from a register in the accel/mag sensor 00422 // Input: 00423 // - subAddress = Register to be read from. 00424 // Output: 00425 // - An 8-bit value read from the requested register. 00426 uint8_t xmReadByte(uint8_t subAddress); 00427 00428 // xmReadBytes() -- Reads a number of bytes -- beginning at an address 00429 // and incrementing from there -- from the accelerometer/magnetometer. 00430 // Input: 00431 // - subAddress = Register to be read from. 00432 // - * dest = A pointer to an array of uint8_t's. Values read will be 00433 // stored in here on return. 00434 // - count = The number of bytes to be read. 00435 // Output: No value is returned, but the `dest` array will store 00436 // the data read upon exit. 00437 void xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count); 00438 00439 // xmWriteByte() -- Write a byte to a register in the accel/mag sensor. 00440 // Input: 00441 // - subAddress = Register to be written to. 00442 // - data = data to be written to the register. 00443 void xmWriteByte(uint8_t subAddress, uint8_t data); 00444 00445 // calcgRes() -- Calculate the resolution of the gyroscope. 00446 // This function will set the value of the gRes variable. gScale must 00447 // be set prior to calling this function. 00448 void calcgRes(); 00449 00450 // calcmRes() -- Calculate the resolution of the magnetometer. 00451 // This function will set the value of the mRes variable. mScale must 00452 // be set prior to calling this function. 00453 void calcmRes(); 00454 00455 // calcaRes() -- Calculate the resolution of the accelerometer. 00456 // This function will set the value of the aRes variable. aScale must 00457 // be set prior to calling this function. 00458 void calcaRes(); 00459 00460 00461 /////////////////// 00462 // I2C Functions // 00463 /////////////////// 00464 I2C* i2c_; 00465 00466 00467 // I2CwriteByte() -- Write a byte out of I2C to a register in the device 00468 // Input: 00469 // - address = The 7-bit I2C address of the slave device. 00470 // - subAddress = The register to be written to. 00471 // - data = Byte to be written to the register. 00472 void I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data); 00473 00474 // I2CreadByte() -- Read a single byte from a register over I2C. 00475 // Input: 00476 // - address = The 7-bit I2C address of the slave device. 00477 // - subAddress = The register to be read from. 00478 // Output: 00479 // - The byte read from the requested address. 00480 uint8_t I2CreadByte(uint8_t address, uint8_t subAddress); 00481 00482 // I2CreadBytes() -- Read a series of bytes, starting at a register via SPI 00483 // Input: 00484 // - address = The 7-bit I2C address of the slave device. 00485 // - subAddress = The register to begin reading. 00486 // - * dest = Pointer to an array where we'll store the readings. 00487 // - count = Number of registers to be read. 00488 // Output: No value is returned by the function, but the registers read are 00489 // all stored in the *dest array given. 00490 void I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count); 00491 }; 00492 00493 #endif // _LSM9DS0_H //
Generated on Thu Jul 21 2022 10:12:20 by 1.7.2