Skip to content

Commit

Permalink
fully generalized
Browse files Browse the repository at this point in the history
  • Loading branch information
@caiodasilva2005
caiodasilva2005 committed Oct 27, 2024
1 parent eee3b4a commit 172529a
Show file tree
Hide file tree
Showing 2 changed files with 85 additions and 140 deletions.
69 changes: 15 additions & 54 deletions general/include/lsm6dso.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,10 +45,8 @@
#define LSM6DSO_REG_ACCEL_Z_AXIS_H 0x2D /* Accelerometer Z axis upper bits */

/* Function Pointers*/
typedef uint8_t (*I2C_Read_Reg)(uint8_t *data, uint8_t reg);
typedef uint8_t (*I2C_Read_Mult_Reg)(uint8_t *data, uint8_t reg,
uint8_t length);
typedef uint8_t (*I2C_Write_Reg)(uint8_t *data, uint8_t reg);
typedef int (*I2C_Read)(uint8_t *data, uint8_t reg, uint8_t length);
typedef int (*I2C_Write)(uint8_t *data, uint8_t reg, uint8_t length);

/**
* @brief Enumeration of axes of data available from the LSM6DSO IMU
Expand Down Expand Up @@ -76,50 +74,20 @@ typedef struct {

int16_t gyro_data[3];

I2C_Read_Reg read_reg;
I2C_Read read_reg;

I2C_Read_Mult_Reg read_mult_reg;

I2C_Write_Reg write_reg;
I2C_Write write_reg;

} lsm6dso_t;

/**
* @brief Initializes the LSM6DSO IMU
*
* @param imu
* @param i2c_handle
* @return HAL_StatusTypeDef
*/
HAL_StatusTypeDef lsm6dso_init(lsm6dso_t *imu, I2C_HandleTypeDef *i2c_handle);

/**
* @brief reads LSM6DSO memory register
*
* @param data
* @param reg
* @return 0 if HAL_OK, 1 if HAL_ERROR
*/
uint8_t lsm6dso_read(uint8_t *data, uint8_t reg);

/**
* @brief reads multiple LSM6DSO memory registers
*
* @param data
* @param reg
* @param length
* @return 0 if HAL_OK, 1 if HAL_ERROR
* @return 0 if OK, other if ERROR
*/
uint8_t lsm6dso_read_mult(uint8_t *data, uint8_t reg, uint8_t length);

/**
* @brief wrties to LSM6DSO memory register
*
* @param data
* @param reg
* @return 0 if HAL_OK, 1 if HAL_ERROR
*/
uint8_t lsm6dso_write(uint8_t *data, uint8_t reg);
int lsm6dso_init(I2C_HandleTypeDef *i2c_handle, I2C_Read read_func,
I2C_Write write_func);

/* IMU Setting Configuration */
/**
Expand All @@ -129,38 +97,31 @@ uint8_t lsm6dso_write(uint8_t *data, uint8_t reg);
* @param odr_sel
* @param fs_sel
* @param lp_f2_enable
* @return HAL_StatusTypeDef
* @return 0 if OK, other if ERROR
*/
HAL_StatusTypeDef lsm6dso_set_accel_cfg(lsm6dso_t *imu, int8_t odr_sel,
int8_t fs_sel, int8_t lp_f2_enable);
int lsm6dso_set_accel_cfg(int8_t odr_sel, int8_t fs_sel, int8_t lp_f2_enable);

/**
* @brief Configures the accelerometer of the LSM6DSO IMU
*
* @param imu
* @param odr_sel
* @param fs_sel
* @param fs_125
* @return HAL_StatusTypeDef
* @return 0 if OK, other if ERROR
*/
HAL_StatusTypeDef lsm6dso_set_gyro_cfg(lsm6dso_t *imu, int8_t odr_sel,
int8_t fs_sel, int8_t fs_125);
int lsm6dso_set_gyro_cfg(int8_t odr_sel, int8_t fs_sel, int8_t fs_125);

