From bd4a256c9b30a87df114757104c0ed4157e01ad7 Mon Sep 17 00:00:00 2001 From: Will Hedgecock Date: Mon, 6 Nov 2023 16:27:14 -0600 Subject: [PATCH] Finish ranging algorithm --- .../firmware/.settings/language.settings.xml | 2 +- .../firmware/launchConfigs/TestFull.launch | 2 +- .../firmware/launchConfigs/TestRTCSet.launch | 2 +- software/firmware/src/app/app_config.h | 28 +- .../src/peripherals/include/ranging.h | 12 +- .../firmware/src/peripherals/src/ranging.c | 75 +-- .../src/tasks/ranging/computation_phase.c | 84 ++- .../src/tasks/ranging/computation_phase.h | 14 +- .../src/tasks/ranging/ranging_phase.c | 586 +++++++++--------- .../src/tasks/ranging/ranging_phase.h | 8 +- .../src/tasks/ranging/schedule_phase.c | 51 +- .../src/tasks/ranging/schedule_phase.h | 2 +- .../firmware/src/tasks/ranging/scheduler.c | 23 +- .../firmware/src/tasks/ranging/status_phase.c | 170 ++--- .../firmware/src/tasks/ranging/status_phase.h | 4 +- 15 files changed, 459 insertions(+), 604 deletions(-) diff --git a/software/firmware/.settings/language.settings.xml b/software/firmware/.settings/language.settings.xml index 73035b0b..438e4d2e 100644 --- a/software/firmware/.settings/language.settings.xml +++ b/software/firmware/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + diff --git a/software/firmware/launchConfigs/TestFull.launch b/software/firmware/launchConfigs/TestFull.launch index ef325b1c..4ecc747e 100644 --- a/software/firmware/launchConfigs/TestFull.launch +++ b/software/firmware/launchConfigs/TestFull.launch @@ -78,6 +78,6 @@ - + diff --git a/software/firmware/launchConfigs/TestRTCSet.launch b/software/firmware/launchConfigs/TestRTCSet.launch index 33fa981a..36d1f130 100644 --- a/software/firmware/launchConfigs/TestRTCSet.launch +++ b/software/firmware/launchConfigs/TestRTCSet.launch @@ -78,6 +78,6 @@ - + diff --git a/software/firmware/src/app/app_config.h b/software/firmware/src/app/app_config.h index af5798e2..6be09220 100644 --- a/software/firmware/src/app/app_config.h +++ b/software/firmware/src/app/app_config.h @@ -48,11 +48,13 @@ typedef enum { BATTERY_EMPTY = 3200, BATTERY_CRITICAL = 3500, BATTERY_NOMINAL = // DW3000 Ranging Radio Configuration ---------------------------------------------------------------------------------- -#define DW_PREAMBLE_LENGTH DWT_PLEN_256 -#define DW_PAC_SIZE DWT_PAC16 +#define DW_PREAMBLE_LENGTH DWT_PLEN_128 +#define DW_PAC_SIZE DWT_PAC8 #define DW_DATA_RATE DWT_BR_6M8 #define DW_SFD_TYPE DWT_SFD_DW_16 -#define DW_SFD_TO (256 + 1 + 16 - 16) // (Preamble length + 1 + SFD length - PAC size) +#define DW_SFD_TO (128 + 1 + 16 - 8) // (Preamble length + 1 + SFD length - PAC size) +#define DW_PREAMBLE_TIMEOUT (128 / 8) // (Preamble length / PAC size) +#define DW_PREAMBLE_LENGTH_US ((1 + 128 + 16) * 64 / 62.89133858) // (1 + Preamble length + SFD length) * 64 / 62.89133858 // Bluetooth LE Configuration ------------------------------------------------------------------------------------------ @@ -95,32 +97,32 @@ typedef enum { BATTERY_EMPTY = 3200, BATTERY_CRITICAL = 3500, BATTERY_NOMINAL = // Ranging Protocol Configuration -------------------------------------------------------------------------------------- #define RADIO_XMIT_CHANNEL 5 -#define NUM_ANTENNAS 3 -#define RADIO_TX_PLUS_RX_DELAY 32770 +#define NUM_XMIT_ANTENNAS 2 +#define NUM_RCV_ANTENNAS 2 +#define TX_ANTENNA_DELAY 16385 +#define RX_ANTENNA_DELAY 16385 #define MIN_VALID_RANGE_MM (-1000) #define MAX_VALID_RANGE_MM (32*1000) #define SCHEDULING_INTERVAL_US 1000000 #define RADIO_WAKEUP_SAFETY_DELAY_US 5000 -#define RECEIVE_EARLY_START_US 100 +#define RECEIVE_EARLY_START_US ((uint32_t)DW_PREAMBLE_LENGTH_US) #define DEVICE_TIMEOUT_SECONDS 60 #define NETWORK_SEARCH_TIME_SECONDS 3 #define MAX_EMPTY_ROUNDS_BEFORE_STATE_CHANGE 3 -#define SCHEDULE_XMIT_ANTENNA 0 #define SCHEDULE_NUM_TOTAL_BROADCASTS 5 #define SCHEDULE_NUM_MASTER_BROADCASTS 2 #define SCHEDULE_RESEND_INTERVAL_US 1000 #define SCHEDULE_BROADCAST_PERIOD_US (SCHEDULE_NUM_TOTAL_BROADCASTS * SCHEDULE_RESEND_INTERVAL_US) -#define RANGING_NUM_SEQUENCES NUM_ANTENNAS -#define RANGING_BROADCAST_INTERVAL_US 1000 -#define RANGING_TIMEOUT_US (100 + RECEIVE_EARLY_START_US) -#define RANGING_NUM_PACKETS_PER_ITERATION ((3 * NUM_ANTENNAS) + NUM_ANTENNAS) -#define RANGING_ITERATION_INTERVAL_US (RANGING_BROADCAST_INTERVAL_US * RANGING_NUM_PACKETS_PER_ITERATION) +#define RANGING_BROADCAST_INTERVAL_US 600 +#define RANGING_TIMEOUT_US ((uint32_t)DW_PREAMBLE_LENGTH_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) -#define RANGE_STATUS_XMIT_ANTENNA 0 #define RANGE_STATUS_NUM_TOTAL_BROADCASTS 4 #define RANGE_STATUS_RESEND_INTERVAL_US 1000 #define RANGE_STATUS_BROADCAST_PERIOD_US (RANGE_STATUS_NUM_TOTAL_BROADCASTS * RANGE_STATUS_RESEND_INTERVAL_US) diff --git a/software/firmware/src/peripherals/include/ranging.h b/software/firmware/src/peripherals/include/ranging.h index 0f12acdc..2cfad777 100644 --- a/software/firmware/src/peripherals/include/ranging.h +++ b/software/firmware/src/peripherals/include/ranging.h @@ -13,11 +13,10 @@ #define SPEED_OF_LIGHT 299711693.79 // In air @ 22C, 101.325kPa, 50% RH #define MODULE_PANID 0x6611 -#define APP_US_TO_DEVICETIMEU64(_microsecu) ((uint64_t)(((_microsecu) / DWT_TIME_UNITS) / 1000000.0)) -#define APP_DEVICETIMEU64_TO_US(_dw_units) ((uint32_t)(((_dw_units) * DWT_TIME_UNITS) * 1000000.0)) -#define DW_DELAY_FROM_US(_us) ((uint32_t)(APP_US_TO_DEVICETIMEU64((_us)) >> 8)) -#define US_DELAY_FROM_DW(_dwt) (APP_DEVICETIMEU64_TO_US(((uint64_t)(_dwt)) << 8)) -#define DW_TIMEOUT_FROM_US(_us) ((uint32_t)((_us) / (512.0 / 499.2))) +#define US_TO_DWT(_microsec) ((uint64_t)((_microsec) * 499.2 * 128.0)) +#define DWT_TO_US(_dw_units) ((uint64_t)((_dw_units) / (499.2 * 128.0))) +#define DW_DELAY_FROM_US(_us) ((uint32_t)(US_TO_DWT((_us)) >> 8)) +#define DW_TIMEOUT_FROM_US(_us) ((uint32_t)((_us) * 499.2 / 512.0)) // Data structures for 802.15.4 packets -------------------------------------------------------------------------------- @@ -25,7 +24,7 @@ typedef struct __attribute__ ((__packed__)) { uint8_t frameCtrl[2]; - uint8_t seqNum; + uint8_t msgType; uint8_t panID[2]; uint8_t destAddr[2]; uint8_t sourceAddr[2]; @@ -52,7 +51,6 @@ bool ranging_radio_rxenable(int mode); uint64_t ranging_radio_readrxtimestamp(void); uint64_t ranging_radio_readtxtimestamp(void); float ranging_radio_received_signal_level(void); -uint64_t ranging_radio_compute_correction_for_signal_level(float signal_level_dbm); int ranging_radio_time_to_millimeters(double dwtime); #endif // #ifndef __RANGING_HEADER_H__ diff --git a/software/firmware/src/peripherals/src/ranging.c b/software/firmware/src/peripherals/src/ranging.c index dc5c75dd..9a362393 100644 --- a/software/firmware/src/peripherals/src/ranging.c +++ b/software/firmware/src/peripherals/src/ranging.c @@ -37,7 +37,7 @@ static void ranging_radio_isr(void *args) static void ranging_radio_spi_slow(void) { const am_hal_iom_config_t spi_slow_config = { - .eInterfaceMode = AM_HAL_IOM_SPI_MODE, .ui32ClockFreq = AM_HAL_IOM_6MHZ, .eSpiMode = AM_HAL_IOM_SPI_MODE_0, + .eInterfaceMode = AM_HAL_IOM_SPI_MODE, .ui32ClockFreq = 6000000, .eSpiMode = AM_HAL_IOM_SPI_MODE_0, .pNBTxnBuf = NULL, .ui32NBTxnBufLength = 0 }; am_hal_iom_power_ctrl(spi_handle, AM_HAL_SYSCTRL_WAKE, false); am_hal_iom_configure(spi_handle, &spi_slow_config); @@ -47,7 +47,7 @@ static void ranging_radio_spi_slow(void) static void ranging_radio_spi_fast(void) { const am_hal_iom_config_t spi_fast_config = { - .eInterfaceMode = AM_HAL_IOM_SPI_MODE, .ui32ClockFreq = AM_HAL_IOM_24MHZ, .eSpiMode = AM_HAL_IOM_SPI_MODE_0, + .eInterfaceMode = AM_HAL_IOM_SPI_MODE, .ui32ClockFreq = 36000000, .eSpiMode = AM_HAL_IOM_SPI_MODE_0, .pNBTxnBuf = NULL, .ui32NBTxnBufLength = 0 }; am_hal_iom_power_ctrl(spi_handle, AM_HAL_SYSCTRL_WAKE, false); am_hal_iom_configure(spi_handle, &spi_fast_config); @@ -132,7 +132,7 @@ void ranging_radio_init(uint8_t *uid) { // Initialize static variables tx_config_ch5 = (dwt_txconfig_t){ 0x34, 0xFFFFFFFF, 0x0 }; // Recommended: 0xFDFDFDFD - tx_config_ch9 = (dwt_txconfig_t){ 0x34, 0xFEFEFEFE, 0x0 }; // Recommended: 0xFEFEFEFE + tx_config_ch9 = (dwt_txconfig_t){ 0x34, 0xFFFFFFFF, 0x0 }; // Recommended: 0xFEFEFEFE spi_functions = (struct dwt_spi_s){ .readfromspi = readfromspi, .writetospi = writetospi, .writetospiwithcrc = NULL, .setslowrate = ranging_radio_spi_slow, .setfastrate = ranging_radio_spi_fast }; driver_interface = (struct dwt_probe_s){ .dw = NULL, .spi = (void*)&spi_functions, .wakeup_device_with_io = NULL }; @@ -322,6 +322,7 @@ void ranging_radio_reset(void) // Disable double-buffer mode, receive timeouts, and auto-ack mode dwt_setdblrxbuffmode(DBL_BUF_STATE_DIS, DBL_BUF_MODE_MAN); + dwt_setpreambledetecttimeout(0); dwt_enableautoack(0, 0); dwt_setrxtimeout(0); @@ -329,8 +330,8 @@ void ranging_radio_reset(void) dwt_configureframefilter(DWT_FF_ENABLE_802_15_4, DWT_FF_DATA_EN); // Clear the internal TX/RX antenna delays - dwt_settxantennadelay(0); - dwt_setrxantennadelay(0); + dwt_settxantennadelay(TX_ANTENNA_DELAY); + dwt_setrxantennadelay(RX_ANTENNA_DELAY); } void ranging_radio_register_callbacks(dwt_cb_t tx_done, dwt_cb_t rx_done, dwt_cb_t rx_timeout, dwt_cb_t rx_err) @@ -459,69 +460,7 @@ float ranging_radio_received_signal_level(void) return (10.0f * log10f((F1*F1 + F2*F2 + F3*F3) / (N*N))) + (6.0f * D) - A; } -uint64_t ranging_radio_compute_correction_for_signal_level(float signal_level_dbm) -{ - const uint32_t signal_level_inverted = (uint32_t)(-signal_level_dbm); - switch (signal_level_inverted) - { - case 61: - case 62: - return (uint64_t)(-0.110f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 63: - case 64: - return (uint64_t)(-0.105f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 65: - case 66: - return (uint64_t)(-0.100f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 67: - case 68: - return (uint64_t)(-0.093f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 69: - case 70: - return (uint64_t)(-0.082f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 71: - case 72: - return (uint64_t)(-0.069f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 73: - case 74: - return (uint64_t)(-0.051f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 75: - case 76: - return (uint64_t)(-0.027f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 77: - case 78: - return 0; - case 79: - case 80: - return (uint64_t)(0.021f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 81: - case 82: - return (uint64_t)(0.035f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 83: - case 84: - return (uint64_t)(0.042f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 85: - case 86: - return (uint64_t)(0.049f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 87: - case 88: - return (uint64_t)(0.062f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 89: - case 90: - return (uint64_t)(0.071f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 91: - case 92: - return (uint64_t)(0.076f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - case 93: - case 94: - return (uint64_t)(0.081f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - default: - return (signal_level_inverted < 61) ? (uint64_t)(-0.11f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)) : - (uint64_t)(0.081f / (SPEED_OF_LIGHT * DWT_TIME_UNITS)); - } -} - int ranging_radio_time_to_millimeters(double dwtime) { - return (int)((dwtime - RADIO_TX_PLUS_RX_DELAY) * SPEED_OF_LIGHT * DWT_TIME_UNITS * 1000.0); + return (int)(dwtime * SPEED_OF_LIGHT * DWT_TIME_UNITS * 1000.0); } diff --git a/software/firmware/src/tasks/ranging/computation_phase.c b/software/firmware/src/tasks/ranging/computation_phase.c index 5c71f106..f52fd217 100644 --- a/software/firmware/src/tasks/ranging/computation_phase.c +++ b/software/firmware/src/tasks/ranging/computation_phase.c @@ -7,7 +7,7 @@ // Static Global Variables --------------------------------------------------------------------------------------------- static ranging_state_t state; -static int distances_millimeters[RANGING_NUM_SEQUENCES]; +static int distances_millimeters[RANGING_NUM_SEQUENCES_PER_RANGE]; // Private Helper Functions -------------------------------------------------------------------------------------------- @@ -37,64 +37,62 @@ void reset_computation_phase(void) memset(&state, 0, sizeof(state)); } -void add_roundtrip0_times(uint8_t eui, uint8_t sequence_number, uint32_t poll_rx_time) +void add_ranging_times_poll_tx(uint8_t eui, uint8_t sequence_number, uint32_t tx_time) { bool response_found = false; for (uint8_t i = 0; i < state.num_responses; ++i) if (state.responses[i].device_eui == eui) { - state.responses[i].poll_rx_times[sequence_number] = poll_rx_time; + state.responses[i].poll_tx_times[sequence_number] = tx_time; response_found = true; break; } if (!response_found) { state.responses[state.num_responses].device_eui = eui; - state.responses[state.num_responses].poll_rx_times[sequence_number] = poll_rx_time; + state.responses[state.num_responses].poll_tx_times[sequence_number] = tx_time; ++state.num_responses; } } -void add_roundtrip1_times(uint8_t eui, uint8_t sequence_number, uint32_t poll_tx_time, uint32_t resp_rx_time, uint32_t final_tx_time) +void add_ranging_times_poll_rx(uint8_t eui, uint8_t sequence_number, uint32_t rx_time) { - bool response_found = false; - for (uint8_t i = 0; i < state.num_responses; ++i) - if (state.responses[i].device_eui == eui) - { - state.responses[i].poll_tx_times[sequence_number] = poll_tx_time; - state.responses[i].resp_rx_times[sequence_number] = resp_rx_time; - state.responses[i].final_tx_times[sequence_number] = final_tx_time; - response_found = true; - break; - } - if (!response_found) - { - state.responses[state.num_responses].device_eui = eui; - state.responses[state.num_responses].poll_tx_times[sequence_number] = poll_tx_time; - state.responses[state.num_responses].resp_rx_times[sequence_number] = resp_rx_time; - state.responses[state.num_responses].final_tx_times[sequence_number] = final_tx_time; - ++state.num_responses; - } + if (state.num_responses && + (state.responses[state.num_responses-1].device_eui == eui) && + (state.responses[state.num_responses-1].poll_tx_times[sequence_number])) + state.responses[state.num_responses-1].poll_rx_times[sequence_number] = rx_time; } -void add_roundtrip2_times(uint8_t eui, uint8_t sequence_number, uint32_t resp_tx_time, uint32_t final_rx_time) +void add_ranging_times_response_tx(uint8_t eui, uint8_t sequence_number, uint32_t tx_time) { - bool response_found = false; - for (uint8_t i = 0; i < state.num_responses; ++i) - if (state.responses[i].device_eui == eui) - { - state.responses[i].resp_tx_times[sequence_number] = resp_tx_time; - state.responses[i].final_rx_times[sequence_number] = final_rx_time; - response_found = true; - break; - } - if (!response_found) - { - state.responses[state.num_responses].device_eui = eui; - state.responses[state.num_responses].resp_tx_times[sequence_number] = resp_tx_time; - state.responses[state.num_responses].final_rx_times[sequence_number] = final_rx_time; - ++state.num_responses; - } + if (state.num_responses && + (state.responses[state.num_responses-1].device_eui == eui) && + (state.responses[state.num_responses-1].poll_rx_times[sequence_number])) + state.responses[state.num_responses-1].resp_tx_times[sequence_number] = tx_time; +} + +void add_ranging_times_response_rx(uint8_t eui, uint8_t sequence_number, uint32_t rx_time) +{ + if (state.num_responses && + (state.responses[state.num_responses-1].device_eui == eui) && + (state.responses[state.num_responses-1].resp_tx_times[sequence_number])) + state.responses[state.num_responses-1].resp_rx_times[sequence_number] = rx_time; +} + +void add_ranging_times_final_tx(uint8_t eui, uint8_t sequence_number, uint32_t tx_time) +{ + if (state.num_responses && + (state.responses[state.num_responses-1].device_eui == eui) && + (state.responses[state.num_responses-1].resp_rx_times[sequence_number])) + state.responses[state.num_responses-1].final_tx_times[sequence_number] = tx_time; +} + +void add_ranging_times_final_rx(uint8_t eui, uint8_t sequence_number, uint32_t rx_time) +{ + if (state.num_responses && + (state.responses[state.num_responses-1].device_eui == eui) && + (state.responses[state.num_responses-1].final_tx_times[sequence_number])) + state.responses[state.num_responses-1].final_rx_times[sequence_number] = rx_time; } void compute_ranges(uint8_t *ranging_results) @@ -107,8 +105,8 @@ void compute_ranges(uint8_t *ranging_results) // Calculate the device distances using symmetric two-way TOFs uint8_t num_valid_distances = 0; memset(distances_millimeters, 0, sizeof(distances_millimeters)); - for (uint8_t i = 0; i < RANGING_NUM_SEQUENCES; ++i) - if (state.responses[dev_index].resp_rx_times[i] && state.responses[dev_index].final_rx_times[i]) + for (uint8_t i = 0; i < RANGING_NUM_SEQUENCES_PER_RANGE; ++i) + if (state.responses[dev_index].final_rx_times[i]) { // Compute the device range from the two-way round-trip times const double Ra = state.responses[dev_index].resp_rx_times[i] - state.responses[dev_index].poll_tx_times[i]; @@ -122,7 +120,7 @@ void compute_ranges(uint8_t *ranging_results) if ((distance_millimeters >= MIN_VALID_RANGE_MM) && (distance_millimeters <= MAX_VALID_RANGE_MM)) insert_sorted(distances_millimeters, distance_millimeters, num_valid_distances++); else - print("WARNING: Disregarding range to EUI %u for subsequence #%u: %d\n", (uint32_t)state.responses[dev_index].device_eui, i, distance_millimeters); + print("WARNING: Disregarding range to EUI 0x%02X for subsequence #%u: %d\n", (uint32_t)state.responses[dev_index].device_eui, i, distance_millimeters); } // Skip this device if too few ranging packets were received diff --git a/software/firmware/src/tasks/ranging/computation_phase.h b/software/firmware/src/tasks/ranging/computation_phase.h index 7cf8ca60..0cab3b71 100644 --- a/software/firmware/src/tasks/ranging/computation_phase.h +++ b/software/firmware/src/tasks/ranging/computation_phase.h @@ -11,8 +11,9 @@ typedef struct { uint8_t device_eui; - uint32_t poll_tx_times[RANGING_NUM_SEQUENCES], poll_rx_times[RANGING_NUM_SEQUENCES], resp_tx_times[RANGING_NUM_SEQUENCES]; - uint32_t resp_rx_times[RANGING_NUM_SEQUENCES], final_tx_times[RANGING_NUM_SEQUENCES], final_rx_times[RANGING_NUM_SEQUENCES]; + uint32_t poll_tx_times[RANGING_NUM_SEQUENCES_PER_RANGE], poll_rx_times[RANGING_NUM_SEQUENCES_PER_RANGE]; + uint32_t resp_tx_times[RANGING_NUM_SEQUENCES_PER_RANGE], resp_rx_times[RANGING_NUM_SEQUENCES_PER_RANGE]; + uint32_t final_tx_times[RANGING_NUM_SEQUENCES_PER_RANGE], final_rx_times[RANGING_NUM_SEQUENCES_PER_RANGE]; } ranging_device_state_t; typedef struct @@ -25,9 +26,12 @@ typedef struct // Public API ---------------------------------------------------------------------------------------------------------- void reset_computation_phase(void); -void add_roundtrip0_times(uint8_t eui, uint8_t sequence_number, uint32_t poll_rx_time); -void add_roundtrip1_times(uint8_t eui, uint8_t sequence_number, uint32_t poll_tx_time, uint32_t resp_rx_time, uint32_t final_tx_time); -void add_roundtrip2_times(uint8_t eui, uint8_t sequence_number, uint32_t resp_tx_time, uint32_t final_rx_time); +void add_ranging_times_poll_tx(uint8_t eui, uint8_t sequence_number, uint32_t tx_time); +void add_ranging_times_poll_rx(uint8_t eui, uint8_t sequence_number, uint32_t rx_time); +void add_ranging_times_response_tx(uint8_t eui, uint8_t sequence_number, uint32_t tx_time); +void add_ranging_times_response_rx(uint8_t eui, uint8_t sequence_number, uint32_t rx_time); +void add_ranging_times_final_tx(uint8_t eui, uint8_t sequence_number, uint32_t tx_time); +void add_ranging_times_final_rx(uint8_t eui, uint8_t sequence_number, uint32_t rx_time); void compute_ranges(uint8_t *ranging_results); bool responses_received(void); diff --git a/software/firmware/src/tasks/ranging/ranging_phase.c b/software/firmware/src/tasks/ranging/ranging_phase.c index a3b3e69d..d84b88be 100644 --- a/software/firmware/src/tasks/ranging/ranging_phase.c +++ b/software/firmware/src/tasks/ranging/ranging_phase.c @@ -10,89 +10,231 @@ static scheduler_phase_t current_phase; static ranging_packet_t ranging_packet; -static uint8_t responder_slot_index, scheduled_slot, total_num_slots, antenna_index; -static int32_t responder_slots[MAX_NUM_RANGING_DEVICES], current_slot; -static int32_t initiator_slot_first, initiator_slot_last; -static uint32_t num_sub_slots, last_rx_delay_time_us; -static bool last_rx_relative_to_transmit; +static uint8_t initiator_index, responder_index, my_index; +static uint8_t initiator_antenna, responder_antenna, schedule_length; +static uint32_t next_action_timestamp, ranging_phase_duration; +static uint64_t reference_time; -// Public functions ---------------------------------------------------------------------------------------------------- +// Private Helper Functions -------------------------------------------------------------------------------------------- + +static inline scheduler_phase_t start_tx(const char *error_message) +{ + dwt_setdelayedtrxtime((uint32_t)((US_TO_DWT(next_action_timestamp) - TX_ANTENNA_DELAY) >> 8) & 0xFFFFFFFE); + 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 RANGING_PHASE; +} + +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) + { + print(error_message); + return RANGING_ERROR; + } + return RANGING_PHASE; +} + +static scheduler_phase_t initiator_handle_tx(void) +{ + // Listen for an incoming RESPONSE or SUMMARY packet + next_action_timestamp += RANGING_BROADCAST_INTERVAL_US; + return start_rx("ERROR: Unable to start listening for RANGING packets after INITIATOR TX\n"); +} + +static scheduler_phase_t initiator_handle_rx(ranging_packet_t *packet) +{ + // Verify the received packet sequence number + next_action_timestamp += RANGING_BROADCAST_INTERVAL_US; + ranging_packet.sequence_number = packet->sequence_number + 1; + if ((packet->sequence_number & 0x03) == 1) + { + // Update the packet round-trip times and transit the FINAL packet + add_ranging_times_poll_tx(packet->header.sourceAddr[0], packet->sequence_number >> 2, ranging_packet.tx_time); + add_ranging_times_poll_rx(packet->header.sourceAddr[0], packet->sequence_number >> 2, packet->rx_time); + add_ranging_times_response_tx(packet->header.sourceAddr[0], packet->sequence_number >> 2, packet->tx_time); + ranging_packet.rx_time = (uint32_t)ranging_radio_readrxtimestamp(); + ranging_packet.tx_time = (uint32_t)(reference_time + US_TO_DWT(next_action_timestamp)) & 0xFFFFFE00; + add_ranging_times_response_rx(packet->header.sourceAddr[0], packet->sequence_number >> 2, ranging_packet.rx_time); + add_ranging_times_final_tx(packet->header.sourceAddr[0], packet->sequence_number >> 2, ranging_packet.tx_time); + return start_tx("ERROR: Failed to transmit RANGING FINAL packet\n"); + } + else if ((packet->sequence_number & 0x03) == 3) + { + // Update the packet round-trip times and the antenna settings + add_ranging_times_final_rx(packet->header.sourceAddr[0], packet->sequence_number >> 2, packet->rx_time); + responder_antenna = (responder_antenna + 1) % NUM_RCV_ANTENNAS; + if (responder_antenna == 0) + { + initiator_antenna = (initiator_antenna + 1) % NUM_XMIT_ANTENNAS; + ranging_radio_choose_antenna(initiator_antenna); + if (initiator_antenna == 0) + { + ranging_packet.sequence_number = 0; + responder_index = (responder_index + 1) % schedule_length; + if (responder_index == 0) + { + // Move to the Range Status Phase of the ranging protocol + current_phase = RANGE_STATUS_PHASE; + uint32_t remaining = schedule_length - my_index - 1; + next_action_timestamp += ((remaining * (remaining - 1) / 2) * RANGING_US_PER_RANGE); + return status_phase_begin(my_index, schedule_length, (uint32_t)((reference_time + US_TO_DWT(next_action_timestamp - RECEIVE_EARLY_START_US)) >> 8) & 0xFFFFFFFE); + } + } + } + + // Transmit the next POLL packet + 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\n"); + } + print("ERROR: Received an unexpected RANGING sequence number for initiator...possible network collision\n"); + return MESSAGE_COLLISION; +} + +static scheduler_phase_t responder_handle_tx(void) +{ + // Verify the received packet sequence number + next_action_timestamp += RANGING_BROADCAST_INTERVAL_US; + if ((ranging_packet.sequence_number & 0x03) == 3) + { + // Update the initiator and responder antenna settings + responder_antenna = (responder_antenna + 1) % NUM_RCV_ANTENNAS; + ranging_radio_choose_antenna(responder_antenna); + if (responder_antenna == 0) + { + initiator_antenna = (initiator_antenna + 1) % NUM_XMIT_ANTENNAS; + if (initiator_antenna == 0) + { + // Update the initiator and responder active indices + responder_index = (responder_index + 1) % schedule_length; + if (responder_index == 0) + { + initiator_index += 1; + if (initiator_index == (schedule_length - 1)) + { + // Move to the Range Status Phase of the ranging protocol + current_phase = RANGE_STATUS_PHASE; + return status_phase_begin(my_index, schedule_length, (uint32_t)((reference_time + US_TO_DWT(next_action_timestamp - RECEIVE_EARLY_START_US)) >> 8) & 0xFFFFFFFE); + } + else + { + next_action_timestamp += (uint32_t)(my_index - initiator_index - 1) * RANGING_US_PER_RANGE; + responder_index = my_index; + } + } + else + { + // Determine if we are the next initiator + if (my_index == ++initiator_index) + { + // Transmit the next POLL packet at the correct time + ranging_packet.sequence_number = 0; + next_action_timestamp += (schedule_length - responder_index) * RANGING_US_PER_RANGE; + ranging_packet.tx_time = (uint32_t)(reference_time + US_TO_DWT(next_action_timestamp)) & 0xFFFFFE00; + responder_index = my_index + 1; + return start_tx("ERROR: Failed to transmit next RANGING POLL packet\n"); + } + else + { + next_action_timestamp += (uint32_t)(schedule_length - responder_index) * RANGING_US_PER_RANGE; + responder_index = my_index; + } + } + } + } + } + + // Enter RX mode at the appropriate time + return start_rx("ERROR: Unable to start listening for RANGING packets after RESPONDER TX\n"); +} + +static scheduler_phase_t responder_handle_rx(ranging_packet_t *packet) +{ + // Update the ranging packet contents for the upcoming transmission + next_action_timestamp += RANGING_BROADCAST_INTERVAL_US; + ranging_packet.sequence_number = packet->sequence_number + 1; + ranging_packet.rx_time = (uint32_t)ranging_radio_readrxtimestamp(); + ranging_packet.tx_time = (uint32_t)(reference_time + US_TO_DWT(next_action_timestamp)) & 0xFFFFFE00; + + // Update the packet round-trip times + if ((packet->sequence_number & 0x03) == 0) + { + add_ranging_times_poll_tx(packet->header.sourceAddr[0], packet->sequence_number >> 2, packet->tx_time); + add_ranging_times_poll_rx(packet->header.sourceAddr[0], packet->sequence_number >> 2, ranging_packet.rx_time); + add_ranging_times_response_tx(packet->header.sourceAddr[0], packet->sequence_number >> 2, ranging_packet.tx_time); + return start_tx("ERROR: Failed to transmit next RANGING RESPONSE packet\n"); + } + else if ((packet->sequence_number & 0x03) == 2) + { + add_ranging_times_response_rx(packet->header.sourceAddr[0], packet->sequence_number >> 2, packet->rx_time); + add_ranging_times_final_tx(packet->header.sourceAddr[0], packet->sequence_number >> 2, packet->tx_time); + add_ranging_times_final_rx(packet->header.sourceAddr[0], packet->sequence_number >> 2, ranging_packet.rx_time); + return start_tx("ERROR: Failed to transmit next RANGING SUMMARY packet\n"); + } + print("ERROR: Received an unexpected RANGING sequence number for responder...possible network collision\n"); + return MESSAGE_COLLISION; +} + + +// Public Functions ---------------------------------------------------------------------------------------------------- void ranging_phase_initialize(const uint8_t *uid) { // Initialize all Ranging Phase parameters - ranging_packet = (ranging_packet_t){ .header = { .frameCtrl = { 0x41, 0x98 }, .seqNum = 0, + ranging_packet = (ranging_packet_t){ .header = { .frameCtrl = { 0x41, 0x88 }, .msgType = RANGING_PACKET, .panID = { MODULE_PANID & 0xFF, MODULE_PANID >> 8 }, .destAddr = { 0xFF, 0xFF }, .sourceAddr = { 0 } }, - .message_type = RANGING_PACKET, .poll_time = 0, .resp_time = 0, .final_time = 0, .footer = { { 0 } } }; + .sequence_number = 0, .tx_time = 0, .rx_time = 0, .footer = { { 0 } } }; memcpy(ranging_packet.header.sourceAddr, uid, sizeof(ranging_packet.header.sourceAddr)); - scheduled_slot = 0xFF; } -scheduler_phase_t ranging_phase_begin(uint8_t ranging_slot, uint8_t num_slots, uint32_t start_delay_us, bool start_relative_to_transmit) +scheduler_phase_t ranging_phase_begin(uint8_t scheduled_slot, uint8_t schedule_size, uint32_t start_delay_dwt) { // Ensure there are at least two devices to begin ranging reset_computation_phase(); - if (num_slots < 2) + if (schedule_size < 2) return RANGE_COMPUTATION_PHASE; // Reset the necessary Ranging Phase parameters current_phase = RANGING_PHASE; - responder_slot_index = antenna_index = 0; - num_sub_slots = (uint32_t)num_slots * (num_slots - 1) / 2; - scheduled_slot = ranging_slot; - total_num_slots = num_slots; - - // Compute the initiator time slots for the device - if ((ranging_slot + 1) < num_slots) - { - initiator_slot_first = 0; - for (uint8_t i = 1; i <= ranging_slot; ++i) - initiator_slot_first += (num_slots - i); - initiator_slot_last = initiator_slot_first + (num_slots - ranging_slot - 2); - } - else - initiator_slot_first = initiator_slot_last = -1; + my_index = scheduled_slot; + schedule_length = schedule_size; + initiator_antenna = responder_antenna = 0; + next_action_timestamp = RECEIVE_EARLY_START_US; + dwt_writetxfctrl(sizeof(ranging_packet_t), 0, 1); + dwt_writetxdata(sizeof(ranging_packet_t) - sizeof(ieee154_footer_t), (uint8_t*)&ranging_packet, 0); + initiator_index = 0; + responder_index = 1; - // Compute the responder time slots for the device - responder_slots[0] = (int32_t)ranging_slot - 1; - for (uint8_t i = 1; i < ranging_slot; ++i) - responder_slots[i] = responder_slots[i-1] + (num_slots - i - 1); - for (uint8_t i = ranging_slot; i < MAX_NUM_RANGING_DEVICES; ++i) - responder_slots[i] = -1; + // Initialize the Ranging Phase start time for calculating timing offsets + reference_time = ((uint64_t)start_delay_dwt) << 8; + ranging_phase_duration = ((uint32_t)schedule_size * (schedule_size - 1) / 2) * RANGING_US_PER_RANGE; + dwt_setreferencetrxtime(start_delay_dwt); // Set up the correct initial antenna and RX timeout duration ranging_radio_choose_antenna(0); + dwt_setpreambledetecttimeout(DW_PREAMBLE_TIMEOUT); dwt_setrxtimeout(DW_TIMEOUT_FROM_US(RANGING_TIMEOUT_US)); // Begin transmission or reception depending on the scheduled time slot - if (ranging_slot == 0) + if (my_index == 0) { - current_slot = 0; - ranging_packet.header.seqNum = 0; - dwt_writetxfctrl(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), 0, 1); - dwt_writetxdata(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), (uint8_t*)&ranging_packet, 0); - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(start_delay_us)); - if (dwt_starttx(start_relative_to_transmit ? DWT_START_TX_DLY_TS : DWT_START_TX_DLY_RS) != DWT_SUCCESS) - { - print("ERROR: Failed to transmit RANGING REQUEST packet\n"); - return RANGING_ERROR; - } + ranging_packet.sequence_number = 0; + ranging_packet.tx_time = (uint32_t)(reference_time + US_TO_DWT(next_action_timestamp)) & 0xFFFFFE00; + return start_tx("ERROR: Failed to transmit initial RANGING POLL packet\n"); } else { - ranging_packet.header.seqNum = 11; - current_slot = (uint8_t)responder_slots[0]; - last_rx_relative_to_transmit = start_relative_to_transmit; - last_rx_delay_time_us = start_delay_us + ((uint32_t)(responder_slots[0]) * RANGING_ITERATION_INTERVAL_US) - RECEIVE_EARLY_START_US; - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(last_rx_delay_time_us)); - if (!ranging_radio_rxenable(start_relative_to_transmit ? DWT_START_RX_DLY_TS : DWT_START_RX_DLY_RS)) - { - print("ERROR: Unable to start listening for RANGING REQUEST packets\n"); - return RANGING_ERROR; - } + responder_index = my_index; + ranging_packet.sequence_number = 0x03; + 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_PHASE; + return RANGING_ERROR; } scheduler_phase_t ranging_phase_tx_complete(void) @@ -100,63 +242,7 @@ scheduler_phase_t ranging_phase_tx_complete(void) // Forward this request to the next phase if not currently in the Ranging Phase if (current_phase != RANGING_PHASE) return status_phase_tx_complete(); - - // Switch to the next antenna when appropriate - if ((ranging_packet.header.seqNum == 3) || (ranging_packet.header.seqNum == 7)) - ranging_radio_choose_antenna(++antenna_index); - - // Handle the ranging phase based on the sequence number of the packet that was just transmitted - if (ranging_packet.header.seqNum == 11) - { - // Finished current ranging iteration...continue to next time slot - antenna_index = 0; - ++responder_slot_index; - ranging_radio_choose_antenna(0); - if ((++current_slot <= initiator_slot_first) && (initiator_slot_first < responder_slots[responder_slot_index])) - { - ranging_packet.header.seqNum = 0; - dwt_writetxfctrl(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), 0, 1); - dwt_writetxdata(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), (uint8_t*)&ranging_packet, 0); - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(RANGING_BROADCAST_INTERVAL_US + ((initiator_slot_first - current_slot) * RANGING_ITERATION_INTERVAL_US))); - current_slot = initiator_slot_first; - if (dwt_starttx(DWT_START_TX_DLY_TS) != DWT_SUCCESS) - { - print("ERROR: Failed to transmit initial RANGING REQUEST packet\n"); - return RANGING_ERROR; - } - return RANGING_PHASE; - } - else if (current_slot <= responder_slots[responder_slot_index]) - { - last_rx_relative_to_transmit = true; - last_rx_delay_time_us = RANGING_BROADCAST_INTERVAL_US + ((uint32_t)(responder_slots[responder_slot_index] - current_slot) * RANGING_ITERATION_INTERVAL_US) - RECEIVE_EARLY_START_US; - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(last_rx_delay_time_us)); - current_slot = (uint8_t)responder_slots[responder_slot_index]; - if (!ranging_radio_rxenable(DWT_START_RX_DLY_TS)) - { - print("ERROR: Unable to start listening for incoming RANGING REQUEST packets\n"); - return RANGING_ERROR; - } - return RANGING_PHASE; - } - } - else - { - // Finished transmitting non-terminal packet...start listening for incoming responses - last_rx_relative_to_transmit = true; - last_rx_delay_time_us = RANGING_BROADCAST_INTERVAL_US - RECEIVE_EARLY_START_US; - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(last_rx_delay_time_us)); - if (!ranging_radio_rxenable(DWT_START_RX_DLY_TS)) - { - print("ERROR: Unable to start listening for next RANGING REQUEST packets\n"); - return RANGING_ERROR; - } - return RANGING_PHASE; - } - - // Move to the Status Phase of the ranging protocol - current_phase = RANGE_STATUS_PHASE; - return status_phase_begin(scheduled_slot, total_num_slots, RANGING_BROADCAST_INTERVAL_US + ((num_sub_slots - current_slot) * RANGING_ITERATION_INTERVAL_US), true); + return (initiator_index == my_index) ? initiator_handle_tx() : responder_handle_tx(); } scheduler_phase_t ranging_phase_rx_complete(ranging_packet_t* packet) @@ -164,87 +250,12 @@ scheduler_phase_t ranging_phase_rx_complete(ranging_packet_t* packet) // Forward this request to the next phase if not currently in the Ranging Phase if (current_phase != RANGING_PHASE) return status_phase_rx_complete((status_success_packet_t*)packet); - else if (packet->message_type != RANGING_PACKET) + else if (packet->header.msgType != RANGING_PACKET) { print("ERROR: Received an unexpected message type during RANGING phase...possible network collision\n"); return MESSAGE_COLLISION; } - - // Switch to the next antenna when appropriate - if ((packet->header.seqNum == 3) || (packet->header.seqNum == 7)) - ranging_radio_choose_antenna(++antenna_index); - - // Compute the roundtrip transmission time when appropriate - if ((packet->header.seqNum == 0) || (packet->header.seqNum == 4) || (packet->header.seqNum == 8)) - { - uint32_t rx_timestamp = (uint32_t)ranging_radio_readrxtimestamp(); - ranging_packet.poll_time = rx_timestamp; - add_roundtrip0_times(packet->header.sourceAddr[0], packet->header.seqNum / 4, rx_timestamp); - } - else if ((packet->header.seqNum == 1) || (packet->header.seqNum == 5) || (packet->header.seqNum == 9)) - { - uint64_t rx_timestamp = ranging_radio_readrxtimestamp(); - ranging_packet.poll_time = (uint32_t)ranging_radio_readtxtimestamp(); - ranging_packet.resp_time = (uint32_t)rx_timestamp; - ranging_packet.final_time = (uint32_t)(rx_timestamp + APP_US_TO_DEVICETIMEU64(RANGING_BROADCAST_INTERVAL_US)) & 0xFFFFFE00UL; - add_roundtrip0_times(packet->header.sourceAddr[0], (packet->header.seqNum - 1) / 4, packet->poll_time); - add_roundtrip1_times(packet->header.sourceAddr[0], (packet->header.seqNum - 1) / 4, ranging_packet.poll_time, ranging_packet.resp_time, ranging_packet.final_time); - } - else if ((packet->header.seqNum == 2) || (packet->header.seqNum == 6) || (packet->header.seqNum == 10)) - { - ranging_packet.resp_time = (uint32_t)ranging_radio_readtxtimestamp(); - ranging_packet.final_time = (uint32_t)ranging_radio_readrxtimestamp(); - add_roundtrip1_times(packet->header.sourceAddr[0], (packet->header.seqNum - 2) / 4, packet->poll_time, packet->resp_time, packet->final_time); - add_roundtrip2_times(packet->header.sourceAddr[0], (packet->header.seqNum - 2) / 4, ranging_packet.resp_time, ranging_packet.final_time); - } - else if ((packet->header.seqNum == 3) || (packet->header.seqNum == 7) || (packet->header.seqNum == 11)) - add_roundtrip2_times(packet->header.sourceAddr[0], (packet->header.seqNum - 3) / 4, packet->resp_time, packet->final_time); - - // Handle the ranging phase based on the sequence number of the packet that was just received - if (packet->header.seqNum == 11) - { - // Finished current ranging iteration...continue to next time slot - ranging_radio_choose_antenna(0); - antenna_index = ranging_packet.header.seqNum = 0; - if (++current_slot <= initiator_slot_last) - { - dwt_writetxfctrl(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), 0, 1); - dwt_writetxdata(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), (uint8_t*)&ranging_packet, 0); - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(RANGING_BROADCAST_INTERVAL_US)); - if (dwt_starttx(DWT_START_TX_DLY_RS) != DWT_SUCCESS) - { - print("ERROR: Failed to transmit next RANGING REQUEST packet\n"); - return RANGING_ERROR; - } - return RANGING_PHASE; - } - } - else - { - // Received non-terminal packet...send response - ranging_packet.header.seqNum = packet->header.seqNum + 1; - if ((packet->header.seqNum == 3) || (packet->header.seqNum == 7)) - { - dwt_writetxfctrl(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), 0, 1); - dwt_writetxdata(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), (uint8_t*)&ranging_packet, 0); - } - else - { - dwt_writetxfctrl(sizeof(ranging_packet_t), 0, 1); - dwt_writetxdata(sizeof(ranging_packet_t), (uint8_t*)&ranging_packet, 0); - } - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(RANGING_BROADCAST_INTERVAL_US)); - if (dwt_starttx(DWT_START_TX_DLY_RS) != DWT_SUCCESS) - { - print("ERROR: Failed to transmit RANGING RESPONSE packet with sequence number %u\n", (uint32_t)ranging_packet.header.seqNum); - return RANGING_ERROR; - } - return RANGING_PHASE; - } - - // Move to the Status Phase of the ranging protocol - current_phase = RANGE_STATUS_PHASE; - return status_phase_begin(scheduled_slot, total_num_slots, RANGING_BROADCAST_INTERVAL_US + ((num_sub_slots - current_slot) * RANGING_ITERATION_INTERVAL_US), false); + return (initiator_index == my_index) ? initiator_handle_rx(packet) : responder_handle_rx(packet); } scheduler_phase_t ranging_phase_rx_error(void) @@ -254,140 +265,97 @@ scheduler_phase_t ranging_phase_rx_error(void) return status_phase_rx_error(); // Handle RX errors differently depending on which sequence number experienced the problem - if ((ranging_packet.header.seqNum == 8) || (ranging_packet.header.seqNum == 10)) - { - // We are the initiator and did not receive a response on the final antenna...move to next time slot - antenna_index = 0; - ranging_radio_choose_antenna(0); - uint32_t delay_time_us = (uint32_t)(12 - ranging_packet.header.seqNum) * RANGING_BROADCAST_INTERVAL_US; - if (++current_slot <= initiator_slot_last) - { - ranging_packet.header.seqNum = 0; - dwt_writetxfctrl(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), 0, 1); - dwt_writetxdata(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), (uint8_t*)&ranging_packet, 0); - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(delay_time_us)); - if (dwt_starttx(DWT_START_TX_DLY_TS) != DWT_SUCCESS) - { - print("ERROR: Failed to transmit RANGING REQUEST packet after error\n"); - return RANGING_ERROR; - } - return RANGING_PHASE; - } - else - { - // Move to the Range Status Phase of the ranging protocol - current_phase = RANGE_STATUS_PHASE; - return status_phase_begin(scheduled_slot, total_num_slots, delay_time_us + ((num_sub_slots - current_slot) * RANGING_ITERATION_INTERVAL_US), true); - } - } - else if ((ranging_packet.header.seqNum == 0) || (ranging_packet.header.seqNum == 2) || (ranging_packet.header.seqNum == 4) || (ranging_packet.header.seqNum == 6)) - { - // We are the initiator and did not receive a response on the non-final antenna, skip to the next antenna - ranging_radio_choose_antenna(++antenna_index); - uint8_t next_sequence_num = 4 * ((4 + ranging_packet.header.seqNum) / 4); - uint32_t delay_time_us = (uint32_t)(next_sequence_num - ranging_packet.header.seqNum) * RANGING_BROADCAST_INTERVAL_US; - ranging_packet.header.seqNum = next_sequence_num; - dwt_writetxfctrl(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), 0, 1); - dwt_writetxdata(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), (uint8_t*)&ranging_packet, 0); - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(delay_time_us)); - if (dwt_starttx(DWT_START_TX_DLY_TS) != DWT_SUCCESS) - { - print("ERROR: Failed to transmit next RANGING REQUEST packet after error\n"); - return RANGING_ERROR; - } - return RANGING_PHASE; - } - else if ((ranging_packet.header.seqNum == 7) || (ranging_packet.header.seqNum == 9)) + if (((ranging_packet.sequence_number & 0x03) == 0) || ((ranging_packet.sequence_number & 0x03) == 2)) { - // We are the responder and did not receive a response on the final antenna...move to next time slot - antenna_index = 0; - ++responder_slot_index; - ranging_radio_choose_antenna(0); - uint32_t delay_time_us = (uint32_t)(12 - ranging_packet.header.seqNum) * RANGING_BROADCAST_INTERVAL_US; - if ((++current_slot <= initiator_slot_first) && (initiator_slot_first < responder_slots[responder_slot_index])) - { - ranging_packet.header.seqNum = 0; - dwt_writetxfctrl(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), 0, 1); - dwt_writetxdata(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), (uint8_t*)&ranging_packet, 0); - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(delay_time_us + ((initiator_slot_first - current_slot) * RANGING_ITERATION_INTERVAL_US))); - current_slot = initiator_slot_first; - if (dwt_starttx(DWT_START_TX_DLY_TS) != DWT_SUCCESS) - { - print("ERROR: Failed to transmit initial RANGING REQUEST packet after error\n"); - return RANGING_ERROR; - } - return RANGING_PHASE; - } - else if (current_slot <= responder_slots[responder_slot_index]) + // We are the initiator and didn't receive a response + uint32_t skip_packets = 3 - (ranging_packet.sequence_number & 0x03); + next_action_timestamp += (skip_packets * RANGING_BROADCAST_INTERVAL_US); + ranging_packet.sequence_number += skip_packets + 1; + + // Update the antenna settings + responder_antenna = (responder_antenna + 1) % NUM_RCV_ANTENNAS; + if (responder_antenna == 0) { - last_rx_delay_time_us = delay_time_us + ((uint32_t)(responder_slots[responder_slot_index] - current_slot) * RANGING_ITERATION_INTERVAL_US) - RECEIVE_EARLY_START_US; - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(last_rx_delay_time_us)); - current_slot = (uint8_t)responder_slots[responder_slot_index]; - if (!ranging_radio_rxenable(DWT_START_RX_DLY_TS)) + initiator_antenna = (initiator_antenna + 1) % NUM_XMIT_ANTENNAS; + ranging_radio_choose_antenna(initiator_antenna); + if (initiator_antenna == 0) { - print("ERROR: Unable to start listening for incoming RANGING REQUEST packets after error\n"); - return RANGING_ERROR; + responder_index = (responder_index + 1) % schedule_length; + if (responder_index == 0) + { + // Move to the Range Status Phase of the ranging protocol + current_phase = RANGE_STATUS_PHASE; + uint32_t remaining = schedule_length - my_index - 1; + next_action_timestamp += ((remaining * (remaining - 1) / 2) * RANGING_US_PER_RANGE); + return status_phase_begin(my_index, schedule_length, (uint32_t)((reference_time + US_TO_DWT(next_action_timestamp - RECEIVE_EARLY_START_US)) >> 8) & 0xFFFFFFFE); + } } - return RANGING_PHASE; } - else - last_rx_delay_time_us += delay_time_us; + + // Transmit the next POLL packet + 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 ((antenna_index + 1) == NUM_ANTENNAS) + else if (((ranging_packet.sequence_number & 0x03) == 1) || ((ranging_packet.sequence_number & 0x03) == 3)) { - // We are the responder and did not receive any responses...move to next time slot - antenna_index = 0; - ++responder_slot_index; - ranging_radio_choose_antenna(0); - last_rx_delay_time_us += (4 * RANGING_BROADCAST_INTERVAL_US); - if ((++current_slot <= initiator_slot_first) && (initiator_slot_first < responder_slots[responder_slot_index])) - { - ranging_packet.header.seqNum = 0; - dwt_writetxfctrl(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), 0, 1); - dwt_writetxdata(sizeof(ranging_packet_t) - (3 * sizeof(ranging_packet.poll_time)), (uint8_t*)&ranging_packet, 0); - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(last_rx_delay_time_us + RECEIVE_EARLY_START_US + ((initiator_slot_first - current_slot) * RANGING_ITERATION_INTERVAL_US))); - current_slot = initiator_slot_first; - if (dwt_starttx(last_rx_relative_to_transmit ? DWT_START_TX_DLY_TS : DWT_START_TX_DLY_RS) != DWT_SUCCESS) - { - print("ERROR: Failed to transmit RANGING REQUEST packet after error\n"); - return RANGING_ERROR; - } - return RANGING_PHASE; - } - else if (current_slot <= responder_slots[responder_slot_index]) + // We are the responder and didn't receive a response + uint32_t skip_packets = 1 + (ranging_packet.sequence_number & 0x03); + next_action_timestamp += (skip_packets * RANGING_BROADCAST_INTERVAL_US); + ranging_packet.sequence_number += skip_packets; + + // Update the initiator and responder antenna settings + responder_antenna = (responder_antenna + 1) % NUM_RCV_ANTENNAS; + ranging_radio_choose_antenna(responder_antenna); + if (responder_antenna == 0) { - last_rx_delay_time_us += (uint32_t)(responder_slots[responder_slot_index] - current_slot) * RANGING_ITERATION_INTERVAL_US; - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(last_rx_delay_time_us)); - current_slot = (uint8_t)responder_slots[responder_slot_index]; - if (!ranging_radio_rxenable(last_rx_relative_to_transmit ? DWT_START_RX_DLY_TS : DWT_START_RX_DLY_RS)) + initiator_antenna = (initiator_antenna + 1) % NUM_XMIT_ANTENNAS; + if (initiator_antenna == 0) { - print("ERROR: Unable to start listening for new RANGING REQUEST packets after error\n"); - return RANGING_ERROR; + // Update the initiator and responder active indices + responder_index = (responder_index + 1) % schedule_length; + if (responder_index == 0) + { + initiator_index += 1; + if (initiator_index == (schedule_length - 1)) + { + // Move to the Range Status Phase of the ranging protocol + current_phase = RANGE_STATUS_PHASE; + return status_phase_begin(my_index, schedule_length, (uint32_t)((reference_time + US_TO_DWT(next_action_timestamp - RECEIVE_EARLY_START_US)) >> 8) & 0xFFFFFFFE); + } + else + { + next_action_timestamp += (uint32_t)(my_index - initiator_index - 1) * RANGING_US_PER_RANGE; + responder_index = my_index; + } + } + else + { + // Determine if we are the next initiator + if (my_index == ++initiator_index) + { + // Transmit the next POLL packet at the correct time + ranging_packet.sequence_number = 0; + next_action_timestamp += ((schedule_length - responder_index) * RANGING_US_PER_RANGE); + responder_index = my_index + 1; + ranging_packet.tx_time = (uint32_t)(reference_time + US_TO_DWT(next_action_timestamp)) & 0xFFFFFE00; + return start_tx("ERROR: Failed to transmit next RANGING POLL packet after an RX error\n"); + } + else + { + next_action_timestamp += (uint32_t)(schedule_length - responder_index) * RANGING_US_PER_RANGE; + responder_index = my_index; + } + } } - return RANGING_PHASE; - } - } - else - { - // We are the responder and did not receive a response on the non-final antenna, skip to the next antenna - ranging_radio_choose_antenna(++antenna_index); - last_rx_delay_time_us += (((ranging_packet.header.seqNum == 11) || (ranging_packet.header.seqNum == 3)) ? 4 : 2) * RANGING_BROADCAST_INTERVAL_US; - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(last_rx_delay_time_us)); - ranging_packet.header.seqNum = 11; - if (!ranging_radio_rxenable(last_rx_relative_to_transmit ? DWT_START_RX_DLY_TS : DWT_START_RX_DLY_RS)) - { - print("ERROR: Failed to start listening for next RANGING packet after error\n"); - return RANGING_ERROR; } - return RANGING_PHASE; - } - // Move to the Range Status Phase of the ranging protocol - current_phase = RANGE_STATUS_PHASE; - return status_phase_begin(scheduled_slot, total_num_slots, last_rx_delay_time_us + RECEIVE_EARLY_START_US + ((num_sub_slots - current_slot) * RANGING_ITERATION_INTERVAL_US), last_rx_relative_to_transmit); + // 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_time_slices(void) +uint32_t ranging_phase_get_duration(void) { - return num_sub_slots; + return ranging_phase_duration; } diff --git a/software/firmware/src/tasks/ranging/ranging_phase.h b/software/firmware/src/tasks/ranging/ranging_phase.h index 940c1fef..f8b14a80 100644 --- a/software/firmware/src/tasks/ranging/ranging_phase.h +++ b/software/firmware/src/tasks/ranging/ranging_phase.h @@ -11,8 +11,8 @@ typedef struct __attribute__ ((__packed__)) { ieee154_header_t header; - uint8_t message_type; - uint32_t poll_time, resp_time, final_time; + uint8_t sequence_number; + uint32_t tx_time, rx_time; ieee154_footer_t footer; } ranging_packet_t; @@ -20,10 +20,10 @@ typedef struct __attribute__ ((__packed__)) // Public API ---------------------------------------------------------------------------------------------------------- void ranging_phase_initialize(const uint8_t *uid); -scheduler_phase_t ranging_phase_begin(uint8_t ranging_slot, uint8_t num_slots, uint32_t start_delay_us, bool start_relative_to_transmit); +scheduler_phase_t ranging_phase_begin(uint8_t scheduled_slot, uint8_t schedule_size, uint32_t start_delay_dwt); 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_time_slices(void); +uint32_t ranging_phase_get_duration(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 928f079f..efe42e80 100644 --- a/software/firmware/src/tasks/ranging/schedule_phase.c +++ b/software/firmware/src/tasks/ranging/schedule_phase.c @@ -10,6 +10,8 @@ static uint8_t scheduled_slot, device_timeouts[MAX_NUM_RANGING_DEVICES]; static schedule_packet_t schedule_packet; static scheduler_phase_t current_phase; +static uint32_t next_action_timestamp; +static uint64_t reference_time; static bool is_master_scheduler; @@ -18,7 +20,7 @@ static bool is_master_scheduler; static void deschedule_device(uint8_t device_index) { // Search for the specified EUI and move all subsequent devices up in the schedule - print("INFO: Descheduling device 0x%02X due to inactivity\n", schedule_packet.schedule[device_index]); + print("INFO: De-scheduling device 0x%02X due to inactivity\n", schedule_packet.schedule[device_index]); for (int i = device_index + 1; i < MAX_NUM_RANGING_DEVICES; ++i) { schedule_packet.schedule[i-1] = schedule_packet.schedule[i]; @@ -34,9 +36,9 @@ static void deschedule_device(uint8_t device_index) void schedule_phase_initialize(const uint8_t *uid, bool is_master, uint32_t epoch_timestamp) { // Initialize all Schedule Phase parameters - schedule_packet = (schedule_packet_t){ .header = { .frameCtrl = { 0x41, 0x98 }, .seqNum = 0, + schedule_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 = epoch_timestamp, .num_devices = 1, + .sequence_number = 0, .epoch_time_unix = epoch_timestamp, .num_devices = 1, .schedule = { 0 }, .footer = { { 0 } } }; memset(device_timeouts, 0, sizeof(device_timeouts)); memcpy(schedule_packet.header.sourceAddr, uid, sizeof(schedule_packet.header.sourceAddr)); @@ -48,11 +50,12 @@ void schedule_phase_initialize(const uint8_t *uid, bool is_master, uint32_t epoc bool schedule_phase_begin(void) { // Reset the necessary Schedule Phase parameters - schedule_packet.header.seqNum = 0; + schedule_packet.sequence_number = 0; current_phase = SCHEDULE_PHASE; + next_action_timestamp = 0; // Set up the correct antenna for schedule transmission - ranging_radio_choose_antenna(SCHEDULE_XMIT_ANTENNA); + ranging_radio_choose_antenna(0); // Begin transmission or reception depending on the current role if (is_master_scheduler) @@ -65,7 +68,7 @@ bool schedule_phase_begin(void) // Schedule packet transmission const uint16_t packet_size = sizeof(schedule_packet_t) - MAX_NUM_RANGING_DEVICES + schedule_packet.num_devices; dwt_writetxfctrl(packet_size, 0, 0); - if ((dwt_writetxdata(packet_size, (uint8_t*)&schedule_packet, 0) != DWT_SUCCESS) || (dwt_starttx(DWT_START_TX_IMMEDIATE) != DWT_SUCCESS)) + 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; @@ -74,6 +77,7 @@ bool schedule_phase_begin(void) else { // Set up packet reception with a timeout + dwt_setpreambledetecttimeout(0); dwt_setrxtimeout(DW_TIMEOUT_FROM_US(1000000.0)); if (!ranging_radio_rxenable(DWT_START_RX_IMMEDIATE)) { @@ -91,10 +95,16 @@ scheduler_phase_t schedule_phase_tx_complete(void) return ranging_phase_tx_complete(); // Retransmit the schedule up to the specified number of times - if ((++schedule_packet.header.seqNum < SCHEDULE_NUM_MASTER_BROADCASTS) && is_master_scheduler) + next_action_timestamp += SCHEDULE_RESEND_INTERVAL_US; + if ((++schedule_packet.sequence_number < SCHEDULE_NUM_MASTER_BROADCASTS) && is_master_scheduler) { - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(SCHEDULE_RESEND_INTERVAL_US)); - if ((dwt_writetxdata(sizeof(schedule_packet.header.seqNum), &schedule_packet.header.seqNum, offsetof(ieee154_header_t, seqNum)) != DWT_SUCCESS) || (dwt_starttx(DWT_START_TX_DLY_TS) != DWT_SUCCESS)) + if (schedule_packet.sequence_number == 1) + { + reference_time = ranging_radio_readtxtimestamp() & 0xFFFFFFFE00UL; + dwt_setreferencetrxtime((uint32_t)(reference_time >> 8)); + } + 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)) { print("ERROR: Failed to retransmit schedule\n"); return RANGING_ERROR; @@ -104,7 +114,8 @@ scheduler_phase_t schedule_phase_tx_complete(void) // Move to the Ranging Phase of the ranging protocol current_phase = RANGING_PHASE; - return ranging_phase_begin(scheduled_slot, schedule_packet.num_devices, ((uint32_t)(SCHEDULE_NUM_TOTAL_BROADCASTS - schedule_packet.header.seqNum + 1)) * SCHEDULE_RESEND_INTERVAL_US, true); + next_action_timestamp += ((uint32_t)(SCHEDULE_NUM_TOTAL_BROADCASTS - schedule_packet.sequence_number)) * SCHEDULE_RESEND_INTERVAL_US; + return ranging_phase_begin(scheduled_slot, schedule_packet.num_devices, (uint32_t)((reference_time + US_TO_DWT(next_action_timestamp - RECEIVE_EARLY_START_US)) >> 8) & 0xFFFFFFFE); } scheduler_phase_t schedule_phase_rx_complete(schedule_packet_t* schedule) @@ -122,7 +133,7 @@ scheduler_phase_t schedule_phase_rx_complete(schedule_packet_t* schedule) } return device_found ? ranging_phase_rx_complete((ranging_packet_t*)schedule) : MESSAGE_COLLISION; } - else if (schedule->message_type != SCHEDULE_PACKET) + else if (schedule->header.msgType != SCHEDULE_PACKET) { // Immediately restart listening for schedule packets if (!ranging_radio_rxenable(DWT_START_RX_IMMEDIATE)) @@ -157,14 +168,19 @@ scheduler_phase_t schedule_phase_rx_complete(schedule_packet_t* schedule) if (!scheduled_slot) return RANGING_ERROR; + // Set up the reference timestamp for scheduling future messages + reference_time = (ranging_radio_readrxtimestamp() - US_TO_DWT((uint32_t)schedule->sequence_number * SCHEDULE_RESEND_INTERVAL_US)) & 0xFFFFFFFE00UL; + dwt_setreferencetrxtime((uint32_t)(reference_time >> 8)); + // Retransmit the schedule at the specified time slot - schedule_packet.header.seqNum = scheduled_slot + SCHEDULE_NUM_MASTER_BROADCASTS - 1; - if ((schedule->header.seqNum < schedule_packet.header.seqNum) && (schedule_packet.header.seqNum < SCHEDULE_NUM_TOTAL_BROADCASTS)) + schedule_packet.sequence_number = scheduled_slot + SCHEDULE_NUM_MASTER_BROADCASTS - 1; + if ((schedule->sequence_number < schedule_packet.sequence_number) && (schedule_packet.sequence_number < SCHEDULE_NUM_TOTAL_BROADCASTS)) { const uint16_t packet_size = sizeof(schedule_packet_t) - MAX_NUM_RANGING_DEVICES + schedule_packet.num_devices; + next_action_timestamp += (uint32_t)(schedule_packet.sequence_number - schedule->sequence_number) * SCHEDULE_RESEND_INTERVAL_US; dwt_writetxfctrl(packet_size, 0, 0); - dwt_setdelayedtrxtime(DW_DELAY_FROM_US((uint32_t)(schedule_packet.header.seqNum - schedule->header.seqNum) * SCHEDULE_RESEND_INTERVAL_US)); - if ((dwt_writetxdata(packet_size, (uint8_t*)&schedule_packet, 0) != DWT_SUCCESS) || (dwt_starttx(DWT_START_TX_DLY_RS) != DWT_SUCCESS)) + 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)) { print("ERROR: Failed to retransmit received schedule\n"); return RANGING_ERROR; @@ -174,7 +190,8 @@ scheduler_phase_t schedule_phase_rx_complete(schedule_packet_t* schedule) // Move to the Ranging Phase of the ranging protocol current_phase = RANGING_PHASE; - return ranging_phase_begin(scheduled_slot, schedule_packet.num_devices, ((uint32_t)(SCHEDULE_NUM_TOTAL_BROADCASTS - schedule->header.seqNum)) * SCHEDULE_RESEND_INTERVAL_US, false); + next_action_timestamp += ((uint32_t)(SCHEDULE_NUM_TOTAL_BROADCASTS - schedule->sequence_number)) * SCHEDULE_RESEND_INTERVAL_US; + return ranging_phase_begin(scheduled_slot, schedule_packet.num_devices, (uint32_t)((reference_time + US_TO_DWT(next_action_timestamp - RECEIVE_EARLY_START_US)) >> 8) & 0xFFFFFFFE); } scheduler_phase_t schedule_phase_rx_error(void) @@ -231,7 +248,7 @@ void schedule_phase_update_device_presence(uint8_t eui) void schedule_phase_handle_device_timeouts(void) { - // Deschedule any devices that have been absent for a long time + // De-schedule any devices that have been absent for a long time for (uint8_t i = 1; i < schedule_packet.num_devices; ++i) if (device_timeouts[i] > DEVICE_TIMEOUT_SECONDS) deschedule_device(i--); diff --git a/software/firmware/src/tasks/ranging/schedule_phase.h b/software/firmware/src/tasks/ranging/schedule_phase.h index 340ff66d..8cfb503e 100644 --- a/software/firmware/src/tasks/ranging/schedule_phase.h +++ b/software/firmware/src/tasks/ranging/schedule_phase.h @@ -11,7 +11,7 @@ typedef struct __attribute__ ((__packed__)) { ieee154_header_t header; - uint8_t message_type; + uint8_t sequence_number; uint32_t epoch_time_unix; uint8_t num_devices; uint8_t schedule[MAX_NUM_RANGING_DEVICES]; diff --git a/software/firmware/src/tasks/ranging/scheduler.c b/software/firmware/src/tasks/ranging/scheduler.c index 3a36a695..bf02975c 100644 --- a/software/firmware/src/tasks/ranging/scheduler.c +++ b/software/firmware/src/tasks/ranging/scheduler.c @@ -17,7 +17,7 @@ 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[768], device_eui, schedule_reception_timeout; +static uint8_t read_buffer[128], device_eui, schedule_reception_timeout; static uint8_t empty_round_timeout, eui[EUI_LEN]; static volatile bool is_running, is_starting; @@ -54,7 +54,7 @@ static void handle_range_computation_phase(bool is_master) ranging_radio_sleep(true); if (!is_master) { - const uint32_t remaing_time_us = 1000000 - RADIO_WAKEUP_SAFETY_DELAY_US - SCHEDULE_BROADCAST_PERIOD_US - (ranging_phase_get_time_slices() * RANGING_ITERATION_INTERVAL_US) - (schedule_phase_get_num_devices() * RANGE_STATUS_BROADCAST_PERIOD_US); + const uint32_t remaing_time_us = 1000000 - RADIO_WAKEUP_SAFETY_DELAY_US - SCHEDULE_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); @@ -67,9 +67,8 @@ static void handle_range_computation_phase(bool is_master) 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)); -#else - print_ranges(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)); } ranging_phase = UNSCHEDULED_TIME_PHASE; } @@ -98,22 +97,17 @@ static void tx_callback(const dwt_cb_data_t *txData) { // Notify the main task to handle the interrupt BaseType_t xHigherPriorityTaskWoken = pdFALSE; + ranging_phase = schedule_phase_tx_complete(); xTaskNotifyFromISR(notification_handle, RANGING_TX_COMPLETE, eSetBits, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } static void rx_callback(const dwt_cb_data_t *rxData) { - // Read packet data and ensure that no errors were encountered - if (rxData->datalength > sizeof(read_buffer)) - { - read_buffer[sizeof(ieee154_header_t)] = UNKNOWN_PACKET; - print("ERROR: Received packet which exceeds maximal length (received %u bytes)!\n", rxData->datalength); - } - // Notify the main task to handle the interrupt BaseType_t xHigherPriorityTaskWoken = pdFALSE; 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); } @@ -122,6 +116,7 @@ static void rx_timeout_callback(const dwt_cb_data_t *rxData) { // Notify the main task to handle the interrupt BaseType_t xHigherPriorityTaskWoken = pdFALSE; + ranging_phase = schedule_phase_rx_error(); xTaskNotifyFromISR(notification_handle, RANGING_RX_TIMEOUT, eSetBits, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } @@ -204,12 +199,6 @@ void scheduler_run(schedule_role_t role, uint32_t timestamp) vTaskDelay(1); ranging_phase = schedule_phase_begin() ? SCHEDULE_PHASE : RANGING_ERROR; } - if ((pending_actions & RANGING_TX_COMPLETE) != 0) - ranging_phase = schedule_phase_tx_complete(); - if ((pending_actions & RANGING_RX_COMPLETE) != 0) - ranging_phase = schedule_phase_rx_complete((schedule_packet_t*)read_buffer); - if ((pending_actions & RANGING_RX_TIMEOUT) != 0) - ranging_phase = schedule_phase_rx_error(); // Carry out logic based on the current reported phase of the ranging protocol switch (ranging_phase) diff --git a/software/firmware/src/tasks/ranging/status_phase.c b/software/firmware/src/tasks/ranging/status_phase.c index 6b533cc7..aa0ee9a1 100644 --- a/software/firmware/src/tasks/ranging/status_phase.c +++ b/software/firmware/src/tasks/ranging/status_phase.c @@ -10,9 +10,34 @@ static status_success_packet_t success_packet; static uint8_t current_slot, scheduled_slot, total_num_slots; -static uint32_t transmitted_seq_num, last_rx_delay_time_us; +static uint32_t transmitted_seq_num, next_action_timestamp; static uint8_t present_devices[MAX_NUM_RANGING_DEVICES], num_present_devices; -static bool last_rx_relative_to_transmit; + + +// Private Helper Functions -------------------------------------------------------------------------------------------- + +static inline scheduler_phase_t start_tx(const char *error_message, status_success_packet_t *packet) +{ + transmitted_seq_num = packet->sequence_number; + dwt_setdelayedtrxtime((uint32_t)((US_TO_DWT(next_action_timestamp) - TX_ANTENNA_DELAY) >> 8) & 0xFFFFFFFE); + if ((dwt_writetxdata(2 * sizeof(success_packet.sequence_number), &packet->sequence_number, offsetof(status_success_packet_t, sequence_number)) != DWT_SUCCESS) || (dwt_starttx(DWT_START_TX_DLY_REF) != DWT_SUCCESS)) + { + print(error_message); + return RANGE_COMPUTATION_PHASE; + } + return RANGE_STATUS_PHASE; +} + +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) + { + print(error_message); + return RANGE_COMPUTATION_PHASE; + } + return RANGE_STATUS_PHASE; +} // Public API Functions ------------------------------------------------------------------------------------------------ @@ -20,92 +45,51 @@ static bool last_rx_relative_to_transmit; void status_phase_initialize(const uint8_t *uid) { // Initialize all Schedule Phase parameters - success_packet = (status_success_packet_t){ .header = { .frameCtrl = { 0x41, 0x98 }, .seqNum = 0, + success_packet = (status_success_packet_t){ .header = { .frameCtrl = { 0x41, 0x88 }, .msgType = STATUS_SUCCESS_PACKET, .panID = { MODULE_PANID & 0xFF, MODULE_PANID >> 8 }, .destAddr = { 0xFF, 0xFF }, .sourceAddr = { 0 } }, - .message_type = STATUS_SUCCESS_PACKET, .success = 0, .footer = { { 0 } } }; + .sequence_number = 0, .success = 0, .footer = { { 0 } } }; memcpy(success_packet.header.sourceAddr, uid, sizeof(success_packet.header.sourceAddr)); - scheduled_slot = 0xFF; } -scheduler_phase_t status_phase_begin(uint8_t status_slot, uint8_t num_slots, uint32_t start_delay_us, bool start_relative_to_transmit) +scheduler_phase_t status_phase_begin(uint8_t status_slot, uint8_t num_slots, uint32_t start_delay_dwt) { // Reset the necessary Schedule Phase parameters current_slot = 1; num_present_devices = 0; total_num_slots = num_slots; scheduled_slot = status_slot; - success_packet.header.seqNum = 0; + success_packet.sequence_number = 0; success_packet.success = responses_received(); + next_action_timestamp = RECEIVE_EARLY_START_US; memset(present_devices, 0, sizeof(present_devices)); dwt_writetxfctrl(sizeof(status_success_packet_t), 0, 0); + dwt_writetxdata(sizeof(status_success_packet_t) - sizeof(ieee154_footer_t), (uint8_t*)&success_packet, 0); - // Set up the correct initial antenna and RX timeout duration - ranging_radio_choose_antenna(RANGE_STATUS_XMIT_ANTENNA); + // 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_setrxtimeout(DW_TIMEOUT_FROM_US(RANGE_STATUS_TIMEOUT_US)); // Begin transmission or reception depending on the scheduled time slot - if (scheduled_slot == current_slot) - { - transmitted_seq_num = 0; - dwt_writetxdata(sizeof(status_success_packet_t), (uint8_t*)&success_packet, 0); - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(start_delay_us)); - if (dwt_starttx(start_relative_to_transmit ? DWT_START_TX_DLY_TS : DWT_START_TX_DLY_RS) != DWT_SUCCESS) - { - print("ERROR: Failed to transmit initial STATUS packet\n"); - return RANGE_COMPUTATION_PHASE; - } - } - else - { - last_rx_relative_to_transmit = start_relative_to_transmit; - last_rx_delay_time_us = start_delay_us - RECEIVE_EARLY_START_US; - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(last_rx_delay_time_us)); - if (!ranging_radio_rxenable(start_relative_to_transmit ? DWT_START_RX_DLY_TS : DWT_START_RX_DLY_RS)) - { - print("ERROR: Unable to start listening for STATUS packets\n"); - return RANGE_COMPUTATION_PHASE; - } - } - return RANGE_STATUS_PHASE; + return (scheduled_slot == current_slot) ? + start_tx("ERROR: Failed to transmit initial STATUS packet\n", &success_packet) : + start_rx("ERROR: Unable to start listening for STATUS packets\n"); } scheduler_phase_t status_phase_tx_complete(void) { + next_action_timestamp += RANGE_STATUS_BROADCAST_PERIOD_US - (transmitted_seq_num * RANGE_STATUS_RESEND_INTERVAL_US); if (++current_slot == scheduled_slot) - { - dwt_writetxdata(sizeof(status_success_packet_t), (uint8_t*)&success_packet, 0); - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(RANGE_STATUS_BROADCAST_PERIOD_US - (transmitted_seq_num * RANGE_STATUS_RESEND_INTERVAL_US))); - transmitted_seq_num = 0; - if (dwt_starttx(DWT_START_TX_DLY_TS) != DWT_SUCCESS) - { - print("ERROR: Failed to transmit STATUS packet after prior transmission\n"); - return RANGE_COMPUTATION_PHASE; - } - return RANGE_STATUS_PHASE; - } + return start_tx("ERROR: Failed to transmit STATUS packet after prior transmission\n", &success_packet); else if (current_slot < total_num_slots) - { - // Move back into listening mode for additional status responses - last_rx_relative_to_transmit = true; - last_rx_delay_time_us = RANGE_STATUS_BROADCAST_PERIOD_US - (transmitted_seq_num * RANGE_STATUS_RESEND_INTERVAL_US) - RECEIVE_EARLY_START_US; - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(last_rx_delay_time_us)); - transmitted_seq_num = 0; - if (!ranging_radio_rxenable(DWT_START_RX_DLY_TS)) - { - print("ERROR: Unable to re-enable listening for STATUS packets\n"); - return RANGE_COMPUTATION_PHASE; - } - return RANGE_STATUS_PHASE; - } - - // Move to the Range Computation Phase of the ranging protocol + return start_rx("ERROR: Unable to re-enable listening for STATUS packets after transmission\n"); return RANGE_COMPUTATION_PHASE; } scheduler_phase_t status_phase_rx_complete(status_success_packet_t* packet) { // Ensure that this packet is of the expected type - if (packet->message_type != STATUS_SUCCESS_PACKET) + if (packet->header.msgType != STATUS_SUCCESS_PACKET) { print("ERROR: Received an unexpected message type during STATUS phase...possible network collision\n"); return MESSAGE_COLLISION; @@ -116,78 +100,34 @@ scheduler_phase_t status_phase_rx_complete(status_success_packet_t* packet) present_devices[num_present_devices++] = packet->header.sourceAddr[0]; // Retransmit the status packet upon reception - const uint32_t seqNum = packet->header.seqNum; - if (scheduled_slot && (scheduled_slot <= RANGE_STATUS_NUM_TOTAL_BROADCASTS) && (packet->header.seqNum < scheduled_slot)) + const uint32_t seqNum = packet->sequence_number; + if (scheduled_slot && (scheduled_slot <= RANGE_STATUS_NUM_TOTAL_BROADCASTS) && (packet->sequence_number < scheduled_slot)) { - packet->header.seqNum = scheduled_slot - 1; - transmitted_seq_num = packet->header.seqNum; - dwt_writetxdata(sizeof(status_success_packet_t), (uint8_t*)packet, 0); - dwt_setdelayedtrxtime(DW_DELAY_FROM_US((transmitted_seq_num - seqNum) * RANGE_STATUS_RESEND_INTERVAL_US)); - if (dwt_starttx(DWT_START_TX_DLY_RS) != DWT_SUCCESS) - { - print("ERROR: Failed to retransmit received STATUS packet\n"); - return RANGE_COMPUTATION_PHASE; - } - return RANGE_STATUS_PHASE; + packet->sequence_number = 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); } else if (++current_slot == scheduled_slot) { - transmitted_seq_num = 0; - dwt_writetxdata(sizeof(status_success_packet_t), (uint8_t*)&success_packet, 0); - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(RANGE_STATUS_BROADCAST_PERIOD_US - (seqNum * RANGE_STATUS_RESEND_INTERVAL_US))); - if (dwt_starttx(DWT_START_TX_DLY_RS) != DWT_SUCCESS) - { - print("ERROR: Failed to transmit STATUS packet\n"); - return RANGE_COMPUTATION_PHASE; - } - return RANGE_STATUS_PHASE; + next_action_timestamp += RANGE_STATUS_BROADCAST_PERIOD_US - (seqNum * RANGE_STATUS_RESEND_INTERVAL_US); + return start_tx("ERROR: Failed to transmit STATUS packet\n", &success_packet); } else if (current_slot < total_num_slots) { - // Move back into listening mode for additional status responses - last_rx_relative_to_transmit = false; - last_rx_delay_time_us = RANGE_STATUS_BROADCAST_PERIOD_US - (seqNum * RANGE_STATUS_RESEND_INTERVAL_US) - RECEIVE_EARLY_START_US; - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(last_rx_delay_time_us)); - if (!ranging_radio_rxenable(DWT_START_RX_DLY_RS)) - { - print("ERROR: Unable to re-enable listening for STATUS packets after reception\n"); - return RANGE_COMPUTATION_PHASE; - } - return RANGE_STATUS_PHASE; + next_action_timestamp += RANGE_STATUS_BROADCAST_PERIOD_US - (seqNum * RANGE_STATUS_RESEND_INTERVAL_US); + return start_rx("ERROR: Unable to re-enable listening for STATUS packets after reception\n"); } - - // Move to the Range Computation Phase of the ranging protocol return RANGE_COMPUTATION_PHASE; } scheduler_phase_t status_phase_rx_error(void) { // Move to the next expected status packet to receive + next_action_timestamp += RANGE_STATUS_BROADCAST_PERIOD_US; if (++current_slot == scheduled_slot) - { - transmitted_seq_num = 0; - dwt_writetxdata(sizeof(status_success_packet_t), (uint8_t*)&success_packet, 0); - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(last_rx_delay_time_us + RECEIVE_EARLY_START_US + RANGE_STATUS_BROADCAST_PERIOD_US)); - if (dwt_starttx(last_rx_relative_to_transmit ? DWT_START_TX_DLY_TS : DWT_START_TX_DLY_RS) != DWT_SUCCESS) - { - print("ERROR: Failed to transmit STATUS packet after error\n"); - return RANGE_COMPUTATION_PHASE; - } - return RANGE_STATUS_PHASE; - } + return start_tx("ERROR: Failed to transmit STATUS packet after error\n", &success_packet); else if (current_slot < total_num_slots) - { - last_rx_delay_time_us += RANGE_STATUS_BROADCAST_PERIOD_US; - dwt_setdelayedtrxtime(DW_DELAY_FROM_US(last_rx_delay_time_us)); - if (!ranging_radio_rxenable(last_rx_relative_to_transmit ? DWT_START_RX_DLY_TS : DWT_START_RX_DLY_RS)) - { - print("ERROR: Unable to re-enable listening for STATUS packets after error\n"); - return RANGE_COMPUTATION_PHASE; - } - return RANGE_STATUS_PHASE; - } - - // Move to the Range Computation Phase of the ranging protocol + return start_rx("ERROR: Unable to re-enable listening for STATUS packets after error\n"); return RANGE_COMPUTATION_PHASE; } diff --git a/software/firmware/src/tasks/ranging/status_phase.h b/software/firmware/src/tasks/ranging/status_phase.h index 36c3562d..8ca80612 100644 --- a/software/firmware/src/tasks/ranging/status_phase.h +++ b/software/firmware/src/tasks/ranging/status_phase.h @@ -11,7 +11,7 @@ typedef struct __attribute__ ((__packed__)) { ieee154_header_t header; - uint8_t message_type, success; + uint8_t sequence_number, success; ieee154_footer_t footer; } status_success_packet_t; @@ -19,7 +19,7 @@ typedef struct __attribute__ ((__packed__)) // Public API ---------------------------------------------------------------------------------------------------------- void status_phase_initialize(const uint8_t *uid); -scheduler_phase_t status_phase_begin(uint8_t status_slot, uint8_t num_slots, uint32_t start_delay_us, bool start_relative_to_transmit); +scheduler_phase_t status_phase_begin(uint8_t status_slot, uint8_t num_slots, uint32_t start_delay_dwt); scheduler_phase_t status_phase_tx_complete(void); scheduler_phase_t status_phase_rx_complete(status_success_packet_t* packet); scheduler_phase_t status_phase_rx_error(void);