dan
/
EM7180_STM32
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

implement ST-like API for LIS2MDL, LPS22HB, and LSM6DSM

master
Daniel Peter Chokola 4 years ago
parent
d0f01e53a4
commit
d19f47865a
7 changed files with 483 additions and 324 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 51
      Drivers/EM7180/Inc/lis2mdl.h
  2. 60
      Drivers/EM7180/Inc/lps22hb.h
  3. 132
      Drivers/EM7180/Inc/lsm6dsm.h
  4. 2
      Drivers/EM7180/Src/em7180.c
  5. 159
      Drivers/EM7180/Src/lis2mdl.c
  6. 134
      Drivers/EM7180/Src/lps22hb.c
  7. 251
      Drivers/EM7180/Src/lsm6dsm.c

51
Drivers/EM7180/Inc/lis2mdl.h
Unescape View File

@ -19,53 +19,38 @@ @@ -19,53 +19,38 @@
/* Includes */
#include <stdint.h>
#include "i2c.h"
/* Definitions */
//Register map for LIS2MDL'
// http://www.st.com/content/ccc/resource/technical/document/datasheet/group3/29/13/d1/e0/9a/4d/4f/30/DM00395193/files/DM00395193.pdf/jcr:content/translations/en.DM00395193.pdf
#define LIS2MDL_OFFSET_X_REG_L 0x45
#define LIS2MDL_OFFSET_X_REG_H 0x46
#define LIS2MDL_OFFSET_Y_REG_L 0x47
#define LIS2MDL_OFFSET_Y_REG_H 0x48
#define LIS2MDL_OFFSET_Z_REG_L 0x49
#define LIS2MDL_OFFSET_Z_REG_H 0x4A
#define LIS2MDL_WHO_AM_I 0x4F
#define LIS2MDL_CFG_REG_A 0x60
#define LIS2MDL_CFG_REG_B 0x61
#define LIS2MDL_CFG_REG_C 0x62
#define LIS2MDL_INT_CTRL_REG 0x63
#define LIS2MDL_INT_SOURCE_REG 0x64
#define LIS2MDL_INT_THS_L_REG 0x65
#define LIS2MDL_INT_THS_H_REG 0x66
#define LIS2MDL_STATUS_REG 0x67
#define LIS2MDL_OUTX_L_REG 0x68
#define LIS2MDL_OUTX_H_REG 0x69
#define LIS2MDL_OUTY_L_REG 0x6A
#define LIS2MDL_OUTY_H_REG 0x6B
#define LIS2MDL_OUTZ_L_REG 0x6C
#define LIS2MDL_OUTZ_H_REG 0x6D
#define LIS2MDL_TEMP_OUT_L_REG 0x6E
#define LIS2MDL_TEMP_OUT_H_REG 0x6F
#define LIS2MDL_ADDRESS 0x1E
#define LIS2MDL_OK (0)
#define LIS2MDL_BAD_ARG (1 << 0)
#define LIS2MDL_BAD_COMM (1 << 1)
#define MODR_10Hz 0x00
#define MODR_20Hz 0x01
#define MODR_50Hz 0x02
#define MODR_100Hz 0x03
#define MFS_14BITS 0 // 0.6 mG per LSB
#define MFS_16BITS 1 // 0.15 mG per LSB
/* Data Structures */
typedef struct lis2mdl_s
typedef int32_t lis2mdl_status_t;
typedef struct lis2mdl_init_s
{
uint8_t m_odr;
} lis2mdl_init_t;
typedef struct lis2mdl_s
{
lis2mdl_init_t *init;
uint8_t i2c_addr;
delay_func_t delay_func;
i2c_read_func_t i2c_read_func;
i2c_write_func_t i2c_write_func;
} lis2mdl_t;
/* Function Prototypes */
void lis2mdl_init(lis2mdl_t *lis2mdl, uint8_t m_odr);
void lis2mdl_config(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c);
lis2mdl_status_t lis2mdl_init(lis2mdl_t *lis2mdl, lis2mdl_init_t *init);
void lis2mdl_set_delay_cb(lis2mdl_t *lis2mdl, delay_func_t delay_func);
void lis2mdl_set_i2c_cbs(lis2mdl_t *lis2mdl, i2c_read_func_t i2c_read_func,
i2c_write_func_t i2c_write_func, uint8_t dev_addr);
lis2mdl_status_t lis2mdl_config(lis2mdl_t *lis2mdl);
#endif

60
Drivers/EM7180/Inc/lps22hb.h
Unescape View File

