Hi,I am using Keil to edit the "DataLog" example application provided in STSW-STLKT01_V2.5.0. I want to use the MotionDI library to get orientation and linear acceleration of my device (STEVAL-STLKT01V1) in real time. For that purpose, I want to insert the MotionDI_update(&data_out, &data_in) function in the DataLog application code. The DataLog application code uses 2 threads to get and print the data from accelerometer and gyroscope. I add the MotionDI_update(&data_out, &data_in) in the WriteData_Thread. However, my code hangs after I get the orientation and acceleration for the first time. How to get rid of this issue?
I tried increasing stack size but no success. I want to continuously get data in real time. My code is attached as well.
int main(void) { HAL_Init(); /* Configure the System clock to 80 MHz */ SystemClock_Config(); MX_GPIO_Init(); MX_DMA_Init(); MX_CRC_Init(); MX_RTC_Init(); // initialize MotionDI library MotionDI_Initialize(&freq); /* Optional: Get version */ //MotionDI_GetLibVersion(lib_version); /* Optional: Modify knobs settings & set the knobs */ MotionDI_getKnobs(&iKnobs); iKnobs.AccKnob.CalType = MDI_CAL_ONETIME; iKnobs.GyrKnob.CalType = MDI_CAL_ONETIME; BSP_SENSOR_ACC_GetOrientation(iKnobs.AccOrientation); BSP_SENSOR_GYR_GetOrientation(iKnobs.GyroOrientation); iKnobs.SFKnob.output_type = MDI_ENGINE_OUTPUT_NED; iKnobs.SFKnob.modx = DECIMATION; MotionDI_setKnobs(&iKnobs); AccCalMode = iKnobs.AccKnob.CalType; GyrCalMode = iKnobs.GyrKnob.CalType; if(LoggingInterface == USB_Datalog) { /* Initialize LED */ BSP_LED_Init(LED1); BSP_LED_Off(LED1); } /* enable USB power on Pwrctrl CR2 register */ HAL_PWREx_EnableVddUSB(); HAL_PWREx_EnableVddIO2(); if(LoggingInterface == USB_Datalog) /* Configure the USB */ { /*** USB CDC Configuration ***/ /* Init Device Library */ USBD_Init(&USBD_Device, &VCP_Desc, 0); /* Add Supported Class */ USBD_RegisterClass(&USBD_Device, USBD_CDC_CLASS); /* Add Interface callbacks for AUDIO and CDC Class */ USBD_CDC_RegisterInterface(&USBD_Device, &USBD_CDC_fops); /* Start Device Process */ USBD_Start(&USBD_Device); } else /* Configure the SDCard */ { DATALOG_SD_Init(); } /* Thread 1 definition */ osThreadDef(THREAD_1, GetData_Thread, osPriorityAboveNormal, 0, configMINIMAL_STACK_SIZE*4); /* Thread 2 definition */ osThreadDef(THREAD_2, WriteData_Thread, osPriorityAboveNormal, 0, configMINIMAL_STACK_SIZE*4); /* Start thread 1 */ GetDataThreadId = osThreadCreate(osThread(THREAD_1), NULL); /* Start thread 2 */ WriteDataThreadId = osThreadCreate(osThread(THREAD_2), NULL); /* Start scheduler */ osKernelStart(); /* We should never get here as control is now taken by the scheduler */ for (;;); }
static void GetData_Thread(void const *argument) { (void) argument; T_SensorsData *mptr; sensorPool_id = osPoolCreate(osPool(sensorPool)); dataQueue_id = osMessageCreate(osMessageQ(dataqueue), NULL); readDataSem_id = osSemaphoreCreate(osSemaphore(readDataSem), 1); osSemaphoreWait(readDataSem_id, osWaitForever); /* Initialize and Enable the available sensors */ Sensor_IO_SPI_CS_Init_All(); MX_X_CUBE_MEMS1_Init(); /* COnfigure LSM6DSM Double Tap interrupt*/ LSM6DSM_Sensor_IO_ITConfig(); if(LoggingInterface == USB_Datalog) { dataTimerStart(); } for (;;) { osSemaphoreWait(readDataSem_id, osWaitForever); if(MEMSInterrupt && LoggingInterface == SDCARD_Datalog) { MEMSInterrupt = 0; if(DoubleTap()) { if(SD_Log_Enabled) { dataTimerStop(); osMessagePut(dataQueue_id, DATALOG_CMD_STARTSTOP, osWaitForever); } else { osMessagePut(dataQueue_id, DATALOG_CMD_STARTSTOP, osWaitForever); } } } else { /* Try to allocate a memory block and check if is not NULL */ mptr = osPoolAlloc(sensorPool_id); if(mptr != NULL) { if(getSensorsData(mptr) == BSP_ERROR_NONE) { /* Push the new memory Block in the Data Queue */ if(osMessagePut(dataQueue_id, (uint32_t)mptr, osWaitForever) != osOK) { Error_Handler(); } } else { Error_Handler(); } } else { Error_Handler(); } } } }
static void WriteData_Thread(void const *argument) { (void) argument; osEvent evt; T_SensorsData *rptr; int size; char data_s[256]; for (;;) { char entry[6]; size=sprintf(entry, "for\n"); osPoolFree(sensorPool_id, rptr); // free memory allocated for message CDC_Fill_Buffer(( uint8_t * )entry, size); evt = osMessageGet(dataQueue_id, osWaitForever); // wait for message if (evt.status == osEventMessage) { char entry[6]; size=sprintf(entry, "if\n"); osPoolFree(sensorPool_id, rptr); // free memory allocated for message CDC_Fill_Buffer(( uint8_t * )entry, size); if(evt.value.v == DATALOG_CMD_STARTSTOP) { if (SD_Log_Enabled) { DATALOG_SD_Log_Disable(); SD_Log_Enabled=0; } else { while(SD_Log_Enabled != 1) { if(DATALOG_SD_Log_Enable()) { SD_Log_Enabled=1; osDelay(100); dataTimerStart(); } else { //DATALOG_SD_Log_Disable(); DATALOG_SD_DeInit(); DATALOG_SD_Init(); osDelay(100); } } } } else { char entry[6]; size=sprintf(entry, "Entry\n"); osPoolFree(sensorPool_id, rptr); // free memory allocated for message CDC_Fill_Buffer(( uint8_t * )entry, size); rptr = evt.value.p; if(LoggingInterface == USB_Datalog) { /* Dynamic Inclinometer API initialization function */ /*** Using Dynamic Inclinometer algorithm ***/ //Timer_OR_DataRate_Interrupt_Handler() //{ MDI_input_t data_in; MDI_output_t data_out; /* Get acceleration X/Y/Z in g */ data_in.Acc[0] = rptr->acc.x; data_in.Acc[1] = rptr->acc.y; data_in.Acc[2] = rptr->acc.z; /* Get angular rate X/Y/Z in dps */ data_in.Gyro[0] = rptr->gyro.x; data_in.Gyro[1] = rptr->gyro.y; data_in.Gyro[2] = rptr->gyro.z; data_in.Timestamp = rptr->ms_counter; //data_in.Timestamp = HAL_GetTick() * 1000; /* Run Dynamic Inclinometer algorithm */ // char msgg[25]; // size=sprintf(msgg, lib_version); // osPoolFree(sensorPool_id, rptr); // free memory allocated for message // CDC_Fill_Buffer(( uint8_t * )msgg, size); char hi[4]; size=sprintf(hi, "Hi\n"); osPoolFree(sensorPool_id, rptr); // free memory allocated for message CDC_Fill_Buffer(( uint8_t * )hi, size); // size = sprintf(data_s, "TimeStamp: %ld\r\n Acc_X: %d, Acc_Y: %d, Acc_Z :%d\r\n Gyro_X:%d, Gyro_Y:%d, Gyro_Z:%d\r\n", // rptr->ms_counter, // (int)data_in.Acc[0], (int)data_in.Acc[1], (int)data_in.Acc[2], // (int)data_in.Gyro[0], (int)data_in.Gyro[1], (int)data_in.Gyro[2]); // osPoolFree(sensorPool_id, rptr); // free memory allocated for message // CDC_Fill_Buffer(( uint8_t * )data_s, size); // // The update function MotionDI_update(&data_out, &data_in); char bye[4]; size=sprintf(bye, "Bye\n"); osPoolFree(sensorPool_id, rptr); // free memory allocated for message CDC_Fill_Buffer(( uint8_t * )bye, size); /* Use data_out – output data from Dynamic Inclinometer algorithm */ //} size = sprintf(data_s, "TimeStamp: %ld\r\n x-Rotation: %d, y-Rotation: %d, z-Rotation: %d\r\n x_accel: %d, y_accel: %d, z_accel: %d\r\n", data_in.Timestamp, (int)data_out.rotation[0], (int)data_out.rotation[1], (int)data_out.rotation[2], (int)data_out.linear_acceleration[0], (int)data_out.linear_acceleration[1], (int)data_out.linear_acceleration[2]); osPoolFree(sensorPool_id, rptr); // free memory allocated for message BSP_LED_Toggle(LED1); CDC_Fill_Buffer(( uint8_t * )data_s, size); } else { size = sprintf(data_s, "%ld, %d, %d, %d, %d, %d, %d, %d, %d, %d, %5.2f, %5.2f, %4.1f\r\n", rptr->ms_counter, (int)rptr->acc.x, (int)rptr->acc.y, (int)rptr->acc.z, (int)rptr->gyro.x, (int)rptr->gyro.y, (int)rptr->gyro.z, (int)rptr->mag.x, (int)rptr->mag.y, (int)rptr->mag.z, rptr->pressure, rptr->temperature, rptr->humidity); osPoolFree(sensorPool_id, rptr); // free memory allocated for message DATALOG_SD_writeBuf(data_s, size); } } } } }