diff --git a/software/firmware/src/app/app_config.h b/software/firmware/src/app/app_config.h index 84398d5e..bad4f362 100644 --- a/software/firmware/src/app/app_config.h +++ b/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__ diff --git a/software/firmware/src/tasks/ranging/ranging_phase.c b/software/firmware/src/tasks/ranging/ranging_phase.c index 81279cb0..770f379b 100644 --- a/software/firmware/src/tasks/ranging/ranging_phase.c +++ b/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); +} diff --git a/software/firmware/src/tasks/ranging/ranging_phase.h b/software/firmware/src/tasks/ranging/ranging_phase.h index f8b14a80..3bc0cd6e 100644 --- a/software/firmware/src/tasks/ranging/ranging_phase.h +++ b/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__ diff --git a/software/firmware/src/tasks/ranging/schedule_phase.c b/software/firmware/src/tasks/ranging/schedule_phase.c index 8ce132c3..6a3349f2 100644 --- a/software/firmware/src/tasks/ranging/schedule_phase.c +++ b/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; } diff --git a/software/firmware/src/tasks/ranging/schedule_phase.h b/software/firmware/src/tasks/ranging/schedule_phase.h index 8cfb503e..145aba9e 100644 --- a/software/firmware/src/tasks/ranging/schedule_phase.h +++ b/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); diff --git a/software/firmware/src/tasks/ranging/scheduler.c b/software/firmware/src/tasks/ranging/scheduler.c index 2abc1f88..11617866 100644 --- a/software/firmware/src/tasks/ranging/scheduler.c +++ b/software/firmware/src/tasks/ranging/scheduler.c @@ -14,18 +14,19 @@ // Static Global Variables --------------------------------------------------------------------------------------------- +static schedule_role_t current_role; static scheduler_phase_t ranging_phase; static TaskHandle_t notification_handle; static am_hal_timer_config_t wakeup_timer_config; static uint8_t ranging_results[MAX_COMPRESSED_RANGE_DATA_LENGTH]; -static uint8_t read_buffer[128], device_eui, schedule_reception_timeout; +static uint8_t read_buffer[128], device_eui, reception_timeout; static uint8_t empty_round_timeout, eui[EUI_LEN]; static volatile bool is_running; // Private Helper Functions -------------------------------------------------------------------------------------------- -static bool fix_network_errors(uint8_t num_ranging_results) +static void fix_network_errors(uint8_t num_ranging_results) { // Have the Scheduler Phase handle any new device timeouts uint8_t num_devices = 0; @@ -44,32 +45,55 @@ static bool fix_network_errors(uint8_t num_ranging_results) #else empty_round_timeout = 0; #endif - return false; } - return true; } -static void handle_range_computation_phase(bool is_master) +static void handle_range_computation_phase(void) { - // Put the radio into deep-sleep mode and set a timer to wake it before the next round + // Put the radio into deep-sleep mode and handle role-specific tasks ranging_radio_sleep(true); - if (!is_master) + switch (current_role) { - const uint32_t remaing_time_us = 1000000 - RADIO_WAKEUP_SAFETY_DELAY_US - SCHEDULE_BROADCAST_PERIOD_US - SUBSCRIPTION_BROADCAST_PERIOD_US - ranging_phase_get_duration() - (schedule_phase_get_num_devices() * RANGE_STATUS_BROADCAST_PERIOD_US); - wakeup_timer_config.ui32Compare0 = (uint32_t)((float)RADIO_WAKEUP_TIMER_TICK_RATE_HZ / (1000000.0f / remaing_time_us)); - am_hal_timer_config(RADIO_WAKEUP_TIMER_NUMBER, &wakeup_timer_config); - am_hal_timer_clear(RADIO_WAKEUP_TIMER_NUMBER); - } + case ROLE_MASTER: + { + // Carry out the ranging algorithm and fix any detected network errors + compute_ranges(ranging_results); + fix_network_errors(ranging_results[0]); + bluetooth_write_range_results(ranging_results, 1 + ((uint16_t)ranging_results[0] * COMPRESSED_RANGE_DATUM_LENGTH)); +#ifndef _TEST_RANGING_TASK + if (ranging_results[0]) + storage_write_ranging_data(schedule_phase_get_timestamp(), ranging_results, 1 + ((uint32_t)ranging_results[0] * COMPRESSED_RANGE_DATUM_LENGTH)); +#endif + print_ranges(schedule_phase_get_timestamp(), ranging_results, 1 + ((uint32_t)ranging_results[0] * COMPRESSED_RANGE_DATUM_LENGTH)); + break; + } + case ROLE_PARTICIPANT: + { + // Set a timer to wake the radio before the next round + const uint32_t remaing_time_us = 1000000 - RADIO_WAKEUP_SAFETY_DELAY_US - SCHEDULE_BROADCAST_PERIOD_US - SUBSCRIPTION_BROADCAST_PERIOD_US - ranging_phase_get_duration() - (schedule_phase_get_num_devices() * RANGE_STATUS_BROADCAST_PERIOD_US); + wakeup_timer_config.ui32Compare0 = (uint32_t)((float)RADIO_WAKEUP_TIMER_TICK_RATE_HZ / (1000000.0f / remaing_time_us)); + am_hal_timer_config(RADIO_WAKEUP_TIMER_NUMBER, &wakeup_timer_config); + am_hal_timer_clear(RADIO_WAKEUP_TIMER_NUMBER); - // Carry out the ranging algorithm and fix any detected network errors - compute_ranges(ranging_results); - if (!is_master || fix_network_errors(ranging_results[0])) - { - bluetooth_write_range_results(ranging_results, 1 + ((uint16_t)ranging_results[0] * COMPRESSED_RANGE_DATUM_LENGTH)); + // Carry out the ranging algorithm and fix any detected network errors + compute_ranges(ranging_results); + bluetooth_write_range_results(ranging_results, 1 + ((uint16_t)ranging_results[0] * COMPRESSED_RANGE_DATUM_LENGTH)); #ifndef _TEST_RANGING_TASK - storage_write_ranging_data(schedule_phase_get_timestamp(), ranging_results, 1 + ((uint32_t)ranging_results[0] * COMPRESSED_RANGE_DATUM_LENGTH)); + if (ranging_results[0]) + storage_write_ranging_data(schedule_phase_get_timestamp(), ranging_results, 1 + ((uint32_t)ranging_results[0] * COMPRESSED_RANGE_DATUM_LENGTH)); #endif - print_ranges(schedule_phase_get_timestamp(), ranging_results, 1 + ((uint32_t)ranging_results[0] * COMPRESSED_RANGE_DATUM_LENGTH)); + print_ranges(schedule_phase_get_timestamp(), ranging_results, 1 + ((uint32_t)ranging_results[0] * COMPRESSED_RANGE_DATUM_LENGTH)); + break; + } + default: + { + // Set a timer to wake the radio before the next round + const uint32_t remaing_time_us = 1000000 - RADIO_WAKEUP_SAFETY_DELAY_US - SCHEDULE_BROADCAST_PERIOD_US - SUBSCRIPTION_BROADCAST_PERIOD_US; + wakeup_timer_config.ui32Compare0 = (uint32_t)((float)RADIO_WAKEUP_TIMER_TICK_RATE_HZ / (1000000.0f / remaing_time_us)); + am_hal_timer_config(RADIO_WAKEUP_TIMER_NUMBER, &wakeup_timer_config); + am_hal_timer_clear(RADIO_WAKEUP_TIMER_NUMBER); + break; + } } ranging_phase = UNSCHEDULED_TIME_PHASE; } @@ -105,21 +129,31 @@ static void tx_callback(const dwt_cb_data_t *txData) static void rx_callback(const dwt_cb_data_t *rxData) { - // Notify the main task to handle the interrupt - BaseType_t xHigherPriorityTaskWoken = pdFALSE; + // Read the received data packet and allow the scheduling protocol to handle it dwt_readrxdata(read_buffer, rxData->datalength, 0); ranging_phase = schedule_phase_rx_complete((schedule_packet_t*)read_buffer); - xTaskNotifyFromISR(notification_handle, RANGING_RX_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 == RANGE_COMPUTATION_PHASE) || (ranging_phase == MESSAGE_COLLISION)) + { + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xTaskNotifyFromISR(notification_handle, RANGING_RX_COMPLETE, eSetBits, &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + } } static void rx_timeout_callback(const dwt_cb_data_t *rxData) { - // Notify the main task to handle the interrupt - BaseType_t xHigherPriorityTaskWoken = pdFALSE; + // Allow the scheduling protocol to handle the interrupt ranging_phase = schedule_phase_rx_error(); - xTaskNotifyFromISR(notification_handle, RANGING_RX_TIMEOUT, 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 == RANGING_ERROR) || (ranging_phase == RADIO_ERROR) || (ranging_phase == RANGE_COMPUTATION_PHASE)) + { + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xTaskNotifyFromISR(notification_handle, RANGING_RX_TIMEOUT, eSetBits, &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + } } @@ -148,7 +182,7 @@ void scheduler_run(schedule_role_t role, uint32_t timestamp) // Initialize all static ranging variables notification_handle = xTaskGetCurrentTaskHandle(); memset(ranging_results, 0, sizeof(ranging_results)); - schedule_reception_timeout = empty_round_timeout = 0; + reception_timeout = empty_round_timeout = 0; ranging_phase = UNSCHEDULED_TIME_PHASE; // Initialize the Schedule, Ranging, Status, and Subscription phases @@ -162,6 +196,7 @@ void scheduler_run(schedule_role_t role, uint32_t timestamp) if (role == ROLE_MASTER) { // Initialize the scheduler timer + current_role = ROLE_MASTER; am_hal_rtc_time_t scheduler_interval = { .ui32ReadError = 0, .ui32CenturyEnable = 0, .ui32Weekday = 0, .ui32Century = 0, .ui32Year = 0, .ui32Month = 0, .ui32DayOfMonth = 0, .ui32Hour = 0, .ui32Minute = 0, .ui32Second = 1, .ui32Hundredths = 0 }; @@ -173,12 +208,13 @@ void scheduler_run(schedule_role_t role, uint32_t timestamp) else { // Initialize the radio wakeup timer + current_role = ROLE_IDLE; am_hal_timer_default_config_set(&wakeup_timer_config); am_hal_timer_interrupt_enable(AM_HAL_TIMER_MASK(RADIO_WAKEUP_TIMER_NUMBER, AM_HAL_TIMER_COMPARE0)); NVIC_SetPriority(TIMER0_IRQn + RADIO_WAKEUP_TIMER_NUMBER, NVIC_configMAX_SYSCALL_INTERRUPT_PRIORITY + 1); NVIC_EnableIRQ(TIMER0_IRQn + RADIO_WAKEUP_TIMER_NUMBER); print("INFO: Searching for an existing network\n"); - schedule_phase_begin(); + ranging_phase = schedule_phase_begin(); } // Loop forever waiting for actions to wake us up @@ -191,42 +227,51 @@ void scheduler_run(schedule_role_t role, uint32_t timestamp) { // Wake up the radio and wait until all schedule updating tasks have completed ranging_radio_wakeup(); - ranging_phase = schedule_phase_begin() ? SCHEDULE_PHASE : RANGING_ERROR; + ranging_phase = schedule_phase_begin(); } // Carry out logic based on the current reported phase of the ranging protocol switch (ranging_phase) { - case RANGING_PHASE: - schedule_reception_timeout = 0; - break; case RANGE_COMPUTATION_PHASE: - handle_range_computation_phase(role == ROLE_MASTER); + reception_timeout = 0; + if (ranging_phase_was_scheduled() && (current_role == ROLE_IDLE)) + { + current_role = ROLE_PARTICIPANT; + bluetooth_set_current_ranging_role(ROLE_PARTICIPANT); + } + handle_range_computation_phase(); + break; + case RADIO_ERROR: + if (current_role == ROLE_MASTER) + ranging_phase = UNSCHEDULED_TIME_PHASE; + else + ranging_phase = schedule_phase_begin(); break; case RANGING_ERROR: - if (role == ROLE_MASTER) + if (current_role == ROLE_MASTER) ranging_phase = UNSCHEDULED_TIME_PHASE; - else if (++schedule_reception_timeout >= NETWORK_SEARCH_TIME_SECONDS) + else if (++reception_timeout >= NETWORK_SEARCH_TIME_SECONDS) { // Stop the ranging task if no network was detected after a period of time print("WARNING: Timed out searching for an existing network\n"); #ifndef _TEST_RANGING_TASK is_running = false; #else - schedule_phase_begin(); - schedule_reception_timeout = 0; + ranging_phase = schedule_phase_begin(); + reception_timeout = 0; #endif } else - schedule_phase_begin(); + ranging_phase = schedule_phase_begin(); break; case MESSAGE_COLLISION: print("WARNING: Stopping ranging due to possible network collision\n"); #ifndef _TEST_RANGING_TASK is_running = false; #else - schedule_phase_begin(); - schedule_reception_timeout = 0; + ranging_phase = schedule_phase_begin(); + reception_timeout = 0; #endif break; default: @@ -234,6 +279,10 @@ void scheduler_run(schedule_role_t role, uint32_t timestamp) } } + // Update our BLE-advertised role to IDLE + current_role = ROLE_IDLE; + bluetooth_set_current_ranging_role(ROLE_IDLE); + // Disable all ranging timers and interrupts const am_hal_rtc_time_t scheduler_interval = { .ui32ReadError = 0, .ui32CenturyEnable = 0, .ui32Weekday = 0, .ui32Century = 0, .ui32Year = 0, diff --git a/software/firmware/src/tasks/ranging/scheduler.h b/software/firmware/src/tasks/ranging/scheduler.h index 07c49b8b..7b3c9a93 100644 --- a/software/firmware/src/tasks/ranging/scheduler.h +++ b/software/firmware/src/tasks/ranging/scheduler.h @@ -26,6 +26,7 @@ typedef enum RANGE_COMPUTATION_PHASE, UNSCHEDULED_TIME_PHASE, RANGING_ERROR, + RADIO_ERROR, MESSAGE_COLLISION } scheduler_phase_t; diff --git a/software/firmware/src/tasks/ranging/status_phase.c b/software/firmware/src/tasks/ranging/status_phase.c index aa0ee9a1..b8bfab6c 100644 --- a/software/firmware/src/tasks/ranging/status_phase.c +++ b/software/firmware/src/tasks/ranging/status_phase.c @@ -67,7 +67,7 @@ scheduler_phase_t status_phase_begin(uint8_t status_slot, uint8_t num_slots, uin // Set up the correct initial start time, antenna, and RX timeout duration ranging_radio_choose_antenna(0); - dwt_setreferencetrxtime(start_delay_dwt + DW_DELAY_FROM_US(400)); + dwt_setreferencetrxtime(start_delay_dwt + DW_DELAY_FROM_US(1000 - RANGING_BROADCAST_INTERVAL_US)); dwt_setrxtimeout(DW_TIMEOUT_FROM_US(RANGE_STATUS_TIMEOUT_US)); // Begin transmission or reception depending on the scheduled time slot @@ -103,7 +103,7 @@ scheduler_phase_t status_phase_rx_complete(status_success_packet_t* packet) const uint32_t seqNum = packet->sequence_number; if (scheduled_slot && (scheduled_slot <= RANGE_STATUS_NUM_TOTAL_BROADCASTS) && (packet->sequence_number < scheduled_slot)) { - packet->sequence_number = scheduled_slot - 1; + packet->sequence_number = (scheduled_slot < current_slot) ? scheduled_slot : (scheduled_slot - 1); next_action_timestamp += (packet->sequence_number - seqNum) * RANGE_STATUS_RESEND_INTERVAL_US; return start_tx("ERROR: Failed to retransmit received STATUS packet\n", packet); } diff --git a/software/firmware/src/tasks/ranging/subscription_phase.c b/software/firmware/src/tasks/ranging/subscription_phase.c index 9ce35150..470cd88c 100644 --- a/software/firmware/src/tasks/ranging/subscription_phase.c +++ b/software/firmware/src/tasks/ranging/subscription_phase.c @@ -11,36 +11,9 @@ static scheduler_phase_t current_phase; static subscription_packet_t subscription_packet; static uint8_t schedule_index, schedule_length; -static uint32_t next_action_timestamp; static uint64_t reference_time; -// Private Helper Functions -------------------------------------------------------------------------------------------- - -static inline scheduler_phase_t start_tx(const char *error_message) -{ - next_action_timestamp += (rand() % (SUBSCRIPTION_TIMEOUT_US - 200)); - dwt_setdelayedtrxtime((uint32_t)((US_TO_DWT(next_action_timestamp) - TX_ANTENNA_DELAY) >> 8) & 0xFFFFFFFE); - if (dwt_starttx(DWT_START_TX_DLY_REF) != DWT_SUCCESS) - { - print(error_message); - return RANGING_ERROR; - } - return SUBSCRIPTION_PHASE; -} - -static inline scheduler_phase_t start_rx(const char *error_message, int mode) -{ - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(next_action_timestamp - RECEIVE_EARLY_START_US)); - if (dwt_rxenable(mode) != DWT_SUCCESS) - { - print(error_message); - return RANGING_ERROR; - } - return SUBSCRIPTION_PHASE; -} - - // Public API Functions ------------------------------------------------------------------------------------------------ void subscription_phase_initialize(const uint8_t *uid) @@ -58,28 +31,31 @@ scheduler_phase_t subscription_phase_begin(uint8_t scheduled_slot, uint8_t sched current_phase = SUBSCRIPTION_PHASE; schedule_index = scheduled_slot; schedule_length = schedule_size; - next_action_timestamp = RECEIVE_EARLY_START_US; reference_time = ((uint64_t)start_delay_dwt) << 8; dwt_setreferencetrxtime(start_delay_dwt); // Reset the necessary Subscription Phase parameters - if (scheduled_slot == UNSCHEDULED_SLOT) + if (schedule_index == UNSCHEDULED_SLOT) { dwt_writetxfctrl(sizeof(subscription_packet_t), 0, 0); - dwt_writetxdata(sizeof(subscription_packet_t) - sizeof(ieee154_footer_t), (uint8_t*)&subscription_packet, 0); - return start_tx("ERROR: Failed to transmit SUBSCRIPTION request packet\n"); + dwt_setdelayedtrxtime((uint32_t)((US_TO_DWT(RECEIVE_EARLY_START_US + (rand() % (SUBSCRIPTION_TIMEOUT_US - 100))) - TX_ANTENNA_DELAY) >> 8) & 0xFFFFFFFE); + if ((dwt_writetxdata(sizeof(subscription_packet_t) - sizeof(ieee154_footer_t), (uint8_t*)&subscription_packet, 0) != DWT_SUCCESS) || (dwt_starttx(DWT_START_TX_DLY_REF) != DWT_SUCCESS)) + print("ERROR: Failed to transmit SUBSCRIPTION request packet\n"); + else + return SUBSCRIPTION_PHASE; } - else if (!scheduled_slot) + else if (!schedule_index) { - // Set up the correct initial antenna and RX timeout duration - ranging_radio_choose_antenna(0); - dwt_setpreambledetecttimeout(0); - dwt_setrxtimeout(DW_TIMEOUT_FROM_US(SUBSCRIPTION_TIMEOUT_US)); dwt_setdelayedtrxtime(0); - return start_rx("ERROR: Unable to start listening for SUBSCRIPTION packets\n", DWT_START_RX_DLY_REF); + 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) + print("ERROR: Unable to start listening for SUBSCRIPTION packets\n"); + else + return SUBSCRIPTION_PHASE; } - // Immediately transition to the Ranging Phase if already scheduled + // Transition to the Ranging Phase at the appropriate future time 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); } @@ -89,12 +65,12 @@ 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(); - return RANGING_ERROR; + return ranging_phase_begin(schedule_index, schedule_length, (uint32_t)((reference_time + US_TO_DWT(SUBSCRIPTION_BROADCAST_PERIOD_US)) >> 8) & 0xFFFFFFFE); } scheduler_phase_t subscription_phase_rx_complete(subscription_packet_t* packet) { - // Ensure that this packet is of the expected type + // Ensure that this packet is of the expected type and schedule the requesting device if (current_phase != SUBSCRIPTION_PHASE) return ranging_phase_rx_complete((ranging_packet_t*)packet); else if (packet->header.msgType != SUBSCRIPTION_PACKET) @@ -102,23 +78,25 @@ scheduler_phase_t subscription_phase_rx_complete(subscription_packet_t* packet) print("ERROR: Received an unexpected message type during SUBSCRIPTION phase...possible network collision\n"); return MESSAGE_COLLISION; } - - // Attempt to schedule the requesting device then re-enable listening - current_phase = RANGING_PHASE; schedule_phase_add_device(packet->header.sourceAddr[0]); - return ranging_phase_begin(schedule_index, schedule_length, (uint32_t)((reference_time + US_TO_DWT(SUBSCRIPTION_BROADCAST_PERIOD_US)) >> 8) & 0xFFFFFFFE); + return subscription_phase_rx_error(); } scheduler_phase_t subscription_phase_rx_error(void) { - // Re-enable listening for additional Subscription packets - uint32_t time_expired_us = DWT_TO_US((uint64_t)(dwt_readsystimestamphi32() - (uint32_t)(reference_time >> 8)) << 8); - if ((time_expired_us + 400) < SUBSCRIPTION_TIMEOUT_US) + // 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)) { - dwt_setrxtimeout(DW_TIMEOUT_FROM_US(SUBSCRIPTION_TIMEOUT_US - time_expired_us)); - start_rx("ERROR: Unable to restart listening for SUBSCRIPTION packets after error\n", DWT_START_RX_IMMEDIATE); - return SUBSCRIPTION_PHASE; + print("INFO: More time left in the Subscription phase...listening again\n"); + dwt_setrxtimeout(DW_TIMEOUT_FROM_US(RECEIVE_EARLY_START_US + SUBSCRIPTION_TIMEOUT_US - time_elapsed_us)); + if (dwt_rxenable(DWT_START_RX_IMMEDIATE) != DWT_SUCCESS) + print("ERROR: Unable to restart listening for SUBSCRIPTION packets\n"); + else + return SUBSCRIPTION_PHASE; } + + // Move on to the Ranging phase 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); } diff --git a/software/firmware/tests/peripherals/test_ranging_radio.c b/software/firmware/tests/peripherals/test_ranging_radio.c index c292ef6e..29cc6cb6 100644 --- a/software/firmware/tests/peripherals/test_ranging_radio.c +++ b/software/firmware/tests/peripherals/test_ranging_radio.c @@ -10,14 +10,14 @@ static uint8_t eui[EUI_LEN], read_buffer[1024]; static void tx_callback(const dwt_cb_data_t *txData) { volatile uint64_t tx_timestamp = ranging_radio_readtxtimestamp(); - print("Callback 'tx_callback' fired at %lu us\n", APP_DEVICETIMEU64_TO_US(tx_timestamp)); + print("Callback 'tx_callback' fired at %lu us\n", DWT_TO_US(tx_timestamp)); } static void rx_done_callback(const dwt_cb_data_t *rxData) { // Check the reception timestamp and re-enable packet reception volatile uint64_t rx_timestamp = ranging_radio_readrxtimestamp(); - print("Callback 'rx_callback' fired at %lu us\n", APP_DEVICETIMEU64_TO_US(rx_timestamp)); + print("Callback 'rx_callback' fired at %lu us\n", DWT_TO_US(rx_timestamp)); ranging_radio_rxenable(DWT_START_RX_IMMEDIATE); dwt_readrxdata(read_buffer, rxData->datalength, 0); print("Message Length: %u, Type: %u, Seq: %u\n", (uint32_t)rxData->datalength, (uint32_t)read_buffer[sizeof(ieee154_header_t)], (uint32_t)read_buffer[2]); @@ -27,7 +27,7 @@ static void rx_error_callback(const dwt_cb_data_t *rxData) { // Re-enable packet reception upon error volatile uint64_t rx_timestamp = ranging_radio_readrxtimestamp(); - print("Callback 'rx_error_callback' fired at %lu us\n", APP_DEVICETIMEU64_TO_US(rx_timestamp)); + print("Callback 'rx_error_callback' fired at %lu us\n", DWT_TO_US(rx_timestamp)); print("Status Flags: %u, RX Flags: %u\n", rxData->status, (uint32_t)rxData->rx_flags); ranging_radio_rxenable(DWT_START_RX_IMMEDIATE); } @@ -57,9 +57,9 @@ void deep_sleep_test(void) void delayed_write_test(void) { // Create a test packet for sending - schedule_packet_t packet = (schedule_packet_t){ .header = { .frameCtrl = { 0x41, 0xC8 }, .seqNum = 0, + schedule_packet_t packet = (schedule_packet_t){ .header = { .frameCtrl = { 0x41, 0x88 }, .msgType = SCHEDULE_PACKET, .panID = { MODULE_PANID & 0xFF, MODULE_PANID >> 8 }, .destAddr = { 0xFF, 0xFF }, .sourceAddr = { 0 } }, - .message_type = SCHEDULE_PACKET, .epoch_time_unix = 12345678, .num_devices = 0, + .sequence_number = 0, .epoch_time_unix = 12345678, .num_devices = 0, .schedule = { 0 }, .footer = { { 0 } } }; uint16_t packet_size = sizeof(packet); diff --git a/software/firmware/tests/tasks/test_ranging_task.c b/software/firmware/tests/tasks/test_ranging_task.c index c0be1541..d63a5209 100644 --- a/software/firmware/tests/tasks/test_ranging_task.c +++ b/software/firmware/tests/tasks/test_ranging_task.c @@ -60,9 +60,6 @@ void TestTask(void *uid) scheduler_stop(); while (is_ranging); xTaskNotify(ranging_task_handle, ROLE_MASTER, eSetValueWithOverwrite); - for (uint8_t i = 3; i < 8; ++i) - scheduler_add_device(i); - scheduler_add_device(0x02); break; } default: @@ -72,13 +69,7 @@ void TestTask(void *uid) } #else if (desired_role == ROLE_MASTER) - { - scheduler_prepare(); xTaskNotify(ranging_task_handle, ROLE_MASTER, eSetValueWithOverwrite); - for (uint8_t i = 3; i < 8; ++i) - scheduler_add_device(i); - scheduler_add_device(0x52); - } else xTaskNotify(ranging_task_handle, ROLE_PARTICIPANT, eSetValueWithOverwrite); vTaskSuspend(test_task_handle);