Skip to content

Commit

Permalink
Initial steps for scheduling via UWB instead of BLE
Browse files Browse the repository at this point in the history
  • Loading branch information
@hedgecrw
hedgecrw committed Nov 8, 2023
1 parent 0abadd8 commit 11cdbec
Show file tree
Hide file tree
Showing 10 changed files with 201 additions and 177 deletions.
4 changes: 4 additions & 0 deletions software/firmware/src/app/app_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,7 @@ typedef enum { BATTERY_EMPTY = 3200, BATTERY_CRITICAL = 3500, BATTERY_NOMINAL =
#define RX_ANTENNA_DELAY 16385
#define MIN_VALID_RANGE_MM (-1000)
#define MAX_VALID_RANGE_MM (32*1000)
#define UNSCHEDULED_SLOT 0xFF

#define SCHEDULING_INTERVAL_US 1000000
#define RADIO_WAKEUP_SAFETY_DELAY_US 5000
Expand All @@ -128,4 +129,7 @@ typedef enum { BATTERY_EMPTY = 3200, BATTERY_CRITICAL = 3500, BATTERY_NOMINAL =
#define RANGE_STATUS_BROADCAST_PERIOD_US (RANGE_STATUS_NUM_TOTAL_BROADCASTS * RANGE_STATUS_RESEND_INTERVAL_US)
#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

#endif // #ifndef __APP_CONFIG_HEADER_H__
124 changes: 22 additions & 102 deletions software/firmware/src/tasks/app_task_ranging.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,10 @@

static uint8_t device_uid_short;
static TaskHandle_t app_task_handle = 0;
static uint8_t device_id_to_schedule[EUI_LEN];
static uint8_t discovered_devices[MAX_NUM_RANGING_DEVICES][1+EUI_LEN];
static volatile bool devices_found, forwarding_request;
static volatile uint32_t seconds_to_activate_buzzer;
static volatile uint8_t num_discovered_devices;
static volatile bool devices_found;


// Private Helper Functions --------------------------------------------------------------------------------------------
Expand All @@ -31,10 +30,6 @@ static void verify_app_configuration(void)
// Retrieve the current state of the application
const bool is_scanning = bluetooth_is_scanning(), is_ranging = ranging_active();

// Advertised role should be UNKNOWN if not ranging
if (!is_ranging && (bluetooth_get_current_ranging_role() != ROLE_UNKNOWN))
bluetooth_set_current_ranging_role(ROLE_UNKNOWN);

// Advertising should always be enabled
if (!bluetooth_is_advertising())
bluetooth_start_advertising();
Expand All @@ -53,66 +48,27 @@ static void handle_notification(app_notification_t notification)
verify_app_configuration();
if ((notification & APP_NOTIFY_NETWORK_FOUND) != 0)
{
// Determine if a master or participant device was located
bool master_device_located = false, participant_device_located = false;
for (uint8_t i = 0; !master_device_located && (i < num_discovered_devices); ++i)
switch (discovered_devices[i][EUI_LEN])
// Search for the non-sleeping device with the highest ID that is higher than our own
int32_t best_device_idx = -1;
uint8_t highest_device_id = device_uid_short;
for (uint8_t i = 0; i < num_discovered_devices; ++i)
if ((discovered_devices[i][EUI_LEN] != ROLE_ASLEEP) && (discovered_devices[i][0] > highest_device_id))
{
case ROLE_MASTER:
master_device_located = true;
bluetooth_join_ranging_network(discovered_devices[i], NULL);
break;
case ROLE_PARTICIPANT:
participant_device_located = true;
break;
default:
break;
best_device_idx = i;
highest_device_id = discovered_devices[i][0];
}

// Join the ranging network based on the state of the detected devices
if (master_device_located)
// If a potential master candidate device was found, attempt to connect to it
if (best_device_idx >= 0)
{
// Set our role as a ranging participant and start the ranging process
bluetooth_set_current_ranging_role(ROLE_PARTICIPANT);
ranging_begin(ROLE_PARTICIPANT);
}
else if (participant_device_located)
{
// Set our role as a ranging participant and start the ranging process
bluetooth_set_current_ranging_role(ROLE_PARTICIPANT);
ranging_begin(ROLE_PARTICIPANT);

// Send a request to join the network to all participant devices
for (uint8_t i = 0; i < num_discovered_devices; ++i)
if (discovered_devices[i][EUI_LEN] == ROLE_PARTICIPANT)
bluetooth_join_ranging_network(discovered_devices[i], NULL);
//bluetooth_set_current_ranging_role(ROLE_PARTICIPANT);
}
else
{
// Search for the non-sleeping device with the highest ID that is higher than our own
int32_t best_device_idx = -1;
uint8_t highest_device_id = device_uid_short;
for (uint8_t i = 0; i < num_discovered_devices; ++i)
if ((discovered_devices[i][EUI_LEN] != ROLE_ASLEEP) && (discovered_devices[i][0] > highest_device_id))
{
best_device_idx = i;
highest_device_id = discovered_devices[i][0];
}

// If a potential master candidate device was found, attempt to connect to it
if (best_device_idx >= 0)
{
// Set our role as a ranging participant and start the ranging process
ranging_begin(ROLE_PARTICIPANT);
bluetooth_set_current_ranging_role(ROLE_PARTICIPANT);
bluetooth_join_ranging_network(discovered_devices[best_device_idx], NULL);
}
else
{
// Reset the scanning interface so that lower ID devices quickly get discovered again
// in case one of them became the master
bluetooth_reset_scanning();
}
ranging_begin(ROLE_MASTER);
//bluetooth_set_current_ranging_role(ROLE_MASTER);
}

// Reset the devices-found flag and verify the app configuration
Expand All @@ -127,41 +83,6 @@ static void handle_notification(app_notification_t notification)
buzzer_indicate_location();
vTaskDelay(pdMS_TO_TICKS(1000));
}
if ((notification & APP_NOTIFY_SCHEDULE_DEVICE) != 0)
{
// Ignore this if we are supposed to be asleep
schedule_role_t role = (schedule_role_t)bluetooth_get_current_ranging_role();
if (role != ROLE_ASLEEP)
{
// Start the ranging protocol as master if not already ranging
if (!ranging_active())
{
role = ROLE_MASTER;
ranging_begin(ROLE_MASTER);
bluetooth_set_current_ranging_role(ROLE_MASTER);
verify_app_configuration();
}

// Schedule the device if we are the master, otherwise forward the request if it has not already been forwarded
if (role == ROLE_MASTER)
ranging_schedule_device(device_id_to_schedule);
else if (!forwarding_request)
{
// Quickly scan for all TotTags in the vicinity
forwarding_request = true;
bluetooth_single_scan(250);

// Try to forward directly to the master device
for (uint8_t i = 0; i < num_discovered_devices; ++i)
if (discovered_devices[i][EUI_LEN] == ROLE_MASTER)
{
bluetooth_join_ranging_network(discovered_devices[i], device_id_to_schedule);
break;
}
forwarding_request = devices_found = false;
}
}
}
}

static void battery_event_handler(battery_event_t battery_event)
Expand All @@ -187,18 +108,23 @@ static void ble_discovery_handler(const uint8_t ble_address[EUI_LEN], uint8_t ra
num_discovered_devices = 1;
memcpy(discovered_devices[0], ble_address, EUI_LEN);
discovered_devices[0][EUI_LEN] = ranging_role;
if (!forwarding_request)
am_hal_timer_clear(BLE_SCANNING_TIMER_NUMBER);
am_hal_timer_clear(BLE_SCANNING_TIMER_NUMBER);
}
else if (num_discovered_devices < MAX_NUM_RANGING_DEVICES)
{
for (uint8_t i = 0; i < num_discovered_devices; ++i)
if (memcmp(discovered_devices[i], ble_address, EUI_LEN) == 0)
return;
memcpy(discovered_devices[num_discovered_devices], ble_address, EUI_LEN);
discovered_devices[num_discovered_devices++][EUI_LEN] = ranging_role;
}
}

