Szekely Istvan
Published © GPL3+

Localization Based on RF Modules Using FPGA

Find the location of a device, using multiple DWM1000-ESP8266 pairs, and an FPGA.

IntermediateWork in progress5 hours2,056
Localization Based on RF Modules Using FPGA

Things used in this project

Hardware components

Zybo
Digilent Zybo
×1
ESP8266-07
×1
DWM1000
×1

Software apps and online services

Arduino IDE
Arduino IDE
MATLAB
MATLAB
Vivado HLS

Hand tools and fabrication machines

Soldering iron (generic)
Soldering iron (generic)
UART cable

Story

Read more

Code

Anchor.ino

C/C++
Use this code for the devices, which position is known.
/*
 * Copyright (c) 2015 by Thomas Trojer <thomas@trojer.net>
 * Decawave DW1000 library for arduino.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * @file RangingAnchor.ino
 * Use this to test two-way ranging functionality with two
 * DW1000. This is the anchor component's code which computes range after
 * exchanging some messages. Addressing and frame filtering is currently done
 * in a custom way, as no MAC features are implemented yet.
 *
 * Complements the "RangingTag" example sketch.
 *
 * @todo
 *  - weighted average of ranging results based on signal quality
 *  - use enum instead of define
 *  - move strings to flash (less RAM consumption)
 */
 
#define ESP8266


#include <SPI.h>
#include "DW1000.h"


// connection pins
const uint8_t PIN_RST = 5; // reset pin
const uint8_t PIN_IRQ = 4; // irq pin
const uint8_t PIN_SS = SS; // spi select pin


// currrent device address
#define DEVICE_ADDRESS 1

// messages used in the ranging protocol
// TODO replace by enum
// data[6]
#define POLL 0
#define POLL_ACK 1
#define RANGE 2
#define RANGE_REPORT 3
#define RANGE_FAILED 255
	
// protocol error state
boolean protocolFailed = false;

// watchdog and reset period
uint32_t lastActivity;
uint32_t resetPeriod = 250;

// timestamps to remember
DW1000Time timePollSent;
DW1000Time timePollReceived;
DW1000Time timePollAckSent;
DW1000Time timePollAckReceived;
DW1000Time timeRangeSent;
DW1000Time timeRangeReceived;

// last computed range/time
DW1000Time timeComputedRange;
	
// number of devices
#define NUMBER_OF_ANCHORS 1

// device type
#define TAG 1
#define ANCHOR 0

// target / personal devide type
byte targetDeviceType = TAG; // data[0]
byte personalDeviceType = ANCHOR; // data[1]
	
// all devices in certain type
#define ALL_DEVICE 0

// target / personal device ID
byte targetDeviceID[2]; // data[2,3]
byte personalDeviceID[2]; // data[4,5]
	
// message flow state
volatile byte expectedMsgId;
	
// data buffer
const uint8_t data_length = 7 + (NUMBER_OF_ANCHORS + 2) * 5;
#define LEN_DATA data_length

// data buffer
byte data[LEN_DATA];

// message sent/received state
volatile boolean sentAck = false;
volatile boolean receivedAck = false;

// reply times (same on both sides for symm. ranging)
uint16_t replyDelayTimeUS = 4000;


void noteActivity() {
	// update activity timestamp, so that we do not reach "resetPeriod"
	lastActivity = millis();
}

void handleSent() {
	// status change on sent success
	sentAck = true;
}

void handleReceived() {
	// status change on received success
	receivedAck = true;
}

void resetInactive() {
	// anchor listens for POLL
	receiver();
	noteActivity();
}

void receiver() {
	expectedMsgId = POLL;
	DW1000.newReceive();
	DW1000.setDefaults();
	// so we don't need to restart the receiver manually
	DW1000.receivePermanently(true);
	DW1000.startReceive();
}

