Receiving a queue from an interrupt and synchronization issues(STM32)

Danman · September 4, 2023, 5:09am

Hello,

I am having issues trying to receive data from a queue that is being sent from an interrupt. I am sending data through a UART_DMA(in a ring buffer) to a queue that should then be received by a task that sends commands to motors(“motor_drive”).

I am parsing the data in the UART callback(“HAL_UART_RxCpltCallback”). I have set my interrupt priority above configMAX_SYSCALL_INTERRUPT_PRIORITY(to 6), but I’m still not able to receive data from the queue.

When the code is running, everything inside of the callback runs fine, but every other task, “led_blink” and “motor_drive” tasks, both run only once. I think this is a priority or queue receiving issue, but I just can’t figure out why my tasks only run once when they are higher priority and using a queue.

Any input on how I could fix this or what could be the issue?

Thanks in advance for any help.

/*****************************
		RC CONTROL
 ****************************/
float max_RC_Value = 1811.0, min_RC_Value = 172.0, throttle_perct = 0.0, tot_TIM = 65535;
int32_t CH1_DC = 0, CH2_DC = 0, CH3_DC = 0, CH4_DC = 0;

/*****************************
SBUS PROTOCOL DECIPHER
 ****************************/
volatile uint16_t failsafe_status; //Failsafe status transmitted if signal lost
volatile uint8_t buffer_SBUS[25]; //SBUS send a 25 byte packet
volatile uint16_t channels_SBUS[18]; //SBUS has 16 channels; Channels are 11 bits each
volatile uint16_t channel_SBUS[18];
static volatile int counter = 0;


void led_blink(void *parameters);
void motor_drive(void *parameters);

QueueHandle_t xChannels;

int main(void)
{

	/*****************************
	  			HANDLES
	 ****************************/
	TaskHandle_t led_handler;
	TaskHandle_t sonar_handler;
	TaskHandle_t encoder_handler;
	TaskHandle_t motor_handler;

	/*****************************
	  			QUEUE
	 ****************************/
	xChannels = xQueueCreate(18, sizeof(channels_SBUS));

/*****************************
	  			DMA
	 ****************************/
  HAL_UART_Receive_DMA(&huart1, buffer_SBUS, 25);

	/*****************************
	 * 			TASKS
	 ****************************/
  	xTaskCreate(led_blink, "Led Blink", configMINIMAL_STACK_SIZE, NULL, 1, &led_handler);
	xTaskCreate(motor_drive, "Motor Driver", 256*4, NULL, 3, &motor_handler);

  vTaskStartScheduler();


  while (1)
  {
  }
}

static void MX_DMA_Init(void)
{

  /* DMA controller clock enable */
  __HAL_RCC_DMA2_CLK_ENABLE();

  /* DMA interrupt init */
  /* DMA2_Stream5_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 6, 0);
  HAL_NVIC_EnableIRQ(DMA2_Stream5_IRQn);

}

  /* EXTI interrupt init*/
  HAL_NVIC_SetPriority(EXTI4_IRQn, 6, 0);
  HAL_NVIC_EnableIRQ(EXTI4_IRQn);
}

