|
|
|
|
@ -14,281 +14,288 @@
@@ -14,281 +14,288 @@
|
|
|
|
|
* Library may be used freely and without limit with attribution. |
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
/* Includes */ |
|
|
|
|
#include <stdint.h> |
|
|
|
|
#include "lsm6dsm.h" |
|
|
|
|
|
|
|
|
|
LSM6DSM::LSM6DSM(uint8_t intPin1, uint8_t intPin2) |
|
|
|
|
/* Private Global Variables */ |
|
|
|
|
static uint8_t _intPin1; |
|
|
|
|
static uint8_t _intPin2; |
|
|
|
|
static float _aRes; |
|
|
|
|
static float _gRes; |
|
|
|
|
|
|
|
|
|
/* Function Prototypes */ |
|
|
|
|
static void lsm6dsm_write_byte(uint8_t address, uint8_t subAddress, |
|
|
|
|
uint8_t data); |
|
|
|
|
static uint8_t lsm6dsm_read_byte(uint8_t address, uint8_t subAddress); |
|
|
|
|
static void lsm6dsm_read(uint8_t address, uint8_t subAddress, uint8_t count, |
|
|
|
|
uint8_t *dest); |
|
|
|
|
|
|
|
|
|
/* Function Definitions */ |
|
|
|
|
lsm6dsm_new(uint8_t pin1, uint8_t pin2) |
|
|
|
|
{ |
|
|
|
|
pinMode(intPin1, INPUT); |
|
|
|
|
_intPin1 = intPin1; |
|
|
|
|
pinMode(intPin2, INPUT); |
|
|
|
|
_intPin2 = intPin2;
|
|
|
|
|
pinMode(pin1, INPUT); |
|
|
|
|
_intPin1 = pin1; |
|
|
|
|
pinMode(pin2, INPUT); |
|
|
|
|
_intPin2 = pin2; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
uint8_t LSM6DSM::getChipID() |
|
|
|
|
uint8_t lsm6dsm_chip_id_get() |
|
|
|
|
{ |
|
|
|
|
uint8_t c = readByte(LSM6DSM_ADDRESS, LSM6DSM_WHO_AM_I); |
|
|
|
|
return c; |
|
|
|
|
uint8_t c = lsm6dsm_read_byte(LSM6DSM_ADDRESS, LSM6DSM_WHO_AM_I); |
|
|
|
|
return c; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float LSM6DSM::getAres(uint8_t Ascale) { |
|
|
|
|
switch (Ascale) |
|
|
|
|
{ |
|
|
|
|
// Possible accelerometer scales (and their register bit settings) are:
|
|
|
|
|
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
|
|
|
|
|
// Here's a bit of an algorithm to calculate DPS/(ADC tick) based on that 2-bit value:
|
|
|
|
|
case AFS_2G: |
|
|
|
|
_aRes = 2.0f/32768.0f; |
|
|
|
|
return _aRes; |
|
|
|
|
break; |
|
|
|
|
case AFS_4G: |
|
|
|
|
_aRes = 4.0f/32768.0f; |
|
|
|
|
return _aRes; |
|
|
|
|
break; |
|
|
|
|
case AFS_8G: |
|
|
|
|
_aRes = 8.0f/32768.0f; |
|
|
|
|
return _aRes; |
|
|
|
|
break; |
|
|
|
|
case AFS_16G: |
|
|
|
|
_aRes = 16.0f/32768.0f; |
|
|
|
|
return _aRes; |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
float lsm6dsm_ares_get(uint8_t ascale) |
|
|
|
|
{ |
|
|
|
|
switch(ascale) |
|
|
|
|
{ |
|
|
|
|
// Possible accelerometer scales (and their register bit settings) are:
|
|
|
|
|
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
|
|
|
|
|
// Here's a bit of an algorithm to calculate DPS/(ADC tick) based on that 2-bit value:
|
|
|
|
|
case AFS_2G: |
|
|
|
|
_aRes = 2.0f / 32768.0f; |
|
|
|
|
return _aRes; |
|
|
|
|
break; |
|
|
|
|
case AFS_4G: |
|
|
|
|
_aRes = 4.0f / 32768.0f; |
|
|
|
|
return _aRes; |
|
|
|
|
break; |
|
|
|
|
case AFS_8G: |
|
|
|
|
_aRes = 8.0f / 32768.0f; |
|
|
|
|
return _aRes; |
|
|
|
|
break; |
|
|
|
|
case AFS_16G: |
|
|
|
|
_aRes = 16.0f / 32768.0f; |
|
|
|
|
return _aRes; |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float LSM6DSM::getGres(uint8_t Gscale) { |
|
|
|
|
switch (Gscale) |
|
|
|
|
{ |
|
|
|
|
// Possible gyro scales (and their register bit settings) are:
|
|
|
|
|
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
|
|
|
|
|
case GFS_245DPS: |
|
|
|
|
_gRes = 245.0f/32768.0f; |
|
|
|
|
return _gRes; |
|
|
|
|
break; |
|
|
|
|
case GFS_500DPS: |
|
|
|
|
_gRes = 500.0f/32768.0f; |
|
|
|
|
return _gRes; |
|
|
|
|
break; |
|
|
|
|
case GFS_1000DPS: |
|
|
|
|
_gRes = 1000.0f/32768.0f; |
|
|
|
|
return _gRes; |
|
|
|
|
break; |
|
|
|
|
case GFS_2000DPS: |
|
|
|
|
_gRes = 2000.0f/32768.0f; |
|
|
|
|
return _gRes; |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
float lsm6dsm_gres_get(uint8_t gscale) |
|
|
|
|
{ |
|
|
|
|
switch(gscale) |
|
|
|
|
{ |
|
|
|
|
// Possible gyro scales (and their register bit settings) are:
|
|
|
|
|
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
|
|
|
|
|
case GFS_245DPS: |
|
|
|
|
_gRes = 245.0f / 32768.0f; |
|
|
|
|
return _gRes; |
|
|
|
|
break; |
|
|
|
|
case GFS_500DPS: |
|
|
|
|
_gRes = 500.0f / 32768.0f; |
|
|
|
|
return _gRes; |
|
|
|
|
break; |
|
|
|
|
case GFS_1000DPS: |
|
|
|
|
_gRes = 1000.0f / 32768.0f; |
|
|
|
|
return _gRes; |
|
|
|
|
break; |
|
|
|
|
case GFS_2000DPS: |
|
|
|
|
_gRes = 2000.0f / 32768.0f; |
|
|
|
|
return _gRes; |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void LSM6DSM::reset() |
|
|
|
|
void lsm6dsm_reset() |
|
|
|
|
{ |
|
|
|
|
// reset device
|
|
|
|
|
uint8_t temp = readByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C); |
|
|
|
|
writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM
|
|
|
|
|
delay(100); // Wait for all registers to reset
|
|
|
|
|
// reset device
|
|
|
|
|
uint8_t temp = lsm6dsm_read_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C); |
|
|
|
|
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM
|
|
|
|
|
delay(100); // Wait for all registers to reset
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void LSM6DSM::init(uint8_t Ascale, uint8_t Gscale, uint8_t AODR, uint8_t GODR) |
|
|
|
|
void lsm6dsm_init(uint8_t ascale, uint8_t gscale, uint8_t AODR, uint8_t GODR) |
|
|
|
|
{ |
|
|
|
|
writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL, AODR << 4 | Ascale << 2); |
|
|
|
|
|
|
|
|
|
writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G, GODR << 4 | Gscale << 2); |
|
|
|
|
|
|
|
|
|
uint8_t temp = readByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C); |
|
|
|
|
// enable block update (bit 6 = 1), auto-increment registers (bit 2 = 1)
|
|
|
|
|
writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x40 | 0x04);
|
|
|
|
|
// by default, interrupts active HIGH, push pull, little endian data
|
|
|
|
|
// (can be changed by writing to bits 5, 4, and 1, resp to above register)
|
|
|
|
|
|
|
|
|
|
// enable accel LP2 (bit 7 = 1), set LP2 tp ODR/9 (bit 6 = 1), enable input_composite (bit 3) for low noise
|
|
|
|
|
writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08 ); |
|
|
|
|
|
|
|
|
|
// interrupt handling
|
|
|
|
|
writeByte(LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read
|
|
|
|
|
writeByte(LSM6DSM_ADDRESS, LSM6DSM_INT1_CTRL, 0x40); // enable significant motion interrupts on INT1
|
|
|
|
|
writeByte(LSM6DSM_ADDRESS, LSM6DSM_INT2_CTRL, 0x03); // enable accel/gyro data ready interrupts on INT2
|
|
|
|
|
} |
|
|
|
|
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL, |
|
|
|
|
AODR << 4 | ascale << 2); |
|
|
|
|
|
|
|
|
|
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G, |
|
|
|
|
GODR << 4 | gscale << 2); |
|
|
|
|
|
|
|
|
|
void LSM6DSM::selfTest() |
|
|
|
|
{ |
|
|
|
|
int16_t temp[7] = {0, 0, 0, 0, 0, 0, 0}; |
|
|
|
|
int16_t accelPTest[3] = {0, 0, 0}, accelNTest[3] = {0, 0, 0}, gyroPTest[3] = {0, 0, 0}, gyroNTest[3] = {0, 0, 0}; |
|
|
|
|
int16_t accelNom[3] = {0, 0, 0}, gyroNom[3] = {0, 0, 0}; |
|
|
|
|
|
|
|
|
|
readData(temp); |
|
|
|
|
accelNom[0] = temp[4]; |
|
|
|
|
accelNom[1] = temp[5]; |
|
|
|
|
accelNom[2] = temp[6]; |
|
|
|
|
gyroNom[0] = temp[1]; |
|
|
|
|
gyroNom[1] = temp[2]; |
|
|
|
|
gyroNom[2] = temp[3]; |
|
|
|
|
|
|
|
|
|
writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test
|
|
|
|
|
delay(100); // let accel respond
|
|
|
|
|
readData(temp); |
|
|
|
|
accelPTest[0] = temp[4]; |
|
|
|
|
accelPTest[1] = temp[5]; |
|
|
|
|
accelPTest[2] = temp[6]; |
|
|
|
|
|
|
|
|
|
writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test
|
|
|
|
|
delay(100); // let accel respond
|
|
|
|
|
readData(temp); |
|
|
|
|
accelNTest[0] = temp[4]; |
|
|
|
|
accelNTest[1] = temp[5]; |
|
|
|
|
accelNTest[2] = temp[6]; |
|
|
|
|
|
|
|
|
|
writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
|
|
|
|
|
delay(100); // let gyro respond
|
|
|
|
|
readData(temp); |
|
|
|
|
gyroPTest[0] = temp[1]; |
|
|
|
|
gyroPTest[1] = temp[2]; |
|
|
|
|
gyroPTest[2] = temp[3]; |
|
|
|
|
|
|
|
|
|
writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
|
|
|
|
|
delay(100); // let gyro respond
|
|
|
|
|
readData(temp); |
|
|
|
|
gyroNTest[0] = temp[1]; |
|
|
|
|
gyroNTest[1] = temp[2]; |
|
|
|
|
gyroNTest[2] = temp[3]; |
|
|
|
|
|
|
|
|
|
writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode
|
|
|
|
|
delay(100); // let accel and gyro respond
|
|
|
|
|
|
|
|
|
|
Serial.println("Accel Self Test:"); |
|
|
|
|
Serial.print("+Ax results:"); Serial.print( (accelPTest[0] - accelNom[0]) * _aRes * 1000.0); Serial.println(" mg"); |
|
|
|
|
Serial.print("-Ax results:"); Serial.println((accelNTest[0] - accelNom[0]) * _aRes * 1000.0); |
|
|
|
|
Serial.print("+Ay results:"); Serial.println((accelPTest[1] - accelNom[1]) * _aRes * 1000.0); |
|
|
|
|
Serial.print("-Ay results:"); Serial.println((accelNTest[1] - accelNom[1]) * _aRes * 1000.0); |
|
|
|
|
Serial.print("+Az results:"); Serial.println((accelPTest[2] - accelNom[2]) * _aRes * 1000.0); |
|
|
|
|
Serial.print("-Az results:"); Serial.println((accelNTest[2] - accelNom[2]) * _aRes * 1000.0); |
|
|
|
|
Serial.println("Should be between 90 and 1700 mg"); |
|
|
|
|
|
|
|
|
|
Serial.println("Gyro Self Test:"); |
|
|
|
|
Serial.print("+Gx results:"); Serial.print((gyroPTest[0] - gyroNom[0]) * _gRes); Serial.println(" dps"); |
|
|
|
|
Serial.print("-Gx results:"); Serial.println((gyroNTest[0] - gyroNom[0]) * _gRes); |
|
|
|
|
Serial.print("+Gy results:"); Serial.println((gyroPTest[1] - gyroNom[1]) * _gRes); |
|
|
|
|
Serial.print("-Gy results:"); Serial.println((gyroNTest[1] - gyroNom[1]) * _gRes); |
|
|
|
|
Serial.print("+Gz results:"); Serial.println((gyroPTest[2] - gyroNom[2]) * _gRes); |
|
|
|
|
Serial.print("-Gz results:"); Serial.println((gyroNTest[2] - gyroNom[2]) * _gRes); |
|
|
|
|
Serial.println("Should be between 20 and 80 dps"); |
|
|
|
|
delay(2000); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
uint8_t temp = lsm6dsm_read_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C); |
|
|
|
|
// enable block update (bit 6 = 1), auto-increment registers (bit 2 = 1)
|
|
|
|
|
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x40 | 0x04); |
|
|
|
|
// by default, interrupts active HIGH, push pull, little endian data
|
|
|
|
|
// (can be changed by writing to bits 5, 4, and 1, resp to above register)
|
|
|
|
|
|
|
|
|
|
// enable accel LP2 (bit 7 = 1), set LP2 tp ODR/9 (bit 6 = 1), enable input_composite (bit 3) for low noise
|
|
|
|
|
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08); |
|
|
|
|
|
|
|
|
|
// interrupt handling
|
|
|
|
|
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read
|
|
|
|
|
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_INT1_CTRL, 0x40); // enable significant motion interrupts on INT1
|
|
|
|
|
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_INT2_CTRL, 0x03); // enable accel/gyro data ready interrupts on INT2
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void LSM6DSM::offsetBias(float * dest1, float * dest2) |
|
|
|
|
void lsm6dsm_selfTest() |
|
|
|
|
{ |
|
|
|
|
int16_t temp[7] = {0, 0, 0, 0, 0, 0, 0}; |
|
|
|
|
int32_t sum[7] = {0, 0, 0, 0, 0, 0, 0}; |
|
|
|
|
|
|
|
|
|
Serial.println("Calculate accel and gyro offset biases: keep sensor flat and motionless!"); |
|
|
|
|
delay(4000); |
|
|
|
|
|
|
|
|
|
for (int ii = 0; ii < 128; ii++) |
|
|
|
|
{ |
|
|
|
|
readData(temp); |
|
|
|
|
sum[1] += temp[1]; |
|
|
|
|
sum[2] += temp[2]; |
|
|
|
|
sum[3] += temp[3]; |
|
|
|
|
sum[4] += temp[4]; |
|
|
|
|
sum[5] += temp[5]; |
|
|
|
|
sum[6] += temp[6]; |
|
|
|
|
delay(50); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
dest1[0] = sum[1]*_gRes/128.0f; |
|
|
|
|
dest1[1] = sum[2]*_gRes/128.0f; |
|
|
|
|
dest1[2] = sum[3]*_gRes/128.0f; |
|
|
|
|
dest2[0] = sum[4]*_aRes/128.0f; |
|
|
|
|
dest2[1] = sum[5]*_aRes/128.0f; |
|
|
|
|
dest2[2] = sum[6]*_aRes/128.0f; |
|
|
|
|
|
|
|
|
|
if(dest2[0] > 0.8f) {dest2[0] -= 1.0f;} // Remove gravity from the x-axis accelerometer bias calculation
|
|
|
|
|
if(dest2[0] < -0.8f) {dest2[0] += 1.0f;} // Remove gravity from the x-axis accelerometer bias calculation
|
|
|
|
|
if(dest2[1] > 0.8f) {dest2[1] -= 1.0f;} // Remove gravity from the y-axis accelerometer bias calculation
|
|
|
|
|
if(dest2[1] < -0.8f) {dest2[1] += 1.0f;} // Remove gravity from the y-axis accelerometer bias calculation
|
|
|
|
|
if(dest2[2] > 0.8f) {dest2[2] -= 1.0f;} // Remove gravity from the z-axis accelerometer bias calculation
|
|
|
|
|
if(dest2[2] < -0.8f) {dest2[2] += 1.0f;} // Remove gravity from the z-axis accelerometer bias calculation
|
|
|
|
|
int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 }; |
|
|
|
|
int16_t accelPTest[3] = { 0, 0, 0 }, accelNTest[3] = { 0, 0, 0 }, |
|
|
|
|
gyroPTest[3] = { 0, 0, 0 }, gyroNTest[3] = { 0, 0, 0 }; |
|
|
|
|
int16_t accelNom[3] = { 0, 0, 0 }, gyroNom[3] = { 0, 0, 0 }; |
|
|
|
|
|
|
|
|
|
readData(temp); |
|
|
|
|
accelNom[0] = temp[4]; |
|
|
|
|
accelNom[1] = temp[5]; |
|
|
|
|
accelNom[2] = temp[6]; |
|
|
|
|
gyroNom[0] = temp[1]; |
|
|
|
|
gyroNom[1] = temp[2]; |
|
|
|
|
gyroNom[2] = temp[3]; |
|
|
|
|
|
|
|
|
|
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test
|
|
|
|
|
delay(100); // let accel respond
|
|
|
|
|
readData(temp); |
|
|
|
|
accelPTest[0] = temp[4]; |
|
|
|
|
accelPTest[1] = temp[5]; |
|
|
|
|
accelPTest[2] = temp[6]; |
|
|
|
|
|
|
|
|
|
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test
|
|
|
|
|
delay(100); // let accel respond
|
|
|
|
|
readData(temp); |
|
|
|
|
accelNTest[0] = temp[4]; |
|
|
|
|
accelNTest[1] = temp[5]; |
|
|
|
|
accelNTest[2] = temp[6]; |
|
|
|
|
|
|
|
|
|
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
|
|
|
|
|
delay(100); // let gyro respond
|
|
|
|
|
readData(temp); |
|
|
|
|
gyroPTest[0] = temp[1]; |
|
|
|
|
gyroPTest[1] = temp[2]; |
|
|
|
|
gyroPTest[2] = temp[3]; |
|
|
|
|
|
|
|
|
|
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
|
|
|
|
|
delay(100); // let gyro respond
|
|
|
|
|
readData(temp); |
|
|
|
|
gyroNTest[0] = temp[1]; |
|
|
|
|
gyroNTest[1] = temp[2]; |
|
|
|
|
gyroNTest[2] = temp[3]; |
|
|
|
|
|
|
|
|
|
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode
|
|
|
|
|
delay(100); // let accel and gyro respond
|
|
|
|
|
|
|
|
|
|
Serial.println("Accel Self Test:"); |
|
|
|
|
Serial.print("+Ax results:"); |
|
|
|
|
Serial.print((accelPTest[0] - accelNom[0]) * _aRes * 1000.0); |
|
|
|
|
Serial.println(" mg"); |
|
|
|
|
Serial.print("-Ax results:"); |
|
|
|
|
Serial.println((accelNTest[0] - accelNom[0]) * _aRes * 1000.0); |
|
|
|
|
Serial.print("+Ay results:"); |
|
|
|
|
Serial.println((accelPTest[1] - accelNom[1]) * _aRes * 1000.0); |
|
|
|
|
Serial.print("-Ay results:"); |
|
|
|
|
Serial.println((accelNTest[1] - accelNom[1]) * _aRes * 1000.0); |
|
|
|
|
Serial.print("+Az results:"); |
|
|
|
|
Serial.println((accelPTest[2] - accelNom[2]) * _aRes * 1000.0); |
|
|
|
|
Serial.print("-Az results:"); |
|
|
|
|
Serial.println((accelNTest[2] - accelNom[2]) * _aRes * 1000.0); |
|
|
|
|
Serial.println("Should be between 90 and 1700 mg"); |
|
|
|
|
|
|
|
|
|
Serial.println("Gyro Self Test:"); |
|
|
|
|
Serial.print("+Gx results:"); |
|
|
|
|
Serial.print((gyroPTest[0] - gyroNom[0]) * _gRes); |
|
|
|
|
Serial.println(" dps"); |
|
|
|
|
Serial.print("-Gx results:"); |
|
|
|
|
Serial.println((gyroNTest[0] - gyroNom[0]) * _gRes); |
|
|
|
|
Serial.print("+Gy results:"); |
|
|
|
|
Serial.println((gyroPTest[1] - gyroNom[1]) * _gRes); |
|
|
|
|
Serial.print("-Gy results:"); |
|
|
|
|
Serial.println((gyroNTest[1] - gyroNom[1]) * _gRes); |
|
|
|
|
Serial.print("+Gz results:"); |
|
|
|
|
Serial.println((gyroPTest[2] - gyroNom[2]) * _gRes); |
|
|
|
|
Serial.print("-Gz results:"); |
|
|
|
|
Serial.println((gyroNTest[2] - gyroNom[2]) * _gRes); |
|
|
|
|
Serial.println("Should be between 20 and 80 dps"); |
|
|
|
|
delay(2000); |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void LSM6DSM::readData(int16_t * destination) |
|
|
|
|
void lsm6dsm_offsetBias(float *dest1, float *dest2) |
|
|
|
|
{ |
|
|
|
|
uint8_t rawData[14]; // x/y/z accel register data stored here
|
|
|
|
|
readBytes(LSM6DSM_ADDRESS, LSM6DSM_OUT_TEMP_L, 14, &rawData[0]); // Read the 14 raw data registers into data array
|
|
|
|
|
destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
|
|
|
|
|
destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
|
|
|
|
|
destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
|
|
|
|
|
destination[3] = ((int16_t)rawData[7] << 8) | rawData[6] ;
|
|
|
|
|
destination[4] = ((int16_t)rawData[9] << 8) | rawData[8] ;
|
|
|
|
|
destination[5] = ((int16_t)rawData[11] << 8) | rawData[10] ;
|
|
|
|
|
destination[6] = ((int16_t)rawData[13] << 8) | rawData[12] ;
|
|
|
|
|
int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 }; |
|
|
|
|
int32_t sum[7] = { 0, 0, 0, 0, 0, 0, 0 }; |
|
|
|
|
|
|
|
|
|
Serial.println( |
|
|
|
|
"Calculate accel and gyro offset biases: keep sensor flat and motionless!"); |
|
|
|
|
delay(4000); |
|
|
|
|
|
|
|
|
|
for(int ii = 0; ii < 128; ii++) |
|
|
|
|
{ |
|
|
|
|
readData(temp); |
|
|
|
|
sum[1] += temp[1]; |
|
|
|
|
sum[2] += temp[2]; |
|
|
|
|
sum[3] += temp[3]; |
|
|
|
|
sum[4] += temp[4]; |
|
|
|
|
sum[5] += temp[5]; |
|
|
|
|
sum[6] += temp[6]; |
|
|
|
|
delay(50); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
dest1[0] = sum[1] * _gRes / 128.0f; |
|
|
|
|
dest1[1] = sum[2] * _gRes / 128.0f; |
|
|
|
|
dest1[2] = sum[3] * _gRes / 128.0f; |
|
|
|
|
dest2[0] = sum[4] * _aRes / 128.0f; |
|
|
|
|
dest2[1] = sum[5] * _aRes / 128.0f; |
|
|
|
|
dest2[2] = sum[6] * _aRes / 128.0f; |
|
|
|
|
|
|
|
|
|
if(dest2[0] > 0.8f) |
|
|
|
|
{ |
|
|
|
|
dest2[0] -= 1.0f; |
|
|
|
|
} // Remove gravity from the x-axis accelerometer bias calculation
|
|
|
|
|
if(dest2[0] < -0.8f) |
|
|
|
|
{ |
|
|
|
|
dest2[0] += 1.0f; |
|
|
|
|
} // Remove gravity from the x-axis accelerometer bias calculation
|
|
|
|
|
if(dest2[1] > 0.8f) |
|
|
|
|
{ |
|
|
|
|
dest2[1] -= 1.0f; |
|
|
|
|
} // Remove gravity from the y-axis accelerometer bias calculation
|
|
|
|
|
if(dest2[1] < -0.8f) |
|
|
|
|
{ |
|
|
|
|
dest2[1] += 1.0f; |
|
|
|
|
} // Remove gravity from the y-axis accelerometer bias calculation
|
|
|
|
|
if(dest2[2] > 0.8f) |
|
|
|
|
{ |
|
|
|
|
dest2[2] -= 1.0f; |
|
|
|
|
} // Remove gravity from the z-axis accelerometer bias calculation
|
|
|
|
|
if(dest2[2] < -0.8f) |
|
|
|
|
{ |
|
|
|
|
dest2[2] += 1.0f; |
|
|
|
|
} // Remove gravity from the z-axis accelerometer bias calculation
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// I2C scan function
|
|
|
|
|
void LSM6DSM::I2Cscan() |
|
|
|
|
void lsm6dsm_read_data(int16_t *destination) |
|
|
|
|
{ |
|
|
|
|
// scan for i2c devices
|
|
|
|
|
byte error, address; |
|
|
|
|
int nDevices; |
|
|
|
|
|
|
|
|
|
Serial.println("Scanning..."); |
|
|
|
|
|
|
|
|
|
nDevices = 0; |
|
|
|
|
for(address = 1; address < 127; address++ )
|
|
|
|
|
{ |
|
|
|
|
// The i2c_scanner uses the return value of
|
|
|
|
|
// the Write.endTransmission to see if
|
|
|
|
|
// a device did acknowledge to the address.
|
|
|
|
|
// Wire.beginTransmission(address);
|
|
|
|
|
// error = Wire.endTransmission();
|
|
|
|
|
error = Wire.transfer(address, NULL, 0, NULL, 0); |
|
|
|
|
|
|
|
|
|
if (error == 0) |
|
|
|
|
{ |
|
|
|
|
Serial.print("I2C device found at address 0x"); |
|
|
|
|
if (address<16)
|
|
|
|
|
Serial.print("0"); |
|
|
|
|
Serial.print(address,HEX); |
|
|
|
|
Serial.println(" !"); |
|
|
|
|
|
|
|
|
|
nDevices++; |
|
|
|
|
} |
|
|
|
|
else if (error==4)
|
|
|
|
|
{ |
|
|
|
|
Serial.print("Unknown error at address 0x"); |
|
|
|
|
if (address<16)
|
|
|
|
|
Serial.print("0"); |
|
|
|
|
Serial.println(address,HEX); |
|
|
|
|
}
|
|
|
|
|
} |
|
|
|
|
if (nDevices == 0) |
|
|
|
|
Serial.println("No I2C devices found\n"); |
|
|
|
|
else |
|
|
|
|
Serial.println("done\n"); |
|
|
|
|
|
|
|
|
|
uint8_t rawdata[14]; // x/y/z accel register data stored here
|
|
|
|
|
lsm6dsm_read(LSM6DSM_ADDRESS, LSM6DSM_OUT_TEMP_L, 14, &rawdata[0]); // Read the 14 raw data registers into data array
|
|
|
|
|
destination[0] = ((int16_t) rawdata[1] << 8) | rawdata[0]; // Turn the MSB and LSB into a signed 16-bit value
|
|
|
|
|
destination[1] = ((int16_t) rawdata[3] << 8) | rawdata[2]; |
|
|
|
|
destination[2] = ((int16_t) rawdata[5] << 8) | rawdata[4]; |
|
|
|
|
destination[3] = ((int16_t) rawdata[7] << 8) | rawdata[6]; |
|
|
|
|
destination[4] = ((int16_t) rawdata[9] << 8) | rawdata[8]; |
|
|
|
|
destination[5] = ((int16_t) rawdata[11] << 8) | rawdata[10]; |
|
|
|
|
destination[6] = ((int16_t) rawdata[13] << 8) | rawdata[12]; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// I2C read/write functions for the LSM6DSM
|
|
|
|
|
static void lsm6dsm_write_byte(uint8_t address, uint8_t subAddress, |
|
|
|
|
uint8_t data) |
|
|
|
|
{ |
|
|
|
|
uint8_t temp[2]; |
|
|
|
|
temp[0] = subAddress; |
|
|
|
|
temp[1] = data; |
|
|
|
|
Wire.transfer(address, &temp[0], 2, NULL, 0); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void LSM6DSM::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) { |
|
|
|
|
uint8_t temp[2]; |
|
|
|
|
temp[0] = subAddress; |
|
|
|
|
temp[1] = data; |
|
|
|
|
Wire.transfer(address, &temp[0], 2, NULL, 0);
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
uint8_t LSM6DSM::readByte(uint8_t address, uint8_t subAddress) { |
|
|
|
|
uint8_t temp[1]; |
|
|
|
|
Wire.transfer(address, &subAddress, 1, &temp[0], 1); |
|
|
|
|
return temp[0]; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void LSM6DSM::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) { |
|
|
|
|
Wire.transfer(address, &subAddress, 1, dest, count);
|
|
|
|
|
} |
|
|
|
|
static uint8_t lsm6dsm_read_byte(uint8_t address, uint8_t subAddress) |
|
|
|
|
{ |
|
|
|
|
uint8_t temp[1]; |
|
|
|
|
Wire.transfer(address, &subAddress, 1, &temp[0], 1); |
|
|
|
|
return temp[0]; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
static void lsm6dsm_read(uint8_t address, uint8_t subAddress, uint8_t count, |
|
|
|
|
uint8_t *dest) |
|
|
|
|
{ |
|
|
|
|
Wire.transfer(address, &subAddress, 1, dest, count); |
|
|
|
|
} |
|
|
|
|
|
