diff --git a/Drivers/EM7180/Inc/lis2mdl.h b/Drivers/EM7180/Inc/lis2mdl.h index a984478..6586c07 100644 --- a/Drivers/EM7180/Inc/lis2mdl.h +++ b/Drivers/EM7180/Inc/lis2mdl.h @@ -19,53 +19,38 @@ /* Includes */ #include -#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 diff --git a/Drivers/EM7180/Inc/lps22hb.h b/Drivers/EM7180/Inc/lps22hb.h index ff8ac59..c65ab31 100644 --- a/Drivers/EM7180/Inc/lps22hb.h +++ b/Drivers/EM7180/Inc/lps22hb.h @@ -19,53 +19,43 @@ /* Includes */ #include -#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 diff --git a/Drivers/EM7180/Inc/lsm6dsm.h b/Drivers/EM7180/Inc/lsm6dsm.h index 350449d..8983637 100644 --- a/Drivers/EM7180/Inc/lsm6dsm.h +++ b/Drivers/EM7180/Inc/lsm6dsm.h @@ -19,124 +19,19 @@ /* Includes */ #include -#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 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 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 @@ #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 @@ #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 diff --git a/Drivers/EM7180/Src/em7180.c b/Drivers/EM7180/Src/em7180.c index 17f2247..7000ad2 100644 --- a/Drivers/EM7180/Src/em7180.c +++ b/Drivers/EM7180/Src/em7180.c @@ -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) { diff --git a/Drivers/EM7180/Src/lis2mdl.c b/Drivers/EM7180/Src/lis2mdl.c index 789acf1..01aee87 100644 --- a/Drivers/EM7180/Src/lis2mdl.c +++ b/Drivers/EM7180/Src/lis2mdl.c @@ -16,90 +16,151 @@ /* Includes */ #include +#include #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_set_delay_cb(lis2mdl_t *lis2mdl, delay_func_t delay_func) +{ + return_if_fail(lis2mdl); + + lis2mdl->delay_func = delay_func; } -void lis2mdl_config(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c) +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) { - // 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->i2c_read_func = i2c_read_func; + lis2mdl->i2c_write_func = i2c_write_func; + lis2mdl->i2c_addr = dev_addr; +} - // 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); +lis2mdl_status_t lis2mdl_config(lis2mdl_t *lis2mdl) +{ + int32_t ret = 0; + + /* 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); - - 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 + uint8_t temp; + lis2mdl_read_byte(LIS2MDL_CFG_REG_A, &temp); + + 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) 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) 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) /* 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) // 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) /* 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 diff --git a/Drivers/EM7180/Src/lps22hb.c b/Drivers/EM7180/Src/lps22hb.c index 90db3a8..c294953 100644 --- a/Drivers/EM7180/Src/lps22hb.c +++ b/Drivers/EM7180/Src/lps22hb.c @@ -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 @@ /* Includes */ #include +#include #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->delay_func = 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) { return_if_fail(lps22hb); - lps22hb->p_odr = p_odr; + lps22hb->i2c_read_func = i2c_read_func; + lps22hb->i2c_write_func = i2c_write_func; + lps22hb->i2c_addr = dev_addr; } -void lps22hb_config(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c) +lps22hb_status_t lps22hb_config(lps22hb_t *lps22hb) { - // 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 + 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 ret ? LPS22HB_BAD_COMM : LPS22HB_OK; } -uint8_t lps22hb_chip_id_get(I2C_HandleTypeDef *hi2c) +lps22hb_status_t lps22hb_status(lps22hb_t *lps22hb, uint8_t *status) { - // 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; - return temp; + /* read the status register of the altimeter */ + ret |= lps22hb_read_byte(LPS22H_STATUS, status); + + return ret ? LPS22HB_BAD_COMM : LPS22HB_OK; } -uint8_t lps22hb_status(I2C_HandleTypeDef *hi2c) +lps22hb_status_t lps22hb_chip_id_get(lps22hb_t *lps22hb, uint8_t *data) { - // 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 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; } diff --git a/Drivers/EM7180/Src/lsm6dsm.c b/Drivers/EM7180/Src/lsm6dsm.c index 5ff36cf..3729c8b 100644 --- a/Drivers/EM7180/Src/lsm6dsm.c +++ b/Drivers/EM7180/Src/lsm6dsm.c @@ -16,56 +16,196 @@ /* Includes */ #include -#include +#include #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) { 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) { 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) 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) 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) 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) /* 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, 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, } -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, destination[5] = ((int16_t) data[11] << 8) | data[10]; destination[6] = ((int16_t) data[13] << 8) | data[12]; } +#endif