diff --git a/software/firmware/.cproject b/software/firmware/.cproject
index 56e81989..828b0179 100644
--- a/software/firmware/.cproject
+++ b/software/firmware/.cproject
@@ -18,7 +18,7 @@
 				</extensions>
 			</storageModule>
 			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration artifactName="${ProjName}" buildProperties="" description="" id="ilg.gnuarmeclipse.managedbuild.cross.toolchain.base.1790597438" name="Default" optionalBuildProperties="org.eclipse.cdt.docker.launcher.containerbuild.property.dockerdpath=,org.eclipse.cdt.docker.launcher.containerbuild.property.selectedvolumes=,org.eclipse.cdt.docker.launcher.containerbuild.property.volumes=" parent="org.eclipse.cdt.build.core.emptycfg">
+				<configuration artifactName="${ProjName}" buildProperties="" description="" id="ilg.gnuarmeclipse.managedbuild.cross.toolchain.base.1790597438" name="Default" optionalBuildProperties="org.eclipse.cdt.docker.launcher.containerbuild.property.dockerdpath=,org.eclipse.cdt.docker.launcher.containerbuild.property.volumes=,org.eclipse.cdt.docker.launcher.containerbuild.property.selectedvolumes=" parent="org.eclipse.cdt.build.core.emptycfg">
 					<folderInfo id="ilg.gnuarmeclipse.managedbuild.cross.toolchain.base.1790597438.1033093985" name="/" resourcePath="">
 						<toolChain id="ilg.gnuarmeclipse.managedbuild.cross.toolchain.base.1406672748" name="Arm Cross GCC" superClass="ilg.gnuarmeclipse.managedbuild.cross.toolchain.base">
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.architecture.275751269" name="Architecture" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.architecture" value="ilg.gnuarmeclipse.managedbuild.cross.option.architecture.arm" valueType="enumerated"/>
@@ -347,7 +347,7 @@
 			</target>
 			<target name="UID" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>${cross_make}</buildCommand>
-				<buildArguments>ID=c0:98:e5:42:01:10</buildArguments>
+				<buildArguments>ID=c0:98:e5:42:01:14</buildArguments>
 				<buildTarget>UID</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
diff --git a/software/firmware/.settings/language.settings.xml b/software/firmware/.settings/language.settings.xml
index 73035b0b..49bbe7e6 100644
--- a/software/firmware/.settings/language.settings.xml
+++ b/software/firmware/.settings/language.settings.xml
@@ -5,7 +5,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider copy-of="extension" id="org.eclipse.cdt.managedbuilder.core.GCCBuildCommandParser"/>
-			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-339510081203349785" id="org.eclipse.embedcdt.managedbuild.cross.arm.core.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT Arm Cross GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="1347002660722691700" id="org.eclipse.embedcdt.managedbuild.cross.arm.core.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT Arm Cross GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
diff --git a/software/firmware/launchConfigs/TestFull.launch b/software/firmware/launchConfigs/TestFull.launch
index 4ecc747e..ef325b1c 100644
--- a/software/firmware/launchConfigs/TestFull.launch
+++ b/software/firmware/launchConfigs/TestFull.launch
@@ -78,6 +78,6 @@
     <listAttribute key="org.eclipse.debug.ui.favoriteGroups">
         <listEntry value="org.eclipse.debug.ui.launchGroup.debug"/>
     </listAttribute>
-    <stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#13;&#10;&lt;memoryBlockExpressionList context=&quot;Context string&quot;/&gt;&#13;&#10;"/>
+    <stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#10;&lt;memoryBlockExpressionList context=&quot;Context string&quot;/&gt;&#10;"/>
     <stringAttribute key="process_factory_id" value="org.eclipse.cdt.dsf.gdb.GdbProcessFactory"/>
 </launchConfiguration>
diff --git a/software/firmware/launchConfigs/TestRanging.launch b/software/firmware/launchConfigs/TestRanging.launch
index 270b55ea..b03ed7e0 100644
--- a/software/firmware/launchConfigs/TestRanging.launch
+++ b/software/firmware/launchConfigs/TestRanging.launch
@@ -78,6 +78,6 @@
     <listAttribute key="org.eclipse.debug.ui.favoriteGroups">
         <listEntry value="org.eclipse.debug.ui.launchGroup.debug"/>
     </listAttribute>