@ -19,53 +19,43 @@ @@ -19,53 +19,43 @@
/* Includes */
#include <stdint.h>
#include "i2c.h"
// See LPS22H "MEMS pressure sensor: 260-1260 hPa absolute digital output barometer" Data Sheet
// http://www.st.com/content/ccc/resource/technical/document/datasheet/bf/c1/4f/23/61/17/44/8a/DM00140895.pdf/files/DM00140895.pdf/jcr:content/translations/en.DM00140895.pdf
#define LPS22H_INTERRUPT_CFG 0x0B
#define LPS22H_THS_P_L 0x0C
#define LPS22H_THS_P_H 0x0D
#define LPS22H_WHOAMI 0x0F // should return 0xB1
#define LPS22H_CTRL_REG1 0x10
#define LPS22H_CTRL_REG2 0x11
#define LPS22H_CTRL_REG3 0x12
#define LPS22H_FIFO_CTRL 0x14
#define LPS22H_REF_P_XL 0x15
#define LPS22H_REF_P_L 0x16
#define LPS22H_REF_P_H 0x17
#define LPS22H_RPDS_L 0x18
#define LPS22H_RPDS_H 0x19
#define LPS22H_RES_CONF 0x1A
#define LPS22H_INT_SOURCE 0x25
#define LPS22H_FIFO_STATUS 0x26
#define LPS22H_STATUS 0x27
#define LPS22H_PRESS_OUT_XL 0x28
#define LPS22H_PRESS_OUT_L 0x29
#define LPS22H_PRESS_OUT_H 0x2A
#define LPS22H_TEMP_OUT_L 0x2B
#define LPS22H_TEMP_OUT_H 0x2C
#define LPS22H_LPFP_RES 0x33
#define LPS22H_ADDRESS 0x5C // Address of altimeter
// Altimeter output data rate
/* Definitions */
#define LPS22HB_OK (0)
#define LPS22HB_BAD_ARG (1 << 0)
#define LPS22HB_BAD_COMM (1 << 1)
/* altimeter output data rate */
#define P_1shot 0x00
#define P_1Hz 0x01
#define P_10Hz 0x02
#define P_25Hz 0x03 // 25 Hz output data rate
#define P_25Hz 0x03
#define P_50Hz 0x04
#define P_75Hz 0x05
/* Data Structures */
typedef struct lps22hb_s
typedef int32_t lps22hb_status_t;
typedef struct lps22hb_init_s
{
I2C_HandleTypeDef *hi2c;
uint8_t p_odr;
} lps22hb_init_t;
typedef struct lps22hb_s
{
lps22hb_init_t *init;
uint8_t i2c_addr;
delay_func_t delay_func;
i2c_read_func_t i2c_read_func;
i2c_write_func_t i2c_write_func;
} lps22hb_t;
/* Function Prototypes */
void lps22hb_init(lps22hb_t *lps22hb, uint8_t p_odr);
void lps22hb_config(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c);
lps22hb_status_t lps22hb_init(lps22hb_t *lps22hb, lps22hb_init_t *init);
void lps22hb_set_delay_cb(lps22hb_t *lps22hb, delay_func_t delay_func);
void lps22hb_set_i2c_cbs(lps22hb_t *lps22hb, i2c_read_func_t i2c_read_func,
i2c_write_func_t i2c_write_func, uint8_t dev_addr);
lps22hb_status_t lps22hb_config(lps22hb_t *lps22hb);
lps22hb_status_t lps22hb_status(lps22hb_t *lps22hb, uint8_t *status);
lps22hb_status_t lps22hb_chip_id_get(lps22hb_t *lps22hb, uint8_t *data);
lps22hb_status_t lps22hb_pressure_get(lps22hb_t *lps22hb, int32_t *data);
lps22hb_status_t lps22hb_temp_get(lps22hb_t *lps22hb, int16_t *data);
#endif

132
Drivers/EM7180/Inc/lsm6dsm.h
Unescape View File

@ -19,124 +19,19 @@ @@ -19,124 +19,19 @@
/* Includes */
#include <stdint.h>
#include "i2c.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
#define LSM6DSM_ADDRESS 0x6A // Address of LSM6DSM accel/gyro when ADO = 0
#define LSM6DSM_OK (0)
#define LSM6DSM_BAD_ARG (1 << 0)
#define LSM6DSM_BAD_COMM (1 << 1)
#define AFS_2G 0x00
#define AFS_4G 0x02
#define AFS_8G 0x03
#define AFS_16G 0x01
#define GFS_250DPS 0x00
#define GFS_500DPS 0x01
#define GFS_1000DPS 0x02
#define GFS_2000DPS 0x03
#define AODR_12_5Hz 0x01 // same for accel and gyro in normal mode
#define AODR_26Hz 0x02
#define AODR_52Hz 0x03
@ -147,7 +42,6 @@ @@ -147,7 +42,6 @@
#define AODR_1660Hz 0x08
#define AODR_3330Hz 0x09
#define AODR_6660Hz 0x0A
#define GODR_12_5Hz 0x01
#define GODR_26Hz 0x02
#define GODR_52Hz 0x03
@ -160,18 +54,28 @@ @@ -160,18 +54,28 @@
#define GODR_6660Hz 0x0A
/* Data Structures */
typedef struct lsm6dsm_s
typedef int32_t lsm6dsm_status_t;
typedef struct lsm6dsm_init_s
{
I2C_HandleTypeDef *hi2c;
uint8_t ascale;
uint8_t gscale;
uint8_t a_odr;
uint8_t g_odr;
} lsm6dsm_init_t;
typedef struct lsm6dsm_s
{
lsm6dsm_init_t *init;
uint8_t i2c_addr;
delay_func_t delay_func;
i2c_read_func_t i2c_read_func;
i2c_write_func_t i2c_write_func;
} lsm6dsm_t;
/* Function Prototypes */
void lsm6dsm_init(lsm6dsm_t *lsm6dsm, uint8_t ascale, uint8_t gscale,
uint8_t a_odr, uint8_t g_odr);
void lsm6dsm_config(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c);
lsm6dsm_status_t lsm6dsm_init(lsm6dsm_t *lsm6dsm, lsm6dsm_init_t *init);
void lsm6dsm_set_delay_cb(lsm6dsm_t *lsm6dsm, delay_func_t 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);
lsm6dsm_status_t lsm6dsm_config(lsm6dsm_t *lsm6dsm);
#endif