void transmitPollAck() {
	expectedMsgId = RANGE;
	DW1000.newTransmit();
	DW1000.setDefaults();
	data[0] = targetDeviceType;
	data[1] = personalDeviceType;
	data[2] = targetDeviceID[1];
	data[3] = targetDeviceID[0];
	data[4] = personalDeviceID[1];
	data[5] = personalDeviceID[0];
	data[6] = POLL_ACK;
	// delay the same amount as ranging tag
	DW1000Time deltaTime = DW1000Time(replyDelayTimeUS, DW1000Time::MICROSECONDS);
	DW1000.setDelay(deltaTime);
	DW1000.setData(data, LEN_DATA);
	DW1000.startTransmit();
}

void transmitRangeReport(float curRange) {
	expectedMsgId = POLL;
	DW1000.newTransmit();
	DW1000.setDefaults();
	data[0] = targetDeviceType;
	data[1] = personalDeviceType;
	data[2] = targetDeviceID[1];
	data[3] = targetDeviceID[0];
	data[4] = personalDeviceID[1];
	data[5] = personalDeviceID[0];
	data[6] = RANGE_REPORT;
	// write final ranging result
	memcpy(data + 7, &curRange, 4);
	DW1000.setData(data, LEN_DATA);
	DW1000.startTransmit();
}

void transmitRangeFailed() {
	expectedMsgId = POLL;
	DW1000.newTransmit();
	DW1000.setDefaults();
	data[0] = targetDeviceType;
	data[1] = personalDeviceType;
	data[2] = targetDeviceID[1];
	data[3] = targetDeviceID[0];
	data[4] = personalDeviceID[1];
	data[5] = personalDeviceID[0];
	data[6] = RANGE_FAILED;
	DW1000.setData(data, LEN_DATA);
	DW1000.startTransmit();
}


/*
 * RANGING ALGORITHMS
 * ------------------
 * Either of the below functions can be used for range computation (see line "CHOSEN
 * RANGING ALGORITHM" in the code).
 * - Asymmetric is more computation intense but least error prone
 * - Symmetric is less computation intense but more error prone to clock drifts
 *
 * The anchors and tags of this reference example use the same reply delay times, hence
 * are capable of symmetric ranging (and of asymmetric ranging anyway).
 */

void DS_TWR_A() {
	// asymmetric double-sided two-way ranging (more computation intense, less error prone)
	DW1000Time round1 = (timePollAckReceived - timePollSent).wrap();
	DW1000Time reply1 = (timePollAckSent - timePollReceived).wrap();
	DW1000Time round2 = (timeRangeReceived - timePollAckSent).wrap();
	DW1000Time reply2 = (timeRangeSent - timePollAckReceived).wrap();
	DW1000Time tof = (round1 * round2 - reply1 * reply2) / (round1 + round2 + reply1 + reply2);
	// set tof timestamp
	timeComputedRange.setTimestamp(tof);
}

void DS_TWR_S() {
	// symmetric double-sided two-way ranging (less computation intense, more error prone on clock drift)
	DW1000Time tof = ((timePollAckReceived - timePollSent) - (timePollAckSent - timePollReceived) +
	  		(timeRangeReceived - timePollAckSent) - (timeRangeSent - timePollAckReceived)) * 0.25f;
	// set tof timestamp
	timeComputedRange.setTimestamp(tof);
}

void SS_TWR() {
	// single-sided two-way ranging
	DW1000Time tof = ((timePollAckReceived - timePollSent) - (timePollAckSent - timePollReceived)) * 0.5f;
	// set tof timestamp
	timeComputedRange.setTimestamp(tof);
}

/*
 * END RANGING ALGORITHMS
 * ----------------------
 */

 
void setup() {
	// DEBUG monitoring
	Serial.begin(115200);
	while (!Serial);
  
	// initialize the driver
	DW1000.begin(PIN_IRQ, PIN_RST);
	DW1000.select(PIN_SS);
  
	// general configuration
	DW1000.newConfiguration();
	DW1000.setDefaults();
	uint16_t dev_addr = DEVICE_ADDRESS;
	DW1000.setDeviceAddress(dev_addr);
		memcpy(personalDeviceID, &dev_addr, 2);
	DW1000.setNetworkId(10);
	DW1000.enableMode(DW1000.MODE_SHORTDATA_FAST_ACCURACY);
	DW1000.commitConfiguration();

	// attach callback for (successfully) sent and received messages
	DW1000.attachSentHandler(handleSent);
	DW1000.attachReceivedHandler(handleReceived);

	// anchor starts in receiving mode, awaiting a ranging poll message
	receiver();
	noteActivity();
}


