Time Of Flight (TOF) Initiator

Introduction
Hardware Prerequisites
Software Prerequisites
Functional Description
- Software Overview
- Limitation
Usage

Introduction

The tof_initiator project implements a simple Bluetooth low energy central device with GATT client functionality. This project can be run on various platforms, including the CC2640R2 Launchpad. By default, the tof_initiator application is configured to filter and connect to peripheral devices with the TI ToF Profile UUID. To modify this behavior, set DEFAULT_DEV_DISC_BY_SVC_UUID to FALSE in tof_initiator.

ToF uses a primary packet format that is different form standard BLE packets. Its physical layer properties are different. It also uses a 64-bit syncword. ToF packets require a TI LaunchPad to detect, but can be interleaved with a standard BLE connection

Hardware Prerequisites

The default ToF Initiator configuration uses the LAUNCHXL-CC2640R2. This hardware configuration is shown in the below image:

For custom hardware, see the Running the SDK on Custom Boards section of the BLE-Stack User’s Guide for Bluetooth 4.2.

Software Prerequisites

For information on what versions of Code Composer Studio and IAR Embedded Workbench to use, see the Release Notes located in the docs/blestack folder. For information on how to import this project into your IDE workspace and build/run, please refer to The CC2640R2F Platform section in the BLE-Stack User’s Guide for Bluetooth 4.2.

Functional Description

Software Overview

Application

Project Files	Description
tof_initiator.c	Top level application. Initialization of hardware, connection settings, button handling and ToF packets sending/receiving.
TOF.c	Implements the ToF control functions. Extracts rssi value and calculate tick numbers in order to determine the distance between 2 devices.
TOFExampleDatasets.c	list of syncwords used in the application.

SyncWords

This file is shared between tof_initiator, tof_responder and tof_passive, to change or increase the list of sync words, you can edit the following array.

