diff --git a/sdcl-firmware/Core/Src/main.c b/sdcl-firmware/Core/Src/main.c
index acb0c86..396d098 100644
--- a/sdcl-firmware/Core/Src/main.c
+++ b/sdcl-firmware/Core/Src/main.c
@@ -48,6 +48,15 @@ typedef enum {
 	M_MANUAL		= 7
 } mission_t;
 
+typedef enum {
+	AS_OFF = 0,
+	AS_MANUAL = 1,
+	AS_READY = 2,
+	AS_DRIVING = 3,
+	AS_FINISHED = 4,
+	AS_EMERGENCY = 5,
+} as_state_t;
+
 /*
  * BO_ 15 SDCL_rx: 3 ABX
  * SG_ as_close_sdc : 0|1@1+ (1,0) [0|1] ""  SDCL
@@ -112,6 +121,7 @@ typedef union {
 
 #define CAN_ID_RX 0x00F
 #define CAN_ID_TX 0x010
+#define CAN_ID_JETSON_RX 0x0E0
 
 // Defined in DBC?
 #define TX_UPDATE_PERIOD 100
@@ -140,6 +150,7 @@ const uint16_t	mission2led[] 	= {0		, AMI_ACCEL_Pin	, AMI_SKIDPAD_Pin	, AMI_AUTO
 const mission_t	mission2next[] 	= {M_MANUAL	, M_SKIDPAD		, M_AUTOX			, M_EBSTEST			, M_INSPECTION		, M_MANUAL		, M_TRACKDRIVE	, M_ACCEL		};
 
 mission_t mission = M_NONE;
+as_state_t state = AS_OFF;
 
 bool setup_done = false;
 
@@ -228,7 +239,11 @@ int main(void)
   canfilterconfig.FilterMode = CAN_FILTERMODE_IDMASK;
   canfilterconfig.FilterScale = CAN_FILTERSCALE_32BIT;
   canfilterconfig.SlaveStartFilterBank = 14;
-
+  if (HAL_CAN_ConfigFilter(&hcan, &canfilterconfig) != HAL_OK) {
+    Error_Handler();
+  }
+  canfilterconfig.FilterBank = 1;
+  canfilterconfig.FilterIdHigh = CAN_ID_JETSON_RX << (16 -11);
   if (HAL_CAN_ConfigFilter(&hcan, &canfilterconfig) != HAL_OK) {
     Error_Handler();
   }
@@ -271,6 +286,7 @@ int main(void)
   }
 
   setup_done = true;
+  uint32_t last_msg = 0;
 
   while (true) {
 
@@ -289,48 +305,53 @@ int main(void)
 	bool CLOSED				= HAL_GPIO_ReadPin(CLOSED_GPIO_Port, CLOSED_Pin)						== GPIO_PIN_RESET;
 	bool REOPENED			= HAL_GPIO_ReadPin(REOPENED_GPIO_Port, REOPENED_Pin)					== GPIO_PIN_RESET;
 
-	bool AMC = HAL_GPIO_ReadPin(AMC_GPIO_Port, AMC_Pin) == GPIO_PIN_SET;
-	// On signal edge. Debouncing usually not needed at these polling rates (10Hz)
-	if (AMC < pAMC) {
+	if (HAL_GetTick() - last_msg >= TX_UPDATE_PERIOD) {
+		last_msg = HAL_GetTick();
+		bool AMC = HAL_GPIO_ReadPin(AMC_GPIO_Port, AMC_Pin) == GPIO_PIN_SET;
+		// On signal edge. Debouncing usually not needed at these polling rates (10Hz)
+		if (AMC < pAMC) {
 
-		// Reset LED to indicate transaction / mission change in progress
-		setMissionLED(mission, GPIO_PIN_RESET);
+			// Reset LED to indicate transaction / mission change in progress
+			setMissionLED(mission, GPIO_PIN_RESET);
 
-		new_mission = mission2next[mission];
-		// New LED will be set once response from ABX is received
+			new_mission = mission2next[mission];
+			// New LED will be set once response from ABX is received
 
-	}
-
-	// TEMP: Only enable WD if in autonomous mode because EMI currently messes it up during R2D
-	if (ASMS > pASMS) {
-		MX_IWDG_Init();
-		WD_initialized = true;
-	}
-
-	txData = (tx_data_t) {
-		.signals = {
-			.asms_state = ASMS,
-			.sdc_state_1 = LV_SENSE_1,
-			.sdc_state_2 = LV_SENSE_2,
-			.sdc_state_3 = SDC_in_3V3,
-			.heartbeat_ok = WD_OK,
-			.sdc_ready = SDC_is_ready,
-			.ts_start_muxed = TS_activate_MUXed,
-			.latch_init_open = INITIAL_OPEN,
-			.latch_closed = CLOSED,
-			.latch_reopened = REOPENED,
-			.as_mission = new_mission
 		}
-	};
 
-	if (HAL_CAN_AddTxMessage(&hcan, &txHeader, txData.raw, &txMailbox) != HAL_OK)
-		Error_Handler();
+		// TEMP: Only enable WD if in autonomous mode because EMI currently messes it up during R2D
+		if (ASMS > pASMS) {
+			MX_IWDG_Init();
+			WD_initialized = true;
+		}
 
-	// Store previous button value to detect signal edges
-	pAMC = AMC;
-	pASMS = ASMS;
+		txData = (tx_data_t) {
+			.signals = {
+				.asms_state = ASMS,
+				.sdc_state_1 = LV_SENSE_1,
+				.sdc_state_2 = LV_SENSE_2,
+				.sdc_state_3 = SDC_in_3V3,
+				.heartbeat_ok = WD_OK,
+				.sdc_ready = SDC_is_ready,
+				.ts_start_muxed = TS_activate_MUXed,
+				.latch_init_open = INITIAL_OPEN,
+				.latch_closed = CLOSED,
+				.latch_reopened = REOPENED,
+				.as_mission = new_mission
+			}
+		};
 
-	HAL_Delay(TX_UPDATE_PERIOD);
+		if (HAL_CAN_AddTxMessage(&hcan, &txHeader, txData.raw, &txMailbox) != HAL_OK)
+			Error_Handler();
+
+		// Store previous button value to detect signal edges
+		pAMC = AMC;
+		pASMS = ASMS;
+	}
+	if (state != AS_OFF && state != AS_MANUAL && WD_initialized) {
+		HAL_IWDG_Refresh(&hiwdg);
+		WD_OK = true;
+	}
 
     /* USER CODE END WHILE */
 