void loop() {
	int32_t curMillis = millis();
	if (!sentAck && !receivedAck) {
		// check if inactive
		if (curMillis - lastActivity > resetPeriod) {
			resetInactive();
		}
		return;
	}
	  
	// continue on any success confirmation
	if (sentAck) {
		sentAck = false;
		byte msgId = data[0];
		if (msgId == POLL_ACK) {
			DW1000.getTransmitTimestamp(timePollAckSent);
			noteActivity();
		}
	}
	if (receivedAck) {
	receivedAck = false;
	// get message and parse
		DW1000.getData(data, LEN_DATA);
		if (data[0] == ANCHOR) {
			if (data[2] == ALL_DEVICE && data[3] == ALL_DEVICE) {
				if (data[6] != expectedMsgId) {
					// unexpected message, start over again (except if already POLL)
					protocolFailed = true;
				}
				switch (data[6]) {
					case POLL:
						// timestamp
						protocolFailed = false;
						DW1000.getReceiveTimestamp(timePollReceived);
						
						// prepare data for transmission
						targetDeviceID[1] = data[4];
						targetDeviceID[0] = data[5];
						
						// wait
						delay(((uint16_t)((personalDeviceID[1] << 8) | personalDeviceID[0]) - 1) * 25);
						
						// transmit poll ack
						transmitPollAck();
						
						// wait
						delay((NUMBER_OF_ANCHORS - (uint16_t)((personalDeviceID[1] << 8) | personalDeviceID[0])) * 25);
						
						noteActivity();
						break;
					case RANGE:
						// timestamp
						DW1000.getReceiveTimestamp(timeRangeReceived);
						
						// wait
						delay(((uint16_t)((personalDeviceID[1] << 8) | personalDeviceID[0]) - 1) * 25);
							
						if (!protocolFailed) {
							// report back
							timePollSent.setTimestamp(data + 7);
							timePollAckReceived.setTimestamp(data + 7 + (uint16_t)((personalDeviceID[1] << 8) | personalDeviceID[0]) * 5);
							timeRangeSent.setTimestamp(data + 12 + NUMBER_OF_ANCHORS * 5);
							// (re-)compute range as two-way ranging is done
							DS_TWR_A(); // chosen ranging algorithm
							//DS_TWR_S();
							//SS_TWR();
							float distance = timeComputedRange.getAsMeters();
							transmitRangeReport(distance);
							
							Serial.println(distance);
						}
						else {
							transmitRangeFailed();
						}
						
						// wait
						delay((NUMBER_OF_ANCHORS - (uint16_t)((personalDeviceID[1] << 8) | personalDeviceID[0])) * 25);

						noteActivity();
						break;
				}
			}
		}
		else { // TAG
			// wait
			if (data[6] != RANGE_FAILED) {
				delay(25*(3-data[6]));
			}
		}
	}
}

Tag.ino

C/C++
Use this code for the devices, which position has to be calculated.
/*
 * Copyright (c) 2015 by Thomas Trojer <thomas@trojer.net>
 * Decawave DW1000 library for arduino.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * @file RangingTag.ino
 * Use this to test two-way ranging functionality with two DW1000. This is
 * the tag component's code which polls for range computation. Addressing and
 * frame filtering is currently done in a custom way, as no MAC features are
 * implemented yet.
 *
 * Complements the "RangingAnchor" example sketch.
 *
 * @todo
 *  - use enum instead of define
 *  - move strings to flash (less RAM consumption)
 */

#define ESP8266


#include <SPI.h>
#include "DW1000.h"


// connection pins
const uint8_t PIN_RST = 5; // reset pin
const uint8_t PIN_IRQ = 4; // irq pin
const uint8_t PIN_SS = SS; // spi select pin


