diff --git a/sdcl-firmware/Core/Src/main.c b/sdcl-firmware/Core/Src/main.c
index 6f461d2..cf04a6b 100644
--- a/sdcl-firmware/Core/Src/main.c
+++ b/sdcl-firmware/Core/Src/main.c
@@ -141,6 +141,8 @@ const mission_t	mission2next[] 	= {M_MANUAL	, M_SKIDPAD		, M_AUTOX			, M_EBSTEST
 
 mission_t mission = M_NONE;
 
+bool setup_done = false;
+
 #ifdef WATCHDOG_STM
 bool pHeartbeat = false;
 bool WD_OK = false;
@@ -268,11 +270,17 @@ int main(void)
 	  }
   }
 
-  MX_IWDG_Init();
-  WD_initialized = true;
+  setup_done = true;
+
+  // TEMP: Only enable WD if in autonomous mode because EMI currently messes it up during R2D
+  if (HAL_GPIO_ReadPin(ASMS_GPIO_Port, ASMS_Pin) == GPIO_PIN_RESET) {
+	  MX_IWDG_Init();
+	  WD_initialized = true;
+  }
 
   while (true) {
 
+	// Compare with RESET for signals obtained via inverting buffer
 	bool TS_activate_MUXed	= HAL_GPIO_ReadPin(TS_activate_MUXed_GPIO_Port, TS_activate_MUXed_Pin)	== GPIO_PIN_RESET;
 	bool ASMS				= HAL_GPIO_ReadPin(ASMS_GPIO_Port, ASMS_Pin)							== GPIO_PIN_RESET;
 #ifdef WATCHDOG_UCC
@@ -513,7 +521,7 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
   }
   pHeartbeat = rxData.signals.heartbeat;
 
-  bool close_sdc = WD_initialized && rxData.signals.as_close_sdc;
+  bool close_sdc = setup_done && rxData.signals.as_close_sdc;
 
 #endif
 
diff --git a/sdcl-firmware/sdcl-firmware Debug.launch b/sdcl-firmware/sdcl-firmware Debug.launch
index f665c1d..a7a2042 100644
--- a/sdcl-firmware/sdcl-firmware Debug.launch	
+++ b/sdcl-firmware/sdcl-firmware Debug.launch	
@@ -38,7 +38,7 @@
     <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_txt_serial_number" value=""/>
     <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.watchdog_config" value="none"/>
     <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkenable_rtos" value="false"/>
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkrestart_configurations" value="{&quot;fItems&quot;:[{&quot;fDisplayName&quot;:&quot;Reset&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Software system reset&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Software system reset&quot;,&quot;fLaunchAttribute&quot;:&quot;system_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Hardware reset&quot;,&quot;fLaunchAttribute&quot;:&quot;hardware_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset hardware\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Core reset&quot;,&quot;fLaunchAttribute&quot;:&quot;core_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset core\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;no_reset&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[&quot;-g&quot;]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]}}]}"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkrestart_configurations" value="{&quot;fVersion&quot;:1,&quot;fItems&quot;:[{&quot;fDisplayName&quot;:&quot;Reset&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Software system reset&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Software system reset&quot;,&quot;fLaunchAttribute&quot;:&quot;system_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Hardware reset&quot;,&quot;fLaunchAttribute&quot;:&quot;hardware_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset hardware\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Core reset&quot;,&quot;fLaunchAttribute&quot;:&quot;core_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset core\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;no_reset&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[&quot;-g&quot;]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]},&quot;fStartApplication&quot;:true}]}"/>
     <booleanAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.enableRtosProxy" value="false"/>
     <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyCustomProperties" value=""/>
     <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriver" value="threadx"/>