//size of pSyncWord needs to be able to fit numSw*2 since 64bit sync words
uint64_t syncWords[TOF_NUM_RFC_BURST_PKTS] =
{
       0x4bb12e98463d7a1e,
       0xdcf2c942287747ae,
       0xff01da71ddf05ac5,
       0xef8ce65dfda4aaff,
       0xcbddcae9aa13eac5,
       0x7b99f603318cb31b,
       0x52f3af7b49c35116,
       0x29abdddf9627992a,
       0x839d185f7e711de3,
       0xd1be9f4070a589a4,
       0xda9c82c0e597cb38,
       0x254aef8b8bdbc5fd,

Increasing the number of burst samples(syncwords) can increase the accuracy, however, the max number of samples allowed depends on how much RAM is left in the application, and also how much time you have between each connection interval.

If you have longer connection interval, you will be able to do more bursts, but again you will have to check how much RAM you have. By increasing 2 syncwords(one for initiator one for responder), the result sample array (ToF_Sample)will increase by 8 bytes, and each syncword also uses 8 bytes (64-bit)

Data Processing

After finishing one burst(run through all the synwords for every frequency channel), the data will be extracted and processed in tof_initiator.c: TOFInitiator_postProcessTof(uint8_t status).

Inside this function, there are data extraction and data filtering, which can all be customized. Here we provide an example of what you can do with the data.

TOF_getBurstStat

First we will take a look function TOF.c :: void TOF_getBurstStat()

The result is returned from radio core in the following format:

typedef struct
{
    uint8_t freqIndex;
    int8_t RSSI;
    uint32_t T1; // = [31..8].[7..0], T1 = time << 8 + (stim/16)<<8 = time << 8 + stim << 4
} rfc_CMD_TOF_sample_t;

From bit[31:8] contains the radio clock tick value for each ToF packet to travel from initiator to responder and back to initiator.

Data extraction:

    for (uint32_t i = 0; i < handle->numBurstSamples / 2; i++, pSample++)
    {
        uint32_t t1 = pSample->T1;

        if (t1 != TOF_TIMEOUTVAL && t1 != 0)
        {
            stats[pSample->freqIndex].tick += pSample->T1 / 256;
            stats[pSample->freqIndex].numOk++;
        }
    }

Data averaging per frequency channel:

    for (uint32_t i = 0; i < handle->numFreqs; ++i)
    {
        stats[i].freq = handle->synthCal[i].freq;
        stats[i].tick /= stats[i].numOk;
    }

Then calculate the variance:

    // Find the variance
    pSample = (ToF_Sample *)handle->pT1RSSIBuf;
    for (uint32_t i = 0; i < handle->numBurstSamples / 2; i++, pSample++)
    {
        uint32_t t1 = pSample->T1;
        if (t1 != TOF_TIMEOUTVAL && t1 != 0)
        {
            double dev = pSample->T1 / 256 - stats[pSample->freqIndex].tick;
            stats[pSample->freqIndex].tickVariance += dev * dev;
        }
    }

    for (uint32_t i = 0; i < handle->numFreqs; ++i)
    {
        stats[i].tickVariance /= stats[i].numOk;
    }

TOFInitiator_calculateRSSI

In tof_initiator.c :: void TOFInitiator_calculateRSSI()

// The maximum value for alpha in the RSSI filter
#define TOF_ALPHA_FILTER_MAX_VALUE            16

// The larger this number is, the effect which the last
// sample will have on RSSI is greater
#define TOF_ALPHA_FILTER_VALUE                4

static void TOFInitiator_calculateRSSI(int lastRssi)
{
  if (TOF_IS_VALID_RSSI(lastRssi))
  {
    tofInitiatorRssi.currentRssi =
        ((TOF_ALPHA_FILTER_MAX_VALUE - tofInitiatorRssi.alpha) * (tofInitiatorRssi.currentRssi) + tofInitiatorRssi.alpha * lastRssi) >> 4;
  }
}

The function is essentially the same as currentRssi = (0.75) x currentRssi + (0.25) x lastRssi. This is a low pass filter which gives more weight to the new rssi value than the old one. This can be customized to whatever suits you better as long as the sum of the weight equals 1(0.75 + 0.25).

TOFInitiator_calcMovingAverage

In tof_initiator.c :: void TOFInitiator_calcMovingAverage(), the following code is Infinite Impulse Response filter.

// IIR filter values
#define TOF_FILTER_ATTACK                     0.3
#define TOF_FILTER_DECAY                      0.3

    avg[i].tick         =  IIR_AVG(TOF_FILTER_ATTACK, TOF_FILTER_DECAY, avg[i].tick, cur[i].tick);
    avg[i].tickVariance =  IIR_AVG(TOF_FILTER_ATTACK, TOF_FILTER_DECAY, avg[i].tickVariance, cur[i].tickVariance);

#define IIR_AVG(attack, decay, old, new) (new > old ? (attack * (float)new + (1-attack) * (float)old) : (decay * (float)new + (1-decay) * (float)old))

Which is 0.3 x tick_new + 0.7 x tick_old if we plug in all the defined numbers.

Here we give more weight to the old data and the thought behind this filter is that a person doesn’t move very fast so this means one measurement is very likely measuring a very similar physical distance to the previous. This is not necessarily true if the measurements are done with a long delay in between, but in the example we do measurements as fast and often as we can.

You can use custom filter to calculate moving average.

TOFInitiator_tofGetTotalAvg

In tof_initiator.c :: void TOFInitiator_tofGetTotalAvg(), we average the tick/tick_variance/numOk over all the frequency channels.

The tick value then is converted to meters by subtracting the calibrated tick value at 0 meters and multiplying with (6.25 * 3). The explanation regarding tick to meter ratio can be found in our software user’s guide

Triggered by RSSI

#define TOF_RSSI_THRESHOLD                    -50
#define TOF_RSSI_THRESHOLD_HYSTERESIS         -5

// Initial RSSI value for the alpha filter (first dummy sample)
#define TOF_ALPHA_FILTER_INITIAL_RSSI         -55

Once the Toggle Auto ToF feature is set, tof will be triggered based on TOF_RSSI_THRESHOLD and TOF_ALPHA_FILTER_INITIAL_RSSI.

If the current rssi is lower than TOF_RSSI_THRESHOLD + TOF_RSSI_THRESHOLD_HYSTERESIS, then the tof_initiator will disable tof at the responder end.

Those are the values you can modify yourself.

Limitation

TOF.c :: static void TOF_configureRunTime(ToF_Handle handle, uint32_t nextTaskTime)

The function above calculates and configures how many ToF samples will be run between each connection event (up to your configured maximum), given the nextTaskTime provided by the BLE connection event callback, which is the relative time until the next connection event.

However, since the BLE stack returns relative time, the BLE STACK does not know how much time it actually has left till the next connection event after one ToF burst, then it will just do only one ToF burst no matter how much time it has left till the next connection event.

Usage

This application uses the UART peripheral to provide an interface for the application. This document will use Tera Term to serve as the display for the output of the CC2640R2 LaunchPad. Note that any other serial terminal can be used. The following default parameters are used for the UART peripheral for display:

UART Param	Default Values
Baud Rate	115200
Data length	8 bits
Parity	None
Stop bits	1 bit
Flow Control	None

Option: Toggle ToF Run/Non-connected ToF

Pressing ToF Run without establishing a BLE connection will cause the device to start looking for ToF packets immediately. Note that the other side (ToF Responder must be manualy set to start ToF as well).

** To get the best results it’s recommended to calibrate the devices by setting them exactly 1 meter apart before enabling ToF Run. **

Option: BLE Connection + ToF

Once the ToF Initiator sample application starts, the output to the terminal will report its address and that it is initialized and ready to begin discovery, as shown below:

As shown, the right button (BTN-2 on the CC2640R2 LaunchPad) can be pressed to begin discovering devices that are broadcasting BLE advertisements. This will report the number of discovered devices:

The left button (BTN-1) can be pressed to go through the available devices. When you have found the device that you are trying to connect to, press the right button to connect. This will display the connected device’s address. Once the TOF service discovery process is complete we will be notified in the following manner:

There are now 3 possible options:

Toggle ToF Run
Toggle Auto ToF
Disconnect

These options can be selected by pressing the right button.

Option: Toggle ToF Run/Connected ToF

This option is used to send a ToF request to the ToF Responder. Once the request is sent, both devices will perform ToF runs on each connection event.