[feature] DW1000 Driver, 802.15.4 Frame & Ranging

Added driver for DW1000 with generic and STM32F4_disco examples Added MAC Frame for IEEE 802.15.4 with unitttests Added Ranging algorithms for UWB modules with unittests
roboterclubaachen · Jun 4, 2018 · 45ea976 · 45ea976
1 parent a1b5a1b
commit 45ea976
Show file tree

Hide file tree

Showing 83 changed files with 8,789 additions and 6 deletions.
diff --git a/.Rhistory b/.Rhistory
diff --git a/..._discovery/radio/nrf24-data/rx/SConstruct → ...ided_two-way_ranging/initiator/SConstruct b/..._discovery/radio/nrf24-data/rx/SConstruct → ...ided_two-way_ranging/initiator/SConstruct
diff --git a/examples/generic/dw1000/double-sided_two-way_ranging/initiator/main.cpp b/examples/generic/dw1000/double-sided_two-way_ranging/initiator/main.cpp
@@ -0,0 +1,195 @@
+/* This is an Example application for one STM32 in combination with a Decawave DM1000 Chip
+*
+* Connections are the following:
+*
+* USART:       STM32F4&F103 Pin
+* TXD          -> PA2
+* RXD          -> PA3 (not used in this example)
+*
+* DM1000 -> SPI-Master-2
+* SPI:		 STM32F4 Pin	   STM32F107
+* Miso         -> PC02		   PB14
+* Mosi         -> PB15		   PB15
+* Clock        -> PB10		   PB13
+*
+* RES          -> PA08		   PA00 (not used in this example)
+* Chipselect   -> PE08		   PB12
+* IRQ          -> PD10		   PB08(not used in this example)
+ *
+ *
+ * What this program does is:
+ * 1) Sends out a DSTW-Ranging Frame on Broadcast
+ * 2) Waits for an Answer and Computes the Range between the both
+ *
+ * For 1 Ranging there will be 4 messages but this technique is more precise than SSTW
+ *
+ *
+ * Init -> Resp |Payload{init DSTWR}
+ * Resp -> Init |Payload{receive and transmittime of Resp}
+ * Init -> Resp |Payload{receive and transmittime of Init}
+ * Resp -> Init |Payload{Time of flight of second ranging round}
+*/
+
+
+#include <xpcc/architecture/platform.hpp>					// include for platform
+#include <xpcc/debug/logger.hpp>							// include for DW1000 Error/Debug Messaging
+#include <xpcc/driver/radio/dw1000/dw1000.hpp>				// DW1000 Driver
+#include <xpcc/communication/Frame802154/Frame802154.hpp>	// MAC Frame for IEEE 802.15.4
+#include <xpcc/positioning/ranging.hpp>						// ranging operations
+#include <xpcc/processing/timer.hpp>						// Timer and Timeouts
+
+//-------------------------Namespaces and renaming----------------------------------
+// change things up for each individual board
+using namespace Board;
+using SPI		=	SpiMaster2;
+using SPISCK	=	GpioOutputB10;
+using SPIMISO	=	GpioInputC2;
+using SPIMOSI	=	GpioOutputB15;
+using RES		=	GpioOutputA8;
+using CS		=	GpioOutputE8;
+using IRQ		=	GpioInputD10;
+
+
+using dwm = xpcc::Dw1000 < SPI, CS, RES, IRQ >;
+using ranging = xpcc::Ranging < dwm >;
+
+//------------------------------LOGGER----------------------------------------------
+#undef	XPCC_LOG_LEVEL
+#define	XPCC_LOG_LEVEL xpcc::log::INFO
+xpcc::IODeviceWrapper< Usart2, xpcc::IOBuffer::BlockIfFull > loggerDevice;
+
+// Set all four logger streams to use the UART
+xpcc::log::Logger xpcc::log::debug(loggerDevice);
+xpcc::log::Logger xpcc::log::info(loggerDevice);
+xpcc::log::Logger xpcc::log::warning(loggerDevice);
+xpcc::log::Logger xpcc::log::error(loggerDevice);
+
+//------------------------------DWM Config------------------------------------------------
+
+static xpcc::dw1000::config_t config =
+{
+	2,								/* Channel number. */
+	xpcc::dw1000::PRF_64M,         /* Pulse repetition frequency. */
+	xpcc::dw1000::PLEN_128,        /* Preamble length. Used in TX only. */
+	xpcc::dw1000::PAC8,            /* Preamble acquisition chunk size. Used in RX only. */
+	9,								/* TX preamble code. Used in TX only. */
+	9,								/* RX preamble code. Used in RX only. */
+	0,								/* 0 to use standard SFD, 1 to use non-standard SFD. */
+	xpcc::dw1000::BR_6M8,			/* Data rate. */
+	xpcc::dw1000::PHRMODE_STD,		/* PHY header mode. */
+	(129 + 8 - 8)					/* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
+};
+
+static constexpr uint16_t hostaddress= 0xBBBB;
+
+
+xpcc::Frame802154 receiveframe;
+xpcc::ShortTimeout timeout;
+bool isrx = false;
+double distance;
+uint8_t buffer[1024];
+
+
+//-------------------------------MAIN-----------------------------------------------------
+int
+main()
+{
+
+	//setup USART
+	GpioOutputA2::connect(Usart2::Tx);
+	GpioInputA3::connect(Usart2::Rx, Gpio::InputType::PullUp);
+	Usart2::initialize<Board::systemClock, 115200>(12);
+	//initialize the board
+	Board::initialize();
+	XPCC_LOG_INFO << "GENERIC DSTW INIT v1.1"<< xpcc::endl;
+	//activate the CS on the DW1000
+	CS::setOutput(xpcc::Gpio::High);
+	//setup SPI
+	SPIMOSI::connect(SPI::Mosi);
+	SPIMISO::connect(SPI::Miso);
+	SPISCK::connect(SPI::Sck);
+	SPI::initialize<Board::systemClock,2500000,xpcc::Tolerance::DontCare>();
+	SPI::setDataMode(SPI::DataMode::Mode0);
+	//Init with the config
+	while (!(dwm::init(config)))
+	{
+		Board::Leds::toggle();
+		xpcc::delayMilliseconds(100);
+	}
+	/* Apply antenna delay value.*/
+	dwm::setRXAntennaDelay(dwm::standardAntennaDelay);
+	dwm::setTXAntennaDelay(dwm::standardAntennaDelay);
+	dwm::hostaddress = hostaddress;
+
+	while (true)
+	{
+		Board::Leds::toggle();
+
+		//--------------------------------------- Start sending init ---------------------------------------------------------------
+		XPCC_LOG_DEBUG << "Send init"<< xpcc::endl;
+		ranging::sendDSTWRinit();
+		timeout.restart(5);
+		while(not(dwm::isFrameSent()||timeout.isExpired()))
+		{}
+		Board::Leds::toggle();
+
+		//------------------------------------------------------------ Receive Message--------------------------------------------------------------------
+		timeout.restart(200);
+		dwm::rxEnable();
+		while(not(dwm::checkForRXError() || (isrx = dwm::checkForRX())  || timeout.isExpired()))
+		{}
+		if (isrx)
+		{
+			Board::Leds::toggle();
+			dwm::readrx(dwm::rxlength(),buffer);
+			receiveframe.loadFrame(dwm::rxlength(),buffer);
+			if (receiveframe.getDestinationAddress16() == hostaddress)
+			{
+				if (ranging::IsRangingFrame(receiveframe))
+				{
+
+					//--------------------------------------- Start get distance and send return ---------------------------------------------------------------
+					distance = ranging::computeSsTwrDistance(receiveframe);
+					dwm::rxdisable();
+					ranging::sendAnswer(receiveframe);
+					//---------------------------------------Wait for TOF from other device---------------------------------------------------------------------
+					timeout.restart(5);
+					while(not(dwm::isFrameSent()||timeout.isExpired())){}
+					dwm::rxreset();
+					dwm::rxEnable();
+					isrx = false;
+					timeout.restart(150);
+					while(not(dwm::checkForRXError() || (isrx = dwm::checkForRX())  || timeout.isExpired())){}
+					if (isrx)
+					{
+						dwm::readrx(dwm::rxlength(),buffer);
+						receiveframe.loadFrame(dwm::rxlength(),buffer);
+						if (receiveframe.getDestinationAddress16() == hostaddress)
+						{
+							if (ranging::IsRangingFrame(receiveframe))
+							{
+								distance = ranging::computeDsTwrDistance(distance,receiveframe);
+								XPCC_LOG_INFO.printf("DISTANCE TO %0x IS %5.3fm \n",receiveframe.getSourceAddress16(),distance);
+							}
+
+						}
+					}
+				}
+				Board::Leds::toggle();
+			}
+			else
+			{
+				dwm::frame_seq_nb = receiveframe.getSequenceNumber() + 1 ;
+				XPCC_LOG_DEBUG << "Wrong Answer --- Address was wrong"<< xpcc::endl;
+			}
+		}
+
+		else if (dwm::checkForRXError())
+		{
+			XPCC_LOG_ERROR<< "RX--ERROR!"<< xpcc::endl;
+		}
+		dwm::rxdisable();
+		while(not(timeout.isExpired())){}
+
+	}
+}
diff --git a/examples/generic/dw1000/double-sided_two-way_ranging/initiator/project.cfg b/examples/generic/dw1000/double-sided_two-way_ranging/initiator/project.cfg
@@ -0,0 +1,22 @@
+[build]
+#board = al_avreb_can
+#board = arduino_uno
+#board = nucleo_f031k6
+#board = nucleo_f103rb
+#board = nucleo_f303k8
+#board = nucleo_f401re
+#board = nucleo_f411re
+#board = nucleo_f429zi
+#board = nucleo_l476rg
+#board = stm32f0_discovery
+#board = stm32f072_discovery
+#board = stm32f103c8t6_blue_pill
+#board = stm32f1_discovery
+#board = stm32f3_discovery
+#board = stm32f429_discovery
+#board = stm32f469_discovery
+board = stm32f4_discovery
+#board = stm32f746g_discovery
+#board = stm32f769i_discovery
+
+buildpath = ${xpccpath}/build/examples/generic/${name}
diff --git a/..._discovery/radio/nrf24-data/tx/SConstruct → ...ided_two-way_ranging/responder/SConstruct b/..._discovery/radio/nrf24-data/tx/SConstruct → ...ided_two-way_ranging/responder/SConstruct
diff --git a/examples/generic/dw1000/double-sided_two-way_ranging/responder/main.cpp b/examples/generic/dw1000/double-sided_two-way_ranging/responder/main.cpp
@@ -0,0 +1,170 @@
+/* This is an Example application for one STM32 in combination with a Decawave DM1000 Chip
+*
+* Connections are the following:
+*
+* USART:       STM32F4&F103 Pin
+* TXD          -> PA2
+* RXD          -> PA3 (not used in this example)
+*
+* DM1000 -> SPI-Master-2
+* SPI:		 STM32F4 Pin	   STM32F107
+* Miso         -> PC02		   PB14
+* Mosi         -> PB15		   PB15
+* Clock        -> PB10		   PB13
+*
+* RES          -> PA08		   PA00 (not used in this example)
+* Chipselect   -> PE08		   PB12
+* IRQ          -> PD10		   PB08(not used in this example)
+*
+*
+ * What this program does is:
+ * 1) Sends out a DSTW-Ranging Frame on Broadcast
+ * 2) Waits for an Answer and Computes the Range between the both
+ *
+ * For 1 Ranging there will be 4 messages but this technique is more precise than SSTW
+ *
+ *
+ * Init -> Resp |Payload{init DSTWR}
+ * Resp -> Init |Payload{receive and transmittime of Resp}
+ * Init -> Resp |Payload{receive and transmittime of Init}
+ * Resp -> Init |Payload{Time of flight of second ranging round}
+*/
+
+
+#include <xpcc/architecture/platform.hpp>					// include for platform
+#include <xpcc/debug/logger.hpp>							// include for DW1000 Error/Debug Messaging
+#include <xpcc/driver/radio/dw1000/dw1000.hpp>			// DW1000 Driver
+#include <xpcc/communication/Frame802154/Frame802154.hpp>	// MAC Frame for IEEE 802.15.4
+#include <xpcc/positioning/ranging.hpp>						// ranging operations for DW1000
+#include <xpcc/processing/timer.hpp>						// Timer and Timeouts
+
+//-------------------------Namespaces and renaming----------------------------------
+// change things up for each individual board
+using namespace Board;
+using SPI		=	SpiMaster2;
+using SPISCK	=	GpioOutputB10;
+using SPIMISO	=	GpioInputC2;
+using SPIMOSI	=	GpioOutputB15;
+using RES		=	GpioOutputA8;
+using CS		=	GpioOutputE8;
+using IRQ		=	GpioInputD10;
+
+
+using dwm = xpcc::Dw1000 < SPI, CS, RES, IRQ >;
+using ranging = xpcc::Ranging < dwm >;
+
+//------------------------------LOGGER----------------------------------------------
+#undef	XPCC_LOG_LEVEL
+#define	XPCC_LOG_LEVEL xpcc::log::DISABLED
+xpcc::IODeviceWrapper< Usart2, xpcc::IOBuffer::BlockIfFull > loggerDevice;
+
+// Set all four logger streams to use the UART
+xpcc::log::Logger xpcc::log::debug(loggerDevice);
+xpcc::log::Logger xpcc::log::info(loggerDevice);
+xpcc::log::Logger xpcc::log::warning(loggerDevice);
+xpcc::log::Logger xpcc::log::error(loggerDevice);
+
+//------------------------------DWM Config------------------------------------------------
+
+static xpcc::dw1000::config_t config =
+{
+    2,													/* Channel number. */
+    xpcc::dw1000::PRF_64M,								/* Pulse repetition frequency. */
+    xpcc::dw1000::PLEN_128,							/* Preamble length. Used in TX only. */
+    xpcc::dw1000::PAC8,								/* Preamble acquisition chunk size. Used in RX only. */
+    9,													/* TX preamble code. Used in TX only. */
+    9,													/* RX preamble code. Used in RX only. */
+    0,													/* 0 to use standard SFD, 1 to use non-standard SFD. */
+    xpcc::dw1000::BR_6M8,								/* Data rate. */
+    xpcc::dw1000::PHRMODE_STD,							/* PHY header mode. */
+    (129 + 8 - 8)										/* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
+};
+
+static constexpr uint16_t hostaddress= 0xAAAA;
+
+xpcc::Frame802154 receiveframe;
+xpcc::ShortTimeout timeout;
+uint8_t buffer[256];
+bool isrx = false;
+uint32_t length;
+
+
+//-------------------------------MAIN-----------------------------------------------------
+int
+main()
+{
+
+	//setup USART
+	GpioOutputA2::connect(Usart2::Tx);
+	GpioInputA3::connect(Usart2::Rx, Gpio::InputType::PullUp);
+	Usart2::initialize<Board::systemClock, 115200>(12);
+	//initialize the board
+	Board::initialize();
+	XPCC_LOG_INFO << "GENERIC DSTW RESP v1.1"<< xpcc::endl;
+	//activate the CS on the DW1000
+	CS::setOutput(xpcc::Gpio::High);
+	//setup SPI
+	SPIMOSI::connect(SPI::Mosi);
+	SPIMISO::connect(SPI::Miso);
+	SPISCK::connect(SPI::Sck);
+	SPI::initialize<Board::systemClock,2500000,xpcc::Tolerance::DontCare>();
+	SPI::setDataMode(SPI::DataMode::Mode0);
+	//Init with the config
+	while (!(dwm::init(config)))
+	{
+		Board::Leds::toggle();
+		xpcc::delayMilliseconds(100);
+	}
+	/* Apply antenna delay value.*/
+	dwm::setRXAntennaDelay(dwm::standardAntennaDelay);
+	dwm::setTXAntennaDelay(dwm::standardAntennaDelay);
+	dwm::hostaddress = hostaddress;
+
+
+	while (true)
+	{
+		isrx = false;
+		dwm::rxEnable();
+		Leds::toggle();
+		while(not((isrx = dwm::checkForRX()) || dwm::checkForRXError()))
+		{}
+		Leds::toggle();
+		if (isrx)
+		{
+			length = dwm::rxlength();
+			dwm::readrx(length, buffer);
+			receiveframe.loadFrame(length, buffer);
+			if (receiveframe.getDestinationAddress16() == dwm::hostaddress || receiveframe.getDestinationAddress16() == 0xFFFF)
+			{
+				Leds::toggle();
+				if(ranging::IsRangingFrame(receiveframe))
+				{
+					ranging::sendAnswer(receiveframe);
+					timeout.restart(5);
+					while(not(dwm::isFrameSent() || timeout.isExpired()))
+					{}
+
+				}
+				Leds::toggle();
+			}
+			else
+			{
+				Leds::toggle();
+				xpcc::delayMilliseconds(100);
+				Leds::toggle();
+			}
+		}
+		else
+		{
+			Leds::toggle();
+			xpcc::delayMilliseconds(50);
+			Leds::toggle();
+			xpcc::delayMilliseconds(50);
+			Leds::toggle();
+			xpcc::delayMilliseconds(50);
+			Leds::toggle();
+		}
+		dwm::trxdisable();
+		dwm::rxreset();
+	}
+}