Question
I have edited the real-time 'DataLog' application provided in STSW-STLKT01_V2.5.0 to include the MotionDI_update function. However, my code hangs after I get the tilt and acceleration values for the first time. How to solve this?
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();
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 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);
//
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);
}
}
}
}
}