Skip to content

Commit

Permalink
Completely new super-efficient ranging algorithm
Browse files Browse the repository at this point in the history
  • Loading branch information
@hedgecrw
hedgecrw committed Nov 13, 2023
1 parent 2298770 commit e6b32d2
Show file tree
Hide file tree
Showing 9 changed files with 194 additions and 332 deletions.
2 changes: 1 addition & 1 deletion software/firmware/.cproject
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration artifactName="${ProjName}" buildProperties="" description="" id="ilg.gnuarmeclipse.managedbuild.cross.toolchain.base.1790597438" name="Default" optionalBuildProperties="org.eclipse.cdt.docker.launcher.containerbuild.property.dockerdpath=,org.eclipse.cdt.docker.launcher.containerbuild.property.volumes=,org.eclipse.cdt.docker.launcher.containerbuild.property.selectedvolumes=" parent="org.eclipse.cdt.build.core.emptycfg">
<configuration artifactName="${ProjName}" buildProperties="" description="" id="ilg.gnuarmeclipse.managedbuild.cross.toolchain.base.1790597438" name="Default" optionalBuildProperties="org.eclipse.cdt.docker.launcher.containerbuild.property.dockerdpath=,org.eclipse.cdt.docker.launcher.containerbuild.property.selectedvolumes=,org.eclipse.cdt.docker.launcher.containerbuild.property.volumes=" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="ilg.gnuarmeclipse.managedbuild.cross.toolchain.base.1790597438.1033093985" name="/" resourcePath="">
<toolChain id="ilg.gnuarmeclipse.managedbuild.cross.toolchain.base.1406672748" name="Arm Cross GCC" superClass="ilg.gnuarmeclipse.managedbuild.cross.toolchain.base">
<option id="ilg.gnuarmeclipse.managedbuild.cross.option.architecture.275751269" name="Architecture" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.architecture" value="ilg.gnuarmeclipse.managedbuild.cross.option.architecture.arm" valueType="enumerated"/>
Expand Down
2 changes: 1 addition & 1 deletion software/firmware/launchConfigs/TestBluetooth.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,6 @@
<listAttribute key="org.eclipse.debug.ui.favoriteGroups">
<listEntry value="org.eclipse.debug.ui.launchGroup.debug"/>
</listAttribute>
<stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#10;&lt;memoryBlockExpressionList context=&quot;Context string&quot;/&gt;&#10;"/>
<stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#13;&#10;&lt;memoryBlockExpressionList context=&quot;Context string&quot;/&gt;&#13;&#10;"/>
<stringAttribute key="process_factory_id" value="org.eclipse.cdt.dsf.gdb.GdbProcessFactory"/>
</launchConfiguration>
2 changes: 1 addition & 1 deletion software/firmware/launchConfigs/TestRTCSet.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,6 @@
<listAttribute key="org.eclipse.debug.ui.favoriteGroups">
<listEntry value="org.eclipse.debug.ui.launchGroup.debug"/>
</listAttribute>
<stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#13;&#10;&lt;memoryBlockExpressionList context=&quot;Context string&quot;/&gt;&#13;&#10;"/>
<stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#10;&lt;memoryBlockExpressionList context=&quot;Context string&quot;/&gt;&#10;"/>
<stringAttribute key="process_factory_id" value="org.eclipse.cdt.dsf.gdb.GdbProcessFactory"/>
</launchConfiguration>
10 changes: 4 additions & 6 deletions software/firmware/src/app/app_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,8 +97,7 @@ typedef enum { BATTERY_EMPTY = 3200, BATTERY_CRITICAL = 3500, BATTERY_NOMINAL =
// Ranging Protocol Configuration --------------------------------------------------------------------------------------

#define RADIO_XMIT_CHANNEL 5
#define NUM_XMIT_ANTENNAS 2
#define NUM_RCV_ANTENNAS 2
#define NUM_XMIT_ANTENNAS 3
#define TX_ANTENNA_DELAY 16385
#define RX_ANTENNA_DELAY 16385
#define MIN_VALID_RANGE_MM (-1000)
Expand All @@ -118,11 +117,10 @@ typedef enum { BATTERY_EMPTY = 3200, BATTERY_CRITICAL = 3500, BATTERY_NOMINAL =
#define SCHEDULE_RESEND_INTERVAL_US 1000
#define SCHEDULE_BROADCAST_PERIOD_US (SCHEDULE_NUM_TOTAL_BROADCASTS * SCHEDULE_RESEND_INTERVAL_US)

#define RANGING_NUM_PACKETS_PER_DEVICE 3
#define RANGING_BROADCAST_INTERVAL_US 600
#define RANGING_TIMEOUT_US (RECEIVE_EARLY_START_US + 56)
#define RANGING_NUM_SEQUENCES_PER_RANGE (NUM_XMIT_ANTENNAS * NUM_RCV_ANTENNAS)
#define RANGING_NUM_PACKETS_PER_RANGE (4 * RANGING_NUM_SEQUENCES_PER_RANGE)
#define RANGING_US_PER_RANGE (RANGING_BROADCAST_INTERVAL_US * RANGING_NUM_PACKETS_PER_RANGE)
#define RANGING_NUM_RANGE_ATTEMPTS NUM_XMIT_ANTENNAS
#define RANGING_TIMEOUT_US (RECEIVE_EARLY_START_US + 130)

#define RANGE_STATUS_NUM_TOTAL_BROADCASTS 4
#define RANGE_STATUS_RESEND_INTERVAL_US 1000
Expand Down
1 change: 1 addition & 0 deletions software/firmware/src/tasks/app_task_ranging.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@ static volatile bool devices_found;
static void verify_app_configuration(void)
{
// Retrieve the current state of the application
print("INFO: Verifying TotTag application configuration\n");
const bool is_scanning = bluetooth_is_scanning(), is_ranging = ranging_active();

// Advertising should always be enabled
Expand Down
88 changes: 35 additions & 53 deletions software/firmware/src/tasks/ranging/computation_phase.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,9 @@

// Static Global Variables ---------------------------------------------------------------------------------------------

static ranging_state_t state;
static int distances_millimeters[RANGING_NUM_SEQUENCES_PER_RANGE];
static ranging_device_state_t state[MAX_NUM_RANGING_DEVICES];
static int distances_millimeters[RANGING_NUM_RANGE_ATTEMPTS];
static uint8_t num_scheduled_devices;


// Private Helper Functions --------------------------------------------------------------------------------------------
Expand All @@ -32,95 +33,73 @@ void insert_sorted(int arr[], int new, unsigned end)

// Public API Functions ------------------------------------------------------------------------------------------------

void reset_computation_phase(void)
void reset_computation_phase(uint8_t schedule_length)
{
num_scheduled_devices = schedule_length;
memset(&state, 0, sizeof(state));
}

void add_ranging_times_poll_tx(uint8_t eui, uint8_t sequence_number, uint32_t tx_time)
void associate_eui_with_index(uint32_t index, uint8_t eui)
{
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] = 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] = tx_time;
++state.num_responses;
}
state[index].device_eui = eui;
}

