pima/Trinity-DevPCB-V1.0
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fixed DMA and INT bugs

Browse Source 
- Fixed indexed task notifications for INT and DMAf for both sensors
- Fixed the INT debugger startup freeze (added != NULL check in
callbacks)
- Fixed DMA notifications not being read properly/too early
- Changed IMU INT configure/enable function as one
- Added proper DMA checking in IMU calibration
This commit is contained in:
pima committed 2026-10-03 17:42:33 +02:00
1 parent 5daac77a37
commit 1409bad40b
67 files changed
+52921 -51367
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

Core/Src/app_freertos.c
+14
View File
@@ -52,6 +52,20 @@
/* USER CODE END FunctionPrototypes */
/* Hook prototypes */
void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName);
/* USER CODE BEGIN 4 */
void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName)
{
/* Run time stack overflow checking is performed if
configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is
called if a stack overflow is detected. */
__disable_irq();
for (;;) { } // breakpoint here
}
/* USER CODE END 4 */
/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
Core/Src/main.c
+61 -59
View File
@@ -44,6 +44,12 @@
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
//#define ICM_NOTIFY_INT_DRDY 0
//#define ICM_NOTIFY_DMA 1
//#define BMM_NOTIFY_INT_DRDY 0
//#define BMM_NOTIFY_DMA 1
//configASSERT(BMM_NOTIFY_DMA < configTASK_NOTIFICATION_ARRAY_ENTRIES);
//configASSERT(BMM_NOTIFY_INT_DRDY < configTASK_NOTIFICATION_ARRAY_ENTRIES);
/* USER CODE END PD */
@@ -359,7 +365,7 @@ static void MX_I2C1_Init(void)
/* USER CODE END I2C1_Init 1 */
hi2c1.Instance = I2C1;
hi2c1.Init.Timing = 0x10B17DB5;
hi2c1.Init.Timing = 0x00C12166;
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
@@ -504,7 +510,7 @@ static void MX_SPI1_Init(void)
hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
hspi1.Init.NSS = SPI_NSS_SOFT;
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_64;
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
@@ -726,7 +732,7 @@ void Start_DataAnalysis(void *argument)
// TODO: Einstellungen setzbar durch trinity control für IMU, Magnetometer, etc.
// HAL_StatusTypeDef status;
char output_buffer[128];
// char output_buffer[128];
ICM45686_Data imu_data;
struct bmm350_mag_temp_data magn_data;
// struct bmm350_raw_mag_data magn_data;
@@ -735,7 +741,7 @@ void Start_DataAnalysis(void *argument)
Quat_Dat.SensorAddress = BMM350_I2C_ADSEL_SET_LOW;
Quat_Dat.EndByte = PACKET_END_BYTE;
MadgwickAHRS_init(&IMUQuat, 6400.0f, 0.15f);
MadgwickAHRS_init(&IMUQuat, 6400.0f, 0.00001f);
/* Infinite loop */
for(;;)
@@ -747,7 +753,7 @@ void Start_DataAnalysis(void *argument)
MadgwickAHRS_update(&IMUQuat, imu_data.processed_imu_data[3], imu_data.processed_imu_data[4], imu_data.processed_imu_data[5], //gx, gy, gz
imu_data.processed_imu_data[0], imu_data.processed_imu_data[1], imu_data.processed_imu_data[2], // ax, ay, yz
magn_data.x, magn_data.y, magn_data.z); // mx, my, mz
//
// MadgwickAHRS_update_IMU(&IMUQuat, imu_data.processed_imu_data[3], imu_data.processed_imu_data[4], imu_data.processed_imu_data[5], //gx, gy, gz
// imu_data.processed_imu_data[0], imu_data.processed_imu_data[1], imu_data.processed_imu_data[2]); // ax, ay, yz)
@@ -805,8 +811,6 @@ void Start_ICM45686(void *argument)
HAL_StatusTypeDef status;
// char output_buffer[128];
xIMU_Task = osThreadGetId();
// ICM
//uint8_t usb_response;
uint8_t raw_imu_data[15];
int16_t imu_sensor_data[7];
ICM45686_Data processed_data;
@@ -815,58 +819,62 @@ void Start_ICM45686(void *argument)
imu.hspi = &hspi1;
imu.GPIO_Port = GPIO_SPI1_IMU_CS_GPIO_Port;
imu.GPIO_Pin = GPIO_SPI1_IMU_CS_Pin;
imu.acc_fs = ICM45686_ACC_FS_4G;
imu.acc_ssf = ICM45686_ACC_SSF[ICM45686_ACC_FS_4G >> 4];
imu.gyro_fs = ICM45686_GYRO_FS_250DPS;
imu.gyro_ssf = ICM45686_GYRO_SSF[ICM45686_GYRO_FS_250DPS >> 4];
imu.acc_fs = ICM45686_ACC_FS_16G;
imu.acc_ssf = ICM45686_ACC_SSF[ICM45686_ACC_FS_16G >> 4];
imu.gyro_fs = ICM45686_GYRO_FS_2000DPS;
imu.gyro_ssf = ICM45686_GYRO_SSF[ICM45686_GYRO_FS_2000DPS >> 4];
imu.odr = ICM45686_ODR_6_4kHz_LN ;
uint8_t tx[15] = {0x00};
tx[0] = 0x00 | 0x80;
uint16_t tx_len = sizeof(tx);
init_icm(&imu);
osDelay(50);
configure_interrupt_icm(&imu);
enable_interrupt_icm(&imu);
// osDelay(500);
// calibrate_icm(&imu, 1000);
configure_and_enable_int_icm(&imu);
ulTaskNotifyTakeIndexed(ICM_NOTIFY_INT_DRDY, pdTRUE, 0);
ulTaskNotifyTakeIndexed(ICM_NOTIFY_DMA, pdTRUE, 0);
calibrate_icm(&imu, 25000);
/* Infinite loop */
for(;;)
{
ulTaskNotifyTake(pdFALSE, osWaitForever);
status = read_icm_dma(&imu, tx, raw_imu_data, sizeof(tx));
ulTaskNotifyTakeIndexed(ICM_NOTIFY_INT_DRDY, pdTRUE, portMAX_DELAY);
status = read_icm_dma(&imu, tx, raw_imu_data, tx_len);
if (status != HAL_OK) {
printf("Reading IMU sensordata failed: %d\n", status);
}
if (ulTaskNotifyTakeIndexed(ICM_NOTIFY_DMA, pdTRUE, pdMS_TO_TICKS(10)) > 0) {
imu_sensor_data[0] = (int16_t)(raw_imu_data[1] << 8 | raw_imu_data[2]); // a_x
imu_sensor_data[1] = (int16_t)(raw_imu_data[3] << 8 | raw_imu_data[4]); // a_y
imu_sensor_data[2] = (int16_t)(raw_imu_data[5] << 8 | raw_imu_data[6]); // a_z
imu_sensor_data[3] = (int16_t)(raw_imu_data[7] << 8 | raw_imu_data[8]); // g_x
imu_sensor_data[4] = (int16_t)(raw_imu_data[9] << 8 | raw_imu_data[10]); // g_y
imu_sensor_data[5] = (int16_t)(raw_imu_data[11] << 8 | raw_imu_data[12]); // g_z
imu_sensor_data[6] = (int16_t)(raw_imu_data[13] << 8 | raw_imu_data[14]); // t
imu_sensor_data[0] = (int16_t)(raw_imu_data[1] << 8 | raw_imu_data[2]); // a_x
imu_sensor_data[1] = (int16_t)(raw_imu_data[3] << 8 | raw_imu_data[4]); // a_y
imu_sensor_data[2] = (int16_t)(raw_imu_data[5] << 8 | raw_imu_data[6]); // a_z
imu_sensor_data[3] = (int16_t)(raw_imu_data[7] << 8 | raw_imu_data[8]); // g_x
imu_sensor_data[4] = (int16_t)(raw_imu_data[9] << 8 | raw_imu_data[10]); // g_y
imu_sensor_data[5] = (int16_t)(raw_imu_data[11] << 8 | raw_imu_data[12]); // g_z
imu_sensor_data[6] = (int16_t)(raw_imu_data[13] << 8 | raw_imu_data[14]); // t
processed_data.processed_imu_data[0] = (float)imu_sensor_data[0] / (float)imu.acc_fs;
processed_data.processed_imu_data[1] = (float)imu_sensor_data[1] / (float)imu.acc_fs;
processed_data.processed_imu_data[2] = (float)imu_sensor_data[2] / (float)imu.acc_fs;
processed_data.processed_imu_data[3] = (float)imu_sensor_data[3] / (float)imu.gyro_ssf;
processed_data.processed_imu_data[4] = (float)imu_sensor_data[4] / (float)imu.gyro_ssf;
processed_data.processed_imu_data[5] = (float)imu_sensor_data[5] / (float)imu.gyro_ssf;
processed_data.processed_imu_data[6] = (float)(imu_sensor_data[6] / 128.0 + 25.0);
if (imu.calibated) {
processed_data.processed_imu_data[0] -= imu.imu_bias[0];
processed_data.processed_imu_data[1] -= imu.imu_bias[1];
processed_data.processed_imu_data[2] -= imu.imu_bias[2];
processed_data.processed_imu_data[3] -= imu.imu_bias[3];
processed_data.processed_imu_data[4] -= imu.imu_bias[4];
processed_data.processed_imu_data[5] -= imu.imu_bias[5];
processed_data.processed_imu_data[6] -= imu.imu_bias[7];
processed_data.processed_imu_data[0] = (float)imu_sensor_data[0] / (float)imu.acc_fs;
processed_data.processed_imu_data[1] = (float)imu_sensor_data[1] / (float)imu.acc_fs;
processed_data.processed_imu_data[2] = (float)imu_sensor_data[2] / (float)imu.acc_fs;
processed_data.processed_imu_data[3] = (float)imu_sensor_data[3] / (float)imu.gyro_ssf;
processed_data.processed_imu_data[4] = (float)imu_sensor_data[4] / (float)imu.gyro_ssf;
processed_data.processed_imu_data[5] = (float)imu_sensor_data[5] / (float)imu.gyro_ssf;
processed_data.processed_imu_data[6] = (float)(imu_sensor_data[6] / 128.0 + 25.0);
if (imu.calibated) {
processed_data.processed_imu_data[0] -= imu.imu_bias[0];
processed_data.processed_imu_data[1] -= imu.imu_bias[1];
processed_data.processed_imu_data[2] -= imu.imu_bias[2];
processed_data.processed_imu_data[3] -= imu.imu_bias[3];
processed_data.processed_imu_data[4] -= imu.imu_bias[4];
processed_data.processed_imu_data[5] -= imu.imu_bias[5];
processed_data.processed_imu_data[6] -= imu.imu_bias[6];
}
osMessageQueuePut(Q_ICMHandle, &processed_data, 0, 5);
} else {
HAL_SPI_Abort(imu.hspi);
HAL_GPIO_WritePin(imu.GPIO_Port, imu.GPIO_Pin, GPIO_PIN_SET);
printf("IMU DMA data transaction timed out");
}
osMessageQueuePut(Q_ICMHandle, &processed_data, 0, 5);
}
/* USER CODE END Start_ICM45686 */
}
@@ -881,44 +889,38 @@ void Start_ICM45686(void *argument)
void Start_BMM350(void *argument)
{
/* USER CODE BEGIN Start_BMM350 */
// HAL_StatusTypeDef status;
// char output_buffer[128];
xMagn_Task = osThreadGetId();
// BMM
struct bmm350_dev dev = {0x00};
int8_t rslt;
struct bmm350_mag_temp_data mag_data;
osDelay(500);
// struct bmm350_raw_mag_data mag_data;
// ODR: 200HZ, AVG:
rslt = init_bmm(&dev);
if (rslt != BMM350_OK) {
printf("BMM350 initialization failed: %d\n", rslt);
printf("Chip id: 0x%02X\n", dev.chip_id);
}
//
rslt = bmm_configure_interrupt(&dev);
if (rslt != BMM350_OK) {
printf("Configuring interrupt failed: %d\n", rslt);
}
// osDelay(200);
ulTaskNotifyTakeIndexed(BMM_NOTIFY_INT_DRDY, pdTRUE, 0);
ulTaskNotifyTakeIndexed(BMM_NOTIFY_DMA, pdTRUE, 0);
// osDelay(50);
/* Infinite loop */
for(;;)
{
// wait for the interrupt
ulTaskNotifyTake(pdFALSE, osWaitForever);
// magnetometer
// ulTaskNotifyTake(pdFALSE, osWaitForever);
// ulTaskNotifyTakeIndexed(pdFALSE, BMM_NOTIFY_INT_DRDY, osWaitForever);
// ulTaskNotifyTakeIndexed(BMM_NOTIFY_INT_DRDY, pdTRUE, osWaitForever);
rslt = bmm350_get_compensated_mag_xyz_temp_data(&mag_data, &dev);
// rslt = bmm350_read_uncomp_mag_temp_data(&mag_data, &dev);
// ulTaskNotifyTake(pdFALSE, osWaitForever);
// ulTaskNotifyTakeIndexed(BMM_NOTIFY_DMA, pdTRUE, osWaitForever);
if (rslt == BMM350_OK)
{
// sprintf(output_buffer, "X: %.2f uT, Y: %.2f uT, Z: %.2f uT, Temp: %.2f C\n",
// mag_data.x, mag_data.y, mag_data.z, mag_data.temperature);
// CDC_Transmit_FS((uint8_t*)output_buffer, (uint16_t)strlen(output_buffer));
osMessageQueuePut(Q_BMMHandle, &mag_data, 0, 5);
}
// osDelay(100);
}
/* USER CODE END Start_BMM350 */
}
Core/Src/sensors.c
+118 -98
View File
@@ -10,65 +10,60 @@
//extern osThreadId_t defaultTaskHandle;
//TaskHandle_t xStartDefaultTask = NULL;
//extern osThreadId_t defaultTaskHandle
osThreadId_t xIMU_Task = NULL;
osThreadId_t xMagn_Task = NULL;
volatile osThreadId_t xIMU_Task = NULL;
volatile osThreadId_t xMagn_Task = NULL;
//osThreadId_t xData_Task = NULL;
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
if (hspi->Instance == SPI1) {
if (hspi->Instance == SPI1 && xIMU_Task != NULL) {
HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
// notify the task that the data is ready
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
vTaskNotifyGiveFromISR(xIMU_Task, &xHigherPriorityTaskWoken);
vTaskNotifyGiveIndexedFromISR(xIMU_Task, ICM_NOTIFY_DMA, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *I2C_address) {
// printf("HAL_I2C_MemRxCpltCallback");
if (I2C_address->Instance == I2C1) {
if (I2C_address->Instance == I2C1 && xMagn_Task != NULL) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
// vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
vTaskNotifyGiveIndexedFromISR(xMagn_Task, BMM_NOTIFY_DMA, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *I2C_address) {
// printf("HAL_I2C_MemTxCpltCallback");
if (I2C_address->Instance == I2C1) {
if (I2C_address->Instance == I2C1 && xMagn_Task != NULL) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
// vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
vTaskNotifyGiveIndexedFromISR(xMagn_Task, BMM_NOTIFY_DMA, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c) {
if (hi2c->Instance == I2C1) {
if (hi2c->Instance == I2C1 && xMagn_Task != NULL) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
printf("ErrorCallback: %ld\n", HAL_I2C_GetError(hi2c));
vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
// vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
vTaskNotifyGiveIndexedFromISR(xMagn_Task, BMM_NOTIFY_DMA, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
// was macht das?
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
if (GPIO_Pin == GPIO_IMU_INT_Pin) {
vTaskNotifyGiveFromISR(xIMU_Task, &xHigherPriorityTaskWoken);
if (GPIO_Pin == GPIO_IMU_INT_Pin && xIMU_Task != NULL) {
// vTaskNotifyGiveFromISR(xIMU_Task, &xHigherPriorityTaskWoken);
vTaskNotifyGiveIndexedFromISR(xIMU_Task, ICM_NOTIFY_INT_DRDY, &xHigherPriorityTaskWoken);
// printf("Triggered ICM45686 EXTI\n");
}
if (GPIO_Pin == GPIO_MAGN_INT_Pin) {
vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
// printf("Triggered BMM350 EXTI\n");
if (GPIO_Pin == GPIO_MAGN_INT_Pin && xMagn_Task != NULL) {
// vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
vTaskNotifyGiveIndexedFromISR(xMagn_Task, BMM_NOTIFY_INT_DRDY, &xHigherPriorityTaskWoken);
}
// was macht das?
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
// ICM 45686
@@ -93,33 +88,10 @@ HAL_StatusTypeDef write_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint16_t
}
void init_icm(ICM45686_HandleTypeDef *imu) {
// TODO: Add settings as enum to choose from
// FIFO
// Interrupt
/* Interrupt functionality is configured via the Interrupt Configuration register. Items that are configurable include the
interrupt pins configuration, the interrupt latching and clearing method, and triggers for the interrupt. Items that
can trigger an interrupt are (1) Clock generator locked to new reference oscillator (used when switching clock
sources); (2) new data is available to be read (from the FIFO and Data registers); (3) accelerometer event
interrupts; (4) FIFO watermark; (5) FIFO overflow. The interrupt status can be read from the Interrupt Status
register.
- Configure interrupt pin on icm
- configure interrupt pin on bmm
- -> push/pull or open drain, latch, polarity
- everything stays as is only that a interrupt callback gets added, that when called wakes up the
corresponding task for the sensor
*/
// APEX???
// EDMP???
uint8_t tx[2], rx[2];
HAL_StatusTypeDef status;
printf("Initializing ICM45686\n");
// WHO AM
// // WHO AM
tx[0] = ICM45686_WHO_AM_I | 0x80;
// HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = read_icm(imu, tx, rx, sizeof(tx));
if (status != HAL_OK) {
printf("Error in reading WHO AM I:%d\n", status);
@@ -127,12 +99,9 @@ void init_icm(ICM45686_HandleTypeDef *imu) {
printf("WHO AM I (0xE9): 0x%02X \n", rx[1]);
}
// acc
// acc config
tx[0] = ICM45686_ACCEL_CONFIG0;
tx[1] = imu->acc_fs | imu->odr;
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = write_icm(imu, tx, sizeof(tx));
if (status != HAL_OK) {
printf("Error in setting ACCEL_CONFIG0:%d\n", status);
@@ -142,9 +111,6 @@ void init_icm(ICM45686_HandleTypeDef *imu) {
tx[0] = ICM45686_ACCEL_CONFIG0 | 0x80;
tx[1] = 0x00;
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = read_icm(imu, tx, rx, sizeof(tx));
if (status != HAL_OK) {
printf("Error reading ACCEL_CONFIG0:%d\n", status);
@@ -152,12 +118,9 @@ void init_icm(ICM45686_HandleTypeDef *imu) {
printf("ACCEL_CONFIG0 read: 0x%02X \n", rx[1]);
}
// gyro
// gyro config
tx[0] = ICM45686_GYRO_CONFIG0;
tx[1] = imu->gyro_fs | imu->odr;
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = write_icm(imu, tx, sizeof(tx));
if (status != HAL_OK) {
printf("Error in setting GYRO_CONFIG0:%d\n", status);
@@ -167,9 +130,6 @@ void init_icm(ICM45686_HandleTypeDef *imu) {
tx[0] = ICM45686_GYRO_CONFIG0 | 0x80;
tx[1] = 0x00;
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = read_icm(imu, tx, rx, sizeof(tx));
if (status != HAL_OK) {
printf("Error reading GYRO_CONFIG0:%d\n", status);
@@ -180,9 +140,6 @@ void init_icm(ICM45686_HandleTypeDef *imu) {
// power settings
tx[0] = ICM45686_PWR_MGMT0;
tx[1] = 0x0F; //0x10 | 0x03;
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = write_icm(imu, tx, sizeof(tx));
if (status != HAL_OK) {
printf("Error in setting PWR_MGMT0:%d\n", status);
@@ -192,17 +149,12 @@ void init_icm(ICM45686_HandleTypeDef *imu) {
tx[0] = ICM45686_PWR_MGMT0 | 0x80;
tx[1] = 0x00;
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = read_icm(imu, tx, rx, sizeof(tx));
if (status != HAL_OK) {
printf("Error reading PWR_MGMT0:%d\n", status);
} else {
printf("PWR_MGMT0 read: 0x%02X \n", rx[1]);
}
osDelay(250);
}
void calibrate_icm(ICM45686_HandleTypeDef *imu, int samples) {
@@ -213,10 +165,17 @@ void calibrate_icm(ICM45686_HandleTypeDef *imu, int samples) {
tx[0] = 0x00 | 0x80;
for (int i = 1; i <= samples; i++) {
status = read_icm_dma(imu, tx, raw_data, sizeof(tx));
if (status != HAL_OK) {
printf("Calibration read ICM failed: %d\n", status);
}
// status = read_icm_dma(imu, tx, raw_data, sizeof(tx));
// osDelay(1);
// if (status != HAL_OK) {
// printf("Calibration read ICM failed: %d\n", status);
// }
ulTaskNotifyTakeIndexed(ICM_NOTIFY_INT_DRDY, pdTRUE, portMAX_DELAY);
status = read_icm_dma(imu, tx, raw_data, sizeof(tx));
if (status != HAL_OK) {
printf("Calibration read ICM failed: %d\n", status);
}
if (ulTaskNotifyTakeIndexed(ICM_NOTIFY_DMA, pdTRUE, pdMS_TO_TICKS(10)) > 0) {
sensor_data[0] += (int16_t)(raw_data[1] << 8 | raw_data[2]); // a_x
sensor_data[1] += (int16_t)(raw_data[3] << 8 | raw_data[4]); // a_y
sensor_data[2] += (int16_t)(raw_data[5] << 8 | raw_data[6]); // a_z
@@ -224,6 +183,7 @@ void calibrate_icm(ICM45686_HandleTypeDef *imu, int samples) {
sensor_data[4] += (int16_t)(raw_data[9] << 8 | raw_data[10]); // g_y
sensor_data[5] += (int16_t)(raw_data[11] << 8 | raw_data[12]); // g_z
sensor_data[6] += (int16_t)(raw_data[13] << 8 | raw_data[14]); // t
}
}
imu->imu_bias[0] = (float)sensor_data[0] / (float)samples;
@@ -239,20 +199,38 @@ void calibrate_icm(ICM45686_HandleTypeDef *imu, int samples) {
imu->calibated = true;
printf("Calibration finished of ICM45686\n");
printf("a_x: %.2f a_y: %.2f a_z: %.2f g_x: %.2f g_y: %.2f g_z: %.2f t: %.2f\n",
printf("ICM calibration bias: a_x: %.2f a_y: %.2f a_z: %.2f g_x: %.2f g_y: %.2f g_z: %.2f t: %.2f\n",
imu->imu_bias[0], imu->imu_bias[1], imu->imu_bias[2],
imu->imu_bias[3], imu->imu_bias[4], imu->imu_bias[5],
imu->imu_bias[6]);
}
void enable_interrupt_icm(ICM45686_HandleTypeDef *imu) {
uint8_t tx[2], rx[2] = {0x00, 0x00};
tx[0] = ICM45686_INT1_CONFIG0;
tx[1] = 0x04;
printf("Enabling IMU INT pin\n");
//void enable_interrupt_icm(ICM45686_HandleTypeDef *imu) {
// uint8_t tx[2], rx[2] = {0x00, 0x00};
//
// tx[0] = ICM45686_INT1_CONFIG0;
// tx[1] = 0x04;
// HAL_StatusTypeDef status = write_icm(imu, tx, sizeof(tx));
// if (status != HAL_OK) {
// printf("Enabling of IMU INT failed: %d\n", status);
// }
//
// tx[0] = ICM45686_INT1_CONFIG0 | 0x80;
// tx[1] = 0x00;
// status = read_icm(imu, tx, rx, sizeof(tx));
// if (status != HAL_OK) {
// printf("Reading IMU INT1_CONFIG0 failed: %d\n", status);
// } else {
// printf("INT1_CONFIG0 read: 0x%02X\n", rx[1]);
// }
//}
void configure_and_enable_int_icm (ICM45686_HandleTypeDef *imu) {
uint8_t tx[2], rx[2] = {0x00, 0x00};
// turn off int
tx[0] = ICM45686_INT1_CONFIG0;
tx[1] = 0x00;
HAL_StatusTypeDef status = write_icm(imu, tx, sizeof(tx));
if (status != HAL_OK) {
printf("Enabling of IMU INT failed: %d\n", status);
@@ -262,19 +240,15 @@ void enable_interrupt_icm(ICM45686_HandleTypeDef *imu) {
tx[1] = 0x00;
status = read_icm(imu, tx, rx, sizeof(tx));
if (status != HAL_OK) {
printf("Reading IMU INT configuration0 failed: %d\n", status);
printf("Reading IMU INT1_CONFIG0 failed: %d\n", status);
} else {
printf("INT1_CONFIG0 read: 0x%02X\n", rx[1]);
}
}
void configure_interrupt_icm (ICM45686_HandleTypeDef *imu) {
uint8_t tx[2], rx[2] = {0x00, 0x00};
// configure int1 to push-pull, pulse, active high
tx[0] = ICM45686_INT1_CONFIG2;
tx[1] = 0x01;
printf("Configuring IMU INT\n");
HAL_StatusTypeDef status = write_icm(imu, tx, sizeof(tx));
status = write_icm(imu, tx, sizeof(tx));
if (status != HAL_OK) {
printf("Configuration of IMU INT failed: %d\n", status);
}
@@ -283,10 +257,27 @@ void configure_interrupt_icm (ICM45686_HandleTypeDef *imu) {
tx[1] = 0x00;
status = read_icm(imu, tx, rx, sizeof(tx));
if (status != HAL_OK) {
printf("Reading IMU INT configuration2 failed: %d\n", status);
printf("Reading IMU INT1_CONFIG2 failed: %d\n", status);
} else {
printf("INT1_CONFIG2 read: 0x%02X\n", rx[1]);
}
// enable int
tx[0] = ICM45686_INT1_CONFIG0;
tx[1] = 0x04;
status = write_icm(imu, tx, sizeof(tx));
if (status != HAL_OK) {
printf("Enabling of IMU INT failed: %d\n", status);
}
tx[0] = ICM45686_INT1_CONFIG0 | 0x80;
tx[1] = 0x00;
status = read_icm(imu, tx, rx, sizeof(tx));
if (status != HAL_OK) {
printf("Reading IMU INT1_CONFIG0 failed: %d\n", status);
} else {
printf("INT1_CONFIG0 read: 0x%02X\n", rx[1]);
}
}
// BMM350
@@ -309,7 +300,9 @@ BMM350_INTF_RET_TYPE bmm350_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32
// Device address is left-shifted by 1 for STM32 HAL
// if (HAL_I2C_Mem_Read(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK) {
if (HAL_I2C_Mem_Read_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, reg_data, length) == HAL_OK) {
if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
// if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
if (ulTaskNotifyTakeIndexed(BMM_NOTIFY_DMA, pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
// if (ulTaskNotifyTakeIndexed(pdTRUE, BMM_NOTIFY_DMA, pdMS_TO_TICKS(10)) == pdTRUE) {
return BMM350_INTF_RET_SUCCESS; // 0
}
}
@@ -317,6 +310,20 @@ BMM350_INTF_RET_TYPE bmm350_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32
}
//BMM350_INTF_RET_TYPE bmm350_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr) {
// // Extract I2C device address from intf_ptr
// uint8_t device_addr = *(uint8_t*)intf_ptr;
//
// // STM32 HAL: Write register address, then read data
// // Device address is left-shifted by 1 for STM32 HAL
// if (HAL_I2C_Mem_Read(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK)
// {
// return BMM350_INTF_RET_SUCCESS; // 0
// }
//
// return -1; // Communication failure
//}
BMM350_INTF_RET_TYPE bmm350_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr){
uint8_t device_addr = *(uint8_t*)intf_ptr;
// uint8_t rslt = HAL_I2C_Mem_Write_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length);
@@ -325,13 +332,26 @@ BMM350_INTF_RET_TYPE bmm350_i2c_write(uint8_t reg_addr, const uint8_t *reg_data,
if (HAL_I2C_Mem_Write_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length) == HAL_OK) {
// if (HAL_I2C_Mem_Write(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK) {
// if (rslt == HAL_OK) {
if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
// if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
if (ulTaskNotifyTakeIndexed(BMM_NOTIFY_DMA, pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
// if (ulTaskNotifyTakeIndexed(pdTRUE, BMM_NOTIFY_DMA, pdMS_TO_TICKS(10)) == pdTRUE) {
return BMM350_INTF_RET_SUCCESS; // 0
}
}
return -1;
}
//BMM350_INTF_RET_TYPE bmm350_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr){
// uint8_t device_addr = *(uint8_t*)intf_ptr;
//
// // STM32 HAL: Write register address + data
// if (HAL_I2C_Mem_Write(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK) {
// return BMM350_INTF_RET_SUCCESS;
// }
//
// return -1;
//}
void bmm350_delay(uint32_t period_us, void *intf_ptr){
(void)intf_ptr; // Unused
@@ -355,7 +375,7 @@ int8_t init_bmm(struct bmm350_dev *dev) {
dev->delay_us = bmm350_delay;
dev->intf_ptr = &dev_addr;
printf("Starting up and configuring BMM350\n");
// printf("Starting up and configuring BMM350\n");
// Initialize BMM350 driver
rslt = bmm350_init(dev);
@@ -370,8 +390,8 @@ int8_t init_bmm(struct bmm350_dev *dev) {
// Set powermode, odr and avg
rslt = bmm350_set_powermode(BMM350_NORMAL_MODE, dev);
printf("Powermode (%d): %d\n", BMM350_NORMAL_MODE, rslt);
rslt = bmm350_set_odr_performance(BMM350_DATA_RATE_200HZ, BMM350_AVERAGING_2, dev);
printf("ODR(%d) and AVG(%d): %d\n", BMM350_DATA_RATE_200HZ, BMM350_AVERAGING_2, rslt);
rslt = bmm350_set_odr_performance(BMM350_DATA_RATE_400HZ, BMM350_NO_AVERAGING, dev);
printf("ODR(%d) and AVG(%d): %d\n", BMM350_DATA_RATE_400HZ, BMM350_NO_AVERAGING, rslt);
// // configure interrupt
// rslt = bmm350_enable_interrupt(BMM350_ENABLE_INTERRUPT, dev);
@@ -385,9 +405,9 @@ int8_t init_bmm(struct bmm350_dev *dev) {
int8_t bmm_configure_interrupt (struct bmm350_dev *dev) {
int8_t rslt;
rslt = bmm350_enable_interrupt(BMM350_ENABLE_INTERRUPT, dev);
printf("Interrupt enable: %d\n", rslt);
printf("Interrupt enable: %d \n", rslt);
rslt = bmm350_configure_interrupt(BMM350_PULSED, BMM350_ACTIVE_HIGH, BMM350_INTR_PUSH_PULL, BMM350_MAP_TO_PIN, dev);
printf("Configure interrupt: %d\n", rslt);
printf("Configure interrupt: %d \n", rslt);
return BMM350_OK;
}
Core/Src/sensors.h
+12 -4
View File
@@ -10,10 +10,17 @@
// I2C Handle
extern I2C_HandleTypeDef hi2c1;
extern osThreadId_t defaultTaskHandle;
extern osThreadId_t xIMU_Task;
extern osThreadId_t xMagn_Task;
extern volatile osThreadId_t xIMU_Task;
extern volatile osThreadId_t xMagn_Task;
//extern osThreadId_t xData_Task;
typedef enum {
ICM_NOTIFY_INT_DRDY = 0,
ICM_NOTIFY_DMA = 1,
BMM_NOTIFY_INT_DRDY = 0,
BMM_NOTIFY_DMA = 1
} SENSOR_NOTIFY_INDEX;
// ICM45686
typedef struct {
SPI_HandleTypeDef *hspi;
@@ -66,7 +73,8 @@ typedef enum {
ICM45686_FIFO_COUNT = 0x12,
ICM45686_FIFO_DATA = 0x14,
ICM45686_INT1_CONFIG2 = 0x18,
ICM45686_INT1_CONFIG0 = 0x16
ICM45686_INT1_CONFIG0 = 0x16,
ICM45686_INT1_STATUS0 = 0x19
} ICM45686_registers;
typedef enum {
@@ -149,7 +157,7 @@ HAL_StatusTypeDef read_icm_dma (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8
HAL_StatusTypeDef write_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint16_t len_tx);
void calibrate_icm (ICM45686_HandleTypeDef * imu, int samples);
void enable_interrupt_icm(ICM45686_HandleTypeDef *imu);
void configure_interrupt_icm (ICM45686_HandleTypeDef *imu);
void configure_and_enable_int_icm (ICM45686_HandleTypeDef *imu);
// BMM350 - Declarations of read/write/delay functions for the BMM350 API
void bmm_init_DWT(void);
Core/Src/stm32wbxx_hal_msp.c
+5 -1
View File
@@ -125,10 +125,14 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c)
GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9;
GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF4_I2C1;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
__HAL_SYSCFG_FASTMODEPLUS_ENABLE(SYSCFG_FASTMODEPLUS_PB8);
__HAL_SYSCFG_FASTMODEPLUS_ENABLE(SYSCFG_FASTMODEPLUS_PB9);
/* Peripheral clock enable */
__HAL_RCC_I2C1_CLK_ENABLE();