More ranging task updates

software/firmware/src/app/app_config.h

@@ -130,6 +130,6 @@ typedef enum { BATTERY_EMPTY = 3200, BATTERY_CRITICAL = 3500, BATTERY_NOMINAL =
 #define RANGE_STATUS_TIMEOUT_US                     (RANGE_STATUS_BROADCAST_PERIOD_US - 900 + RECEIVE_EARLY_START_US)
 
 #define SUBSCRIPTION_BROADCAST_PERIOD_US            2000
-#define SUBSCRIPTION_TIMEOUT_US                     1400
+#define SUBSCRIPTION_TIMEOUT_US                     1000
 
 #endif  // #ifndef __APP_CONFIG_HEADER_H__

software/firmware/src/tasks/ranging/ranging_phase.c

@@ -24,7 +24,7 @@ static inline scheduler_phase_t start_tx(const char *error_message)
    if ((dwt_writetxdata(sizeof(ranging_packet_t) - sizeof(ieee154_header_t) - sizeof(ieee154_footer_t), &ranging_packet.sequence_number, offsetof(ranging_packet_t, sequence_number)) != DWT_SUCCESS) || (dwt_starttx(DWT_START_TX_DLY_REF) != DWT_SUCCESS))
    {
       print(error_message);
-      return RANGING_ERROR;
+      return RADIO_ERROR;
    }
    return RANGING_PHASE;
 }
@@ -35,7 +35,7 @@ static inline scheduler_phase_t start_rx(const char *error_message)
    if (dwt_rxenable(DWT_START_RX_DLY_REF) != DWT_SUCCESS)
    {
       print(error_message);
-      return RANGING_ERROR;
+      return RADIO_ERROR;
    }
    return RANGING_PHASE;
 }
@@ -196,13 +196,13 @@ scheduler_phase_t ranging_phase_begin(uint8_t scheduled_slot, uint8_t schedule_s
 {
    // Ensure there are at least two devices to begin ranging
    reset_computation_phase();
+   my_index = scheduled_slot;
    ranging_phase_duration = ((uint32_t)schedule_size * (schedule_size - 1) / 2) * RANGING_US_PER_RANGE;
-   if ((schedule_size < 2) || (scheduled_slot == UNSCHEDULED_SLOT))
+   if ((schedule_size < 2) || (my_index == UNSCHEDULED_SLOT))
       return RANGE_COMPUTATION_PHASE;
 
    // Reset the necessary Ranging Phase parameters
    current_phase = RANGING_PHASE;
-   my_index = scheduled_slot;
    schedule_length = schedule_size;
    initiator_antenna = responder_antenna = 0;
    next_action_timestamp = RECEIVE_EARLY_START_US;
@@ -234,7 +234,6 @@ scheduler_phase_t ranging_phase_begin(uint8_t scheduled_slot, uint8_t schedule_s
       next_action_timestamp += (uint32_t)(my_index - 1) * RANGING_US_PER_RANGE;
       return start_rx("ERROR: Unable to start listening for RANGING POLL packets\n");
    }
-   return RANGING_ERROR;
 }
 
 scheduler_phase_t ranging_phase_tx_complete(void)
@@ -296,7 +295,7 @@ scheduler_phase_t ranging_phase_rx_error(void)
       ranging_packet.tx_time = (uint32_t)(reference_time + US_TO_DWT(next_action_timestamp)) & 0xFFFFFE00;
       return start_tx("ERROR: Failed to transmit RANGING POLL packet after INITIATOR RX error\n");
    }
-   else if (((ranging_packet.sequence_number & 0x03) == 1) || ((ranging_packet.sequence_number & 0x03) == 3))
+   else  // if (((ranging_packet.sequence_number & 0x03) == 1) || ((ranging_packet.sequence_number & 0x03) == 3))
    {
       // We are the responder and didn't receive a response
       uint32_t skip_packets = 1 + (ranging_packet.sequence_number & 0x03);
@@ -352,10 +351,14 @@ scheduler_phase_t ranging_phase_rx_error(void)
       // Attempt to receive the next ranging packet
       return start_rx("ERROR: Unable to start listening for RANGING packets after RESPONDER RX error\n");
    }
-   return RANGING_ERROR;
 }
 
 uint32_t ranging_phase_get_duration(void)
 {
    return ranging_phase_duration;
 }
+
+bool ranging_phase_was_scheduled(void)
+{
+   return (my_index != UNSCHEDULED_SLOT);
+}

software/firmware/src/tasks/ranging/ranging_phase.h

@@ -25,5 +25,6 @@ scheduler_phase_t ranging_phase_tx_complete(void);
 scheduler_phase_t ranging_phase_rx_complete(ranging_packet_t* packet);
 scheduler_phase_t ranging_phase_rx_error(void);
 uint32_t ranging_phase_get_duration(void);
+bool ranging_phase_was_scheduled(void);
 
 #endif  // #ifndef __RANGING_PHASE_HEADER_H__

software/firmware/src/tasks/ranging/schedule_phase.c

@@ -47,7 +47,7 @@ void schedule_phase_initialize(const uint8_t *uid, bool is_master, uint32_t epoc
    scheduled_slot = 0;
 }
 
