This discussion has been locked.

You can no longer post new replies to this discussion. If you have a question you can start a new discussion

RTOS problem in Keil

QAbbas over 4 years ago

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.

Fullscreen

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
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 */
            
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

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 (;;);

}

Fullscreen

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
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();
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

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();
      }
    }
  }
}

Fullscreen

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
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];
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

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);
        }
      }
    }
  }
}