// currrent device address
#define TAG_NUMBER 1
const uint8_t deviceAddress = 256 * TAG_NUMBER;
#define DEVICE_ADDRESS deviceAddress

// messages used in the ranging protocol // data[4]
// TODO replace by enum
#define POLL 0
#define POLL_ACK 1
#define RANGE 2
#define RANGE_REPORT 3
#define RANGE_FAILED 255

// message flow state
volatile byte expectedMsgId = POLL_ACK;
	
// watchdog and reset period
uint32_t lastActivity;
uint32_t resetPeriod = 250;
	
// number of devices
#define NUMBER_OF_ANCHORS 1
#define NUMBER_OF_TAGS 1

// range of different devices
float currentRangeAnchors[NUMBER_OF_ANCHORS];
float currentRangeTags[NUMBER_OF_TAGS];

// device counter
uint8_t anchorCounter = 0;

// device type
#define TAG 1
#define ANCHOR 0

// target / personal devide type
byte targetDeviceType; //data[0]
byte personalDeviceType = TAG; //data[1]

// all devices in certain type
#define ALL_DEVICE 0

// target / personal device ID
byte targetDeviceID[2]; // data[2,3]
byte personalDeviceID[2]; // data[4,5]
		
// data buffer
const uint8_t data_length = 7 + (NUMBER_OF_ANCHORS + 2) * 5;
#define LEN_DATA data_length
		
// timestamps to remember
DW1000Time timePollAckReceived[NUMBER_OF_ANCHORS];
		
// status of the ranging
boolean rangingDone;
		
// identify any ranging problems
boolean rangingFailed;

// data buffer
byte data[LEN_DATA];

// message sent/received state
volatile boolean sentAck = false;
volatile boolean receivedAck = false;

// reply times (same on both sides for symm. ranging)
uint16_t replyDelayTimeUS = 4000;

// timestamps to remember
DW1000Time timePollSent;
DW1000Time timeRangeSent;


void noteActivity() {
    // update activity timestamp, so that we do not reach "resetPeriod"
    lastActivity = millis();
}

void handleSent() {
    // status change on sent success
    sentAck = true;
}

void handleReceived() {
    // status change on received success
    receivedAck = true;
}

void receiver() {
    DW1000.newReceive();
    DW1000.setDefaults();
    // so we don't need to restart the receiver manually
    DW1000.receivePermanently(true);
    DW1000.startReceive();
}

void resetInactive() {
	// tag sends POLL and listens for POLL_ACK
	transmitPoll();
	noteActivity();
}
  
void transmitPoll() {
	rangingDone = false;
	rangingFailed = false;
	expectedMsgId = POLL_ACK;
	DW1000.newTransmit();
	DW1000.setDefaults();
	data[0] = targetDeviceType;
	data[1] = personalDeviceType;
	data[2] = targetDeviceID[1];
	data[3] = targetDeviceID[0];
	data[4] = personalDeviceID[1];
	data[5] = personalDeviceID[0];
	data[6] = POLL;
	DW1000.setData(data, LEN_DATA);
	DW1000.startTransmit();
}
  
void transmitRange() {
	expectedMsgId = RANGE_REPORT;
	DW1000.newTransmit();
	DW1000.setDefaults();
	data[0] = targetDeviceType;
	data[1] = personalDeviceType;
	data[2] = targetDeviceID[1];
	data[3] = targetDeviceID[0];
	data[4] = personalDeviceID[1];
	data[5] = personalDeviceID[0];
	data[6] = RANGE;
	// delay sending the message and remember expected future sent timestamp
	DW1000Time deltaTime = DW1000Time(replyDelayTimeUS, DW1000Time::MICROSECONDS);
	timeRangeSent = DW1000.setDelay(deltaTime);
	timePollSent.getTimestamp(data + 7);
	int i;
	for (i=0; i<NUMBER_OF_ANCHORS; ++i) {
		timePollAckReceived[i].getTimestamp(data + 12 + i * 5);
	}
	timeRangeSent.getTimestamp(data + 12 + i * 5);
	DW1000.setData(data, LEN_DATA);
	DW1000.startTransmit();
	//Serial.print("Expect RANGE to be sent @ "); Serial.println(timeRangeSent.getAsFloat());
}
	