void add_ranging_times_poll_tx(uint32_t index, uint8_t sequence_number, uint32_t tx_time)
{
state[index].poll_tx_times[sequence_number] = tx_time;
}

void add_ranging_times_poll_rx(uint8_t eui, uint8_t sequence_number, uint32_t rx_time)
void add_ranging_times_poll_rx(uint32_t index, 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].poll_tx_times[sequence_number]))
state.responses[state.num_responses-1].poll_rx_times[sequence_number] = rx_time;
state[index].poll_rx_times[sequence_number] = rx_time;
}

void add_ranging_times_response_tx(uint8_t eui, uint8_t sequence_number, uint32_t tx_time)
void add_ranging_times_response_tx(uint32_t index, 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].poll_rx_times[sequence_number]))
state.responses[state.num_responses-1].resp_tx_times[sequence_number] = tx_time;
state[index].resp_tx_times[sequence_number] = tx_time;
}

void add_ranging_times_response_rx(uint8_t eui, uint8_t sequence_number, uint32_t rx_time)
void add_ranging_times_response_rx(uint32_t index, 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;
state[index].resp_rx_times[sequence_number] = rx_time;
}

void add_ranging_times_final_tx(uint8_t eui, uint8_t sequence_number, uint32_t tx_time)
void add_ranging_times_final_tx(uint32_t index, 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;
state[index].final_tx_times[sequence_number] = tx_time;
}

void add_ranging_times_final_rx(uint8_t eui, uint8_t sequence_number, uint32_t rx_time)
void add_ranging_times_final_rx(uint32_t index, 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;
state[index].final_rx_times[sequence_number] = rx_time;
}