2
Drivers/EM7180/Src/em7180.c
Unescape View File

@ -272,7 +272,7 @@ em7180_status_t em7180_tempdata_get(em7180_t *em7180, int16_t *destination) @@ -272,7 +272,7 @@ em7180_status_t em7180_tempdata_get(em7180_t *em7180, int16_t *destination)
return ret ? EM7180_BAD_COMM : EM7180_OK;
}
/* FIXME: haven't explored the usagge/usefulness of these yet: */
/* FIXME: haven't explored the usage/usefulness of these yet: */
#if(0)
uint8_t em7180_status(em7180_t *em7180)
{

159
Drivers/EM7180/Src/lis2mdl.c
Unescape View File

@ -16,90 +16,151 @@ @@ -16,90 +16,151 @@
/* Includes */
#include <stdint.h>
#include <stddef.h>
#include "em7180_common.h"
#include "lis2mdl.h"
/* Definitions */
/*
* Register map for LIS2MDL
* http://www.st.com/content/ccc/resource/technical/document/datasheet/group3/29/13/d1/e0/9a/4d/4f/30/DM00395193/files/DM00395193.pdf/jcr:content/translations/en.DM00395193.pdf
*/
#define LIS2MDL_OFFSET_X_REG_L 0x45
#define LIS2MDL_OFFSET_X_REG_H 0x46
#define LIS2MDL_OFFSET_Y_REG_L 0x47
#define LIS2MDL_OFFSET_Y_REG_H 0x48
#define LIS2MDL_OFFSET_Z_REG_L 0x49
#define LIS2MDL_OFFSET_Z_REG_H 0x4A
#define LIS2MDL_WHO_AM_I 0x4F
#define LIS2MDL_CFG_REG_A 0x60
#define LIS2MDL_CFG_REG_B 0x61
#define LIS2MDL_CFG_REG_C 0x62
#define LIS2MDL_INT_CTRL_REG 0x63
#define LIS2MDL_INT_SOURCE_REG 0x64
#define LIS2MDL_INT_THS_L_REG 0x65
#define LIS2MDL_INT_THS_H_REG 0x66
#define LIS2MDL_STATUS_REG 0x67
#define LIS2MDL_OUTX_L_REG 0x68
#define LIS2MDL_OUTX_H_REG 0x69
#define LIS2MDL_OUTY_L_REG 0x6A
#define LIS2MDL_OUTY_H_REG 0x6B
#define LIS2MDL_OUTZ_L_REG 0x6C
#define LIS2MDL_OUTZ_H_REG 0x6D
#define LIS2MDL_TEMP_OUT_L_REG 0x6E
#define LIS2MDL_TEMP_OUT_H_REG 0x6F
/* Macros */
#define lis2mdl_read_byte(addr, byte) i2c_read_byte((lis2mdl->i2c_read_func), (lis2mdl->i2c_addr), (addr), (byte))
#define lis2mdl_write_byte(addr, byte) i2c_write_byte((lis2mdl->i2c_write_func), (lis2mdl->i2c_addr), (addr), (byte))
#define lis2mdl_read(addr, data, len) i2c_read((lis2mdl->i2c_read_func), (lis2mdl->i2c_addr), (addr), (data), (len))
#define lis2mdl_write(addr, data, len) i2c_write((lis2mdl->i2c_write_func), (lis2mdl->i2c_addr), (addr), (data), (len))
/* Private Global Variables */
/* Function Prototypes */
/* Function Definitions */
void lis2mdl_init(lis2mdl_t *lis2mdl, uint8_t m_odr)
lis2mdl_status_t lis2mdl_init(lis2mdl_t *lis2mdl, lis2mdl_init_t *init)
{
if(!lis2mdl)
int8_t *ptr = (int8_t*) lis2mdl;
size_t i;
return_val_if_fail(lis2mdl, LIS2MDL_BAD_ARG);
return_val_if_fail(init, LIS2MDL_BAD_ARG);
/* zero lis2mdl_t struct */
for(i = 0; i < sizeof(lis2mdl_t); i++)
{
return;
*ptr++ = 0;
}
lis2mdl->m_odr = m_odr;
lis2mdl->init = init;
return LIS2MDL_OK;
}
void lis2mdl_config(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c)
void lis2mdl_set_delay_cb(lis2mdl_t *lis2mdl, delay_func_t delay_func)
{
// enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00)
broken_i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A,
0x80 | lis2mdl->m_odr << 2);
return_if_fail(lis2mdl);
// enable low pass filter (bit 0 == 1), set to ODR/4
broken_i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01);
lis2mdl->delay_func = delay_func;
}
void lis2mdl_set_i2c_cbs(lis2mdl_t *lis2mdl, i2c_read_func_t i2c_read_func,
i2c_write_func_t i2c_write_func, uint8_t dev_addr)
{
return_if_fail(lis2mdl);
lis2mdl->i2c_read_func = i2c_read_func;
lis2mdl->i2c_write_func = i2c_write_func;
lis2mdl->i2c_addr = dev_addr;
}
lis2mdl_status_t lis2mdl_config(lis2mdl_t *lis2mdl)
{
int32_t ret = 0;
// enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1)
broken_i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C,
0x01 | 0x10);
/* enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00) */
ret |= lis2mdl_write_byte(LIS2MDL_CFG_REG_A,
0x80 | lis2mdl->init->m_odr << 2);
/* enable low pass filter (bit 0 == 1), set to ODR/4 */
ret |= lis2mdl_write_byte(LIS2MDL_CFG_REG_B, 0x01);
/* enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1) */
ret |= lis2mdl_write_byte(LIS2MDL_CFG_REG_C, 0x01 | 0x10);
return ret ? LIS2MDL_BAD_COMM : LIS2MDL_OK;
}
uint8_t lis2mdl_chip_id_get(I2C_HandleTypeDef *hi2c)
/* FIXME: haven't explored the usage/usefulness of these yet: */
#if(0)
uint8_t lis2mdl_chip_id_get(lis2mdl_t *lis2mdl)
{
uint8_t c = broken_i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I);
uint8_t c;
lis2mdl_read_byte(LIS2MDL_WHO_AM_I, &c);
return c;
}
void lis2mdl_reset(I2C_HandleTypeDef *hi2c)
void lis2mdl_reset(lis2mdl_t *lis2mdl)
{
// reset device
uint8_t temp = broken_i2c_read_byte(hi2c, LIS2MDL_ADDRESS,
LIS2MDL_CFG_REG_A);
uint8_t temp;
lis2mdl_read_byte(LIS2MDL_CFG_REG_A, &temp);
broken_i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A,
temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL
HAL_Delay(1);
broken_i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A,
temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL
HAL_Delay(100); // Wait for all registers to reset
lis2mdl_write_byte(LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL
lis2mdl->delay_func(1);
lis2mdl_write_byte(LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL
lis2mdl->delay_func(100); // Wait for all registers to reset
}
uint8_t lis2mdl_status(I2C_HandleTypeDef *hi2c)
uint8_t lis2mdl_status(lis2mdl_t *lis2mdl)
{
// Read the status register of the altimeter
uint8_t temp = broken_i2c_read_byte(hi2c, LIS2MDL_ADDRESS,
LIS2MDL_STATUS_REG);
uint8_t temp;
lis2mdl_read_byte(LIS2MDL_STATUS_REG, &temp);
return temp;
}
void lis2mdl_data_get(I2C_HandleTypeDef *hi2c, int16_t *destination)
void lis2mdl_data_get(lis2mdl_t *lis2mdl, int16_t *destination)
{
uint8_t data[6]; // x/y/z mag register data stored here
broken_i2c_read(hi2c, LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), data,
8); // Read the 6 raw data registers into data array
lis2mdl_read((0x80 | LIS2MDL_OUTX_L_REG), data, 8); // Read the 6 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];
}
int16_t lis2mdl_temp_get()
int16_t lis2mdl_temp_get(lis2mdl_t *lis2mdl)
{
uint8_t data[2]; // x/y/z mag register data stored here
lis2mdl_read_bytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_TEMP_OUT_L_REG), 2,
&data[0]); // Read the 8 raw data registers into data array
lis2mdl_read(0x80 | LIS2MDL_TEMP_OUT_L_REG, data, 2); // Read the 8 raw data registers into data array
int16_t temp = ((int16_t) data[1] << 8) | data[0]; // Turn the MSB and LSB into a signed 16-bit value
return temp;
}
void lis2mdl_offset_bias(I2C_HandleTypeDef *hi2c, float *dest1, float *dest2)
void lis2mdl_offset_bias(lis2mdl_t *lis2mdl, float *dest1, float *dest2)
{
int32_t mag_bias[3] = { 0, 0, 0 }, mag_scale[3] = { 0, 0, 0 };
int16_t mag_max[3] = { -32767, -32767, -32767 }, mag_min[3] =
@ -107,11 +168,11 @@ void lis2mdl_offset_bias(I2C_HandleTypeDef *hi2c, float *dest1, float *dest2) @@ -107,11 +168,11 @@ void lis2mdl_offset_bias(I2C_HandleTypeDef *hi2c, float *dest1, float *dest2)
float m_res = 0.0015f;
/* Serial.println("Calculate mag offset bias: move all around to sample the complete response surface!"); */
HAL_Delay(4000);
lis2mdl->delay_func(4000);
for(int ii = 0; ii < 4000; ii++)
{
lis2mdl_data_get(hi2c, mag_temp);
lis2mdl_data_get(lis2mdl, mag_temp);
for(int jj = 0; jj < 3; jj++)
{
if(mag_temp[jj] > mag_max[jj])
@ -123,7 +184,7 @@ void lis2mdl_offset_bias(I2C_HandleTypeDef *hi2c, float *dest1, float *dest2) @@ -123,7 +184,7 @@ void lis2mdl_offset_bias(I2C_HandleTypeDef *hi2c, float *dest1, float *dest2)
mag_min[jj] = mag_temp[jj];
}
}
HAL_Delay(12);
lis2mdl->delay_func(12);
}
m_res = 0.0015f; // fixed sensitivity
@ -151,8 +212,9 @@ void lis2mdl_offset_bias(I2C_HandleTypeDef *hi2c, float *dest1, float *dest2) @@ -151,8 +212,9 @@ void lis2mdl_offset_bias(I2C_HandleTypeDef *hi2c, float *dest1, float *dest2)
/* Serial.println("Mag Calibration done!"); */
}
void lis2mdl_self_test(I2C_HandleTypeDef *hi2c)
void lis2mdl_self_test(lis2mdl_t *lis2mdl)
{
uint8_t c;
int16_t temp[3] = { 0, 0, 0 };
float magTest[3] = { 0., 0., 0. };
float magNom[3] = { 0., 0., 0. };
@ -162,39 +224,39 @@ void lis2mdl_self_test(I2C_HandleTypeDef *hi2c) @@ -162,39 +224,39 @@ void lis2mdl_self_test(I2C_HandleTypeDef *hi2c)
// first, get average response with self test disabled
for(int ii = 0; ii < 50; ii++)
{
lis2mdl_data_get(hi2c, temp);
lis2mdl_data_get(lis2mdl, temp);
sum[0] += temp[0];
sum[1] += temp[1];
sum[2] += temp[2];
HAL_Delay(50);
lis2mdl->delay_func(50);
}
magNom[0] = (float) sum[0] / 50.0f;
magNom[1] = (float) sum[1] / 50.0f;
magNom[2] = (float) sum[2] / 50.0f;
uint8_t c = broken_i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C);
broken_i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test
HAL_Delay(100); // let mag respond
lis2mdl_read_byte(LIS2MDL_CFG_REG_C, &c);
lis2mdl_write_byte(LIS2MDL_CFG_REG_C, c | 0x02); // enable self test
lis2mdl->delay_func(100); // let mag respond
sum[0] = 0;
sum[1] = 0;
sum[2] = 0;
for(int ii = 0; ii < 50; ii++)
{
lis2mdl_data_get(hi2c, temp);
lis2mdl_data_get(lis2mdl, temp);
sum[0] += temp[0];
sum[1] += temp[1];
sum[2] += temp[2];
HAL_Delay(50);
lis2mdl->delay_func(50);
}
magTest[0] = (float) sum[0] / 50.0f;
magTest[1] = (float) sum[1] / 50.0f;
magTest[2] = (float) sum[2] / 50.0f;
broken_i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode
HAL_Delay(100); // let mag respond
lis2mdl_write_byte(LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode
lis2mdl->delay_func(100); // let mag respond
/* Serial.println("Mag Self Test:"); */
/* Serial.print("Mx results:"); */
@ -205,5 +267,6 @@ void lis2mdl_self_test(I2C_HandleTypeDef *hi2c) @@ -205,5 +267,6 @@ void lis2mdl_self_test(I2C_HandleTypeDef *hi2c)
/* Serial.print("Mz results:"); */
/* Serial.println((magTest[1] - magNom[1]) * m_res * 1000.0); */
/* Serial.println("Should be between 15 and 500 mG"); */
HAL_Delay(2000); // give some time to read the screen
/* lis2mdl->delay_func(2000); // give some time to read the screen */
}
#endif