/* Data Aquisition */
/**
* @brief Retrieves accelerometer data from the LSM6DSO IMU
*
* @param imu
* @return HAL_StatusTypeDef
* @return 0 if OK, other if ERROR
*/
HAL_StatusTypeDef lsm6dso_read_accel(lsm6dso_t *imu);
int lsm6dso_read_accel();

/**
* @brief Retreives the gyroscope data from the LSM6DSO IMU
*
* @param imu
* @return HAL_StatusTypeDef
* @return 0 if OK, other if ERROR
*/
HAL_StatusTypeDef lsm6dso_read_gyro(lsm6dso_t *imu);
int lsm6dso_read_gyro();

#endif // LSM6DSO_H
156 changes: 70 additions & 86 deletions general/src/lsm6dso.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,45 +13,28 @@
4 /* The range of values in g's returned from accelerometer */
#define GYRO_RANGE 1000 /* The range of values from the gyro in dps */

lsm6dso_t *imu;
lsm6dso_t imu;

static inline HAL_StatusTypeDef lsm6dso_read_reg(lsm6dso_t *imu, uint8_t *data,
uint8_t reg)
static inline int lsm6dso_read_reg(uint8_t *data, uint8_t reg)
{
return HAL_I2C_Mem_Read(imu->i2c_handle, LSM6DSO_I2C_ADDRESS, reg,
I2C_MEMADD_SIZE_8BIT, data, 1, HAL_MAX_DELAY);
return imu.read_reg(data, reg, 1);
}

static inline HAL_StatusTypeDef lsm6dso_read_mult_reg(lsm6dso_t *imu,
uint8_t *data,
uint8_t reg,
uint8_t length)
static inline int lsm6dso_read_mult_reg(uint8_t *data, uint8_t reg,
uint8_t length)
{
return HAL_I2C_Mem_Read(imu->i2c_handle, LSM6DSO_I2C_ADDRESS, reg,
I2C_MEMADD_SIZE_8BIT, data, length,
HAL_MAX_DELAY);
return imu.read_reg(data, reg, length);
}

static inline HAL_StatusTypeDef lsm6dso_write_reg(lsm6dso_t *imu, uint8_t reg,
uint8_t *data)
static inline int lsm6dso_write_reg(uint8_t *data, uint8_t reg)
{
return HAL_I2C_Mem_Write(imu->i2c_handle, LSM6DSO_I2C_ADDRESS, reg, I2C_MEMADD_SIZE_8BIT,
data, 1, HAL_MAX_DELAY);
return imu.write_reg(data, reg, 1);
}

uint8_t lsm6dso_read(uint8_t *data, uint8_t reg)
static inline int lsm6dso_write_mult_reg(uint8_t *data, uint8_t reg,
uint8_t length)
{
return lsm6dso_read_reg(imu, data, reg);
}

uint8_t lsm6dso_read_mult(uint8_t *data, uint8_t reg, uint8_t length)
{
return lsm6dso_read_mult_reg(imu, data, reg, length);
}

uint8_t lsm6dso_write(uint8_t *data, uint8_t reg)
{
return lsm6dso_write_reg(imu, reg, data);
return imu.write_reg(data, reg, length);
}

static int16_t accel_data_convert(int16_t raw_accel)
Expand All @@ -75,136 +58,137 @@ static int16_t gyro_data_convert(int16_t gyro_accel)
return (int16_t)(((int32_t)val * GYRO_RANGE * 100) / REG_RESOLUTION);
}

static HAL_StatusTypeDef lsm6dso_ping_imu(lsm6dso_t *imu)
static int lsm6dso_ping_imu()
{
uint8_t reg_data;
HAL_StatusTypeDef status;
int status;

status = lsm6dso_read_reg(imu, &reg_data, LSM6DSO_REG_DEVICE_ID);
if (status != HAL_OK)
status = lsm6dso_read_reg(&reg_data, LSM6DSO_REG_DEVICE_ID);
if (status != 0)
return status;

if (reg_data != 0x6C)
return HAL_ERROR;
return -1;

return HAL_OK;
return 0;
}