void am_timer04_isr(void)
{
// Immediately stop scanning for additional devices
bluetooth_stop_scanning();

// Notify the main task to handle the interrupt
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
am_hal_timer_interrupt_clear(AM_HAL_TIMER_MASK(BLE_SCANNING_TIMER_NUMBER, AM_HAL_TIMER_COMPARE_BOTH));
Expand Down Expand Up @@ -227,13 +153,6 @@ void app_notify(app_notification_t notification, bool from_isr)
}
}

void app_schedule_device(const uint8_t *uid)
{
// Notify application of the device scheduling request
memcpy(device_id_to_schedule, uid, EUI_LEN);
app_notify(APP_NOTIFY_SCHEDULE_DEVICE, false);
}

void app_activate_find_my_tottag(uint32_t seconds_to_activate)
{
// Notify application of the request to active FindMyTottag
Expand Down Expand Up @@ -272,9 +191,10 @@ void AppTaskRanging(void *uid)
storage_retrieve_experiment_details(&current_experiment);

// Wait until the BLE stack has been fully initialized
devices_found = forwarding_request = false;
devices_found = false;
while (!bluetooth_is_initialized())
vTaskDelay(1);
bluetooth_set_current_ranging_role(ROLE_UNKNOWN);

// Update the BLE address whitelist
bluetooth_clear_whitelist();
Expand Down
3 changes: 0 additions & 3 deletions software/firmware/src/tasks/app_tasks.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@ typedef enum {
APP_NOTIFY_NETWORK_LOST = 0b00000010,
APP_NOTIFY_NETWORK_FOUND = 0b00000100,
APP_NOTIFY_BATTERY_EVENT = 0b00100000,
APP_NOTIFY_SCHEDULE_DEVICE = 0b01000000,
APP_NOTIFY_FIND_MY_TOTTAG_ACTIVATED = 0b10000000
} app_notification_t;

Expand All @@ -32,14 +31,12 @@ typedef struct __attribute__ ((__packed__))

// Application Task Public Functions
void app_notify(app_notification_t notification, bool from_isr);
void app_schedule_device(const uint8_t *uid);
void app_activate_find_my_tottag(uint32_t seconds_to_activate);

// Ranging Task Public Functions
void ranging_begin(schedule_role_t role);
void ranging_end(void);
bool ranging_active(void);
void ranging_schedule_device(const uint8_t *device_id);

// Storage Task Public Functions
void storage_flush_and_shutdown(void);
Expand Down
4 changes: 2 additions & 2 deletions software/firmware/src/tasks/ranging/ranging_phase.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -196,7 +196,8 @@ 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();
if (schedule_size < 2)
ranging_phase_duration = ((uint32_t)schedule_size * (schedule_size - 1) / 2) * RANGING_US_PER_RANGE;
if ((schedule_size < 2) || (scheduled_slot == UNSCHEDULED_SLOT))
return RANGE_COMPUTATION_PHASE;

// Reset the necessary Ranging Phase parameters
Expand All @@ -212,7 +213,6 @@ scheduler_phase_t ranging_phase_begin(uint8_t scheduled_slot, uint8_t schedule_s

// 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
Expand Down
38 changes: 12 additions & 26 deletions software/firmware/src/tasks/ranging/schedule_phase.c
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
// Header Inclusions ---------------------------------------------------------------------------------------------------

#include "logging.h"
#include "ranging_phase.h"
#include "schedule_phase.h"
#include "subscription_phase.h"


// Static Global Variables ---------------------------------------------------------------------------------------------
Expand Down Expand Up @@ -92,7 +92,7 @@ scheduler_phase_t schedule_phase_tx_complete(void)
{
// Forward this request to the next phase if not currently in the Schedule Phase
if (current_phase != SCHEDULE_PHASE)
return ranging_phase_tx_complete();
return subscription_phase_tx_complete();

// Retransmit the schedule up to the specified number of times
next_action_timestamp += SCHEDULE_RESEND_INTERVAL_US;
Expand All @@ -112,27 +112,17 @@ scheduler_phase_t schedule_phase_tx_complete(void)
return SCHEDULE_PHASE;
}

// Move to the Ranging Phase of the ranging protocol
current_phase = RANGING_PHASE;
// Move to the Subscription Phase of the ranging protocol
current_phase = SUBSCRIPTION_PHASE;
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);
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);
}

