Localization Based on RF Modules Using FPGA

/*
 * 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]));
			}
		}
	}
}