@@ -512,43 +533,46 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
     Error_Handler();
 
   // Discard if it's not for us (shouldn't happen thanks to filter, but just to be sure)
-  if (rxHeader.StdId != CAN_ID_RX)
-	  return;
+  if (rxHeader.StdId == CAN_ID_RX) {
+//	  return;
 
 #ifdef WATCHDOG_STM
 
-  if (rxData.signals.heartbeat != pHeartbeat) {
-	  if (WD_initialized)
-		  HAL_IWDG_Refresh(&hiwdg);
-	  if (heartbeat_received) {
-		  WD_OK = true;
+	  if (rxData.signals.heartbeat != pHeartbeat) {
+		  if (WD_initialized)
+			  HAL_IWDG_Refresh(&hiwdg);
+		  if (heartbeat_received) {
+			  WD_OK = true;
+		  }
+		  heartbeat_received = true;
+		  //HAL_GPIO_WritePin(Watchdog_GPIO_Port, Watchdog_Pin, GPIO_PIN_SET);
 	  }
-	  heartbeat_received = true;
-  	  //HAL_GPIO_WritePin(Watchdog_GPIO_Port, Watchdog_Pin, GPIO_PIN_SET);
+	  pHeartbeat = rxData.signals.heartbeat;
+
+	  bool close_sdc = setup_done && rxData.signals.as_close_sdc && WD_OK;
+
+	#endif
+
+	#ifdef WATCHDOG_UCC
+	  HAL_GPIO_WritePin(Watchdog_GPIO_Port, Watchdog_Pin, rxData.signals.heartbeat);
+
+	  bool close_sdc = rxData.signals.as_close_sdc;
+	#endif
+
+	  // Set whether to close the relay. The port is inverted due to multiple bodges
+	  GPIO_PinState close_sdc_val = close_sdc ? GPIO_PIN_RESET : GPIO_PIN_SET;
+	  HAL_GPIO_WritePin(AS_close_SDC_GPIO_Port, AS_close_SDC_Pin, close_sdc_val);
+
+	  // Reset old mission LED
+	  setMissionLED(mission, GPIO_PIN_RESET);
+	  mission = rxData.signals.as_mission;
+	  setMissionLED(mission, GPIO_PIN_SET);
+
+	  // Set ASB Error status
+	  HAL_GPIO_WritePin(ASB_Error_GPIO_Port, ASB_Error_Pin, rxData.signals.asb_error);
+  } else if (rxHeader.StdId == CAN_ID_JETSON_RX) {
+	  state = (rxData.raw[0] >> 3) & 0b111;
   }
-  pHeartbeat = rxData.signals.heartbeat;
-
-  bool close_sdc = setup_done && rxData.signals.as_close_sdc && WD_OK;
-
-#endif
-
-#ifdef WATCHDOG_UCC
-  HAL_GPIO_WritePin(Watchdog_GPIO_Port, Watchdog_Pin, rxData.signals.heartbeat);
-
-  bool close_sdc = rxData.signals.as_close_sdc;
-#endif
-
-  // Set whether to close the relay. The port is inverted due to multiple bodges
-  GPIO_PinState close_sdc_val = close_sdc ? GPIO_PIN_RESET : GPIO_PIN_SET;
-  HAL_GPIO_WritePin(AS_close_SDC_GPIO_Port, AS_close_SDC_Pin, close_sdc_val);
-
-  // Reset old mission LED
-  setMissionLED(mission, GPIO_PIN_RESET);
-  mission = rxData.signals.as_mission;
-  setMissionLED(mission, GPIO_PIN_SET);
-
-  // Set ASB Error status
-  HAL_GPIO_WritePin(ASB_Error_GPIO_Port, ASB_Error_Pin, rxData.signals.asb_error);
 
 }