Skip to content

Commit

Permalink
Merge branch 'main' into bug/MessageFiltering
Browse files Browse the repository at this point in the history
  • Loading branch information
@dnakhooda
dnakhooda committed Oct 31, 2024
2 parents 5a1659d + 20415db commit cefabad
Show file tree
Hide file tree
Showing 3 changed files with 72 additions and 73 deletions.
2 changes: 0 additions & 2 deletions cangen/__init__.py
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,4 @@
from cangen.RustSynth import RustSynth
from cangen.RustSynthFromJSON import RustSynthFromJSON
from cangen.YAMLParser import YAMLParser
from cangen.CANMsg import CANMsg
from cangen.CANMsg import EncodableCANMsg
from cangen.CANField import NetField
Expand Down
44 changes: 23 additions & 21 deletions general/include/lsm6dso.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,11 +44,16 @@
#define LSM6DSO_REG_ACCEL_Z_AXIS_L 0x2C /* Accelerometer Z axis lower bits */
#define LSM6DSO_REG_ACCEL_Z_AXIS_H 0x2D /* Accelerometer Z axis upper bits */

/* Function Pointers*/
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
*
*/
enum lsm6dso_axes {

LSM6DSO_X_AXIS = 0,
LSM6DSO_Y_AXIS = 1,
LSM6DSO_Z_AXIS = 2
Expand All @@ -59,8 +64,6 @@ enum lsm6dso_axes {
*
*/
typedef struct {
I2C_HandleTypeDef *i2c_handle;

uint8_t accel_config; // TODO: We should make these cfg packed unions with
// bitfield structs

Expand All @@ -70,16 +73,18 @@ typedef struct {

int16_t gyro_data[3];

I2C_Read read_reg;

I2C_Write write_reg;

} lsm6dso_t;

/**
* @brief Initializes the LSM6DSO IMU
*
* @param imu
* @param i2c_handle
* @return HAL_StatusTypeDef
* @return 0 if OK, other if ERROR
*/
HAL_StatusTypeDef lsm6dso_init(lsm6dso_t *imu, I2C_HandleTypeDef *i2c_handle);
int lsm6dso_init(lsm6dso_t *imu, I2C_Read read_func, I2C_Write write_func);

/* IMU Setting Configuration */
/**
Expand All @@ -89,38 +94,35 @@ HAL_StatusTypeDef lsm6dso_init(lsm6dso_t *imu, I2C_HandleTypeDef *i2c_handle);
* @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(lsm6dso_t *imu, 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(lsm6dso_t *imu, 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(lsm6dso_t *imu);

/**
* @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(lsm6dso_t *imu);

#endif // LSM6DSO_H
99 changes: 49 additions & 50 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 */

static inline HAL_StatusTypeDef lsm6dso_read_reg(lsm6dso_t *imu, uint8_t *data,
uint8_t reg)
static inline int lsm6dso_read_reg(lsm6dso_t *imu, 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(lsm6dso_t *imu, 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(lsm6dso_t *imu, 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);
}

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);
}

static int16_t accel_data_convert(int16_t raw_accel)
Expand All @@ -58,47 +56,45 @@ 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(lsm6dso_t *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)
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(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;

return lsm6dso_write_reg(imu, LSM6DSO_REG_ACCEL_CTRL,
&imu->accel_config);
return lsm6dso_write_reg(imu, &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(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;

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

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

imu->i2c_handle = i2c_handle;
int status;

imu->accel_data[LSM6DSO_X_AXIS] = 0;
imu->accel_data[LSM6DSO_Y_AXIS] = 0;
Expand All @@ -108,86 +104,89 @@ HAL_StatusTypeDef lsm6dso_init(lsm6dso_t *imu, I2C_HandleTypeDef *i2c_handle)
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)
if (status != 0)
return status;

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

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

return HAL_OK;
return 0;
}

HAL_StatusTypeDef lsm6dso_read_accel(lsm6dso_t *imu)
int lsm6dso_read_accel(lsm6dso_t *imu)
{
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,
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,
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,
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);

return HAL_OK;
return 0;
}

HAL_StatusTypeDef lsm6dso_read_gyro(lsm6dso_t *imu)
int lsm6dso_read_gyro(lsm6dso_t *imu)
{
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,
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,
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,
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);

return HAL_OK;
return 0;
}

0 comments on commit cefabad

Please sign in to comment.