-bool schedule_phase_begin(void)
+scheduler_phase_t schedule_phase_begin(void)
 {
    // Reset the necessary Schedule Phase parameters
    schedule_packet.sequence_number = 0;
@@ -71,7 +71,7 @@ bool schedule_phase_begin(void)
       if ((dwt_writetxdata(packet_size - sizeof(ieee154_footer_t), (uint8_t*)&schedule_packet, 0) != DWT_SUCCESS) || (dwt_starttx(DWT_START_TX_IMMEDIATE) != DWT_SUCCESS))
       {
          print("ERROR: Failed to transmit schedule with length %u\n", (uint32_t)packet_size);
-         return false;
+         return RADIO_ERROR;
       }
    }
    else
@@ -82,10 +82,10 @@ bool schedule_phase_begin(void)
       if (!ranging_radio_rxenable(DWT_START_RX_IMMEDIATE))
       {
          print("ERROR: Unable to start listening for schedule packets\n");
-         return false;
+         return RADIO_ERROR;
       }
    }
-   return true;
+   return SCHEDULE_PHASE;
 }
 
 scheduler_phase_t schedule_phase_tx_complete(void)
@@ -96,7 +96,7 @@ scheduler_phase_t schedule_phase_tx_complete(void)
 
    // Retransmit the schedule up to the specified number of times
    next_action_timestamp += SCHEDULE_RESEND_INTERVAL_US;
-   if ((++schedule_packet.sequence_number < SCHEDULE_NUM_MASTER_BROADCASTS) && is_master_scheduler)
+   while ((++schedule_packet.sequence_number < SCHEDULE_NUM_MASTER_BROADCASTS) && is_master_scheduler)
    {
       if (schedule_packet.sequence_number == 1)
       {
@@ -106,10 +106,11 @@ scheduler_phase_t schedule_phase_tx_complete(void)
       dwt_setdelayedtrxtime((uint32_t)((US_TO_DWT(next_action_timestamp) - TX_ANTENNA_DELAY) >> 8) & 0xFFFFFFFE);
       if ((dwt_writetxdata(sizeof(schedule_packet.sequence_number), &schedule_packet.sequence_number, offsetof(schedule_packet_t, sequence_number)) != DWT_SUCCESS) || (dwt_starttx(DWT_START_TX_DLY_REF) != DWT_SUCCESS))
       {
+         next_action_timestamp += SCHEDULE_RESEND_INTERVAL_US;
          print("ERROR: Failed to retransmit schedule\n");
-         return RANGING_ERROR;
       }
-      return SCHEDULE_PHASE;
+      else
+         return SCHEDULE_PHASE;
    }
 
    // Move to the Subscription Phase of the ranging protocol
@@ -129,18 +130,11 @@ scheduler_phase_t schedule_phase_rx_complete(schedule_packet_t* schedule)
       if (!ranging_radio_rxenable(DWT_START_RX_IMMEDIATE))
       {
          print("ERROR: Unable to restart listening for schedule packets\n");
-         return RANGING_ERROR;
+         return RADIO_ERROR;
       }
       return SCHEDULE_PHASE;
    }
 
-   // Ensure that the received schedule length is valid
-   if (schedule->num_devices > MAX_NUM_RANGING_DEVICES)
-   {
-      print("ERROR: Received a schedule with too many devices included\n");
-      return RANGING_ERROR;
-   }
-
    // Unpack the received schedule
    scheduled_slot = UNSCHEDULED_SLOT;
    schedule_packet.epoch_time_unix = schedule->epoch_time_unix;
@@ -168,8 +162,10 @@ scheduler_phase_t schedule_phase_rx_complete(schedule_packet_t* schedule)
       dwt_setdelayedtrxtime((uint32_t)((US_TO_DWT(next_action_timestamp) - TX_ANTENNA_DELAY) >> 8) & 0xFFFFFFFE);
       if ((dwt_writetxdata(packet_size - sizeof(ieee154_footer_t), (uint8_t*)&schedule_packet, 0) != DWT_SUCCESS) || (dwt_starttx(DWT_START_TX_DLY_REF) != DWT_SUCCESS))
       {
+         current_phase = SUBSCRIPTION_PHASE;
          print("ERROR: Failed to retransmit received schedule\n");
-         return RANGING_ERROR;
+         next_action_timestamp += ((uint32_t)(SCHEDULE_NUM_TOTAL_BROADCASTS - schedule_packet.sequence_number)) * SCHEDULE_RESEND_INTERVAL_US;
+         return subscription_phase_begin(scheduled_slot, schedule_packet.num_devices, (uint32_t)((reference_time + US_TO_DWT(next_action_timestamp - RECEIVE_EARLY_START_US)) >> 8) & 0xFFFFFFFE);
       }
       return SCHEDULE_PHASE;
    }

software/firmware/src/tasks/ranging/schedule_phase.h

@@ -22,7 +22,7 @@ typedef struct __attribute__ ((__packed__))
 // Public API ----------------------------------------------------------------------------------------------------------
 
 void schedule_phase_initialize(const uint8_t *uid, bool is_master, uint32_t epoch_timestamp);
-bool schedule_phase_begin(void);
+scheduler_phase_t schedule_phase_begin(void);
 scheduler_phase_t schedule_phase_tx_complete(void);
 scheduler_phase_t schedule_phase_rx_complete(schedule_packet_t* schedule);
 scheduler_phase_t schedule_phase_rx_error(void);