Skip to content

Commit

Permalink
Finish ranging algorithm
Browse files Browse the repository at this point in the history
  • Loading branch information
@hedgecrw
hedgecrw committed Nov 6, 2023
1 parent 862278b commit bd4a256
Show file tree
Hide file tree
Showing 15 changed files with 459 additions and 604 deletions.
2 changes: 1 addition & 1 deletion software/firmware/.settings/language.settings.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider copy-of="extension" id="org.eclipse.cdt.managedbuilder.core.GCCBuildCommandParser"/>
<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-339510081203349785" id="org.eclipse.embedcdt.managedbuild.cross.arm.core.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT Arm Cross GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-495364040879034531" id="org.eclipse.embedcdt.managedbuild.cross.arm.core.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT Arm Cross GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>
Expand Down
2 changes: 1 addition & 1 deletion software/firmware/launchConfigs/TestFull.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;&#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>
28 changes: 15 additions & 13 deletions software/firmware/src/app/app_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 ------------------------------------------------------------------------------------------
Expand Down Expand Up @@ -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)
Expand Down
12 changes: 5 additions & 7 deletions software/firmware/src/peripherals/include/ranging.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,19 +13,18 @@
#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 --------------------------------------------------------------------------------

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];
Expand All @@ -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__
75 changes: 7 additions & 68 deletions software/firmware/src/peripherals/src/ranging.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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 };
Expand Down Expand Up @@ -322,15 +322,16 @@ 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);

// Set this device so that it only receives regular and extended data packets
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)
Expand Down Expand Up @@ -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);
}
84 changes: 41 additions & 43 deletions software/firmware/src/tasks/ranging/computation_phase.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 --------------------------------------------------------------------------------------------
Expand Down Expand Up @@ -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)
Expand All @@ -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];
Expand All @@ -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
Expand Down
Loading

0 comments on commit bd4a256

Please sign in to comment.