diff --git a/cangen/__init__.py b/cangen/__init__.py index 926ac82..4c415fa 100644 --- a/cangen/__init__.py +++ b/cangen/__init__.py @@ -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 diff --git a/general/include/lsm6dso.h b/general/include/lsm6dso.h index 362a0c2..6ac3d55 100644 --- a/general/include/lsm6dso.h +++ b/general/include/lsm6dso.h @@ -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 @@ -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 @@ -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 */ /** @@ -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 diff --git a/general/src/lsm6dso.c b/general/src/lsm6dso.c index 4ffb5c6..6db0ab6 100644 --- a/general/src/lsm6dso.c +++ b/general/src/lsm6dso.c @@ -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) @@ -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, ®_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; @@ -108,48 +104,51 @@ 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 */ @@ -157,31 +156,31 @@ HAL_StatusTypeDef lsm6dso_read_accel(lsm6dso_t *imu) 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 */ @@ -189,5 +188,5 @@ HAL_StatusTypeDef lsm6dso_read_gyro(lsm6dso_t *imu) 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; } diff --git a/platforms/stm32f405/include/can.h b/platforms/stm32f405/include/can.h index 69fb2ea..2fdc1d5 100644 --- a/platforms/stm32f405/include/can.h +++ b/platforms/stm32f405/include/can.h @@ -15,8 +15,6 @@ typedef struct { CAN_HandleTypeDef *hcan; - const uint32_t *id_list; - uint8_t id_list_len; } can_t; typedef struct { @@ -26,6 +24,7 @@ typedef struct { } can_msg_t; HAL_StatusTypeDef can_init(can_t *can); +HAL_StatusTypeDef can_add_filter(can_t *can, uint32_t *id_list); HAL_StatusTypeDef can_send_msg(can_t *can, can_msg_t *msg); HAL_StatusTypeDef can_send_extended_msg(can_t *can, can_msg_t *msg); diff --git a/platforms/stm32f405/src/can.c b/platforms/stm32f405/src/can.c index cd82bb1..8ee66dd 100644 --- a/platforms/stm32f405/src/can.c +++ b/platforms/stm32f405/src/can.c @@ -9,59 +9,9 @@ HAL_StatusTypeDef can_init(can_t *can) { - /* set up filter */ - uint16_t high_id = can->id_list[0]; - uint16_t low_id = can->id_list[0]; - - for (uint8_t i = 0; i < can->id_list_len; i++) { - if (can->id_list[i] > high_id) - high_id = can->id_list[i]; - if (can->id_list[i] < low_id) - low_id = can->id_list[i]; - } - - // uint32_t full_id = ((uint32_t)high_id << 16) | low_id; - - CAN_FilterTypeDef sFilterConfig; - - sFilterConfig.FilterBank = 0; - sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; - sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; - sFilterConfig.FilterIdHigh = 0x0000; - sFilterConfig.FilterIdLow = 0x0000; - sFilterConfig.FilterMaskIdHigh = 0x0000; - sFilterConfig.FilterMaskIdLow = 0x0000; - sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0; - sFilterConfig.FilterActivation = ENABLE; - sFilterConfig.SlaveStartFilterBank = 14; - - // sFilterConfig.FilterBank = 0; /* Filter bank number - // (0 to 27 for most STM32 series) */ sFilterConfig.FilterMode = - // CAN_FILTERMODE_IDLIST; /* Identifier list mode */ - // sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; /* 32-bit identifier - // list */ - - // sFilterConfig.FilterIdHigh = (full_id & 0xFFFF0000U) >> 5; - // sFilterConfig.FilterIdLow = (full_id & 0xFFFFU) << 5; - - // sFilterConfig.FilterMaskIdHigh = 0xFFFF << 5; /* Set to all ones for - // ID range */ sFilterConfig.FilterMaskIdLow = 0xFFFF; /* Set to - // all ones for ID range */ - - // sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0; /* FIFO to assign the - // filter to */ sFilterConfig.FilterActivation = ENABLE; /* Enable - // the filter */ - - uint8_t err = 0; - err = HAL_CAN_ConfigFilter(can->hcan, &sFilterConfig); - if (err != HAL_OK) - return err; - /* set up interrupt & activate CAN */ - HAL_CAN_IRQHandler(can->hcan); - - err = HAL_CAN_ActivateNotification(can->hcan, - CAN_IT_RX_FIFO0_MSG_PENDING); + int8_t err = HAL_CAN_ActivateNotification(can->hcan, + CAN_IT_RX_FIFO0_MSG_PENDING); if (err != HAL_OK) return err; err = HAL_CAN_Start(can->hcan); @@ -71,6 +21,32 @@ HAL_StatusTypeDef can_init(can_t *can) return err; } +HAL_StatusTypeDef can_add_filter(can_t *can, uint32_t id_list[4]) +{ + static int filterBank = 0; + + if (filterBank > 7) + return HAL_ERROR; + + CAN_FilterTypeDef filter; + + filter.FilterActivation = ENABLE; + filter.FilterFIFOAssignment = CAN_FILTER_FIFO0; + filter.FilterScale = CAN_FILTERSCALE_16BIT; + filter.FilterMode = CAN_FILTERMODE_IDLIST; + + filter.FilterIdLow = id_list[0] << 5u; + filter.FilterMaskIdLow = id_list[1] << 5u; + filter.FilterIdHigh = id_list[2] << 5u; + filter.FilterMaskIdHigh = id_list[3] << 5u; + + filter.FilterBank = filterBank; + + filterBank++; + + return HAL_CAN_ConfigFilter(can->hcan, &filter); +} + HAL_StatusTypeDef can_send_msg(can_t *can, can_msg_t *msg) { CAN_TxHeaderTypeDef tx_header;