HAL_StatusTypeDef lsm6dso_set_accel_cfg(lsm6dso_t *imu, int8_t odr_sel,
int8_t fs_sel, int8_t lp_f2_enable)
int lsm6dso_set_accel_cfg(int8_t odr_sel, int8_t fs_sel, int8_t lp_f2_enable)
{
uint8_t config =
(((odr_sel << 4) | (fs_sel << 2) | (lp_f2_enable << 1)) << 1);
imu->accel_config = config;
imu.accel_config = config;

return lsm6dso_write_reg(imu, LSM6DSO_REG_ACCEL_CTRL,
&imu->accel_config);
return lsm6dso_write_reg(&imu.accel_config, LSM6DSO_REG_ACCEL_CTRL);
}

HAL_StatusTypeDef lsm6dso_set_gyro_cfg(lsm6dso_t *imu, int8_t odr_sel,
int8_t fs_sel, int8_t fs_125)
int lsm6dso_set_gyro_cfg(int8_t odr_sel, int8_t fs_sel, int8_t fs_125)
{
uint8_t config =
(((odr_sel << 4) | (fs_sel << 2) | (fs_125 << 1)) << 1);
imu->gyro_config = config;
imu.gyro_config = config;

return lsm6dso_write_reg(imu, LSM6DSO_REG_GYRO_CTRL, &imu->gyro_config);
return lsm6dso_write_reg(&imu.gyro_config, LSM6DSO_REG_GYRO_CTRL);
}

HAL_StatusTypeDef lsm6dso_init(lsm6dso_t *imu, I2C_HandleTypeDef *i2c_handle)
int lsm6dso_init(I2C_HandleTypeDef *i2c_handle, I2C_Read read_func,
I2C_Write write_func)
{
HAL_StatusTypeDef status;
int status;

imu.i2c_handle = i2c_handle;

imu->i2c_handle = i2c_handle;
imu.accel_data[LSM6DSO_X_AXIS] = 0;
imu.accel_data[LSM6DSO_Y_AXIS] = 0;
imu.accel_data[LSM6DSO_Z_AXIS] = 0;

imu->accel_data[LSM6DSO_X_AXIS] = 0;
imu->accel_data[LSM6DSO_Y_AXIS] = 0;
imu->accel_data[LSM6DSO_Z_AXIS] = 0;
imu.gyro_data[LSM6DSO_X_AXIS] = 0;
imu.gyro_data[LSM6DSO_Y_AXIS] = 0;
imu.gyro_data[LSM6DSO_Z_AXIS] = 0;

imu->gyro_data[LSM6DSO_X_AXIS] = 0;
imu->gyro_data[LSM6DSO_Y_AXIS] = 0;
imu->gyro_data[LSM6DSO_Z_AXIS] = 0;
imu.read_reg = read_func;
imu.write_reg = write_func;

/* Quick check to make sure I2C is working */
status = lsm6dso_ping_imu(imu);
if (status != HAL_OK)
status = lsm6dso_ping_imu();
if (status != 0)
return status;

/*
Configure IMU to default params
Refer to datasheet pages 56-57
*/
status = lsm6dso_set_accel_cfg(imu, 0x08, 0x02, 0x01);
if (status != HAL_OK)
Configure IMU to default params
Refer to datasheet pages 56-57
*/
status = lsm6dso_set_accel_cfg(0x08, 0x02, 0x01);
if (status != 0)
return status;

status = lsm6dso_set_gyro_cfg(imu, 0x08, 0x02, 0x00);
if (status != HAL_OK)
status = lsm6dso_set_gyro_cfg(0x08, 0x02, 0x00);
if (status != 0)
return status;

return HAL_OK;
return 0;
}

