ST Americas mbed Team
/
Nucleo_Sensor_Shield
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: Nucleo_Sensors_Demo m2x-temp_ethernet_demo m2x-MEMS_ACKme_Wifi_demo m2x_MEMS_Ublox_Cellular_demo ... more
Fork of
Nucleo_Sensor_Shield
by 
Daniel Griffin
Diff: Components/LSM6DS0/lsm6ds0.cpp
- Revision:
- 1:354f61ffdcfb
- Parent:
- 0:0433918efb54
--- a/Components/LSM6DS0/lsm6ds0.cpp Sat Dec 13 01:09:48 2014 +0000
+++ b/Components/LSM6DS0/lsm6ds0.cpp Sat Dec 13 02:33:16 2014 +0000
@@ -61,19 +61,22 @@
//IMU_6AXES_IO_Read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
ret = dev_i2c.i2c_read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
- tempReg &= LSM6DS0_G_FS_MASK;
-
- switch(tempReg)
+ if (ret == 0)
{
- case LSM6DS0_G_FS_245:
- sensitivity = 8.75;
- break;
- case LSM6DS0_G_FS_500:
- sensitivity = 17.50;
- break;
- case LSM6DS0_G_FS_2000:
- sensitivity = 70;
- break;
+ tempReg &= LSM6DS0_G_FS_MASK;
+
+ switch(tempReg)
+ {
+ case LSM6DS0_G_FS_245:
+ sensitivity = 8.75;
+ break;
+ case LSM6DS0_G_FS_500:
+ sensitivity = 17.50;
+ break;
+ case LSM6DS0_G_FS_2000:
+ sensitivity = 70;
+ break;
+ }
}
pData->AXIS_X = (int32_t)(pDataRaw[0] * sensitivity);
@@ -92,20 +95,31 @@
uint8_t tempReg[2] = {0,0};
int ret;
+ pData[0] = pData[1] = pData[2] = 0;
+
//IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_G + 0x80, 2);
ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_G + 0x80, 2);
- pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
-
- //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_G + 0x80, 2);
- ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_G + 0x80, 2);
+ if (ret == 0)
+ {
+ pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
- pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
+ //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_G + 0x80, 2);
+ ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_G + 0x80, 2);
+ }
+
+ if (ret == 0)
+ {
+ pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
- //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_G + 0x80, 2);
- ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_G + 0x80, 2);
+ //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_G + 0x80, 2);
+ ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_G + 0x80, 2);
+ }
- pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
+ if (ret == 0)
+ {
+ pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
+ }
}
@@ -126,19 +140,22 @@
//IMU_6AXES_IO_Read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
ret = dev_i2c.i2c_read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
- tempReg &= LSM6DS0_XL_FS_MASK;
-
- switch(tempReg)
+ if (ret == 0)
{
- case LSM6DS0_XL_FS_2G:
- sensitivity = 0.061;
- break;
- case LSM6DS0_XL_FS_4G:
- sensitivity = 0.122;
- break;
- case LSM6DS0_XL_FS_8G:
- sensitivity = 0.244;
- break;
+ tempReg &= LSM6DS0_XL_FS_MASK;
+
+ switch(tempReg)
+ {
+ case LSM6DS0_XL_FS_2G:
+ sensitivity = 0.061;
+ break;
+ case LSM6DS0_XL_FS_4G:
+ sensitivity = 0.122;
+ break;
+ case LSM6DS0_XL_FS_8G:
+ sensitivity = 0.244;
+ break;
+ }
}
pData->AXIS_X = (int32_t)(pDataRaw[0] * sensitivity);
@@ -157,20 +174,31 @@
uint8_t tempReg[2] = {0,0};
int ret;
+ pData[0] = pData[1] = pData[2] = 0;
+
//IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_XL + 0x80, 2);
ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_XL + 0x80, 2);
- pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
-
- //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_XL + 0x80, 2);
- ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_XL + 0x80, 2);
+ if (ret == 0)
+ {
+ pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
- pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
-
- //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_XL + 0x80, 2);
- ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_XL + 0x80, 2);
-
- pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
+ //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_XL + 0x80, 2);
+ ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_XL + 0x80, 2);
+ }
+
+ if (ret == 0)
+ {
+ pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
+
+ //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_XL + 0x80, 2);
+ ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_XL + 0x80, 2);
+ }
+
+ if (ret == 0)
+ {
+ pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
+ }
}
/**
@@ -188,7 +216,7 @@
ret = dev_i2c.i2c_read(&tmp, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_WHO_AM_I_ADDR, 1);
/* Return the ID */
- return (uint8_t)tmp;
+ return ((ret == 0) ? (uint8_t)tmp : 0);
}
/**
@@ -206,81 +234,99 @@
//IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
- /* Output Data Rate selection */
- tmp1 &= ~(LSM6DS0_G_ODR_MASK);
- tmp1 |= LSM6DS0_G_ODR_119HZ;
+ if (ret == 0)
+ {
+ /* Output Data Rate selection */
+ tmp1 &= ~(LSM6DS0_G_ODR_MASK);
+ tmp1 |= LSM6DS0_G_ODR_119HZ;
+
+ /* Full scale selection */
+ tmp1 &= ~(LSM6DS0_G_FS_MASK);
+ tmp1 |= LSM6DS0_G_FS_2000;
- /* Full scale selection */
- tmp1 &= ~(LSM6DS0_G_FS_MASK);
- tmp1 |= LSM6DS0_G_FS_2000;
+ //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
+ ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
+ }
+
+ if (ret == 0)
+ {
+ //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
+ ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
+ }
- //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
- ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
+ if (ret == 0)
+ {
+ /* Enable X axis selection */
+ tmp1 &= ~(LSM6DS0_G_XEN_MASK);
+ tmp1 |= LSM6DS0_G_XEN_ENABLE;
- //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
- ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
+ /* Enable Y axis selection */
+ tmp1 &= ~(LSM6DS0_G_YEN_MASK);
+ tmp1 |= LSM6DS0_G_YEN_ENABLE;
+
+ /* Enable Z axis selection */
+ tmp1 &= ~(LSM6DS0_G_ZEN_MASK);
+ tmp1 |= LSM6DS0_G_ZEN_ENABLE;
+
+ //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
+ ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
+ }
- /* Enable X axis selection */
- tmp1 &= ~(LSM6DS0_G_XEN_MASK);
- tmp1 |= LSM6DS0_G_XEN_ENABLE;
+/******************************/
+/***** Accelerometer init *****/
+ if (ret == 0)
+ {
+ //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
+ ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
+ }
+
+ if (ret == 0)
+ {
+ /* Output Data Rate selection */
+ tmp1 &= ~(LSM6DS0_XL_ODR_MASK);
+ tmp1 |= LSM6DS0_XL_ODR_119HZ;
- /* Enable Y axis selection */
- tmp1 &= ~(LSM6DS0_G_YEN_MASK);
- tmp1 |= LSM6DS0_G_YEN_ENABLE;
+ /* Full scale selection */
+ tmp1 &= ~(LSM6DS0_XL_FS_MASK);
+ tmp1 |= LSM6DS0_XL_FS_2G;
- /* Enable Z axis selection */
- tmp1 &= ~(LSM6DS0_G_ZEN_MASK);
- tmp1 |= LSM6DS0_G_ZEN_ENABLE;
+ //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
+ ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
+ }
+
+ if (ret == 0)
+ {
+ //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
+ ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
+ }
+
+ if (ret == 0)
+ {
+ /* Enable X axis selection */
+ tmp1 &= ~(LSM6DS0_XL_XEN_MASK);
+ tmp1 |= LSM6DS0_XL_XEN_ENABLE;
- //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
- ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
+ /* Enable Y axis selection */
+ tmp1 &= ~(LSM6DS0_XL_YEN_MASK);
+ tmp1 |= LSM6DS0_XL_YEN_ENABLE;
+ /* Enable Z axis selection */
+ tmp1 &= ~(LSM6DS0_XL_ZEN_MASK);
+ tmp1 |= LSM6DS0_XL_ZEN_ENABLE;
+
+ //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
+ ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
+ }
/******************************/
-/***** Accelerometer init *****/
-
- //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
- ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
-
- /* Output Data Rate selection */
- tmp1 &= ~(LSM6DS0_XL_ODR_MASK);
- tmp1 |= LSM6DS0_XL_ODR_119HZ;
-
- /* Full scale selection */
- tmp1 &= ~(LSM6DS0_XL_FS_MASK);
- tmp1 |= LSM6DS0_XL_FS_2G;
-
- //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
- ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
-
-
- //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
- ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
-
- /* Enable X axis selection */
- tmp1 &= ~(LSM6DS0_XL_XEN_MASK);
- tmp1 |= LSM6DS0_XL_XEN_ENABLE;
-
- /* Enable Y axis selection */
- tmp1 &= ~(LSM6DS0_XL_YEN_MASK);
- tmp1 |= LSM6DS0_XL_YEN_ENABLE;
-
- /* Enable Z axis selection */
- tmp1 &= ~(LSM6DS0_XL_ZEN_MASK);
- tmp1 |= LSM6DS0_XL_ZEN_ENABLE;
-
- //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
- ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
-
-/******************************/
-
- if(ReadID() == I_AM_LSM6DS0_XG)
- {
+ if (ret == 0)
+ {
+ if(ReadID() == I_AM_LSM6DS0_XG)
+ {
LSM6DS0Initialized = 1;
//ret = HUM_TEMP_OK;
+ }
}
return;
}
-
-