void compute_ranges(uint8_t *ranging_results)
{
// Iterate through all responses to calculate the range from this to that device
ranging_results[0] = 0;
uint8_t output_buffer_index = 1;
for (uint8_t dev_index = 0; dev_index < state.num_responses; ++dev_index)
for (uint8_t dev_index = 0; dev_index < num_scheduled_devices; ++dev_index)
{
// 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_PER_RANGE; ++i)
if (state.responses[dev_index].final_rx_times[i])
for (uint8_t i = 0; i < RANGING_NUM_RANGE_ATTEMPTS; ++i)
if (state[dev_index].device_eui && state[dev_index].poll_rx_times[i] && state[dev_index].resp_rx_times[i] && state[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];
const double Rb = state.responses[dev_index].final_rx_times[i] - state.responses[dev_index].resp_tx_times[i];
const double Da = state.responses[dev_index].final_tx_times[i] - state.responses[dev_index].resp_rx_times[i];
const double Db = state.responses[dev_index].resp_tx_times[i] - state.responses[dev_index].poll_rx_times[i];
const double Ra = state[dev_index].resp_rx_times[i] - state[dev_index].poll_tx_times[i];
const double Rb = state[dev_index].final_rx_times[i] - state[dev_index].resp_tx_times[i];
const double Da = state[dev_index].final_tx_times[i] - state[dev_index].resp_rx_times[i];
const double Db = state[dev_index].resp_tx_times[i] - state[dev_index].poll_rx_times[i];
const double TOF = ((Ra * Rb) - (Da * Db)) / (Ra + Rb + Da + Db);
const int distance_millimeters = ranging_radio_time_to_millimeters(TOF);

// Check that the distance we have at this point is at all reasonable
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 0x%02X 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[dev_index].device_eui, i, distance_millimeters);
}

// Skip this device if too few ranging packets were received
Expand All @@ -135,7 +114,7 @@ void compute_ranges(uint8_t *ranging_results)
if (range_millimeters < MAX_VALID_RANGE_MM)
{
// Copy valid ranges into the ID/range output buffer
ranging_results[output_buffer_index++] = state.responses[dev_index].device_eui;
ranging_results[output_buffer_index++] = state[dev_index].device_eui;
*((int16_t*)&ranging_results[output_buffer_index]) = range_millimeters;
output_buffer_index += sizeof(range_millimeters);
++ranging_results[0];
Expand All @@ -146,5 +125,8 @@ void compute_ranges(uint8_t *ranging_results)

bool responses_received(void)
{
return state.num_responses;
for (uint8_t dev_index = 0; dev_index < num_scheduled_devices; ++dev_index)
if (state[dev_index].device_eui)
return true;
return false;
}
27 changes: 11 additions & 16 deletions software/firmware/src/tasks/ranging/computation_phase.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,27 +11,22 @@
typedef struct
{
uint8_t device_eui;
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];
uint32_t poll_tx_times[RANGING_NUM_RANGE_ATTEMPTS], poll_rx_times[RANGING_NUM_RANGE_ATTEMPTS];
uint32_t resp_tx_times[RANGING_NUM_RANGE_ATTEMPTS], resp_rx_times[RANGING_NUM_RANGE_ATTEMPTS];
uint32_t final_tx_times[RANGING_NUM_RANGE_ATTEMPTS], final_rx_times[RANGING_NUM_RANGE_ATTEMPTS];
} ranging_device_state_t;

typedef struct
{
uint8_t num_responses;
ranging_device_state_t responses[MAX_NUM_RANGING_DEVICES];
} ranging_state_t;


// Public API ----------------------------------------------------------------------------------------------------------

void reset_computation_phase(void);
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 reset_computation_phase(uint8_t schedule_length);
void associate_eui_with_index(uint32_t index, uint8_t eui);
void add_ranging_times_poll_tx(uint32_t index, uint8_t sequence_number, uint32_t tx_time);
void add_ranging_times_poll_rx(uint32_t index, uint8_t sequence_number, uint32_t rx_time);
void add_ranging_times_response_tx(uint32_t index, uint8_t sequence_number, uint32_t tx_time);
void add_ranging_times_response_rx(uint32_t index, uint8_t sequence_number, uint32_t rx_time);
void add_ranging_times_final_tx(uint32_t index, uint8_t sequence_number, uint32_t tx_time);
void add_ranging_times_final_rx(uint32_t index, uint8_t sequence_number, uint32_t rx_time);
void compute_ranges(uint8_t *ranging_results);
bool responses_received(void);

Expand Down
Loading

0 comments on commit e6b32d2

Please sign in to comment.