134
Drivers/EM7180/Src/lps22hb.c
Unescape View File

@ -7,7 +7,7 @@ @@ -7,7 +7,7 @@
* Author: Daniel Peter Chokola
*
* Adapted From:
* EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly
* EM7180_LSM6DSM_XXYYZZ_LPS22HB_Butterfly
* by: Kris Winer
* 09/23/2017 Copyright Tlera Corporation
*
@ -16,63 +16,141 @@ @@ -16,63 +16,141 @@
/* Includes */
#include <stdint.h>
#include <stddef.h>
#include "em7180_common.h"
#include "lps22hb.h"
/* Definitions */
/*
* LPS22H Registers
* http://www.st.com/content/ccc/resource/technical/document/datasheet/bf/c1/4f/23/61/17/44/8a/DM00140895.pdf/files/DM00140895.pdf/jcr:content/translations/en.DM00140895.pdf
*/
#define LPS22H_INTERRUPT_CFG 0x0B
#define LPS22H_THS_P_L 0x0C
#define LPS22H_THS_P_H 0x0D
#define LPS22H_WHOAMI 0x0F // should return 0xB1
#define LPS22H_CTRL_REG1 0x10
#define LPS22H_CTRL_REG2 0x11
#define LPS22H_CTRL_REG3 0x12
#define LPS22H_FIFO_CTRL 0x14
#define LPS22H_REF_P_XL 0x15
#define LPS22H_REF_P_L 0x16
#define LPS22H_REF_P_H 0x17
#define LPS22H_RPDS_L 0x18
#define LPS22H_RPDS_H 0x19
#define LPS22H_RES_CONF 0x1A
#define LPS22H_INT_SOURCE 0x25
#define LPS22H_FIFO_STATUS 0x26
#define LPS22H_STATUS 0x27
#define LPS22H_PRESS_OUT_XL 0x28
#define LPS22H_PRESS_OUT_L 0x29
#define LPS22H_PRESS_OUT_H 0x2A
#define LPS22H_TEMP_OUT_L 0x2B
#define LPS22H_TEMP_OUT_H 0x2C
#define LPS22H_LPFP_RES 0x33
/* Macros */
#define lps22hb_read_byte(addr, byte) i2c_read_byte((lps22hb->i2c_read_func), (lps22hb->i2c_addr), (addr), (byte))
#define lps22hb_write_byte(addr, byte) i2c_write_byte((lps22hb->i2c_write_func), (lps22hb->i2c_addr), (addr), (byte))
#define lps22hb_read(addr, data, len) i2c_read((lps22hb->i2c_read_func), (lps22hb->i2c_addr), (addr), (data), (len))
#define lps22hb_write(addr, data, len) i2c_write((lps22hb->i2c_write_func), (lps22hb->i2c_addr), (addr), (data), (len))
/* Private Global Variables */
/* Function Prototypes */
/* Function Definitions */
void lps22hb_init(lps22hb_t *lps22hb, uint8_t p_odr)
lps22hb_status_t lps22hb_init(lps22hb_t *lps22hb, lps22hb_init_t *init)
{
int8_t *ptr = (int8_t*) lps22hb;
size_t i;
return_val_if_fail(lps22hb, LPS22HB_BAD_ARG);
return_val_if_fail(init, LPS22HB_BAD_ARG);
/* zero lps22hb_t struct */
for(i = 0; i < sizeof(lps22hb_t); i++)
{
*ptr++ = 0;
}
lps22hb->init = init;
return LPS22HB_OK;
}
void lps22hb_set_delay_cb(lps22hb_t *lps22hb, delay_func_t delay_func)
{
return_if_fail(lps22hb);
lps22hb->p_odr = p_odr;
lps22hb->delay_func = delay_func;
}
void lps22hb_config(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c)
void lps22hb_set_i2c_cbs(lps22hb_t *lps22hb, i2c_read_func_t i2c_read_func,
i2c_write_func_t i2c_write_func, uint8_t dev_addr)
{
// set sample rate by setting bits 6:4
// enable low-pass filter by setting bit 3 to one
// bit 2 == 0 means bandwidth is odr/9, bit 2 == 1 means bandwidth is odr/20
// make sure data not updated during read by setting block data udate (bit 1) to 1
broken_i2c_write_byte(hi2c, LPS22H_ADDRESS, LPS22H_CTRL_REG1,
lps22hb->p_odr << 4 | 0x08 | 0x02);
broken_i2c_write_byte(hi2c, LPS22H_ADDRESS, LPS22H_CTRL_REG3, 0x04); // enable data ready as interrupt source
return_if_fail(lps22hb);
lps22hb->i2c_read_func = i2c_read_func;
lps22hb->i2c_write_func = i2c_write_func;
lps22hb->i2c_addr = dev_addr;
}
uint8_t lps22hb_chip_id_get(I2C_HandleTypeDef *hi2c)
lps22hb_status_t lps22hb_config(lps22hb_t *lps22hb)
{
// Read the WHO_AM_I register of the altimeter this is a good test of communication
uint8_t temp = broken_i2c_read_byte(hi2c, LPS22H_ADDRESS, LPS22H_WHOAMI); // Read WHO_AM_I register for LPS22H
int32_t ret = 0;
/*
* set sample rate by setting bits 6:4
* enable low-pass filter by setting bit 3 to one
* bit 2 == 0 means bandwidth is odr/9, bit 2 == 1 means bandwidth is odr/20
* make sure data not updated during read by setting block data udate (bit 1) to 1
*/
ret |= lps22hb_write_byte(LPS22H_CTRL_REG1,
lps22hb->init->p_odr << 4 | 0x08 | 0x02);
ret |= lps22hb_write_byte(LPS22H_CTRL_REG3, 0x04); // enable data ready as interrupt source
return temp;
return ret ? LPS22HB_BAD_COMM : LPS22HB_OK;
}
uint8_t lps22hb_status(I2C_HandleTypeDef *hi2c)
lps22hb_status_t lps22hb_status(lps22hb_t *lps22hb, uint8_t *status)
{
// Read the status register of the altimeter
uint8_t temp = broken_i2c_read_byte(hi2c, LPS22H_ADDRESS, LPS22H_STATUS);
int32_t ret = 0;
/* read the status register of the altimeter */
ret |= lps22hb_read_byte(LPS22H_STATUS, status);
return ret ? LPS22HB_BAD_COMM : LPS22HB_OK;
}
lps22hb_status_t lps22hb_chip_id_get(lps22hb_t *lps22hb, uint8_t *data)
{
int32_t ret = 0;
/* read the WHO_AM_I register of the altimeter; this is a good test of communication */
ret |= lps22hb_read_byte(LPS22H_WHOAMI, data); // Read WHO_AM_I register for LPS22H
return temp;
return ret ? LPS22HB_BAD_COMM : LPS22HB_OK;
}
int32_t lps22hb_pressure_get(I2C_HandleTypeDef *hi2c)
lps22hb_status_t lps22hb_pressure_get(lps22hb_t *lps22hb, int32_t *data)
{
uint8_t data[3]; // 24-bit pressure register data stored here
int32_t ret = 0;
uint8_t temp[3]; // 24-bit pressure register data stored here
broken_i2c_read(hi2c, LPS22H_ADDRESS, (LPS22H_PRESS_OUT_XL | 0x80), data,
3); // bit 7 must be one to read multiple bytes
ret |= lps22hb_read(LPS22H_PRESS_OUT_XL | 0x80, temp, 3); // bit 7 must be one to read multiple bytes
*data = ((int32_t) temp[2] << 16 | (int32_t) temp[1] << 8 | temp[0]);
return ((int32_t) data[2] << 16 | (int32_t) data[1] << 8 | data[0]);
return ret ? LPS22HB_BAD_COMM : LPS22HB_OK;
}
int16_t lps22hb_temp_get(I2C_HandleTypeDef *hi2c)
lps22hb_status_t lps22hb_temp_get(lps22hb_t *lps22hb, int16_t *data)
{
uint8_t data[2]; // 16-bit pressure register data stored here
int32_t ret = 0;
uint8_t temp[2]; // 16-bit pressure register data stored here
broken_i2c_read(hi2c, LPS22H_ADDRESS, (LPS22H_TEMP_OUT_L | 0x80), data, 2); // bit 7 must be one to read multiple bytes
ret |= lps22hb_read(LPS22H_TEMP_OUT_L | 0x80, temp, 2); // bit 7 must be one to read multiple bytes
*data = ((int16_t) temp[1] << 8 | temp[0]);
return ((int16_t) data[1] << 8 | data[0]);
return ret ? LPS22HB_BAD_COMM : LPS22HB_OK;
}

