dan
/
EM7180_STM32
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
101 Commits
1 Branch
0 Tags
3.5 MiB
 
Tree: 4b6714b617
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '4b6714b617'
${ noResults }
EM7180_STM32/Drivers/EM7180/Src/lsm6dsm.c

431 lines
14 KiB
Raw Blame History

/*
* lsm6dsm.c
* The LSM6DSM is a sensor hub with embedded accelerometer and gyroscope, here
* used as 6 DoF in a 10 DoF absolute orientation solution.
*
* Created on: Jan 18, 2021
* Author: Daniel Peter Chokola
*
* Adapted From:
* EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly
* by: Kris Winer
* 09/23/2017 Copyright Tlera Corporation
*
* Library may be used freely and without limit with attribution.
*/
/* Includes */
#include <imu_common.h>
#include <lsm6dsm.h>
#include <stddef.h>
/* Definitions */
/*
* LSM6DSM registers
* http://www.st.com/content/ccc/resource/technical/document/datasheet/76/27/cf/88/c5/03/42/6b/DM00218116.pdf/files/DM00218116.pdf/jcr:content/translations/en.DM00218116.pdf
*/
#define LSM6DSM_FUNC_CFG_ACCESS 0x01
#define LSM6DSM_SENSOR_SYNC_TIME_FRAME 0x04
#define LSM6DSM_SENSOR_SYNC_RES_RATIO 0x05
#define LSM6DSM_FIFO_CTRL1 0x06
#define LSM6DSM_FIFO_CTRL2 0x07
#define LSM6DSM_FIFO_CTRL3 0x08
#define LSM6DSM_FIFO_CTRL4 0x09
#define LSM6DSM_FIFO_CTRL5 0x0A
#define LSM6DSM_DRDY_PULSE_CFG 0x0B
#define LSM6DSM_INT1_CTRL 0x0D
#define LSM6DSM_INT2_CTRL 0x0E
#define LSM6DSM_WHO_AM_I 0x0F // should be 0x6A
#define LSM6DSM_CTRL1_XL 0x10
#define LSM6DSM_CTRL2_G 0x11
#define LSM6DSM_CTRL3_C 0x12
#define LSM6DSM_CTRL4_C 0x13
#define LSM6DSM_CTRL5_C 0x14
#define LSM6DSM_CTRL6_C 0x15
#define LSM6DSM_CTRL7_G 0x16
#define LSM6DSM_CTRL8_XL 0x17
#define LSM6DSM_CTRL9_XL 0x18
#define LSM6DSM_CTRL10_C 0x19
#define LSM6DSM_MASTER_CONFIG 0x1A
#define LSM6DSM_WAKE_UP_SRC 0x1B
#define LSM6DSM_TAP_SRC 0x1C
#define LSM6DSM_D6D_SRC 0x1D
#define LSM6DSM_STATUS_REG 0x1E
#define LSM6DSM_OUT_TEMP_L 0x20
#define LSM6DSM_OUT_TEMP_H 0x21
#define LSM6DSM_OUTX_L_G 0x22
#define LSM6DSM_OUTX_H_G 0x23
#define LSM6DSM_OUTY_L_G 0x24
#define LSM6DSM_OUTY_H_G 0x25
#define LSM6DSM_OUTZ_L_G 0x26
#define LSM6DSM_OUTZ_H_G 0x27
#define LSM6DSM_OUTX_L_XL 0x28
#define LSM6DSM_OUTX_H_XL 0x29
#define LSM6DSM_OUTY_L_XL 0x2A
#define LSM6DSM_OUTY_H_XL 0x2B
#define LSM6DSM_OUTZ_L_XL 0x2C
#define LSM6DSM_OUTZ_H_XL 0x2D
#define LSM6DSM_SENSORHUB1_REG 0x2E
#define LSM6DSM_SENSORHUB2_REG 0x2F
#define LSM6DSM_SENSORHUB3_REG 0x30
#define LSM6DSM_SENSORHUB4_REG 0x31
#define LSM6DSM_SENSORHUB5_REG 0x32
#define LSM6DSM_SENSORHUB6_REG 0x33
#define LSM6DSM_SENSORHUB7_REG 0x34
#define LSM6DSM_SENSORHUB8_REG 0x35
#define LSM6DSM_SENSORHUB9_REG 0x36
#define LSM6DSM_SENSORHUB10_REG 0x37
#define LSM6DSM_SENSORHUB11_REG 0x38
#define LSM6DSM_SENSORHUB12_REG 0x39
#define LSM6DSM_FIFO_STATUS1 0x3A
#define LSM6DSM_FIFO_STATUS2 0x3B
#define LSM6DSM_FIFO_STATUS3 0x3C
#define LSM6DSM_FIFO_STATUS4 0x3D
#define LSM6DSM_FIFO_DATA_OUT_L 0x3E
#define LSM6DSM_FIFO_DATA_OUT_H 0x3F
#define LSM6DSM_TIMESTAMP0_REG 0x40
#define LSM6DSM_TIMESTAMP1_REG 0x41
#define LSM6DSM_TIMESTAMP2_REG 0x42
#define LSM6DSM_STEP_TIMESTAMP_L 0x49
#define LSM6DSM_STEP_TIMESTAMP_H 0x4A
#define LSM6DSM_STEP_COUNTER_L 0x4B
#define LSM6DSM_STEP_COUNTER_H 0x4C
#define LSM6DSM_SENSORHUB13_REG 0x4D
#define LSM6DSM_SENSORHUB14_REG 0x4E
#define LSM6DSM_SENSORHUB15_REG 0x4F
#define LSM6DSM_SENSORHUB16_REG 0x50
#define LSM6DSM_SENSORHUB17_REG 0x51
#define LSM6DSM_SENSORHUB18_REG 0x52
#define LSM6DSM_FUNC_SRC1 0x53
#define LSM6DSM_FUNC_SRC2 0x54
#define LSM6DSM_WRIST_TILT_IA 0x55
#define LSM6DSM_TAP_CFG 0x58
#define LSM6DSM_TAP_THS_6D 0x59
#define LSM6DSM_INT_DUR2 0x5A
#define LSM6DSM_WAKE_UP_THS 0x5B
#define LSM6DSM_WAKE_UP_DUR 0x5C
#define LSM6DSM_FREE_FALL 0x5D
#define LSM6DSM_MD1_CFG 0x5E
#define LSM6DSM_MD2_CFG 0x5F
#define LSM6DSM_MASTER_MODE_CODE 0x60
#define LSM6DSM_SENS_SYNC_SPI_ERROR_CODE 0x61
#define LSM6DSM_OUT_MAG_RAW_X_L 0x66
#define LSM6DSM_OUT_MAG_RAW_X_H 0x67
#define LSM6DSM_OUT_MAG_RAW_Y_L 0x68
#define LSM6DSM_OUT_MAG_RAW_Y_H 0x69
#define LSM6DSM_OUT_MAG_RAW_Z_L 0x6A
#define LSM6DSM_OUT_MAG_RAW_Z_H 0x6B
#define LSM6DSM_INT_OIS 0x6F
#define LSM6DSM_CTRL1_OIS 0x70
#define LSM6DSM_CTRL2_OIS 0x71
#define LSM6DSM_CTRL3_OIS 0x72
#define LSM6DSM_X_OFS_USR 0x73
#define LSM6DSM_Y_OFS_USR 0x74
#define LSM6DSM_Z_OFS_USR 0x75
/* Macros */
#define lsm6dsm_read_byte(addr, byte) i2c_read_byte((lsm6dsm->i2c_read_func), (lsm6dsm->i2c_addr), (addr), (byte))
#define lsm6dsm_write_byte(addr, byte) i2c_write_byte((lsm6dsm->i2c_write_func), (lsm6dsm->i2c_addr), (addr), (byte))
#define lsm6dsm_read(addr, data, len) i2c_read((lsm6dsm->i2c_read_func), (lsm6dsm->i2c_addr), (addr), (data), (len))
#define lsm6dsm_write(addr, data, len) i2c_write((lsm6dsm->i2c_write_func), (lsm6dsm->i2c_addr), (addr), (data), (len))
/* Private Global Variables */
/* Function Prototypes */
/* Function Definitions */
lsm6dsm_status_t lsm6dsm_init(lsm6dsm_t *lsm6dsm, lsm6dsm_init_t *init)
{
int8_t *ptr = (int8_t*) lsm6dsm;
size_t i;
return_val_if_fail(lsm6dsm, LSM6DSM_BAD_ARG);
return_val_if_fail(init, LSM6DSM_BAD_ARG);
/* zero lsm6dsm_t struct */
for(i = 0; i < sizeof(lsm6dsm_t); i++)
{
*ptr++ = 0;
}
lsm6dsm->init = init;
return LSM6DSM_OK;
}
void lsm6dsm_set_delay_cb(lsm6dsm_t *lsm6dsm, delay_func_t delay_func)
{
return_if_fail(lsm6dsm);
lsm6dsm->delay_func = delay_func;
}
void lsm6dsm_set_i2c_cbs(lsm6dsm_t *lsm6dsm, i2c_read_func_t i2c_read_func,
i2c_write_func_t i2c_write_func, uint8_t dev_addr)
{
return_if_fail(lsm6dsm);
lsm6dsm->i2c_read_func = i2c_read_func;
lsm6dsm->i2c_write_func = i2c_write_func;
lsm6dsm->i2c_addr = dev_addr;
}
lsm6dsm_status_t lsm6dsm_config(lsm6dsm_t *lsm6dsm)
{
int32_t ret = 0;
uint8_t temp;
ret |= lsm6dsm_write_byte(
LSM6DSM_CTRL1_XL,
lsm6dsm->init->a_odr << 4 | lsm6dsm->init->ascale << 2);
ret |= lsm6dsm_write_byte(
LSM6DSM_CTRL2_G,
lsm6dsm->init->g_odr << 4 | lsm6dsm->init->gscale << 2);
ret |= lsm6dsm_read_byte(LSM6DSM_CTRL3_C, &temp);
// enable block update (bit 6 = 1), auto-increment registers (bit 2 = 1)
ret |= lsm6dsm_write_byte(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
ret |= lsm6dsm_write_byte(LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08);
// interrupt handling
ret |= lsm6dsm_write_byte(LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read
ret |= lsm6dsm_write_byte(LSM6DSM_INT1_CTRL, 0x40); // enable significant motion interrupts on INT1
ret |= lsm6dsm_write_byte(LSM6DSM_INT2_CTRL, 0x03); // enable accel/gyro data ready interrupts on INT2
return ret ? LSM6DSM_BAD_COMM : LSM6DSM_OK;
}
/* FIXME: haven't explored the usage/usefulness of these yet: */
#if(0)
uint8_t lsm6dsm_chip_id_get(lsm6dsm_t *lsm6dsm)
{
uint8_t c;
lsm6dsm_read_byte(LSM6DSM_WHO_AM_I, &c);
return c;
}
float lsm6dsm_ares_get(lsm6dsm_t *lsm6dsm)
{
float a_res;
switch(lsm6dsm->init->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:
a_res = 2.0f / 32768.0f;
break;
case AFS_4G:
a_res = 4.0f / 32768.0f;
break;
case AFS_8G:
a_res = 8.0f / 32768.0f;
break;
case AFS_16G:
a_res = 16.0f / 32768.0f;
break;
default:
a_res = NAN;
break;
}
return a_res;
}
float lsm6dsm_gres_get(lsm6dsm_t *lsm6dsm)
{
float g_res;
switch(lsm6dsm->init->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_250DPS:
g_res = 250.0f / 32768.0f;
break;
case GFS_500DPS:
g_res = 500.0f / 32768.0f;
break;
case GFS_1000DPS:
g_res = 1000.0f / 32768.0f;
break;
case GFS_2000DPS:
g_res = 2000.0f / 32768.0f;
break;
default:
g_res = NAN;
break;
}
return g_res;
}
void lsm6dsm_reset(lsm6dsm_t *lsm6dsm)
{
// reset device
uint8_t temp;
lsm6dsm_read_byte(LSM6DSM_CTRL3_C, &temp);
lsm6dsm_write_byte(LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM
lsm6dsm->delay_func(100); // Wait for all registers to reset
}
void lsm6dsm_self_test(lsm6dsm_t *lsm6dsm)
{
int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 };
int16_t accelPTest[3] = { 0, 0, 0 };
int16_t accelNTest[3] = { 0, 0, 0 };
int16_t gyroPTest[3] = { 0, 0, 0 };
int16_t gyroNTest[3] = { 0, 0, 0 };
int16_t accelNom[3] = { 0, 0, 0 };
int16_t gyroNom[3] = { 0, 0, 0 };
lsm6dsm_read_data(lsm6dsm, 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_CTRL5_C, 0x01); // positive accel self test
lsm6dsm->delay_func(100); // let accel respond
lsm6dsm_read_data(lsm6dsm, temp);
accelPTest[0] = temp[4];
accelPTest[1] = temp[5];
accelPTest[2] = temp[6];
lsm6dsm_write_byte(LSM6DSM_CTRL5_C, 0x03); // negative accel self test
lsm6dsm->delay_func(100); // let accel respond
lsm6dsm_read_data(lsm6dsm, temp);
accelNTest[0] = temp[4];
accelNTest[1] = temp[5];
accelNTest[2] = temp[6];
lsm6dsm_write_byte(LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
lsm6dsm->delay_func(100); // let gyro respond
lsm6dsm_read_data(lsm6dsm, temp);
gyroPTest[0] = temp[1];
gyroPTest[1] = temp[2];
gyroPTest[2] = temp[3];
lsm6dsm_write_byte(LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
lsm6dsm->delay_func(100); // let gyro respond
lsm6dsm_read_data(lsm6dsm, temp);
gyroNTest[0] = temp[1];
gyroNTest[1] = temp[2];
gyroNTest[2] = temp[3];
lsm6dsm_write_byte(LSM6DSM_CTRL5_C, 0x00); // normal mode
lsm6dsm->delay_func(100); // let accel and gyro respond
/* Serial.println("Accel Self Test:"); */
/* Serial.print("+Ax results:"); */
/* Serial.print((accelPTest[0] - accelNom[0]) * a_res * 1000.0); */
/* Serial.println(" mg"); */
/* Serial.print("-Ax results:"); */
/* Serial.println((accelNTest[0] - accelNom[0]) * a_res * 1000.0); */
/* Serial.print("+Ay results:"); */
/* Serial.println((accelPTest[1] - accelNom[1]) * a_res * 1000.0); */
/* Serial.print("-Ay results:"); */
/* Serial.println((accelNTest[1] - accelNom[1]) * a_res * 1000.0); */
/* Serial.print("+Az results:"); */
/* Serial.println((accelPTest[2] - accelNom[2]) * a_res * 1000.0); */
/* Serial.print("-Az results:"); */
/* Serial.println((accelNTest[2] - accelNom[2]) * a_res * 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]) * g_res); */
/* Serial.println(" dps"); */
/* Serial.print("-Gx results:"); */
/* Serial.println((gyroNTest[0] - gyroNom[0]) * g_res); */
/* Serial.print("+Gy results:"); */
/* Serial.println((gyroPTest[1] - gyroNom[1]) * g_res); */
/* Serial.print("-Gy results:"); */
/* Serial.println((gyroNTest[1] - gyroNom[1]) * g_res); */
/* Serial.print("+Gz results:"); */
/* Serial.println((gyroPTest[2] - gyroNom[2]) * g_res); */
/* Serial.print("-Gz results:"); */
/* Serial.println((gyroNTest[2] - gyroNom[2]) * g_res); */
/* Serial.println("Should be between 20 and 80 dps"); */
lsm6dsm->delay_func(2000);
}
void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, float *dest1, float *dest2)
{
int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 };
int32_t sum[7] = { 0, 0, 0, 0, 0, 0, 0 };
float a_res = lsm6dsm_ares_get(lsm6dsm);
float g_res = lsm6dsm_gres_get(lsm6dsm);
/* Serial.println("Calculate accel and gyro offset biases: keep sensor flat and motionless!"); */
lsm6dsm->delay_func(4000);
for(int ii = 0; ii < 128; ii++)
{
lsm6dsm_read_data(lsm6dsm, 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];
lsm6dsm->delay_func(50);
}
dest1[0] = sum[1] * g_res / 128.0f;
dest1[1] = sum[2] * g_res / 128.0f;
dest1[2] = sum[3] * g_res / 128.0f;
dest2[0] = sum[4] * a_res / 128.0f;
dest2[1] = sum[5] * a_res / 128.0f;
dest2[2] = sum[6] * a_res / 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
}
static void lsm6dsm_read_data(lsm6dsm_t *lsm6dsm, int16_t *destination)
{
uint8_t data[14]; // x/y/z accel register data stored here
lsm6dsm_read(LSM6DSM_OUT_TEMP_L, data, 14); // Read the 14 raw data registers into data array
destination[0] = ((int16_t) data[1] << 8) | data[0]; // Turn the MSB and LSB into a signed 16-bit value
destination[1] = ((int16_t) data[3] << 8) | data[2];
destination[2] = ((int16_t) data[5] << 8) | data[4];
destination[3] = ((int16_t) data[7] << 8) | data[6];
destination[4] = ((int16_t) data[9] << 8) | data[8];
destination[5] = ((int16_t) data[11] << 8) | data[10];
destination[6] = ((int16_t) data[13] << 8) | data[12];
}
#endif