Fixed DMA and INT bugs
- 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
1 parent
5daac77a37
commit
1409bad40b
67 files changed
+52921
-51367
No files matched your search
+61
-59
@@ -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 */
|
||||
}
|
||||
|
||||
Reference in new issue
Block a user