251
Drivers/EM7180/Src/lsm6dsm.c
Unescape View File

@ -16,56 +16,196 @@ @@ -16,56 +16,196 @@
/* Includes */
#include <stdint.h>
#include <math.h>
#include <stddef.h>
#include "em7180_common.h"
#include "lsm6dsm.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 */
static void lsm6dsm_read_data(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c,
int16_t *destination);
/* Function Definitions */
void lsm6dsm_init(lsm6dsm_t *lsm6dsm, uint8_t ascale, uint8_t gscale,
uint8_t a_odr, uint8_t g_odr)
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->ascale = ascale;
lsm6dsm->gscale = gscale;
lsm6dsm->a_odr = a_odr;
lsm6dsm->g_odr = g_odr;
lsm6dsm->delay_func = delay_func;
}
void lsm6dsm_config(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
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)
{
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL,
lsm6dsm->a_odr << 4 | lsm6dsm->ascale << 2);
return_if_fail(lsm6dsm);
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G,
lsm6dsm->g_odr << 4 | lsm6dsm->gscale << 2);
lsm6dsm->i2c_read_func = i2c_read_func;
lsm6dsm->i2c_write_func = i2c_write_func;
lsm6dsm->i2c_addr = dev_addr;
}
uint8_t temp = broken_i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
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)
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C,
temp | 0x40 | 0x04);
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
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL,
0x80 | 0x40 | 0x08);
ret |= lsm6dsm_write_byte(LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08);
// interrupt handling
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_INT1_CTRL, 0x40); // enable significant motion interrupts on INT1
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_INT2_CTRL, 0x03); // enable accel/gyro data ready interrupts on INT2
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;
}
uint8_t lsm6dsm_chip_id_get(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
/* FIXME: haven't explored the usage/usefulness of these yet: */
#if(0)
uint8_t lsm6dsm_chip_id_get(lsm6dsm_t *lsm6dsm)
{
uint8_t c = broken_i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_WHO_AM_I);
uint8_t c;
lsm6dsm_read_byte(LSM6DSM_WHO_AM_I, &c);
return c;
}
@ -74,7 +214,7 @@ float lsm6dsm_ares_get(lsm6dsm_t *lsm6dsm) @@ -74,7 +214,7 @@ float lsm6dsm_ares_get(lsm6dsm_t *lsm6dsm)
{
float a_res;
switch(lsm6dsm->ascale)
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).
@ -103,7 +243,7 @@ float lsm6dsm_gres_get(lsm6dsm_t *lsm6dsm) @@ -103,7 +243,7 @@ float lsm6dsm_gres_get(lsm6dsm_t *lsm6dsm)
{
float g_res;
switch(lsm6dsm->gscale)
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).
@ -127,16 +267,17 @@ float lsm6dsm_gres_get(lsm6dsm_t *lsm6dsm) @@ -127,16 +267,17 @@ float lsm6dsm_gres_get(lsm6dsm_t *lsm6dsm)
return g_res;
}
void lsm6dsm_reset(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
void lsm6dsm_reset(lsm6dsm_t *lsm6dsm)
{
// reset device
uint8_t temp = broken_i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
uint8_t temp;
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM
HAL_Delay(100); // Wait for all registers to reset
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, I2C_HandleTypeDef *hi2c)
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 };
@ -146,7 +287,7 @@ void lsm6dsm_self_test(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c) @@ -146,7 +287,7 @@ void lsm6dsm_self_test(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
int16_t accelNom[3] = { 0, 0, 0 };
int16_t gyroNom[3] = { 0, 0, 0 };
lsm6dsm_read_data(lsm6dsm, hi2c, temp);
lsm6dsm_read_data(lsm6dsm, temp);
accelNom[0] = temp[4];
accelNom[1] = temp[5];
accelNom[2] = temp[6];
@ -154,36 +295,36 @@ void lsm6dsm_self_test(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c) @@ -154,36 +295,36 @@ void lsm6dsm_self_test(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
gyroNom[1] = temp[2];
gyroNom[2] = temp[3];
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test
HAL_Delay(100); // let accel respond
lsm6dsm_read_data(lsm6dsm, hi2c, temp);
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];
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test
HAL_Delay(100); // let accel respond
lsm6dsm_read_data(lsm6dsm, hi2c, temp);
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];
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
HAL_Delay(100); // let gyro respond
lsm6dsm_read_data(lsm6dsm, hi2c, temp);
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];
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
HAL_Delay(100); // let gyro respond
lsm6dsm_read_data(lsm6dsm, hi2c, temp);
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];
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode
HAL_Delay(100); // let accel and gyro respond
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:"); */
@ -216,12 +357,10 @@ void lsm6dsm_self_test(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c) @@ -216,12 +357,10 @@ void lsm6dsm_self_test(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
/* Serial.print("-Gz results:"); */
/* Serial.println((gyroNTest[2] - gyroNom[2]) * g_res); */
/* Serial.println("Should be between 20 and 80 dps"); */
HAL_Delay(2000);
lsm6dsm->delay_func(2000);
}
void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c,
float *dest1, float *dest2)
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 };
@ -229,18 +368,18 @@ void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c, @@ -229,18 +368,18 @@ void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c,
float g_res = lsm6dsm_gres_get(lsm6dsm);
/* Serial.println("Calculate accel and gyro offset biases: keep sensor flat and motionless!"); */
HAL_Delay(4000);
lsm6dsm->delay_func(4000);
for(int ii = 0; ii < 128; ii++)
{
lsm6dsm_read_data(lsm6dsm, hi2c, temp);
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];
HAL_Delay(50);
lsm6dsm->delay_func(50);
}
dest1[0] = sum[1] * g_res / 128.0f;
@ -277,12 +416,11 @@ void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c, @@ -277,12 +416,11 @@ void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c,
}
static void lsm6dsm_read_data(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c,
int16_t *destination)
static void lsm6dsm_read_data(lsm6dsm_t *lsm6dsm, int16_t *destination)
{
uint8_t data[14]; // x/y/z accel register data stored here
broken_i2c_read(hi2c, LSM6DSM_ADDRESS, LSM6DSM_OUT_TEMP_L, data, 14); // Read the 14 raw data registers into data array
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];
@ -291,3 +429,4 @@ static void lsm6dsm_read_data(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c, @@ -291,3 +429,4 @@ static void lsm6dsm_read_data(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c,
destination[5] = ((int16_t) data[11] << 8) | data[10];
destination[6] = ((int16_t) data[13] << 8) | data[12];
}
#endif

Write Preview
Loading…
Cancel
Save