HAL_StatusTypeDef lsm6dso_read_accel(lsm6dso_t *imu)
int lsm6dso_read_accel()
{
union {
uint8_t buf[2];
int16_t data;
} accel_x_raw, accel_y_raw, accel_z_raw;
HAL_StatusTypeDef status;
int status;

/* Getting raw data from registers */
status = lsm6dso_read_mult_reg(imu, accel_x_raw.buf,
status = lsm6dso_read_mult_reg(accel_x_raw.buf,
LSM6DSO_REG_ACCEL_X_AXIS_L, 2);
if (status != HAL_OK)
if (status != 0)
return status;

status = lsm6dso_read_mult_reg(imu, accel_y_raw.buf,
status = lsm6dso_read_mult_reg(accel_y_raw.buf,
LSM6DSO_REG_ACCEL_Y_AXIS_L, 2);
if (status != HAL_OK)
if (status != 0)
return status;

status = lsm6dso_read_mult_reg(imu, accel_z_raw.buf,
status = lsm6dso_read_mult_reg(accel_z_raw.buf,
LSM6DSO_REG_ACCEL_Z_AXIS_L, 2);
if (status != HAL_OK)
if (status != 0)
return status;

/* Setting imu struct values to converted measurements */
imu->accel_data[LSM6DSO_X_AXIS] = accel_data_convert(accel_x_raw.data);
imu->accel_data[LSM6DSO_Y_AXIS] = accel_data_convert(accel_y_raw.data);
imu->accel_data[LSM6DSO_Z_AXIS] = accel_data_convert(accel_z_raw.data);
imu.accel_data[LSM6DSO_X_AXIS] = accel_data_convert(accel_x_raw.data);
imu.accel_data[LSM6DSO_Y_AXIS] = accel_data_convert(accel_y_raw.data);
imu.accel_data[LSM6DSO_Z_AXIS] = accel_data_convert(accel_z_raw.data);

return HAL_OK;
return 0;
}

HAL_StatusTypeDef lsm6dso_read_gyro(lsm6dso_t *imu)
int lsm6dso_read_gyro()
{
union {
uint8_t buf[2];
int16_t data;
} gyro_x_raw, gyro_y_raw, gyro_z_raw;
HAL_StatusTypeDef status;
int status;

/* Aquire raw data from registers */
status = lsm6dso_read_mult_reg(imu, gyro_x_raw.buf,
status = lsm6dso_read_mult_reg(gyro_x_raw.buf,
LSM6DSO_REG_GYRO_X_AXIS_L, 2);
if (status != HAL_OK)
if (status != 0)
return status;

status = lsm6dso_read_mult_reg(imu, gyro_y_raw.buf,
status = lsm6dso_read_mult_reg(gyro_y_raw.buf,
LSM6DSO_REG_GYRO_Y_AXIS_L, 2);
if (status != HAL_OK)
if (status != 0)
return status;

status = lsm6dso_read_mult_reg(imu, gyro_z_raw.buf,
status = lsm6dso_read_mult_reg(gyro_z_raw.buf,
LSM6DSO_REG_GYRO_Z_AXIS_L, 2);
if (status != HAL_OK)
if (status != 0)
return status;

/* Setting imu struct values to converted measurements */
imu->gyro_data[LSM6DSO_X_AXIS] = gyro_data_convert(gyro_x_raw.data);
imu->gyro_data[LSM6DSO_Y_AXIS] = gyro_data_convert(gyro_y_raw.data);
imu->gyro_data[LSM6DSO_Z_AXIS] = gyro_data_convert(gyro_z_raw.data);
imu.gyro_data[LSM6DSO_X_AXIS] = gyro_data_convert(gyro_x_raw.data);
imu.gyro_data[LSM6DSO_Y_AXIS] = gyro_data_convert(gyro_y_raw.data);
imu.gyro_data[LSM6DSO_Z_AXIS] = gyro_data_convert(gyro_z_raw.data);

return HAL_OK;
return 0;
}

0 comments on commit 172529a

Please sign in to comment.