/* USER CODE BEGIN 4 */
void led_blink(void *parameters)
{

	while(1)
	{
		HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_5);
		//vTaskDelay(1000); // Delay for 100 milliseconds
	}
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	BaseType_t xHigherPriorityTaskWoken = pdFALSE;
	if (buffer_SBUS[0] == 0x0F)
	{
		channels_SBUS[0] = (buffer_SBUS[1] >> 0 | (buffer_SBUS[2] << 8)) & 0x07FF;
		channels_SBUS[1] = (buffer_SBUS[2] >> 3 | (buffer_SBUS[3] << 5)) & 0x07FF;
		channels_SBUS[2] = (buffer_SBUS[3] >> 6 | (buffer_SBUS[4] << 2) | buffer_SBUS[5] << 10) & 0x07FF;
		channels_SBUS[3] = (buffer_SBUS[5] >> 1 | (buffer_SBUS[6] << 7)) & 0x07FF;
		channels_SBUS[4] = (buffer_SBUS[6] >> 4 | (buffer_SBUS[7] << 4)) & 0x07FF;
		channels_SBUS[5] = (buffer_SBUS[7] >> 7 | (buffer_SBUS[8] << 1) | buffer_SBUS[9] << 9) & 0x07FF;
		channels_SBUS[6] = (buffer_SBUS[9] >> 2 | (buffer_SBUS[10] << 6)) & 0x07FF;
		channels_SBUS[7] = (buffer_SBUS[10] >> 5 | (buffer_SBUS[11] << 3)) & 0x07FF;
		channels_SBUS[8] = (buffer_SBUS[12] << 0 | (buffer_SBUS[13] << 8)) & 0x07FF;
		channels_SBUS[9] = (buffer_SBUS[13] >> 3 | (buffer_SBUS[14] << 5)) & 0x07FF;
		channels_SBUS[10] = (buffer_SBUS[14] >> 6 | (buffer_SBUS[15] << 2) | buffer_SBUS[16] << 10) & 0x07FF;
		channels_SBUS[11] = (buffer_SBUS[16] >> 1 | (buffer_SBUS[17] << 7)) & 0x07FF;
		channels_SBUS[12] = (buffer_SBUS[17] >> 4 | (buffer_SBUS[18] << 4)) & 0x07FF;
		channels_SBUS[13] = (buffer_SBUS[18] >> 7 | (buffer_SBUS[19] << 1) | buffer_SBUS[20] << 9) & 0x07FF;
		channels_SBUS[14] = (buffer_SBUS[20] >> 2 | (buffer_SBUS[21] << 6)) & 0x07FF;
		channels_SBUS[15] = (buffer_SBUS[21] >> 5 | (buffer_SBUS[22] << 3)) & 0x07FF;

		if (buffer_SBUS[23] & (1 << 0))
		{
			channels_SBUS[16] = 1; //If byte 23(digital channel) is 1, then 17th channel is 1
		}else{
			channels_SBUS[16] = 0;
		}

		if (buffer_SBUS[23] & (1 << 1)) //Digital Channel 18.
		{
			channels_SBUS[17] = 1;
		}else {
			channels_SBUS[17] = 0;
		}

		//Failsafe for lost signal
		if (buffer_SBUS[23] & (1 << 2)) {
			failsafe_status = 0;
		}


	}
	xQueueSendFromISR(xChannels, &channels_SBUS, &xHigherPriorityTaskWoken);

	if (xHigherPriorityTaskWoken == pdTRUE)
	{
	   portYIELD();
	}
}


