diff --git a/Software/VNA_embedded/Application/Drivers/FPGA/FPGA.cpp b/Software/VNA_embedded/Application/Drivers/FPGA/FPGA.cpp
index 2181eaa..5083b63 100644
--- a/Software/VNA_embedded/Application/Drivers/FPGA/FPGA.cpp
+++ b/Software/VNA_embedded/Application/Drivers/FPGA/FPGA.cpp
@@ -402,12 +402,23 @@ void FPGA::ResetADCLimits() {
 	High(CS);
 }
 
-void FPGA::ResumeHaltedSweep() {
+bool FPGA::ResumeHaltedSweep() {
+	uint32_t start = HAL_GetTick();
 	uint16_t cmd = 0x2000;
+	uint16_t status;
 	SwitchBytes(cmd);
-	Low(CS);
-	HAL_SPI_Transmit(&FPGA_SPI, (uint8_t*) &cmd, 2, 100);
-	High(CS);
+	do {
+		if(HAL_GetTick() - start > 100) {
+			LOG_WARN("Failed to resume sweep, timed out");
+			return false;
+		}
+		Low(CS);
+		HAL_SPI_TransmitReceive(&FPGA_SPI, (uint8_t*) &cmd, (uint8_t*) &status, 2, 100);
+		High(CS);
+		SwitchBytes(status);
+	} while(status & 0x0010);
+//	LOG_DEBUG("Status: 0x%04x", GetStatus());
+	return true;
 }
 
 void FPGA::SetupDFT(uint32_t f_firstBin, uint32_t f_binSpacing) {
diff --git a/Software/VNA_embedded/Application/Drivers/FPGA/FPGA.hpp b/Software/VNA_embedded/Application/Drivers/FPGA/FPGA.hpp
index 149870b..88da655 100644
--- a/Software/VNA_embedded/Application/Drivers/FPGA/FPGA.hpp
+++ b/Software/VNA_embedded/Application/Drivers/FPGA/FPGA.hpp
@@ -132,7 +132,7 @@ void StartDFT();
 DFTResult ReadDFTResult();
 ADCLimits GetADCLimits();
 void ResetADCLimits();
-void ResumeHaltedSweep();
+bool ResumeHaltedSweep();
 uint16_t GetStatus();
 
 void OverwriteHardware(uint8_t attenuation, LowpassFilter filter, bool lowband, bool port1_enabled, bool port2_enabled);
diff --git a/Software/VNA_embedded/Application/Drivers/Log.cpp b/Software/VNA_embedded/Application/Drivers/Log.cpp
index 858a070..308c67d 100644
--- a/Software/VNA_embedded/Application/Drivers/Log.cpp
+++ b/Software/VNA_embedded/Application/Drivers/Log.cpp
@@ -79,7 +79,7 @@ void Log_Init() {
 #endif
 
 	/* USART interrupt Init */
-	HAL_NVIC_SetPriority(NVIC_ISR, 0, 0);
+	HAL_NVIC_SetPriority(NVIC_ISR, 3, 0);
 	HAL_NVIC_EnableIRQ(NVIC_ISR);
 }
 
diff --git a/Software/VNA_embedded/Application/Drivers/delay.cpp b/Software/VNA_embedded/Application/Drivers/delay.cpp
index 3d9915c..46258f3 100644
--- a/Software/VNA_embedded/Application/Drivers/delay.cpp
+++ b/Software/VNA_embedded/Application/Drivers/delay.cpp
@@ -9,10 +9,10 @@ void Delay::Init() {
 
 	// enable update interrupt
 	TIM1->DIER |= TIM_DIER_UIE;
-    HAL_NVIC_SetPriority(TIM1_UP_TIM16_IRQn, 6, 0);
+    HAL_NVIC_SetPriority(TIM1_UP_TIM16_IRQn, 0, 0);
     HAL_NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn);
 
-	TIM1->CR1 |= TIM_CR1_CEN;
+	TIM1->CR1 |= TIM_CR1_CEN | TIM_CR1_UIFREMAP;
 }
 
 uint64_t Delay::get_us() {
@@ -22,7 +22,7 @@ uint64_t Delay::get_us() {
 	uint64_t ret;
 	if(buf & 0x80000000) {
 		// UIF bit set, timer overflow not handled yet
-		ret = t_us + UINT16_MAX + timer_value;
+		ret = t_us + UINT16_MAX + 1 + timer_value;
 	} else {
 		ret = t_us + timer_value;
 	}
@@ -46,7 +46,7 @@ void TIM1_UP_TIM16_IRQHandler() {
 	// clear bit
 	TIM1->SR &= ~TIM_SR_UIF;
 	// update count
-	t_us += UINT16_MAX;
+	t_us += UINT16_MAX + 1;
 }
 
 }
diff --git a/Software/VNA_embedded/Application/Hardware.cpp b/Software/VNA_embedded/Application/Hardware.cpp
index 200d5ef..3b00427 100644
--- a/Software/VNA_embedded/Application/Hardware.cpp
+++ b/Software/VNA_embedded/Application/Hardware.cpp
@@ -21,7 +21,7 @@ HW::Mode activeMode;
 static bool unlevel = false;
 
 static Protocol::ReferenceSettings ref;
-static uint64_t lastISR;
+static volatile uint64_t lastISR;
 
 static uint32_t IF1 = HW::DefaultIF1;
 static uint32_t IF2 = HW::DefaultIF2;
@@ -293,7 +293,16 @@ HW::AmplitudeSettings HW::GetAmplitudeSettings(int16_t cdbm, uint64_t freq, bool
 
 bool HW::TimedOut() {
 	constexpr uint64_t timeout = 1000000;
-	if(activeMode != Mode::Idle && activeMode != Mode::Generator && Delay::get_us() - lastISR > timeout) {
+	auto bufISR = lastISR;
+	uint64_t now = Delay::get_us();
+	uint64_t timeSinceLast = now - bufISR;
+	if(activeMode != Mode::Idle && activeMode != Mode::Generator && timeSinceLast > timeout) {
+		LOG_WARN("Timed out, last ISR was at %lu%06lu, now %lu%06lu"
+				, (uint32_t) (bufISR / 1000000), (uint32_t)(bufISR%1000000)
+				, (uint32_t) (now / 1000000), (uint32_t)(now%1000000));
+		if(activeMode == Mode::VNA) {
+			VNA::PrintStatus();
+		}
 		return true;
 	} else {
 		return false;
diff --git a/Software/VNA_embedded/Application/VNA.cpp b/Software/VNA_embedded/Application/VNA.cpp
index 08e0a97..e8059a9 100644
--- a/Software/VNA_embedded/Application/VNA.cpp
+++ b/Software/VNA_embedded/Application/VNA.cpp
@@ -498,3 +498,16 @@ void VNA::Stop() {
 	active = false;
 	FPGA::AbortSweep();
 }
+
+void VNA::PrintStatus() {
+	HAL_Delay(10);
+	LOG_INFO("VNA status:");
+	HAL_Delay(10);
+	LOG_INFO("Active: %d", active);
+	HAL_Delay(10);
+	LOG_INFO("Points: %d/%d", pointCnt, settings.points);
+	HAL_Delay(10);
+	LOG_INFO("Stages: %d/%d", stageCnt, stages);
+	HAL_Delay(10);
+	LOG_INFO("FPGA status: 0x%04x", FPGA::GetStatus());
+}
diff --git a/Software/VNA_embedded/Application/VNA.hpp b/Software/VNA_embedded/Application/VNA.hpp
index 3fae0f6..2960c95 100644
--- a/Software/VNA_embedded/Application/VNA.hpp
+++ b/Software/VNA_embedded/Application/VNA.hpp
@@ -12,5 +12,7 @@ void Work();
 void SweepHalted();
 void Stop();
 
+void PrintStatus();
+
 }
 
diff --git a/Software/VNA_embedded/Inc/FreeRTOSConfig.h b/Software/VNA_embedded/Inc/FreeRTOSConfig.h
index 196dc02..d372258 100644
--- a/Software/VNA_embedded/Inc/FreeRTOSConfig.h
+++ b/Software/VNA_embedded/Inc/FreeRTOSConfig.h
@@ -79,6 +79,9 @@
 #define configUSE_CO_ROUTINES                    0
 #define configMAX_CO_ROUTINE_PRIORITIES          ( 2 )
 
+/* The following flag must be enabled only when using newlib */
+#define configUSE_NEWLIB_REENTRANT          1
+
 /* Set the following definitions to 1 to include the API function, or zero
 to exclude the API function. */
 #define INCLUDE_vTaskPrioritySet             0
diff --git a/Software/VNA_embedded/VNA_embedded.ioc b/Software/VNA_embedded/VNA_embedded.ioc
index 8aa12ab..8897fba 100644
--- a/Software/VNA_embedded/VNA_embedded.ioc
+++ b/Software/VNA_embedded/VNA_embedded.ioc
@@ -86,12 +86,13 @@ FREERTOS.INCLUDE_vTaskDelete=0
 FREERTOS.INCLUDE_vTaskPrioritySet=0
 FREERTOS.INCLUDE_vTaskSuspend=1
 FREERTOS.INCLUDE_xTaskResumeFromISR=0
-FREERTOS.IPParameters=Tasks01,INCLUDE_vTaskDelete,INCLUDE_vTaskPrioritySet,INCLUDE_uxTaskPriorityGet,INCLUDE_xTaskResumeFromISR,INCLUDE_vTaskSuspend,MEMORY_ALLOCATION,configTOTAL_HEAP_SIZE,configENABLE_BACKWARD_COMPATIBILITY,configUSE_MUTEXES,FootprintOK
+FREERTOS.IPParameters=Tasks01,INCLUDE_vTaskDelete,INCLUDE_vTaskPrioritySet,INCLUDE_uxTaskPriorityGet,INCLUDE_xTaskResumeFromISR,INCLUDE_vTaskSuspend,MEMORY_ALLOCATION,configTOTAL_HEAP_SIZE,configENABLE_BACKWARD_COMPATIBILITY,configUSE_MUTEXES,FootprintOK,configUSE_NEWLIB_REENTRANT
 FREERTOS.MEMORY_ALLOCATION=1
 FREERTOS.Tasks01=defaultTask,0,1024,StartDefaultTask,Default,NULL,Static,defaultTaskBuffer,defaultTaskControlBlock
 FREERTOS.configENABLE_BACKWARD_COMPATIBILITY=1
 FREERTOS.configTOTAL_HEAP_SIZE=2048
 FREERTOS.configUSE_MUTEXES=1
+FREERTOS.configUSE_NEWLIB_REENTRANT=1
 File.Version=6
 I2C2.I2C_Speed_Mode=I2C_Fast
 I2C2.IPParameters=Timing,I2C_Speed_Mode
@@ -173,7 +174,7 @@ NVIC.SavedPendsvIrqHandlerGenerated=true
 NVIC.SavedSvcallIrqHandlerGenerated=true
 NVIC.SavedSystickIrqHandlerGenerated=true
 NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true
-NVIC.TIM1_TRG_COM_TIM17_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true
+NVIC.TIM1_TRG_COM_TIM17_IRQn=true\:0\:0\:true\:false\:true\:false\:false\:true
 NVIC.TimeBase=TIM1_TRG_COM_TIM17_IRQn
 NVIC.TimeBaseIP=TIM17
 NVIC.UCPD1_IRQn=true\:7\:0\:true\:false\:true\:true\:true\:false