scheduler_phase_t schedule_phase_rx_complete(schedule_packet_t* schedule)
{
// Forward this request to the next phase if not currently in the Schedule Phase
if (current_phase != SCHEDULE_PHASE)
{
// Determine if a colliding network is present
bool device_found = false;
for (uint8_t i = 0; i < schedule_packet.num_devices; ++i)
if (schedule_packet.schedule[i] == schedule->header.sourceAddr[0])
{
device_found = true;
break;
}
return device_found ? ranging_phase_rx_complete((ranging_packet_t*)schedule) : MESSAGE_COLLISION;
}
return subscription_phase_rx_complete((subscription_packet_t*)schedule);
else if (schedule->header.msgType != SCHEDULE_PACKET)
{
// Immediately restart listening for schedule packets
Expand All @@ -152,7 +142,7 @@ scheduler_phase_t schedule_phase_rx_complete(schedule_packet_t* schedule)
}

// Unpack the received schedule
scheduled_slot = 0;
scheduled_slot = UNSCHEDULED_SLOT;
schedule_packet.epoch_time_unix = schedule->epoch_time_unix;
schedule_packet.num_devices = schedule->num_devices;
for (uint8_t i = 0; i < schedule->num_devices; ++i)
Expand All @@ -164,17 +154,13 @@ scheduler_phase_t schedule_phase_rx_complete(schedule_packet_t* schedule)
for (uint8_t i = schedule->num_devices; i < MAX_NUM_RANGING_DEVICES; ++i)
schedule_packet.schedule[i] = 0;

// Ensure that the schedule included a slot for this device
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.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))
if ((scheduled_slot != UNSCHEDULED_SLOT) && (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;
Expand All @@ -188,17 +174,17 @@ scheduler_phase_t schedule_phase_rx_complete(schedule_packet_t* schedule)
return SCHEDULE_PHASE;
}

// Move to the Ranging Phase of the ranging protocol
current_phase = RANGING_PHASE;
// Move to the Subscription Phase of the ranging protocol
current_phase = SUBSCRIPTION_PHASE;
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);
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);
}

scheduler_phase_t schedule_phase_rx_error(void)
{
// Forward this request to the next phase if not currently in the Schedule Phase
if (current_phase != SCHEDULE_PHASE)
return ranging_phase_rx_error();
return subscription_phase_rx_error();
return RANGING_ERROR;
}

Expand Down
Loading

0 comments on commit 11cdbec

Please sign in to comment.