void motor_drive(void *parameters)
{

	while(1)
	{
			counter++;
			if(xQueueReceive(xChannels, &channel_SBUS, portMAX_DELAY))
			{
			counter++;
			/*****************************
			 * THROTTLE CONTROL
			 ****************************/
			if(channel_SBUS[0]){
				throttle_perct = 1 - ((max_RC_Value - ((float)channel_SBUS[0] - 172)) / max_RC_Value);

				if(channel_SBUS[1] >= 890 && channel_SBUS[1] <= 1090){ // 990 is middle condition, 172 low, 1808 high

					/*****************************
					* FORWARD
					****************************/
					if(channel_SBUS[2] >= 1090 && channel_SBUS[2] <= 1811){
						// Front Left Forward
						HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, 0);
						HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, 1);

						// Back Left Forward
						HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, 0);
						HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, 1);

						// Front Right Forwards
						HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, 1);
						HAL_GPIO_WritePin(GPIOC, GPIO_PIN_0, 0);

						// Back Right Forward
						HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, 0);
						HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, 1);

					TIM2->CCR1 = CH1_DC;
					TIM2->CCR2 = CH2_DC;
					TIM2->CCR3 = CH3_DC;
					TIM2->CCR4 = CH4_DC;
					CH1_DC = CH2_DC = CH3_DC = CH4_DC = throttle_perct * tot_TIM;

					/*****************************
					* BACKWARDS
					****************************/
					} else if(channel_SBUS[2] >= 172 && channel_SBUS[2] <= 890){
						// Front Left Backwards
						HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, 1);
						HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, 0);

						// Back Left Backwards
						HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, 1);
						HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, 0);

						// Front Right Backwards
						HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, 0);
						HAL_GPIO_WritePin(GPIOC, GPIO_PIN_0, 1);

						// Back Right Backwards
						HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, 1);
						HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, 0);

						TIM2->CCR1 = CH1_DC;
						TIM2->CCR2 = CH2_DC;
						TIM2->CCR3 = CH3_DC;
						TIM2->CCR4 = CH4_DC;
						CH1_DC = CH2_DC = CH3_DC = CH4_DC = throttle_perct * tot_TIM;
					} else{
						TIM2->CCR1 = CH1_DC;
						TIM2->CCR2 = CH2_DC;
						TIM2->CCR3 = CH3_DC;
						TIM2->CCR4 = CH4_DC;
						CH1_DC = CH2_DC = CH3_DC = CH4_DC = 0;
					}
				/*****************************
				* TURN LEFT
				****************************/
				} else if(channel_SBUS[1] < 890){
					// Front Left Backwards
					HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, 1);
					HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, 0);

					// Back Left Backwards
					HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, 1);
					HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, 0);

					// Front Right Forwards
					HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, 1);
					HAL_GPIO_WritePin(GPIOC, GPIO_PIN_0, 0);

					// Back Right Forward
					HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, 0);
					HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, 1);

					TIM2->CCR1 = CH1_DC;
					TIM2->CCR2 = CH2_DC;
					TIM2->CCR3 = CH3_DC;
					TIM2->CCR4 = CH4_DC;
					CH1_DC = CH2_DC = CH3_DC = CH4_DC = throttle_perct * tot_TIM;

				/*****************************
				* TURN RIGHT
				****************************/
				} else if(channel_SBUS[1] > 1090){
					// Front Left Forward
					HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, 0);
					HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, 1);

					// Back Left Forward
					HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, 0);
					HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, 1);

					// Front Right Backwards
					HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, 0);
					HAL_GPIO_WritePin(GPIOC, GPIO_PIN_0, 1);

					// Back Right Backwards
					HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, 1);
					HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, 0);

					TIM2->CCR1 = CH1_DC;
					TIM2->CCR2 = CH2_DC;
					TIM2->CCR3 = CH3_DC;
					TIM2->CCR4 = CH4_DC;
					CH1_DC = CH2_DC = CH3_DC = CH4_DC = throttle_perct * tot_TIM;
					}
			}
		}
		vTaskDelay(pdMS_TO_TICKS(1000));
	}
}

aggarg · September 4, 2023, 6:08am

How did you confirm this? Can you put a breakpoint in your ISR an verify that the control reaches xQueueSendFromISR?

Make the following change in the calls to xQueueSendFromISR and xQueueReceive:

xQueueSendFromISR(xChannels, &( channels_SBUS[ 0 ] ), &xHigherPriorityTaskWoken);

xQueueReceive(xChannels, &( channel_SBUS[ 0 ] ), portMAX_DELAY)

Danman · September 4, 2023, 7:34am

I confirmed that everything in the callback runs fine by following variables with a live editor option in STM32IDE. To verify it was reaching the xQueueSendFromISR I inserted a counter variable that would increment every time it passed the command. The variable would keep on incrementing as the program ran. I also tried with a breakpoint and it does reach the command.

I implemented the changes to the code and no luck. It runs the same as before.

Any other suggestions?

Thanks

aggarg · September 4, 2023, 7:40am

Can you check the return value of xQueueSendFromISR to see if it was successful?

RAc · September 4, 2023, 9:19am

Check your led blink task. It will trash the cpu.

Danman · September 4, 2023, 5:33pm

The return value of xQueueSendFromISR returns pdTRUE and then returns errQUEUE_FULL after it reaches it’s capacity of 18.

Danman · September 4, 2023, 5:33pm

Yea I have tried running it without the LED blink task running at all but no luck still.

RAc · September 4, 2023, 8:07pm

Is your system running at all, or are you stuck in a fault? The easiest way to determine that is to pull the variable that gets incremented during the system tick isr (I believe it is called xTickCt or something the like) into a real time monitoring watch window. If that keeps incrementing when you are in the funny state, check on the usual suspects (eg track the return values of all system calls, make sure you post to and read from the same queue etc); if not, check whether you are in the infinite loop of a fault handler. I believe this troubleshooting algorithm is fairly well documented.