diff --git a/epicardium/ble/uart.c b/epicardium/ble/uart.c
index 2a178fd82c072a45ef224950db000718b88f64df..55bf3ee8d6ebeb4df026303d5a51a6a40728155f 100644
--- a/epicardium/ble/uart.c
+++ b/epicardium/ble/uart.c
@@ -125,11 +125,10 @@ static uint8_t UARTWriteCback(
 	return ATT_SUCCESS;
 }
 
-static int ble_uart_lasttick = 0;
 static bool done;
 static bool again;
 
-void ble_uart_flush(void)
+static void ble_uart_flush(void)
 {
 	if (ble_uart_buf_tx_fill == 0) {
 		return;
@@ -138,12 +137,11 @@ void ble_uart_flush(void)
 	dmConnId_t connId = AppConnIsOpen();
 	if (connId != DM_CONN_ID_NONE) {
 		if (AttsCccEnabled(connId, UART_TX_CH_CCC_IDX)) {
-			done  = false;
-			again = true;
+			done   = false;
+			again  = true;
+			int t0 = xTaskGetTickCount();
 
-			// TODO: Modify for no initial delay
-			while (!done && ((xTaskGetTickCount() -
-					  ble_uart_lasttick) < 1000)) {
+			while (!done && ((xTaskGetTickCount() - t0) < 1000)) {
 				if (again) {
 					again = false;
 					AttsHandleValueNtf(
@@ -153,15 +151,22 @@ void ble_uart_flush(void)
 						ble_uart_tx_buf
 					);
 				}
-				vTaskDelay(5);
+				/* This function is supposed to only be called
+				 * from the API scheduler with lowest priority.
+				 *
+				 * If that is not the case anymore, use a delay
+				 * instead of the yield. Ideally refactor to avoid
+				 * the delay.
+				 */
+				//vTaskDelay(5);
+				taskYIELD();
 			}
-			ble_uart_lasttick = xTaskGetTickCount();
 		}
 	}
 	ble_uart_buf_tx_fill = 0;
 }
 
-void ble_uart_write_char(uint8_t c)
+static void ble_uart_write_char(uint8_t c)
 {
 	ble_uart_tx_buf[ble_uart_buf_tx_fill] = c;
 	ble_uart_buf_tx_fill++;