-    <stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#10;&lt;memoryBlockExpressionList context=&quot;Context string&quot;/&gt;&#10;"/>
+    <stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#13;&#10;&lt;memoryBlockExpressionList context=&quot;Context string&quot;/&gt;&#13;&#10;"/>
     <stringAttribute key="process_factory_id" value="org.eclipse.cdt.dsf.gdb.GdbProcessFactory"/>
 </launchConfiguration>
diff --git a/software/firmware/src/app/app_config.h b/software/firmware/src/app/app_config.h
index bad4f362..ad1adaf2 100644
--- a/software/firmware/src/app/app_config.h
+++ b/software/firmware/src/app/app_config.h
@@ -119,7 +119,7 @@ typedef enum { BATTERY_EMPTY = 3200, BATTERY_CRITICAL = 3500, BATTERY_NOMINAL =
 #define SCHEDULE_BROADCAST_PERIOD_US                (SCHEDULE_NUM_TOTAL_BROADCASTS * SCHEDULE_RESEND_INTERVAL_US)
 
 #define RANGING_BROADCAST_INTERVAL_US               600
-#define RANGING_TIMEOUT_US                          ((uint32_t)DW_PREAMBLE_LENGTH_US + 56)
+#define RANGING_TIMEOUT_US                          (RECEIVE_EARLY_START_US + 56)
 #define RANGING_NUM_SEQUENCES_PER_RANGE             (NUM_XMIT_ANTENNAS * NUM_RCV_ANTENNAS)
 #define RANGING_NUM_PACKETS_PER_RANGE               (4 * RANGING_NUM_SEQUENCES_PER_RANGE)
 #define RANGING_US_PER_RANGE                        (RANGING_BROADCAST_INTERVAL_US * RANGING_NUM_PACKETS_PER_RANGE)
diff --git a/software/firmware/src/tasks/ranging/ranging_phase.c b/software/firmware/src/tasks/ranging/ranging_phase.c
index 770f379b..b3771964 100644
--- a/software/firmware/src/tasks/ranging/ranging_phase.c
+++ b/software/firmware/src/tasks/ranging/ranging_phase.c
@@ -32,7 +32,7 @@ static inline scheduler_phase_t start_tx(const char *error_message)
 static inline scheduler_phase_t start_rx(const char *error_message)
 {
    dwt_setdelayedtrxtime(DW_DELAY_FROM_US(next_action_timestamp - RECEIVE_EARLY_START_US));
-   if (dwt_rxenable(DWT_START_RX_DLY_REF) != DWT_SUCCESS)
+   if (dwt_rxenable(DWT_START_RX_DLY_REF | DWT_IDLE_ON_DLY_ERR) != DWT_SUCCESS)
    {
       print(error_message);
       return RADIO_ERROR;
diff --git a/software/firmware/src/tasks/ranging/scheduler.c b/software/firmware/src/tasks/ranging/scheduler.c
index 11617866..96dc4aa3 100644
--- a/software/firmware/src/tasks/ranging/scheduler.c
+++ b/software/firmware/src/tasks/ranging/scheduler.c
@@ -120,11 +120,16 @@ void scheduler_rtc_isr(void)
 
 static void tx_callback(const dwt_cb_data_t *txData)
 {
-   // Notify the main task to handle the interrupt
-   BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+   // Allow the scheduling protocol to handle the interrupt
    ranging_phase = schedule_phase_tx_complete();
-   xTaskNotifyFromISR(notification_handle, RANGING_TX_COMPLETE, eSetBits, &xHigherPriorityTaskWoken);
-   portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+
+   // Determine if the main task needs to be woken up to handle the current ranging phase
+   if ((ranging_phase == RADIO_ERROR) || (ranging_phase == RANGE_COMPUTATION_PHASE))
+   {
+      BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+      xTaskNotifyFromISR(notification_handle, RANGING_TX_COMPLETE, eSetBits, &xHigherPriorityTaskWoken);
+      portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+   }
 }
 
 static void rx_callback(const dwt_cb_data_t *rxData)
@@ -134,7 +139,7 @@ static void rx_callback(const dwt_cb_data_t *rxData)
    ranging_phase = schedule_phase_rx_complete((schedule_packet_t*)read_buffer);
 
    // Determine if the main task needs to be woken up to handle the current ranging phase
-   if ((ranging_phase == RANGE_COMPUTATION_PHASE) || (ranging_phase == MESSAGE_COLLISION))
+   if ((ranging_phase == RANGE_COMPUTATION_PHASE) || (ranging_phase == MESSAGE_COLLISION) || (ranging_phase == RADIO_ERROR))
    {
       BaseType_t xHigherPriorityTaskWoken = pdFALSE;
       xTaskNotifyFromISR(notification_handle, RANGING_RX_COMPLETE, eSetBits, &xHigherPriorityTaskWoken);
diff --git a/software/firmware/src/tasks/ranging/status_phase.c b/software/firmware/src/tasks/ranging/status_phase.c
index b8bfab6c..f3070037 100644
--- a/software/firmware/src/tasks/ranging/status_phase.c
+++ b/software/firmware/src/tasks/ranging/status_phase.c
@@ -31,7 +31,7 @@ static inline scheduler_phase_t start_tx(const char *error_message, status_succe
 static inline scheduler_phase_t start_rx(const char *error_message)
 {
    dwt_setdelayedtrxtime(DW_DELAY_FROM_US(next_action_timestamp - RECEIVE_EARLY_START_US));
-   if (dwt_rxenable(DWT_START_RX_DLY_REF) != DWT_SUCCESS)
+   if (dwt_rxenable(DWT_START_RX_DLY_REF | DWT_IDLE_ON_DLY_ERR) != DWT_SUCCESS)
    {
       print(error_message);
       return RANGE_COMPUTATION_PHASE;
diff --git a/software/firmware/src/tasks/ranging/subscription_phase.c b/software/firmware/src/tasks/ranging/subscription_phase.c
index 470cd88c..60788e7a 100644
--- a/software/firmware/src/tasks/ranging/subscription_phase.c
+++ b/software/firmware/src/tasks/ranging/subscription_phase.c
@@ -33,6 +33,7 @@ scheduler_phase_t subscription_phase_begin(uint8_t scheduled_slot, uint8_t sched
    schedule_length = schedule_size;
    reference_time = ((uint64_t)start_delay_dwt) << 8;
    dwt_setreferencetrxtime(start_delay_dwt);
+   ranging_radio_choose_antenna(0);
 
    // Reset the necessary Subscription Phase parameters
    if (schedule_index == UNSCHEDULED_SLOT)
@@ -49,7 +50,7 @@ scheduler_phase_t subscription_phase_begin(uint8_t scheduled_slot, uint8_t sched
       dwt_setdelayedtrxtime(0);
       dwt_setpreambledetecttimeout(0);
       dwt_setrxtimeout(DW_TIMEOUT_FROM_US(RECEIVE_EARLY_START_US + SUBSCRIPTION_TIMEOUT_US));
-      if (dwt_rxenable(DWT_START_RX_DLY_REF) != DWT_SUCCESS)
+      if (dwt_rxenable(DWT_START_RX_DLY_REF | DWT_IDLE_ON_DLY_ERR) != DWT_SUCCESS)
          print("ERROR: Unable to start listening for SUBSCRIPTION packets\n");
       else
          return SUBSCRIPTION_PHASE;
@@ -65,6 +66,7 @@ scheduler_phase_t subscription_phase_tx_complete(void)
    // Forward this request to the next phase if not currently in the Subscription Phase
    if (current_phase != SUBSCRIPTION_PHASE)
       return ranging_phase_tx_complete();
+   current_phase = RANGING_PHASE;
    return ranging_phase_begin(schedule_index, schedule_length, (uint32_t)((reference_time + US_TO_DWT(SUBSCRIPTION_BROADCAST_PERIOD_US)) >> 8) & 0xFFFFFFFE);
 }
 
@@ -84,6 +86,10 @@ scheduler_phase_t subscription_phase_rx_complete(subscription_packet_t* packet)
 
 scheduler_phase_t subscription_phase_rx_error(void)
 {
+   // Forward this request to the next phase if not currently in the Subscription Phase
+   if (current_phase != SUBSCRIPTION_PHASE)
+      return ranging_phase_rx_error();
+
    // Attempt to re-enable listening for additional Subscription packets
    uint32_t time_elapsed_us = DWT_TO_US((uint64_t)(dwt_readsystimestamphi32() - (uint32_t)(reference_time >> 8)) << 8);
    if ((time_elapsed_us + 600) <= (RECEIVE_EARLY_START_US + SUBSCRIPTION_TIMEOUT_US))