Skip to content

Commit

Permalink
imu pointer in param
Browse files Browse the repository at this point in the history
  • Loading branch information
@caiodasilva2005
caiodasilva2005 committed Oct 27, 2024
1 parent 2d4813d commit f8aa44d
Showing 1 changed file with 47 additions and 46 deletions.
93 changes: 47 additions & 46 deletions general/src/lsm6dso.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,28 +13,26 @@
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;

static inline int lsm6dso_read_reg(uint8_t *data, uint8_t reg)
static inline int lsm6dso_read_reg(lsm6dso_t *imu, uint8_t *data, uint8_t reg)
{
return imu.read_reg(data, reg, 1);
return imu->read_reg(data, reg, 1);
}

static inline int lsm6dso_read_mult_reg(uint8_t *data, uint8_t reg,
uint8_t length)
static inline int lsm6dso_read_mult_reg(lsm6dso_t *imu, uint8_t *data,
uint8_t reg, uint8_t length)
{
return imu.read_reg(data, reg, length);
return imu->read_reg(data, reg, length);
}

static inline int lsm6dso_write_reg(uint8_t *data, uint8_t reg)
static inline int lsm6dso_write_reg(lsm6dso_t *imu, uint8_t *data, uint8_t reg)
{
return imu.write_reg(data, reg, 1);
return imu->write_reg(data, reg, 1);
}

static inline int lsm6dso_write_mult_reg(uint8_t *data, uint8_t reg,
uint8_t length)
static inline int lsm6dso_write_mult_reg(lsm6dso_t *imu, uint8_t *data,
uint8_t reg, uint8_t length)
{
return imu.write_reg(data, reg, length);
return imu->write_reg(data, reg, length);
}

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

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

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

Expand All @@ -73,60 +71,63 @@ static int lsm6dso_ping_imu()
return 0;
}

int lsm6dso_set_accel_cfg(int8_t odr_sel, int8_t fs_sel, int8_t lp_f2_enable)
int lsm6dso_set_accel_cfg(lsm6dso_t *imu, 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.accel_config, LSM6DSO_REG_ACCEL_CTRL);
return lsm6dso_write_reg(imu, imu->accel_config,
LSM6DSO_REG_ACCEL_CTRL);
}

int lsm6dso_set_gyro_cfg(int8_t odr_sel, int8_t fs_sel, int8_t fs_125)
int lsm6dso_set_gyro_cfg(lsm6dso_t *imu, 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.gyro_config, LSM6DSO_REG_GYRO_CTRL);
return lsm6dso_write_reg(imu, imu->gyro_config, LSM6DSO_REG_GYRO_CTRL);
}

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

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;
imu->read_reg = read_func;
imu->write_reg = write_func;

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

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

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

return 0;
}

int lsm6dso_read_accel()
int lsm6dso_read_accel(lsm6dso_t *imu)
{
union {
uint8_t buf[2];
Expand All @@ -135,30 +136,30 @@ int lsm6dso_read_accel()
int status;

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

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

status = lsm6dso_read_mult_reg(accel_z_raw.buf,
status = lsm6dso_read_mult_reg(imu, accel_z_raw.buf,
LSM6DSO_REG_ACCEL_Z_AXIS_L, 2);
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 0;
}

int lsm6dso_read_gyro()
int lsm6dso_read_gyro(lsm6dso_t *imu)
{
union {
uint8_t buf[2];
Expand All @@ -167,25 +168,25 @@ int lsm6dso_read_gyro()
int status;

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

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

status = lsm6dso_read_mult_reg(gyro_z_raw.buf,
status = lsm6dso_read_mult_reg(imu, gyro_z_raw.buf,
LSM6DSO_REG_GYRO_Z_AXIS_L, 2);
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 0;
}

0 comments on commit f8aa44d

Please sign in to comment.