void wrongAnchor() {
	delay((NUMBER_OF_ANCHORS - anchorCounter) * 20);
	anchorCounter = 0;
	transmitPoll();
}


void setup() {
    // DEBUG monitoring
    Serial.begin(115200);
	while (!Serial);
    
    // initialize the driver
    DW1000.begin(PIN_IRQ, PIN_RST);
    DW1000.select(PIN_SS);

    // general configuration
    DW1000.newConfiguration();
    DW1000.setDefaults();
    uint16_t dev_addr = DEVICE_ADDRESS;
    DW1000.setDeviceAddress(dev_addr);
	memcpy(personalDeviceID, &dev_addr, 2);
    DW1000.setNetworkId(10);
    DW1000.enableMode(DW1000.MODE_SHORTDATA_FAST_ACCURACY);
    DW1000.commitConfiguration();
  
    // attach callback for (successfully) sent and received messages
    DW1000.attachSentHandler(handleSent);
    DW1000.attachReceivedHandler(handleReceived);
    
    // anchor starts by transmitting a POLL message
    receiver();
	
	targetDeviceID[0] = ALL_DEVICE;
	targetDeviceID[1] = ALL_DEVICE;
	
	transmitPoll();
    noteActivity();
}


void loop() {
	int32_t curMillis = millis();
    if (!sentAck && !receivedAck) {
        // check if inactive
        if (curMillis - lastActivity > resetPeriod) {
            resetInactive();
        }
        return;
    }
  
    // continue on any success confirmation
    if (sentAck) {
        sentAck = false;
        byte msgId = data[0];
        if (msgId == POLL) {
            DW1000.getTransmitTimestamp(timePollSent);
            //Serial.print("Sent POLL @ "); Serial.println(timePollSent.getAsFloat());
        } else if (msgId == RANGE) {
            DW1000.getTransmitTimestamp(timeRangeSent);
            noteActivity();
        }
    }
    if (receivedAck) {
        receivedAck = false;
        // get message and parse
		DW1000.getData(data, LEN_DATA);
		if (data[0] == ANCHOR) {
			// wait
		}
		else { // TAG
			if (data[2] == personalDeviceID[1] && data[3] == personalDeviceID[0]) { 
				// Anchor
				if (data[1] == ANCHOR) {
					if (data[6] != expectedMsgId) {
						// unexpected message, start over again
						//Serial.print("Received wrong message # "); Serial.println(data[6]);
						if (expectedMsgId == RANGE_REPORT) {
							rangingFailed = true;
						}
						else {
							// REPALCEMENT NEEDED
							transmitPoll();
						}
					}
					uint8_t anchorNumber = (data[4] << 8 | data[5]) - 1;
					switch (data[6]) {
						case POLL_ACK:
							if (anchorNumber == anchorCounter) {
								DW1000.getReceiveTimestamp(timePollAckReceived[anchorNumber]);
								++anchorCounter;
								if (anchorCounter == NUMBER_OF_ANCHORS) {
									anchorCounter = 0;
									delay(15);
									transmitRange();
								}
							} 
							else wrongAnchor();
							
							noteActivity();
							break;
						case RANGE_REPORT:
						case RANGE_FAILED:
							if (anchorNumber == anchorCounter) {
								++anchorCounter;
								if (anchorCounter == NUMBER_OF_ANCHORS) {
									rangingDone = true;
									anchorCounter = 0;
								}
								
								if (!rangingFailed) {
									if (!Serial) {
										Serial.begin(115200);
										while (!Serial);
									}
								}
								// wait
							}
							else wrongAnchor();
							
							if (rangingDone) {
								delay(15);
								transmitPoll();
							}
							
							noteActivity();
							break;
					}
				}
				// Tag
				else {
					
				}
			}
			else {
				// wait
			}
		}
    }
}

Github

This repository contains the necessry srource files, to be able to upload the code to the ESP.

Credits

Szekely Istvan

Szekely Istvan

1 project • 3 followers

Comments