From 45ea976c25e5f082d1545a8528b07becdc0aede6 Mon Sep 17 00:00:00 2001 From: Marten Junga Date: Tue, 23 Jan 2018 09:29:52 +0100 Subject: [PATCH] [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 --- .Rhistory | 0 .../initiator}/SConstruct | 0 .../initiator/main.cpp | 195 +++ .../initiator/project.cfg | 22 + .../responder}/SConstruct | 0 .../responder/main.cpp | 170 +++ .../responder/project.cfg | 22 + .../simple_rx_tx_dw1000/Receive}/SConstruct | 2 +- .../simple_rx_tx_dw1000/Receive/main.cpp | 130 ++ .../simple_rx_tx_dw1000/Receive/project.cfg | 22 + .../simple_rx_tx_dw1000/Send}/SConstruct | 2 +- .../dw1000/simple_rx_tx_dw1000/Send/main.cpp | 116 ++ .../simple_rx_tx_dw1000/Send/project.cfg | 22 + .../initiator/SConstruct | 4 + .../initiator/main.cpp | 174 +++ .../initiator/project.cfg | 22 + .../responder/SConstruct | 4 + .../responder/main.cpp | 170 +++ .../responder/project.cfg | 22 + .../trilateration_dw1000/Anchor/SConstruct | 4 + .../trilateration_dw1000/Anchor/main.cpp | 156 ++ .../trilateration_dw1000/Anchor/project.cfg | 22 + .../trilateration_dw1000/Tag/SConstruct | 4 + .../dw1000/trilateration_dw1000/Tag/main.cpp | 229 +++ .../trilateration_dw1000/Tag/project.cfg | 22 + .../initiator/SConstruct | 4 + .../initiator/main.cpp | 192 +++ .../initiator}/project.cfg | 0 .../responder/SConstruct | 4 + .../responder/main.cpp | 154 ++ .../responder}/project.cfg | 0 .../radio/dw1000/rx_tx_dw1000/SConstruct | 4 + .../radio/dw1000/rx_tx_dw1000/main.cpp | 204 +++ .../radio/dw1000/rx_tx_dw1000/project.cfg | 6 + .../simple_rx_tx_dw1000/Receive/SConstruct | 4 + .../simple_rx_tx_dw1000/Receive/main.cpp | 115 ++ .../simple_rx_tx_dw1000/Receive/project.cfg | 6 + .../simple_rx_tx_dw1000/Send/SConstruct | 4 + .../dw1000/simple_rx_tx_dw1000/Send/main.cpp | 106 ++ .../simple_rx_tx_dw1000/Send/project.cfg | 6 + .../initiator/SConstruct | 4 + .../initiator/main.cpp | 161 ++ .../initiator/project.cfg | 6 + .../responder/SConstruct | 4 + .../responder/main.cpp | 154 ++ .../responder/project.cfg | 6 + .../radio/nrf24-scanner/SConstruct | 4 - .../radio/nrf24/nrf24-basic-comm/SConstruct | 4 + .../{ => nrf24}/nrf24-basic-comm/main.cpp | 0 .../radio/nrf24/nrf24-basic-comm/project.cfg | 6 + .../radio/{ => nrf24}/nrf24-data/README.md | 0 .../radio/nrf24/nrf24-data/rx/SConstruct | 4 + .../radio/{ => nrf24}/nrf24-data/rx/main.cpp | 0 .../{ => nrf24}/nrf24-data/rx/project.cfg | 0 .../radio/nrf24/nrf24-data/tx/SConstruct | 4 + .../radio/{ => nrf24}/nrf24-data/tx/main.cpp | 0 .../{ => nrf24}/nrf24-data/tx/project.cfg | 0 .../radio/nrf24/nrf24-phy-test/SConstruct | 4 + .../radio/{ => nrf24}/nrf24-phy-test/main.cpp | 0 .../radio/nrf24/nrf24-phy-test/project.cfg | 6 + .../radio/nrf24/nrf24-scanner/SConstruct | 4 + .../radio/{ => nrf24}/nrf24-scanner/main.cpp | 0 .../{ => nrf24}/nrf24-scanner/project.cfg | 0 src/xpcc/communication.hpp | 1 + src/xpcc/communication/Frame802154.hpp | 21 + .../communication/Frame802154/Frame802154.cpp | 446 ++++++ .../communication/Frame802154/Frame802154.hpp | 339 +++++ .../Frame802154/test/Frame802154_test.cpp | 421 ++++++ .../Frame802154/test/Frame802154_test.hpp | 28 + src/xpcc/driver/radio/dw1000/deca_regs.hpp | 1341 +++++++++++++++++ src/xpcc/driver/radio/dw1000/dw1000.cpp | 30 + src/xpcc/driver/radio/dw1000/dw1000.hpp | 728 +++++++++ src/xpcc/driver/radio/dw1000/dw1000_impl.hpp | 1294 ++++++++++++++++ src/xpcc/driver/radio/dw1000/params.hpp | 321 ++++ src/xpcc/positioning.hpp | 17 + src/xpcc/positioning/multilateration.cpp | 199 +++ src/xpcc/positioning/multilateration.hpp | 143 ++ src/xpcc/positioning/ranging.hpp | 207 +++ src/xpcc/positioning/ranging_impl.hpp | 375 +++++ src/xpcc/positioning/test/ranging_test.cpp | 90 ++ src/xpcc/positioning/test/ranging_test.hpp | 24 + src/xpcc/positioning/test/testdevice.hpp | 38 + src/xpcc/positioning/test/testdevice_impl.hpp | 16 + 83 files changed, 8789 insertions(+), 6 deletions(-) create mode 100644 .Rhistory rename examples/{stm32f4_discovery/radio/nrf24-data/rx => generic/dw1000/double-sided_two-way_ranging/initiator}/SConstruct (100%) create mode 100644 examples/generic/dw1000/double-sided_two-way_ranging/initiator/main.cpp create mode 100644 examples/generic/dw1000/double-sided_two-way_ranging/initiator/project.cfg rename examples/{stm32f4_discovery/radio/nrf24-data/tx => generic/dw1000/double-sided_two-way_ranging/responder}/SConstruct (100%) create mode 100644 examples/generic/dw1000/double-sided_two-way_ranging/responder/main.cpp create mode 100644 examples/generic/dw1000/double-sided_two-way_ranging/responder/project.cfg rename examples/{stm32f4_discovery/radio/nrf24-basic-comm => generic/dw1000/simple_rx_tx_dw1000/Receive}/SConstruct (86%) create mode 100644 examples/generic/dw1000/simple_rx_tx_dw1000/Receive/main.cpp create mode 100644 examples/generic/dw1000/simple_rx_tx_dw1000/Receive/project.cfg rename examples/{stm32f4_discovery/radio/nrf24-phy-test => generic/dw1000/simple_rx_tx_dw1000/Send}/SConstruct (86%) create mode 100644 examples/generic/dw1000/simple_rx_tx_dw1000/Send/main.cpp create mode 100644 examples/generic/dw1000/simple_rx_tx_dw1000/Send/project.cfg create mode 100644 examples/generic/dw1000/single-sided_two-way_ranging_dw1000/initiator/SConstruct create mode 100644 examples/generic/dw1000/single-sided_two-way_ranging_dw1000/initiator/main.cpp create mode 100644 examples/generic/dw1000/single-sided_two-way_ranging_dw1000/initiator/project.cfg create mode 100644 examples/generic/dw1000/single-sided_two-way_ranging_dw1000/responder/SConstruct create mode 100644 examples/generic/dw1000/single-sided_two-way_ranging_dw1000/responder/main.cpp create mode 100644 examples/generic/dw1000/single-sided_two-way_ranging_dw1000/responder/project.cfg create mode 100644 examples/generic/dw1000/trilateration_dw1000/Anchor/SConstruct create mode 100644 examples/generic/dw1000/trilateration_dw1000/Anchor/main.cpp create mode 100644 examples/generic/dw1000/trilateration_dw1000/Anchor/project.cfg create mode 100644 examples/generic/dw1000/trilateration_dw1000/Tag/SConstruct create mode 100644 examples/generic/dw1000/trilateration_dw1000/Tag/main.cpp create mode 100644 examples/generic/dw1000/trilateration_dw1000/Tag/project.cfg create mode 100644 examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/initiator/SConstruct create mode 100644 examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/initiator/main.cpp rename examples/stm32f4_discovery/radio/{nrf24-basic-comm => dw1000/double-sided_two-way_ranging_dw1000/initiator}/project.cfg (100%) create mode 100644 examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/responder/SConstruct create mode 100644 examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/responder/main.cpp rename examples/stm32f4_discovery/radio/{nrf24-phy-test => dw1000/double-sided_two-way_ranging_dw1000/responder}/project.cfg (100%) create mode 100644 examples/stm32f4_discovery/radio/dw1000/rx_tx_dw1000/SConstruct create mode 100644 examples/stm32f4_discovery/radio/dw1000/rx_tx_dw1000/main.cpp create mode 100644 examples/stm32f4_discovery/radio/dw1000/rx_tx_dw1000/project.cfg create mode 100644 examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Receive/SConstruct create mode 100644 examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Receive/main.cpp create mode 100644 examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Receive/project.cfg create mode 100644 examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Send/SConstruct create mode 100644 examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Send/main.cpp create mode 100644 examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Send/project.cfg create mode 100644 examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/initiator/SConstruct create mode 100644 examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/initiator/main.cpp create mode 100644 examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/initiator/project.cfg create mode 100644 examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/responder/SConstruct create mode 100644 examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/responder/main.cpp create mode 100644 examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/responder/project.cfg delete mode 100644 examples/stm32f4_discovery/radio/nrf24-scanner/SConstruct create mode 100644 examples/stm32f4_discovery/radio/nrf24/nrf24-basic-comm/SConstruct rename examples/stm32f4_discovery/radio/{ => nrf24}/nrf24-basic-comm/main.cpp (100%) create mode 100644 examples/stm32f4_discovery/radio/nrf24/nrf24-basic-comm/project.cfg rename examples/stm32f4_discovery/radio/{ => nrf24}/nrf24-data/README.md (100%) create mode 100644 examples/stm32f4_discovery/radio/nrf24/nrf24-data/rx/SConstruct rename examples/stm32f4_discovery/radio/{ => nrf24}/nrf24-data/rx/main.cpp (100%) rename examples/stm32f4_discovery/radio/{ => nrf24}/nrf24-data/rx/project.cfg (100%) create mode 100644 examples/stm32f4_discovery/radio/nrf24/nrf24-data/tx/SConstruct rename examples/stm32f4_discovery/radio/{ => nrf24}/nrf24-data/tx/main.cpp (100%) rename examples/stm32f4_discovery/radio/{ => nrf24}/nrf24-data/tx/project.cfg (100%) create mode 100644 examples/stm32f4_discovery/radio/nrf24/nrf24-phy-test/SConstruct rename examples/stm32f4_discovery/radio/{ => nrf24}/nrf24-phy-test/main.cpp (100%) create mode 100644 examples/stm32f4_discovery/radio/nrf24/nrf24-phy-test/project.cfg create mode 100644 examples/stm32f4_discovery/radio/nrf24/nrf24-scanner/SConstruct rename examples/stm32f4_discovery/radio/{ => nrf24}/nrf24-scanner/main.cpp (100%) rename examples/stm32f4_discovery/radio/{ => nrf24}/nrf24-scanner/project.cfg (100%) create mode 100644 src/xpcc/communication/Frame802154.hpp create mode 100644 src/xpcc/communication/Frame802154/Frame802154.cpp create mode 100644 src/xpcc/communication/Frame802154/Frame802154.hpp create mode 100644 src/xpcc/communication/Frame802154/test/Frame802154_test.cpp create mode 100644 src/xpcc/communication/Frame802154/test/Frame802154_test.hpp create mode 100644 src/xpcc/driver/radio/dw1000/deca_regs.hpp create mode 100644 src/xpcc/driver/radio/dw1000/dw1000.cpp create mode 100644 src/xpcc/driver/radio/dw1000/dw1000.hpp create mode 100644 src/xpcc/driver/radio/dw1000/dw1000_impl.hpp create mode 100644 src/xpcc/driver/radio/dw1000/params.hpp create mode 100644 src/xpcc/positioning.hpp create mode 100644 src/xpcc/positioning/multilateration.cpp create mode 100644 src/xpcc/positioning/multilateration.hpp create mode 100644 src/xpcc/positioning/ranging.hpp create mode 100644 src/xpcc/positioning/ranging_impl.hpp create mode 100644 src/xpcc/positioning/test/ranging_test.cpp create mode 100644 src/xpcc/positioning/test/ranging_test.hpp create mode 100644 src/xpcc/positioning/test/testdevice.hpp create mode 100644 src/xpcc/positioning/test/testdevice_impl.hpp diff --git a/.Rhistory b/.Rhistory new file mode 100644 index 000000000..e69de29bb diff --git a/examples/stm32f4_discovery/radio/nrf24-data/rx/SConstruct b/examples/generic/dw1000/double-sided_two-way_ranging/initiator/SConstruct similarity index 100% rename from examples/stm32f4_discovery/radio/nrf24-data/rx/SConstruct rename to examples/generic/dw1000/double-sided_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 new file mode 100644 index 000000000..393444dbe --- /dev/null +++ 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 // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver +#include // MAC Frame for IEEE 802.15.4 +#include // ranging operations +#include // 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(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(); + 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 new file mode 100644 index 000000000..05a11e0cd --- /dev/null +++ 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/examples/stm32f4_discovery/radio/nrf24-data/tx/SConstruct b/examples/generic/dw1000/double-sided_two-way_ranging/responder/SConstruct similarity index 100% rename from examples/stm32f4_discovery/radio/nrf24-data/tx/SConstruct rename to examples/generic/dw1000/double-sided_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 new file mode 100644 index 000000000..40e84c392 --- /dev/null +++ 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 // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver +#include // MAC Frame for IEEE 802.15.4 +#include // ranging operations for DW1000 +#include // 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(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(); + 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(); + } +} diff --git a/examples/generic/dw1000/double-sided_two-way_ranging/responder/project.cfg b/examples/generic/dw1000/double-sided_two-way_ranging/responder/project.cfg new file mode 100644 index 000000000..05a11e0cd --- /dev/null +++ b/examples/generic/dw1000/double-sided_two-way_ranging/responder/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/examples/stm32f4_discovery/radio/nrf24-basic-comm/SConstruct b/examples/generic/dw1000/simple_rx_tx_dw1000/Receive/SConstruct similarity index 86% rename from examples/stm32f4_discovery/radio/nrf24-basic-comm/SConstruct rename to examples/generic/dw1000/simple_rx_tx_dw1000/Receive/SConstruct index eba41dec8..6a322fa5d 100644 --- a/examples/stm32f4_discovery/radio/nrf24-basic-comm/SConstruct +++ b/examples/generic/dw1000/simple_rx_tx_dw1000/Receive/SConstruct @@ -1,4 +1,4 @@ # path to the xpcc root directory -xpccpath = '../../../..' +xpccpath = '../../../../..' # execute the common SConstruct file exec(compile(open(xpccpath + '/scons/SConstruct', "rb").read(), xpccpath + '/scons/SConstruct', 'exec')) diff --git a/examples/generic/dw1000/simple_rx_tx_dw1000/Receive/main.cpp b/examples/generic/dw1000/simple_rx_tx_dw1000/Receive/main.cpp new file mode 100644 index 000000000..71da80d96 --- /dev/null +++ b/examples/generic/dw1000/simple_rx_tx_dw1000/Receive/main.cpp @@ -0,0 +1,130 @@ +/* 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) +* +* +*/ + + +#include // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver +#include // MAC Frame for IEEE 802.15.4 +#include // ranging operations for DW1000 +#include // 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; + + +//-------------------------------MAIN----------------------------------------------------- +int +main() +{ + //setup USART + GpioOutputA2::connect(Usart2::Tx); + GpioInputA3::connect(Usart2::Rx, Gpio::InputType::PullUp); + Usart2::initialize(12); + //initialize the board + Board::initialize(); + XPCC_LOG_INFO << "Simple Generic Receive 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(); + 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) //-----------MAINLOOP-------------- + { + dwm::rxEnable(); + while (not(dwm::checkForRX() || dwm::checkForRXError())) + { + Board::Leds::toggle(); + } + Board::Leds::toggle(); + if (dwm::checkForRX()) + {Board::Leds::toggle(); + xpcc::delayMilliseconds(100); + Board::Leds::toggle(); + } + else + { + Board::Leds::toggle(); + xpcc::delayMilliseconds(50); + Board::Leds::toggle(); + xpcc::delayMilliseconds(50); + Board::Leds::toggle(); + xpcc::delayMilliseconds(50); + Board::Leds::toggle(); + } + dwm::rxreset(); + } +} diff --git a/examples/generic/dw1000/simple_rx_tx_dw1000/Receive/project.cfg b/examples/generic/dw1000/simple_rx_tx_dw1000/Receive/project.cfg new file mode 100644 index 000000000..05a11e0cd --- /dev/null +++ b/examples/generic/dw1000/simple_rx_tx_dw1000/Receive/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/examples/stm32f4_discovery/radio/nrf24-phy-test/SConstruct b/examples/generic/dw1000/simple_rx_tx_dw1000/Send/SConstruct similarity index 86% rename from examples/stm32f4_discovery/radio/nrf24-phy-test/SConstruct rename to examples/generic/dw1000/simple_rx_tx_dw1000/Send/SConstruct index eba41dec8..6a322fa5d 100644 --- a/examples/stm32f4_discovery/radio/nrf24-phy-test/SConstruct +++ b/examples/generic/dw1000/simple_rx_tx_dw1000/Send/SConstruct @@ -1,4 +1,4 @@ # path to the xpcc root directory -xpccpath = '../../../..' +xpccpath = '../../../../..' # execute the common SConstruct file exec(compile(open(xpccpath + '/scons/SConstruct', "rb").read(), xpccpath + '/scons/SConstruct', 'exec')) diff --git a/examples/generic/dw1000/simple_rx_tx_dw1000/Send/main.cpp b/examples/generic/dw1000/simple_rx_tx_dw1000/Send/main.cpp new file mode 100644 index 000000000..1973bae84 --- /dev/null +++ b/examples/generic/dw1000/simple_rx_tx_dw1000/Send/main.cpp @@ -0,0 +1,116 @@ +/* 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) +* +* +*/ + + + +#include // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver +#include // ranging operations for DW1000 +#include // 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= 0xABCD; + + +//-------------------------------MAIN----------------------------------------------------- +int +main() +{ + //setup USART + GpioOutputA2::connect(Usart2::Tx); + GpioInputA3::connect(Usart2::Rx, Gpio::InputType::PullUp); + Usart2::initialize(12); + //initialize the board + Board::initialize(); + XPCC_LOG_INFO << "Simple Generic Send 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(); + 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) //-----------MAINLOOP-------------- + { + Board::Leds::toggle(); + uint8_t buffer[] = {0xFF,0x00,0x00}; //Testpayload of 0xFF the two 0s are for the checksum + dwm::send(3,buffer); // send frame over DW1000 + xpcc::delayMilliseconds(100); + Board::Leds::toggle(); + xpcc::delayMilliseconds(900); + dwm::trxdisable(); + } +} diff --git a/examples/generic/dw1000/simple_rx_tx_dw1000/Send/project.cfg b/examples/generic/dw1000/simple_rx_tx_dw1000/Send/project.cfg new file mode 100644 index 000000000..05a11e0cd --- /dev/null +++ b/examples/generic/dw1000/simple_rx_tx_dw1000/Send/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/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/initiator/SConstruct b/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/initiator/SConstruct new file mode 100644 index 000000000..1c1994cb3 --- /dev/null +++ b/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/initiator/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/initiator/main.cpp b/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/initiator/main.cpp new file mode 100644 index 000000000..d64029614 --- /dev/null +++ b/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/initiator/main.cpp @@ -0,0 +1,174 @@ +/* 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 // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver +#include // MAC Frame for IEEE 802.15.4 +#include // ranging operations for DW1000 +#include // 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; +uint8_t buffer[256]; +bool isrx = false; +uint32_t length; +float distance; + + +//-------------------------------MAIN----------------------------------------------------- +int +main() +{ + + //setup USART + GpioOutputA2::connect(Usart2::Tx); + GpioInputA3::connect(Usart2::Rx, Gpio::InputType::PullUp); + Usart2::initialize(12); + //initialize the board + Board::initialize(); + XPCC_LOG_INFO << "GENERIC SSTWR 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(); + 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 --------------------------------------------------------------- + XPCC_LOG_DEBUG << "Send init"<< xpcc::endl; + ranging::sendSSTWRinitAt(0xAAAA); + timeout.restart(5); + while(not(dwm::isFrameSent()||timeout.isExpired())){} + Board::Leds::toggle(); + + //------------------------------------------------------------ Receive Message-------------------------------------------------------------------- + timeout.restart(100); + dwm::rxEnable(); + Board::Leds::toggle(); + while(not(dwm::checkForRXError() || (isrx = dwm::checkForRX()) || timeout.isExpired())) {} + Board::Leds::toggle(); + + if (isrx) + { + dwm::readrx(dwm::rxlength(),buffer); + receiveframe.loadFrame(dwm::rxlength(),buffer); + XPCC_LOG_DEBUG << "Got Answer"<< xpcc::endl; + if (receiveframe.getDestinationAddress16() == hostaddress) + { + if (ranging::IsRangingFrame(receiveframe)) + { + distance = ranging::computeSsTwrDistance(receiveframe); + XPCC_LOG_INFO.printf("DISTANCE TO %0x IS %5.2fm \n",receiveframe.getSourceAddress16(),distance); + timeout.restart(1000); + } + } + 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; + timeout.restart(10); + } + + dwm::rxdisable(); + while(not(timeout.isExpired())) {} + + } +} + diff --git a/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/initiator/project.cfg b/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/initiator/project.cfg new file mode 100644 index 000000000..05a11e0cd --- /dev/null +++ b/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/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/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/responder/SConstruct b/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/responder/SConstruct new file mode 100644 index 000000000..1c1994cb3 --- /dev/null +++ b/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/responder/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/responder/main.cpp b/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/responder/main.cpp new file mode 100644 index 000000000..cf62717ce --- /dev/null +++ b/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/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 // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver +#include // MAC Frame for IEEE 802.15.4 +#include // ranging operations +#include // 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(12); + //initialize the board + Board::initialize(); + XPCC_LOG_INFO << "GENERIC 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(); + 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(); + } +} diff --git a/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/responder/project.cfg b/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/responder/project.cfg new file mode 100644 index 000000000..05a11e0cd --- /dev/null +++ b/examples/generic/dw1000/single-sided_two-way_ranging_dw1000/responder/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/examples/generic/dw1000/trilateration_dw1000/Anchor/SConstruct b/examples/generic/dw1000/trilateration_dw1000/Anchor/SConstruct new file mode 100644 index 000000000..1c1994cb3 --- /dev/null +++ b/examples/generic/dw1000/trilateration_dw1000/Anchor/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/generic/dw1000/trilateration_dw1000/Anchor/main.cpp b/examples/generic/dw1000/trilateration_dw1000/Anchor/main.cpp new file mode 100644 index 000000000..d15b8a3a8 --- /dev/null +++ b/examples/generic/dw1000/trilateration_dw1000/Anchor/main.cpp @@ -0,0 +1,156 @@ +/* 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) +*/ + + +#include // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver +#include // MAC Frame for IEEE 802.15.4 +#include // ranging operations +#include // Timer and Timeouts + +//-------------------------Namespaces and renaming---------------------------------- +// change things up for each individual board +using namespace Board; +using SPI = SpiMaster2; +using SPISCK = GpioOutputB13; +using SPIMISO = GpioInputB14; +using SPIMOSI = GpioOutputB15; +using RES = GpioOutputA8; +using CS = GpioOutputB12; +using IRQ = GpioInputB8; + + +using dwm = xpcc::Dw1000 < SPI, CS, RES, IRQ >; +using ranging = xpcc::Ranging < dwm >; + +static constexpr uint16_t hostaddress= 0xA000; + +//------------------------------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. */ +}; + + + +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(12); + //initialize the board + Board::initialize(); + XPCC_LOG_INFO << "GENERIC 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(); + SPI::setDataMode(SPI::DataMode::Mode0); + //Init with the config + while (!(dwm::init(config))) + { + Board::Leds::toggle(); + XPCC_LOG_INFO << "INIT FAILED"<< xpcc::endl; + 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(); + } + } + else + { + Leds::toggle(); + xpcc::delayMilliseconds(50); + Leds::toggle(); + xpcc::delayMilliseconds(50); + Leds::toggle(); + xpcc::delayMilliseconds(50); + Leds::toggle(); + } + dwm::trxdisable(); + dwm::rxreset(); + } +} diff --git a/examples/generic/dw1000/trilateration_dw1000/Anchor/project.cfg b/examples/generic/dw1000/trilateration_dw1000/Anchor/project.cfg new file mode 100644 index 000000000..d00f5448f --- /dev/null +++ b/examples/generic/dw1000/trilateration_dw1000/Anchor/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/examples/generic/dw1000/trilateration_dw1000/Tag/SConstruct b/examples/generic/dw1000/trilateration_dw1000/Tag/SConstruct new file mode 100644 index 000000000..1c1994cb3 --- /dev/null +++ b/examples/generic/dw1000/trilateration_dw1000/Tag/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/generic/dw1000/trilateration_dw1000/Tag/main.cpp b/examples/generic/dw1000/trilateration_dw1000/Tag/main.cpp new file mode 100644 index 000000000..a3044e04d --- /dev/null +++ b/examples/generic/dw1000/trilateration_dw1000/Tag/main.cpp @@ -0,0 +1,229 @@ +/* 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) +* +* +* This Program performs a trilateration with three Anchor Points +* For this a double sided two way ranging is done with every Anchor +* With the results it estimating the X and Y Postition for the Tag. +* Additionally a Residuum is given wich can be interpreted as position on the Z-axis or the error of the ranging +* +* +*/ + + +#include // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver +#include // MAC Frame for IEEE 802.15.4 +#include // ranging operations for DW1000 +#include // trilateration operations for DW1000 +#include // 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::DEBUG +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); + + + +/* +* The Following Variables are subject of being changed. +* Please fit them individually for your setup! +* Each anchor needs its unique Position and Hostname else any trilateration try will fail +*/ +static constexpr uint16_t AnchorAdresses[] = {0xA000, 0xA001,0xA003}; +static constexpr uint16_t hostaddress= 0xAACD; +floatunit distanceToAnchor[3] = {0.0, 0.0, 0.0}; +xpcc::Vector anchor0Position = {0.0, 0.0, 0.0}; +xpcc::Vector anchor1Position = {2.0, 0.0, 0.0}; +xpcc::Vector anchor2Position = {0.0, 2.0, 0.0}; +xpcc::Vector position = {0.0, 0.0, 0.0}; + +//------------------------------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. */ +}; + +uint8_t anchor = 0; +xpcc::Frame802154 receiveframe; +xpcc::ShortTimeout timeout; +floatunit distance; +int counter = 0 ; +bool isrx = false; +uint8_t buffer[256]; + +//-------------------------------MAIN----------------------------------------------------- +int +main() +{ + + //setup USART + GpioOutputA2::connect(Usart2::Tx); + GpioInputA3::connect(Usart2::Rx, Gpio::InputType::PullUp); + Usart2::initialize(12); + //initialize the board + Board::initialize(); + XPCC_LOG_INFO << "GENERIC Trilateration 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(); + 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) + { + //--------------------------------------- Start sending init --------------------------------------------------------------- + //XPCC_LOG_DEBUG << "Send init"<< xpcc::endl; + if (dwm::isChannelFree()) + { + ranging::sendDSTWRinitAt(AnchorAdresses[anchor]); + timeout.restart(5); + while(not(dwm::isFrameSent()||timeout.isExpired())) + {} + //------------------------------------------------------------ Receive Message-------------------------------------------------------------------- + timeout.restart(10); + dwm::rxEnable(); + 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)) + { + //---------------------------------------Start get distance and send return --------------------------------------------------------------- + distance = ranging::computeSsTwrDistance(receiveframe); + ranging::sendAnswer(receiveframe); + //---------------------------------------Wait for TOF from other device-------------------------------------------------------------------- + timeout.restart(5); + while(not(dwm::isFrameSent()||timeout.isExpired())) + {} + dwm::rxEnable(); + isrx = false; + timeout.restart(4); + while(not(dwm::checkForRXError() || (isrx = dwm::checkForRX()) || timeout.isExpired())) + {} + //---------------------------------------Getting last ranging and compute distance--------------------------------------- + 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); + distanceToAnchor[anchor] = distance; + } + } + } + else if (dwm::checkForRXError()) + { + XPCC_LOG_ERROR.printf ("RX--ERROR! with %x\n",AnchorAdresses[anchor]) ; + + } + else + { + XPCC_LOG_DEBUG.printf ("Timeout with %x\n",AnchorAdresses[anchor]); + } + } + } + else + { + dwm::frame_seq_nb = receiveframe.getSequenceNumber() + 1 ; + XPCC_LOG_INFO.printf ("Wrong Answer --- Address was wrong from %x\n",AnchorAdresses[anchor]); + } + } + + else if (dwm::checkForRXError()) + { + XPCC_LOG_ERROR.printf ("RX--ERROR! with %x\n",AnchorAdresses[anchor]) ; + timeout.restart(10); + } + else + { + XPCC_LOG_DEBUG.printf ("Timeout with %x\n",AnchorAdresses[anchor]); + } + // Increase the anchor iteration Var + if (anchor < 2) { + anchor++;} + // If anchor iteration var == 2 compute trilateration + else{ + xpcc::multilateration::trilateration(position,anchor0Position,anchor1Position,anchor2Position,distanceToAnchor[0],distanceToAnchor[1],distanceToAnchor[2]); + XPCC_LOG_INFO.printf ("Distance A000:%.2f , A001: %.2f , A003: %.2f \n",distanceToAnchor[0],distanceToAnchor[1],distanceToAnchor[2]); + XPCC_LOG_INFO.printf ("The actual Position is: X: %.2f , Y: %.2f Residuum: %.2f \n",position[0],position[1],position[2]); + anchor = 0; + timeout.restart(200); + + } + dwm::rxdisable(); + } + else {timeout.restart(40);} + while(not(timeout.isExpired())) + {} + + + } +} diff --git a/examples/generic/dw1000/trilateration_dw1000/Tag/project.cfg b/examples/generic/dw1000/trilateration_dw1000/Tag/project.cfg new file mode 100644 index 000000000..05a11e0cd --- /dev/null +++ b/examples/generic/dw1000/trilateration_dw1000/Tag/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/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/initiator/SConstruct b/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/initiator/SConstruct new file mode 100644 index 000000000..59ba5fb11 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/initiator/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/initiator/main.cpp b/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/initiator/main.cpp new file mode 100644 index 000000000..cf3cf2d08 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/initiator/main.cpp @@ -0,0 +1,192 @@ +/* This is an Example application for one STM32F4 in combination with a Decawave DM1000 Chip + * Connections are the following: + * + * USART: STM32 Pin + * TXD -> PA2 + * RXD -> PA3 (not used in this example) + * + * DM1000: + * SPI: STM32 Pin + * Miso -> PB15 + * Mosi -> PC2 + * Clock -> PB10 + * + * RES -> PA8 + * Chipselect -> PE8 + * IRQ -> PD10 (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 // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver +#include // MAC Frame for IEEE 802.15.4 +#include // ranging operations for DW1000 +#include // Timer and Timeouts + + +// ------------------------------XPCC---------------------------------------------- +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); + +//Namespaces and renaming +using namespace Board; +using SPI = SpiMaster2; +using RES = GpioOutputA8; +using CS = GpioOutputE8; +using IRQ = GpioInputD10; + +using dwm = xpcc::Dw1000 < SPI, CS, RES, IRQ >; +using ranging = xpcc::Ranging < dwm >; + + +//------------------------------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. */ +}; + +/* Default antenna delay values for 64 MHz PRF*/ +static constexpr int TX_ANT_DLY = 16436; +static constexpr int RX_ANT_DLY = 16436; +static constexpr uint16_t hostaddress= 0xBBBB; + + +xpcc::Frame802154 receiveframe; +xpcc::ShortTimeout timeout; +bool isrx = false; +uint32_t responserx, responsetx; +uint64_t owntx,ownrx; +double distance; +uint8_t buffer[1024]; + + +//-------------------------------MAIN----------------------------------------------------- +int +main() +{ + //setup USART + GpioOutputA2::connect(Usart2::Tx); + GpioInputA3::connect(Usart2::Rx, Gpio::InputType::PullUp); + Usart2::initialize(12); + //initialize the board + Board::initialize(); + //activate the CS on the DW1000 + CS::setOutput(xpcc::Gpio::High); + //setup SPI + GpioOutputB15::connect(SPI::Mosi); + GpioInputC2::connect(SPI::Miso); + GpioOutputB10::connect(SPI::Sck); + SPI::initialize(); + SPI::setDataMode(SPI::DataMode::Mode0); + //Init with the config + while (!(dwm::init(config))) + {LedRed::set();} + LedRed::reset(); + /* Apply antenna delay value.*/ + dwm::setRXAntennaDelay(RX_ANT_DLY); + dwm::setTXAntennaDelay(TX_ANT_DLY); + + dwm::hostaddress = hostaddress; + + while (true) + { + LedBlue::set(); + + //--------------------------------------- Start sending init --------------------------------------------------------------- + XPCC_LOG_DEBUG << "Send init"<< xpcc::endl; + ranging::sendDSTWRinit(); + timeout.restart(5); + while(not(dwm::isFrameSent()||timeout.isExpired())){} + LedBlue::reset(); + + //------------------------------------------------------------ Receive Message-------------------------------------------------------------------- + timeout.restart(5); + dwm::rxEnable(); + LedOrange::set(); + while(not(dwm::checkForRXError() || (isrx = dwm::checkForRX()) || timeout.isExpired())){} + LedOrange::reset(); + if (isrx) + { + 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); + LedOrange::set(); + while(not(dwm::checkForRXError() || (isrx = dwm::checkForRX()) || timeout.isExpired())){} + LedOrange::reset(); + if (isrx) + { + LedGreen::toggle(); + 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.2fm \n",receiveframe.getSourceAddress16(),distance); + timeout.restart(5); + } + + } + } + } + } + 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; + timeout.restart(10); + } + dwm::rxdisable(); + while(not(timeout.isExpired())){} + + } +} diff --git a/examples/stm32f4_discovery/radio/nrf24-basic-comm/project.cfg b/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/initiator/project.cfg similarity index 100% rename from examples/stm32f4_discovery/radio/nrf24-basic-comm/project.cfg rename to examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/initiator/project.cfg diff --git a/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/responder/SConstruct b/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/responder/SConstruct new file mode 100644 index 000000000..59ba5fb11 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/responder/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/responder/main.cpp b/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/responder/main.cpp new file mode 100644 index 000000000..044fb53c4 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/responder/main.cpp @@ -0,0 +1,154 @@ +/* This is an Example application for one STM32F4 in combination with a Decawave DM1000 Chip + * Connections are the following: + * + * USART: STM32 Pin + * TXD -> PA2 + * RXD -> PA3 (not used in this example) + * + * DM1000: + * SPI: STM32 Pin + * Miso -> PB15 + * Mosi -> PC2 + * Clock -> PB10 + * + * RES -> PA8 + * Chipselect -> PE8 + * IRQ -> PD10 (not used in this example) + * + * What this program does is: + * 1) Waits for any ranging Frame on the channel and sends a corrosponding answer +*/ + + +#include // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver +#include // MAC Frame for IEEE 802.15.4 +#include // ranging operations for DW1000 +#include // Timer and Timeouts + + +// ------------------------------XPCC---------------------------------------------- +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); + +//Namespaces and renaming +using namespace Board; +using SPI = SpiMaster2; +using RES = GpioOutputA8; +using CS = GpioOutputE8; +using IRQ = GpioInputD10; + +using dwm = xpcc::Dw1000 < SPI, CS, RES, IRQ >; +using ranging = xpcc::Ranging < dwm >; + + +//------------------------------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. */ +}; + + +/* Default antenna delay values for 64 MHz PRF*/ +static constexpr int TX_ANT_DLY = 16436; +static constexpr int RX_ANT_DLY = 16436; +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(12); + //initialize the board + Board::initialize(); + //activate the CS on the DW1000 + CS::setOutput(xpcc::Gpio::High); + //setup SPI + GpioOutputB15::connect(SPI::Mosi); + GpioInputC2::connect(SPI::Miso); + GpioOutputB10::connect(SPI::Sck); + SPI::initialize(); + SPI::setDataMode(SPI::DataMode::Mode0); + //Init with the config + while (!dwm::init(config)) + {LedRed::set();} + LedRed::reset(); + /* Apply antenna delay value.*/ + dwm::setRXAntennaDelay(RX_ANT_DLY); + dwm::setTXAntennaDelay(TX_ANT_DLY); + + dwm::hostaddress = hostaddress; + + + while (true) + { + isrx = false; + dwm::rxEnable(); + LedOrange::set(); + while(not(isrx = dwm::checkForRX() || dwm::checkForRXError())) + {} + LedOrange::reset(); + if (isrx) + { + length = dwm::rxlength(); + dwm::readrx(length, buffer); + receiveframe.loadFrame(length, buffer); + if (receiveframe.getDestinationAddress16() == hostaddress || receiveframe.getDestinationAddress16() == 0xFFFF) + { + LedGreen::set(); + if(ranging::IsRangingFrame(receiveframe)) + { + ranging::sendAnswer(receiveframe); + timeout.restart(5); + while(not(dwm::isFrameSent() || timeout.isExpired())) + {} + + } + LedGreen::reset(); + } + else + { + LedBlue::set(); + xpcc::delayMilliseconds(100); + LedBlue::reset(); + } + + } + else + { + LedRed::set(); + xpcc::delayMilliseconds(100); + LedRed::reset(); + } + dwm::trxdisable(); + dwm::rxreset(); + + } +} diff --git a/examples/stm32f4_discovery/radio/nrf24-phy-test/project.cfg b/examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/responder/project.cfg similarity index 100% rename from examples/stm32f4_discovery/radio/nrf24-phy-test/project.cfg rename to examples/stm32f4_discovery/radio/dw1000/double-sided_two-way_ranging_dw1000/responder/project.cfg diff --git a/examples/stm32f4_discovery/radio/dw1000/rx_tx_dw1000/SConstruct b/examples/stm32f4_discovery/radio/dw1000/rx_tx_dw1000/SConstruct new file mode 100644 index 000000000..1c1994cb3 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/rx_tx_dw1000/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/stm32f4_discovery/radio/dw1000/rx_tx_dw1000/main.cpp b/examples/stm32f4_discovery/radio/dw1000/rx_tx_dw1000/main.cpp new file mode 100644 index 000000000..354772174 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/rx_tx_dw1000/main.cpp @@ -0,0 +1,204 @@ +/* This is an Example application for two STM32F4 in combination with a Decawave DM1000 Chip + * Flash this App on both sides + * Connections are the following: + * + * USART: STM32 Pin + * TXD -> PA1 + * RXD -> PA2 (not used in this example) + * + * DM1000: + * SPI: STM32 Pin + * Miso -> PB15 + * Mosi -> PC2 + * Clock -> PB10 + * + * RES -> PA8 + * Chipselect -> PE8 + * IRQ -> PD10 (not used in this example) + * + * What this program does is: + * 1) Send a broadcast message with the word "PING" to all nearby UWB receivers configured with the same Parameters (Orange LED) + * 2) Receive 3 Seconds long all messages (Blue LED) + * 3) If a Message was received, check if it is a Broadcasted Message and if its Payload contains "PING" + * -> 4a) If both is true answer the message with a "PONG"(Green LED) + * -> 5a) Goto (2) + * 4b) If its Payload contains "PONG" wait 100ms and Goto (1) (Green LED) + * 4c) If a Message was received and it includes any other payload than "PING" or "PONG" Goto (1) (Red LED) + * 4d) If nothing was received goto (1) +*/ + + +#include // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver +#include // MAC Frame for IEEE 802.15.4 +#include // ranging operations for DW1000 +#include // Timer and Timeouts + + +// ------------------------------XPCC---------------------------------------------- +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); + +//Namespaces and renaming +using namespace Board; +using SPI = SpiMaster2; +using RES = GpioOutputA8; +using CS = GpioOutputE8; +using IRQ = GpioInputD10; + +using dwm = xpcc::Dw1000 < SPI, CS, RES, IRQ >; +using ranging = xpcc::Ranging < dwm >; + + +//------------------------------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. */ +}; + +/* Default antenna delay values for 64 MHz PRF*/ +static constexpr int TX_ANT_DLY = 16436; +static constexpr int RX_ANT_DLY = 16436; + +static constexpr uint16_t hostaddress= 0xABCD; + + +//------------------------------Variables-------------------------------------------- +xpcc::Frame802154 sendframe; +xpcc::Frame802154 receiveframe; +xpcc::ShortTimeout timeout1,timeout2,sendtimeout; + +uint8_t buffer[256]; +uint8_t initpayload[5] = {0xFF,'P','I','N','G'}; +uint8_t resppayload[5] = {0xEF,'P','O','N','G'}; + +//------------------------------Helper Function--------------------------------------- +void presetsendframe() // This Function sets up the sendframe +{ + sendframe.setControl(0x8841); // 16Bit Addresses short PAN DATA + sendframe.setSequenceNumber(dwm::frame_seq_nb); //Set SequenceNumber + sendframe.setDestinationPANAddress(0xDECA); //Set PANAddress + sendframe.setDestinationAddress16(0xFFFF); //Set destinationaddress + sendframe.setSourceAddress16(hostaddress); //Set Sourceaddress to own address +} + +//-------------------------------MAIN----------------------------------------------------- +int +main() +{ + Board::initialize(); + //Configure USART + GpioOutputA2::connect(Usart2::Tx); + GpioInputA3::connect(Usart2::Rx, Gpio::InputType::PullUp); + Usart2::initialize(12); + + XPCC_LOG_DEBUG << xpcc::endl << xpcc::endl << xpcc::endl << "-----------------Simple TX RX v1.1 *mja*-----------------" << xpcc::endl << xpcc::endl; + + + CS::setOutput(xpcc::Gpio::High); //activate the CS on the DW1000 + //setup SPI + GpioOutputB15::connect(SPI::Mosi); + GpioInputC2::connect(SPI::Miso); + GpioOutputB10::connect(SPI::Sck); + SPI::initialize(); + SPI::setDataMode(SPI::DataMode::Mode0); + + while (!dwm::init(config)) //Init with the config + {}; + /* Apply antenna delay value.*/ + dwm::setRXAntennaDelay(RX_ANT_DLY); + dwm::setTXAntennaDelay(TX_ANT_DLY); + presetsendframe(); // set the sendframe given to the function below + timeout2.restart(0); // set TO2 to expired + + while (true) //-----------MAINLOOP-------------- + { + // --- Send Message + if (timeout2.isExpired()) + { + LedOrange::toggle(); + sendframe.setSequenceNumber(dwm::frame_seq_nb); //add actual framesequence numper to sendframe + sendframe.setPayload(5,initpayload); // set init payload {0xFF,'P','I','N','G'} for sendframe + sendframe.addPayload(1,&dwm::frame_seq_nb); // add sequencenumber to payload (Testing) + sendframe.getFrame(buffer); // load sendframe to buffer + dwm::send(sendframe.length,buffer); // send frame over DW1000 + sendtimeout.restart(5); + while(!dwm::isFrameSent()||sendtimeout.isExpired()) + {} //wait until the DW1000 sent the frame + XPCC_LOG_DEBUG << "----------------------------Send Init-Frame--------------------- "<< xpcc::endl; + dwm::frame_seq_nb++; //increase the sequence number + LedOrange::toggle(); + } + // Receive Message + timeout1.restart(3000); + dwm::rxEnable(); // Einable the reception of the DW1000 + LedBlue::toggle(); + while(not(dwm::checkForRX() || timeout1.isExpired() || dwm::checkForRXError())) + {} //wait until frame received/rxerror or + LedBlue::toggle(); + if (dwm::checkForRX()) + { + dwm::readrx(dwm::rxlength() ,buffer); //get the received data into the buffer + receiveframe.loadFrame(dwm::rxlength(), buffer); //write the buffer + if (receiveframe.getDestinationAddress16() == hostaddress || receiveframe.getDestinationAddress16() == 0xFFFF) + { + if (receiveframe.payloadlength>0) { + receiveframe.getPayload(receiveframe.payloadlength , buffer); + XPCC_LOG_DEBUG << xpcc::endl << "----------------Received Frame-------------"<< xpcc::endl; + receiveframe.debugToString(); + if (buffer[0] == 0xFF) + { + LedGreen::toggle(); + timeout2.restart(500); + dwm::frame_seq_nb = receiveframe.getSequenceNumber()+1; + sendframe.setSequenceNumber(dwm::frame_seq_nb); + sendframe.setPayload(5,resppayload); + sendframe.addPayload(1,&dwm::frame_seq_nb); + sendframe.getFrame(buffer); + XPCC_LOG_DEBUG << xpcc::endl << "-----------------Send Response-Frame------------------ "<< xpcc::endl; + dwm::send(sendframe.length,buffer); + sendtimeout.restart(5); + while(!dwm::isFrameSent()||sendtimeout.isExpired()) + {} //wait until the DW1000 sent the frame + dwm::frame_seq_nb++; + LedGreen::toggle(); + } + else + { + LedGreen::toggle(); + dwm::frame_seq_nb = receiveframe.getSequenceNumber()+1; + xpcc::delayMilliseconds (100); + LedGreen::toggle(); + } + } + } + } + else if (dwm::checkForRXError()) + { + LedRed::toggle(); + XPCC_LOG_DEBUG << xpcc::endl << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!RX-ERROR!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!11"<< xpcc::endl; + timeout1.restart(0); + timeout2.restart(0); + LedRed::toggle(); + } + timeout1.restart(0); + dwm::trxdisable(); + dwm::rxreset(); + } +} diff --git a/examples/stm32f4_discovery/radio/dw1000/rx_tx_dw1000/project.cfg b/examples/stm32f4_discovery/radio/dw1000/rx_tx_dw1000/project.cfg new file mode 100644 index 000000000..64dfba123 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/rx_tx_dw1000/project.cfg @@ -0,0 +1,6 @@ +[build] +board = stm32f4_discovery +buildpath = ${xpccpath}/build/stm32f4_discovery/radio/${name} + +[parameters] +uart.stm32.2.tx_buffer = 1024 diff --git a/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Receive/SConstruct b/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Receive/SConstruct new file mode 100644 index 000000000..59ba5fb11 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Receive/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Receive/main.cpp b/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Receive/main.cpp new file mode 100644 index 000000000..6c1a8441e --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Receive/main.cpp @@ -0,0 +1,115 @@ +/* This is an Example application for one STM32F4 in combination with a Decawave DM1000 Chip + * Connections are the following: + * + * USART: STM32 Pin + * TXD -> PA2 + * RXD -> PA3 (not used in this example) + * + * DM1000: + * SPI: STM32 Pin + * Miso -> PB15 + * Mosi -> PC2 + * Clock -> PB10 + * + * RES -> PA8 + * Chipselect -> PE8 + * IRQ -> PD10 (not used in this example) + * + * What this program does is: + * 1) Blinks if anything was received (Orange while waiting,Red for Receive Errors Green for good Transmissions) +*/ + + +#include // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver + +// ------------------------------XPCC---------------------------------------------- +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); + +//Namespaces and renaming +using namespace Board; +using SPI = SpiMaster2; +using RES = GpioOutputA8; +using CS = GpioOutputE8; +using IRQ = GpioInputD10; + +using dwm = xpcc::Dw1000 < SPI, CS, RES, IRQ >; + + +//------------------------------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. */ +}; + +/* Default antenna delay values for 64 MHz PRF*/ +static constexpr int TX_ANT_DLY = 16436; +static constexpr int RX_ANT_DLY = 16436; +static constexpr uint16_t hostaddress= 0xAAAA; + + +//-------------------------------MAIN----------------------------------------------------- +int +main() +{ + //setup USART + GpioOutputA2::connect(Usart2::Tx); + GpioInputA3::connect(Usart2::Rx, Gpio::InputType::PullUp); + Usart2::initialize(12); + //initialize the board + Board::initialize(); + //activate the CS on the DW1000 + CS::setOutput(xpcc::Gpio::High); + //setup SPI + GpioOutputB15::connect(SPI::Mosi); + GpioInputC2::connect(SPI::Miso); + GpioOutputB10::connect(SPI::Sck); + SPI::initialize(); + SPI::setDataMode(SPI::DataMode::Mode0); + //Init with the config + while (!dwm::init(config)) + {LedRed::set();} + LedRed::reset(); + /* Apply antenna delay value.*/ + dwm::setRXAntennaDelay(RX_ANT_DLY); + dwm::setTXAntennaDelay(TX_ANT_DLY); + + while (true) //-----------MAINLOOP-------------- + { + dwm::rxEnable(); + while (not(dwm::checkForRX() || dwm::checkForRXError())) + { + LedOrange::set(); + } + LedOrange::reset(); + if (dwm::checkForRX()) + {LedGreen::toggle(); + xpcc::delayMilliseconds(100); + LedGreen::toggle(); + } + else + { + LedRed::toggle(); + xpcc::delayMilliseconds(100); + LedRed::toggle(); + } + dwm::rxreset(); + } +} diff --git a/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Receive/project.cfg b/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Receive/project.cfg new file mode 100644 index 000000000..64dfba123 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Receive/project.cfg @@ -0,0 +1,6 @@ +[build] +board = stm32f4_discovery +buildpath = ${xpccpath}/build/stm32f4_discovery/radio/${name} + +[parameters] +uart.stm32.2.tx_buffer = 1024 diff --git a/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Send/SConstruct b/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Send/SConstruct new file mode 100644 index 000000000..59ba5fb11 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Send/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Send/main.cpp b/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Send/main.cpp new file mode 100644 index 000000000..d13e37311 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Send/main.cpp @@ -0,0 +1,106 @@ +/* This is an Example application for a STM32F4 in combination with a Decawave DM1000 Chip + * Connections are the following: + * + * USART: STM32 Pin + * TXD -> PA2 + * RXD -> PA3 (not used in this example) + * + * DM1000: + * SPI: STM32 Pin + * Miso -> PB15 + * Mosi -> PC2 + * Clock -> PB10 + * + * RES -> PA8 + * Chipselect -> PE8 + * IRQ -> PD10 (not used in this example) + * + * What this program does is: + * 1) Send a broadcast message to all nearby UWB receivers configured with the same Parameters every Second (Green LED) +*/ + + +#include // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver + + +// ------------------------------XPCC---------------------------------------------- +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); + +//Namespaces and renaming +using namespace Board; +using SPI = SpiMaster2; +using RES = GpioOutputA8; +using CS = GpioOutputE8; +using IRQ = GpioInputD10; + +using dwm = xpcc::Dw1000 < SPI, CS, RES, IRQ >; + + +//------------------------------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. */ +}; + + +/* Default antenna delay values for 64 MHz PRF*/ +static constexpr int TX_ANT_DLY = 16436; +static constexpr int RX_ANT_DLY = 16436; +static constexpr uint16_t hostaddress= 0xABCD; + + +//-------------------------------MAIN----------------------------------------------------- +int +main() +{ + //setup USART + GpioOutputA2::connect(Usart2::Tx); + GpioInputA3::connect(Usart2::Rx, Gpio::InputType::PullUp); + Usart2::initialize(12); + + + Board::initialize(); + CS::setOutput(xpcc::Gpio::High); //activate the CS on the DW1000 + //setup SPI + GpioOutputB15::connect(SPI::Mosi); + GpioInputC2::connect(SPI::Miso); + GpioOutputB10::connect(SPI::Sck); + SPI::initialize(); + SPI::setDataMode(SPI::DataMode::Mode0); + + while (!dwm::init(config)) //Init with the config + {LedRed::set();} + LedRed::reset(); + /* Apply antenna delay value.*/ + dwm::setRXAntennaDelay(RX_ANT_DLY); + dwm::setTXAntennaDelay(TX_ANT_DLY); + + while (true) //-----------MAINLOOP-------------- + { + LedGreen::toggle(); + uint8_t buffer[] = {0xFF,0x00,0x00}; + dwm::send(3,buffer); // send frame over DW1000 + xpcc::delayMilliseconds(100); + LedGreen::toggle(); + xpcc::delayMilliseconds(900); + dwm::trxdisable(); + } +} diff --git a/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Send/project.cfg b/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Send/project.cfg new file mode 100644 index 000000000..64dfba123 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/simple_rx_tx_dw1000/Send/project.cfg @@ -0,0 +1,6 @@ +[build] +board = stm32f4_discovery +buildpath = ${xpccpath}/build/stm32f4_discovery/radio/${name} + +[parameters] +uart.stm32.2.tx_buffer = 1024 diff --git a/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/initiator/SConstruct b/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/initiator/SConstruct new file mode 100644 index 000000000..59ba5fb11 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/initiator/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/initiator/main.cpp b/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/initiator/main.cpp new file mode 100644 index 000000000..826d3cceb --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/initiator/main.cpp @@ -0,0 +1,161 @@ +/* This is an Example application for one STM32F4 in combination with a Decawave DM1000 Chip + * Connections are the following: + * + * USART: STM32 Pin + * TXD -> PA2 + * RXD -> PA3 (not used in this example) + * + * DM1000: + * SPI: STM32 Pin + * Miso -> PB15 + * Mosi -> PC2 + * Clock -> PB10 + * + * RES -> PA8 + * Chipselect -> PE8 + * IRQ -> PD10 (not used in this example) + * + * What this program does is: + * 1) Sends out a SSTW-Ranging Frame on Broadcast + * 2) Waits for 1000ms for an Answer and Computes the Range between the both +*/ + + +#include // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver +#include // MAC Frame for IEEE 802.15.4 +#include // ranging operations for DW1000 +#include // Timer and Timeouts + + +// ------------------------------XPCC---------------------------------------------- +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); + +//Namespaces and renaming +using namespace Board; +using SPI = SpiMaster2; +using RES = GpioOutputA8; +using CS = GpioOutputE8; +using IRQ = GpioInputD10; + +using dwm = xpcc::Dw1000 < SPI, CS, RES, IRQ >; +using ranging = xpcc::Ranging < dwm >; + + +//------------------------------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. */ +}; + +/* Default antenna delay values for 64 MHz PRF*/ +static constexpr int TX_ANT_DLY = 16436; +static constexpr int RX_ANT_DLY = 16436; +static constexpr uint16_t hostaddress= 0xBBBB; + + +xpcc::Frame802154 receiveframe; +xpcc::ShortTimeout timeout; +bool isrx = false; +uint32_t responserx, responsetx; +uint64_t owntx,ownrx; +double distance; +uint8_t buffer[1024]; + + +//-------------------------------MAIN----------------------------------------------------- +int +main() +{ + //setup USART + GpioOutputA2::connect(Usart2::Tx); + GpioInputA3::connect(Usart2::Rx, Gpio::InputType::PullUp); + Usart2::initialize(12); + //initialize the board + Board::initialize(); + //activate the CS on the DW1000 + CS::setOutput(xpcc::Gpio::High); + //setup SPI + GpioOutputB15::connect(SPI::Mosi); + GpioInputC2::connect(SPI::Miso); + GpioOutputB10::connect(SPI::Sck); + SPI::initialize(); + SPI::setDataMode(SPI::DataMode::Mode0); + //Init with the config + while (!(dwm::init(config))) + {LedRed::set();} + LedRed::reset(); + /* Apply antenna delay value.*/ + dwm::setRXAntennaDelay(RX_ANT_DLY); + dwm::setTXAntennaDelay(TX_ANT_DLY); + + dwm::hostaddress = hostaddress; + + while (true) + { + LedBlue::set(); + + //--------------------------------------- Start sending --------------------------------------------------------------- + XPCC_LOG_DEBUG << "Send init"<< xpcc::endl; + ranging::sendSSTWRinitAt(0xAAAA); + timeout.restart(5); + while(not(dwm::isFrameSent()||timeout.isExpired())){} + LedBlue::reset(); + + //------------------------------------------------------------ Receive Message-------------------------------------------------------------------- + timeout.restart(100); + dwm::rxEnable(); + LedOrange::set(); + while(not(dwm::checkForRXError() || (isrx = dwm::checkForRX()) || timeout.isExpired())) {} + LedOrange::reset(); + + if (isrx) + { + dwm::readrx(dwm::rxlength(),buffer); + receiveframe.loadFrame(dwm::rxlength(),buffer); + XPCC_LOG_DEBUG << "Got Answer"<< xpcc::endl; + if (receiveframe.getDestinationAddress16() == hostaddress) + { + if (ranging::IsRangingFrame(receiveframe)) + { + distance = ranging::computeSsTwrDistance(receiveframe); + XPCC_LOG_INFO.printf("DISTANCE TO %0x IS %5.2fm \n",receiveframe.getSourceAddress16(),distance); + timeout.restart(1000); + } + } + 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; + timeout.restart(10); + } + + dwm::rxdisable(); + while(not(timeout.isExpired())) {} + + } +} + diff --git a/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/initiator/project.cfg b/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/initiator/project.cfg new file mode 100644 index 000000000..64dfba123 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/initiator/project.cfg @@ -0,0 +1,6 @@ +[build] +board = stm32f4_discovery +buildpath = ${xpccpath}/build/stm32f4_discovery/radio/${name} + +[parameters] +uart.stm32.2.tx_buffer = 1024 diff --git a/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/responder/SConstruct b/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/responder/SConstruct new file mode 100644 index 000000000..59ba5fb11 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/responder/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/responder/main.cpp b/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/responder/main.cpp new file mode 100644 index 000000000..044fb53c4 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/responder/main.cpp @@ -0,0 +1,154 @@ +/* This is an Example application for one STM32F4 in combination with a Decawave DM1000 Chip + * Connections are the following: + * + * USART: STM32 Pin + * TXD -> PA2 + * RXD -> PA3 (not used in this example) + * + * DM1000: + * SPI: STM32 Pin + * Miso -> PB15 + * Mosi -> PC2 + * Clock -> PB10 + * + * RES -> PA8 + * Chipselect -> PE8 + * IRQ -> PD10 (not used in this example) + * + * What this program does is: + * 1) Waits for any ranging Frame on the channel and sends a corrosponding answer +*/ + + +#include // include for platform +#include // include for DW1000 Error/Debug Messaging +#include // DW1000 Driver +#include // MAC Frame for IEEE 802.15.4 +#include // ranging operations for DW1000 +#include // Timer and Timeouts + + +// ------------------------------XPCC---------------------------------------------- +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); + +//Namespaces and renaming +using namespace Board; +using SPI = SpiMaster2; +using RES = GpioOutputA8; +using CS = GpioOutputE8; +using IRQ = GpioInputD10; + +using dwm = xpcc::Dw1000 < SPI, CS, RES, IRQ >; +using ranging = xpcc::Ranging < dwm >; + + +//------------------------------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. */ +}; + + +/* Default antenna delay values for 64 MHz PRF*/ +static constexpr int TX_ANT_DLY = 16436; +static constexpr int RX_ANT_DLY = 16436; +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(12); + //initialize the board + Board::initialize(); + //activate the CS on the DW1000 + CS::setOutput(xpcc::Gpio::High); + //setup SPI + GpioOutputB15::connect(SPI::Mosi); + GpioInputC2::connect(SPI::Miso); + GpioOutputB10::connect(SPI::Sck); + SPI::initialize(); + SPI::setDataMode(SPI::DataMode::Mode0); + //Init with the config + while (!dwm::init(config)) + {LedRed::set();} + LedRed::reset(); + /* Apply antenna delay value.*/ + dwm::setRXAntennaDelay(RX_ANT_DLY); + dwm::setTXAntennaDelay(TX_ANT_DLY); + + dwm::hostaddress = hostaddress; + + + while (true) + { + isrx = false; + dwm::rxEnable(); + LedOrange::set(); + while(not(isrx = dwm::checkForRX() || dwm::checkForRXError())) + {} + LedOrange::reset(); + if (isrx) + { + length = dwm::rxlength(); + dwm::readrx(length, buffer); + receiveframe.loadFrame(length, buffer); + if (receiveframe.getDestinationAddress16() == hostaddress || receiveframe.getDestinationAddress16() == 0xFFFF) + { + LedGreen::set(); + if(ranging::IsRangingFrame(receiveframe)) + { + ranging::sendAnswer(receiveframe); + timeout.restart(5); + while(not(dwm::isFrameSent() || timeout.isExpired())) + {} + + } + LedGreen::reset(); + } + else + { + LedBlue::set(); + xpcc::delayMilliseconds(100); + LedBlue::reset(); + } + + } + else + { + LedRed::set(); + xpcc::delayMilliseconds(100); + LedRed::reset(); + } + dwm::trxdisable(); + dwm::rxreset(); + + } +} diff --git a/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/responder/project.cfg b/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/responder/project.cfg new file mode 100644 index 000000000..64dfba123 --- /dev/null +++ b/examples/stm32f4_discovery/radio/dw1000/single-sided_two-way_ranging_dw1000/responder/project.cfg @@ -0,0 +1,6 @@ +[build] +board = stm32f4_discovery +buildpath = ${xpccpath}/build/stm32f4_discovery/radio/${name} + +[parameters] +uart.stm32.2.tx_buffer = 1024 diff --git a/examples/stm32f4_discovery/radio/nrf24-scanner/SConstruct b/examples/stm32f4_discovery/radio/nrf24-scanner/SConstruct deleted file mode 100644 index eba41dec8..000000000 --- a/examples/stm32f4_discovery/radio/nrf24-scanner/SConstruct +++ /dev/null @@ -1,4 +0,0 @@ -# path to the xpcc root directory -xpccpath = '../../../..' -# execute the common SConstruct file -exec(compile(open(xpccpath + '/scons/SConstruct', "rb").read(), xpccpath + '/scons/SConstruct', 'exec')) diff --git a/examples/stm32f4_discovery/radio/nrf24/nrf24-basic-comm/SConstruct b/examples/stm32f4_discovery/radio/nrf24/nrf24-basic-comm/SConstruct new file mode 100644 index 000000000..1c1994cb3 --- /dev/null +++ b/examples/stm32f4_discovery/radio/nrf24/nrf24-basic-comm/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/stm32f4_discovery/radio/nrf24-basic-comm/main.cpp b/examples/stm32f4_discovery/radio/nrf24/nrf24-basic-comm/main.cpp similarity index 100% rename from examples/stm32f4_discovery/radio/nrf24-basic-comm/main.cpp rename to examples/stm32f4_discovery/radio/nrf24/nrf24-basic-comm/main.cpp diff --git a/examples/stm32f4_discovery/radio/nrf24/nrf24-basic-comm/project.cfg b/examples/stm32f4_discovery/radio/nrf24/nrf24-basic-comm/project.cfg new file mode 100644 index 000000000..64dfba123 --- /dev/null +++ b/examples/stm32f4_discovery/radio/nrf24/nrf24-basic-comm/project.cfg @@ -0,0 +1,6 @@ +[build] +board = stm32f4_discovery +buildpath = ${xpccpath}/build/stm32f4_discovery/radio/${name} + +[parameters] +uart.stm32.2.tx_buffer = 1024 diff --git a/examples/stm32f4_discovery/radio/nrf24-data/README.md b/examples/stm32f4_discovery/radio/nrf24/nrf24-data/README.md similarity index 100% rename from examples/stm32f4_discovery/radio/nrf24-data/README.md rename to examples/stm32f4_discovery/radio/nrf24/nrf24-data/README.md diff --git a/examples/stm32f4_discovery/radio/nrf24/nrf24-data/rx/SConstruct b/examples/stm32f4_discovery/radio/nrf24/nrf24-data/rx/SConstruct new file mode 100644 index 000000000..59ba5fb11 --- /dev/null +++ b/examples/stm32f4_discovery/radio/nrf24/nrf24-data/rx/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/stm32f4_discovery/radio/nrf24-data/rx/main.cpp b/examples/stm32f4_discovery/radio/nrf24/nrf24-data/rx/main.cpp similarity index 100% rename from examples/stm32f4_discovery/radio/nrf24-data/rx/main.cpp rename to examples/stm32f4_discovery/radio/nrf24/nrf24-data/rx/main.cpp diff --git a/examples/stm32f4_discovery/radio/nrf24-data/rx/project.cfg b/examples/stm32f4_discovery/radio/nrf24/nrf24-data/rx/project.cfg similarity index 100% rename from examples/stm32f4_discovery/radio/nrf24-data/rx/project.cfg rename to examples/stm32f4_discovery/radio/nrf24/nrf24-data/rx/project.cfg diff --git a/examples/stm32f4_discovery/radio/nrf24/nrf24-data/tx/SConstruct b/examples/stm32f4_discovery/radio/nrf24/nrf24-data/tx/SConstruct new file mode 100644 index 000000000..59ba5fb11 --- /dev/null +++ b/examples/stm32f4_discovery/radio/nrf24/nrf24-data/tx/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/stm32f4_discovery/radio/nrf24-data/tx/main.cpp b/examples/stm32f4_discovery/radio/nrf24/nrf24-data/tx/main.cpp similarity index 100% rename from examples/stm32f4_discovery/radio/nrf24-data/tx/main.cpp rename to examples/stm32f4_discovery/radio/nrf24/nrf24-data/tx/main.cpp diff --git a/examples/stm32f4_discovery/radio/nrf24-data/tx/project.cfg b/examples/stm32f4_discovery/radio/nrf24/nrf24-data/tx/project.cfg similarity index 100% rename from examples/stm32f4_discovery/radio/nrf24-data/tx/project.cfg rename to examples/stm32f4_discovery/radio/nrf24/nrf24-data/tx/project.cfg diff --git a/examples/stm32f4_discovery/radio/nrf24/nrf24-phy-test/SConstruct b/examples/stm32f4_discovery/radio/nrf24/nrf24-phy-test/SConstruct new file mode 100644 index 000000000..1c1994cb3 --- /dev/null +++ b/examples/stm32f4_discovery/radio/nrf24/nrf24-phy-test/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/stm32f4_discovery/radio/nrf24-phy-test/main.cpp b/examples/stm32f4_discovery/radio/nrf24/nrf24-phy-test/main.cpp similarity index 100% rename from examples/stm32f4_discovery/radio/nrf24-phy-test/main.cpp rename to examples/stm32f4_discovery/radio/nrf24/nrf24-phy-test/main.cpp diff --git a/examples/stm32f4_discovery/radio/nrf24/nrf24-phy-test/project.cfg b/examples/stm32f4_discovery/radio/nrf24/nrf24-phy-test/project.cfg new file mode 100644 index 000000000..64dfba123 --- /dev/null +++ b/examples/stm32f4_discovery/radio/nrf24/nrf24-phy-test/project.cfg @@ -0,0 +1,6 @@ +[build] +board = stm32f4_discovery +buildpath = ${xpccpath}/build/stm32f4_discovery/radio/${name} + +[parameters] +uart.stm32.2.tx_buffer = 1024 diff --git a/examples/stm32f4_discovery/radio/nrf24/nrf24-scanner/SConstruct b/examples/stm32f4_discovery/radio/nrf24/nrf24-scanner/SConstruct new file mode 100644 index 000000000..1c1994cb3 --- /dev/null +++ b/examples/stm32f4_discovery/radio/nrf24/nrf24-scanner/SConstruct @@ -0,0 +1,4 @@ +# path to the xpcc root directory +xpccpath = '../../../../..' +# execute the common SConstruct file +execfile(xpccpath + '/scons/SConstruct') diff --git a/examples/stm32f4_discovery/radio/nrf24-scanner/main.cpp b/examples/stm32f4_discovery/radio/nrf24/nrf24-scanner/main.cpp similarity index 100% rename from examples/stm32f4_discovery/radio/nrf24-scanner/main.cpp rename to examples/stm32f4_discovery/radio/nrf24/nrf24-scanner/main.cpp diff --git a/examples/stm32f4_discovery/radio/nrf24-scanner/project.cfg b/examples/stm32f4_discovery/radio/nrf24/nrf24-scanner/project.cfg similarity index 100% rename from examples/stm32f4_discovery/radio/nrf24-scanner/project.cfg rename to examples/stm32f4_discovery/radio/nrf24/nrf24-scanner/project.cfg diff --git a/src/xpcc/communication.hpp b/src/xpcc/communication.hpp index 4aac8465d..f8d4843c9 100644 --- a/src/xpcc/communication.hpp +++ b/src/xpcc/communication.hpp @@ -18,3 +18,4 @@ //#include "communication/amnb.hpp" #include "communication/sab.hpp" #include "communication/sab2.hpp" +#include "communication/Frame802154.hpp" diff --git a/src/xpcc/communication/Frame802154.hpp b/src/xpcc/communication/Frame802154.hpp new file mode 100644 index 000000000..111d6a203 --- /dev/null +++ b/src/xpcc/communication/Frame802154.hpp @@ -0,0 +1,21 @@ +// coding: utf-8 +/* Copyright (c) 2009, Roboterclub Aachen e.V. + * All Rights Reserved. + * + * The file is part of the xpcc library and is released under the 3-clause BSD + * license. See the file `LICENSE` for the full license governing this code. + */ +// ---------------------------------------------------------------------------- + +/** + * @ingroup communication + * @defgroup 802154Frame IEEE 802.15.4 Frame + * + * + * + * + * @author Marten Junga + */ + + +#include "Frame802154/Frame802154.hpp" diff --git a/src/xpcc/communication/Frame802154/Frame802154.cpp b/src/xpcc/communication/Frame802154/Frame802154.cpp new file mode 100644 index 000000000..d1135a7cb --- /dev/null +++ b/src/xpcc/communication/Frame802154/Frame802154.cpp @@ -0,0 +1,446 @@ +/** +* Copyright (c) 2018, Marten Junga (Github.com/Maju-Ketchup) +* All Rights Reserved. +* +* The file is part of the xpcc library and is released under the 3-clause BSD +* license. See the file `LICENSE` for the full license governing this code. +* +* +* The headder contains the class implementation of the IEEE standart 802.15.4-2011 Frame +* current max size is 255 bytes but some devices are able to send 1023 bytes +* Set always control first +* +*/ + +#ifndef XPCC_FRAME802154_CPP +#define XPCC_FRAME802154_CPP + +#endif // XPCC_FRAME802154_CPP + +#include "Frame802154.hpp" +using namespace xpcc; + +Frame802154::Frame802154() +{ + setControl(0x00UL); + setSequenceNumber(0); + frame[payloadend] = 0; + frame[payloadend + 1] = 0 ; +} + +Frame802154::Frame802154(int size, uint8_t data[]) + +{ + int i; + for (i=0;i> 8); + frame[1] = uint8_t(control); + setbeginvalues(); + frame[payloadend] = 0; + frame[payloadend + 1] = 0 ; + +} + +uint16_t +Frame802154::getControl() +{ + return (((uint16_t)frame[0] << 8) | frame[1]); +} + +void +Frame802154::setSequenceNumber(uint8_t number) +{ + frame[2]=number; +} + +uint8_t +Frame802154::getSequenceNumber() +{ + return frame[2]; +} + +void +Frame802154::setDestinationPANAddress(uint16_t panaddress) +{ + if ((getControl() >> Header::DEST_ADDRESSING_MODE) % 4 != 0) + + { + frame[3] = uint8_t(panaddress); + frame[4] = uint8_t(panaddress >> 8); + } +} + +uint16_t +Frame802154::getDestinationPANAddress() +{ + if ((getControl() >> Header::DEST_ADDRESSING_MODE) % 4 != 0) + { + return (((uint16_t)frame[4] << 8) | frame[3]); + } + else + { + return 0; + } +} + +void +Frame802154::setDestinationAddress64(uint64_t address) +{ + if (((getControl() >> Header::DEST_ADDRESSING_MODE) & Header::LONG_ADDRESS) == Header::LONG_ADDRESS) + { + frame[5] = uint8_t(address); + frame[6] = uint8_t(address >> 8); + frame[7] = uint8_t(address >> 16); + frame[8] = uint8_t(address >> 24); + frame[9] = uint8_t(address >> 32); + frame[10] = uint8_t(address >> 40); + frame[11] = uint8_t(address >> 48); + frame[12] = uint8_t(address >> 56); + } + else if (((getControl() >> Header::DEST_ADDRESSING_MODE) & Header::SHORT_ADDRESS) == Header::SHORT_ADDRESS) + { + frame[5] = uint8_t(address); + frame[6] = uint8_t(address >> 8); + + } +} + + +uint64_t +Frame802154::getDestinationAddress64() +{ + if (((getControl() >> Header::DEST_ADDRESSING_MODE) & Header::LONG_ADDRESS )== Header::LONG_ADDRESS) + { + return ((uint64_t)frame[12]<<56 | + (uint64_t)frame[11]<<48 | + (uint64_t)frame[10]<<40 | + (uint64_t)frame[9]<<32 | + (uint32_t)frame[8]<<24 | + (uint32_t)frame[7]<<16 | + (uint32_t)frame[6]<<8 | + frame[5]); + } + else if (((getControl() >> Header::DEST_ADDRESSING_MODE) & Header::SHORT_ADDRESS) == Header::SHORT_ADDRESS) + { + return ( uint64_t(frame[5]| frame[6]<<8 )); + } + else + { + return 0; + } +} + +void +Frame802154::setDestinationAddress16(uint16_t address) +{ + if (((getControl() >> Header::DEST_ADDRESSING_MODE) & Header::LONG_ADDRESS) == Header::LONG_ADDRESS) + { + frame[5] = uint8_t(address); + frame[6] = uint8_t(address >> 8); + frame[7] = uint8_t(0); + frame[8] = uint8_t(0); + frame[9] = uint8_t(0); + frame[10] = uint8_t(0); + frame[11] = uint8_t(0); + frame[12] = uint8_t(0); + } + else if (((getControl() >> Header::DEST_ADDRESSING_MODE) & Header::SHORT_ADDRESS) == Header::SHORT_ADDRESS) + { + frame[6] = uint8_t(address>> 8); + frame[5] = uint8_t(address); + } + +} + +uint16_t +Frame802154::getDestinationAddress16() +{ + if (((getControl() >> Header::DEST_ADDRESSING_MODE) & Header::LONG_ADDRESS )== Header::LONG_ADDRESS) + { + return ((uint32_t)frame[6]<<8 | frame[5]); + } + else if (((getControl() >> Header::DEST_ADDRESSING_MODE) & Header::SHORT_ADDRESS) == Header::SHORT_ADDRESS) + { + return ( uint64_t(frame[5]| frame[6]<<8 )); + } + else + { + return 0; + } +} + +void +Frame802154::setSourcePANAddress(uint16_t panaddress) +{ + if (((getControl() >> Header::SOURCE_ADDRESSING_MODE) % 4 != 0) && not(getControl() & (PAN_ID_COMPRESSION) ) ) + { + + frame[SourcePANAddressbegin] = uint8_t(panaddress); + frame[SourcePANAddressbegin+1] = uint8_t(panaddress >> 8); + + } + else if (getControl() & PAN_ID_COMPRESSION) + { + setDestinationPANAddress(panaddress); + } +} + +uint16_t +Frame802154::getSourcePANAddress() +{ + if (((getControl()<< Header::SOURCE_ADDRESSING_MODE) != 0) && not(getControl() & PAN_ID_COMPRESSION)) + { + if (((getControl() >> Header::SOURCE_ADDRESSING_MODE) % 4) != NOPAN_AND_NOADRESSFIELD) + {return ( (uint16_t)frame[SourcePANAddressbegin+1]<<8 | frame[SourcePANAddressbegin]); } + else + { + return 0; + } + } + else if (getControl() & PAN_ID_COMPRESSION) + { + return (getDestinationPANAddress()); + } + else + {return 0;} +} + +void +Frame802154::setSourceAddress64(uint64_t address) +{ + if (((getControl() >> Header::SOURCE_ADDRESSING_MODE) & Header::LONG_ADDRESS)== Header::LONG_ADDRESS) + { + frame[SourceAddressbegin +0] = uint8_t(address); + frame[SourceAddressbegin +1] = uint8_t(address >> 8); + frame[SourceAddressbegin +2] = uint8_t(address >> 16); + frame[SourceAddressbegin +3] = uint8_t(address >> 24); + frame[SourceAddressbegin +4] = uint8_t(address >> 32); + frame[SourceAddressbegin +5] = uint8_t(address >> 40); + frame[SourceAddressbegin +6] = uint8_t(address >> 48); + frame[SourceAddressbegin +7] = uint8_t(address >> 56); + } + else if (((getControl() >> Header::SOURCE_ADDRESSING_MODE) & Header::SHORT_ADDRESS )== Header::SHORT_ADDRESS) + { + + frame[SourceAddressbegin+0] = uint8_t(address); + frame[SourceAddressbegin+1] = uint8_t(address >> 8); + + } + +} + +uint64_t +Frame802154::getSourceAddress64() +{ + if (((getControl() >> Header::SOURCE_ADDRESSING_MODE) & Header::LONG_ADDRESS ) == Header::LONG_ADDRESS ) + { + return ( + (uint64_t)frame[SourceAddressbegin+7]<<56| + (uint64_t)frame[SourceAddressbegin+6]<<48| + (uint64_t)frame[SourceAddressbegin+5]<<40| + (uint64_t)frame[SourceAddressbegin+4]<<32| + (uint64_t)frame[SourceAddressbegin+3]<<24| + (uint64_t)frame[SourceAddressbegin+2]<<16| + (uint64_t)frame[SourceAddressbegin+1]<<8 | + (uint64_t)frame[SourceAddressbegin+0]<<0 ); + } + else if(((getControl() >> Header::SOURCE_ADDRESSING_MODE) & Header::SHORT_ADDRESS )== Header::SHORT_ADDRESS) + { + return (uint64_t)frame[SourceAddressbegin+1] <<8 | frame[SourceAddressbegin+0]; + } + + else + { + return 0; + } +} + +void +Frame802154::setSourceAddress16(uint16_t address) +{ + if (((getControl() >> Header::SOURCE_ADDRESSING_MODE) & Header::LONG_ADDRESS ) == Header::LONG_ADDRESS ) + { + frame[SourceAddressbegin +0] = uint8_t(address); + frame[SourceAddressbegin +1] = uint8_t(address >> 8); + frame[SourceAddressbegin +2] = uint8_t(0); + frame[SourceAddressbegin +3] = uint8_t(0); + frame[SourceAddressbegin +4] = uint8_t(0); + frame[SourceAddressbegin +5] = uint8_t(0); + frame[SourceAddressbegin +6] = uint8_t(0); + frame[SourceAddressbegin +7] = uint8_t(0); + } + else if (((getControl()>> Header::SOURCE_ADDRESSING_MODE) & Header::SHORT_ADDRESS) == Header::SHORT_ADDRESS) + { + frame[SourceAddressbegin+0] = address; + frame[SourceAddressbegin+1] = address >> 8; + } +} + +uint16_t +Frame802154::getSourceAddress16() +{ + if (((getControl() >> Header::SOURCE_ADDRESSING_MODE) & Header::LONG_ADDRESS ) == Header::LONG_ADDRESS ) + { + return ((uint64_t)frame[SourceAddressbegin+1]<<8 + |(uint64_t)frame[SourceAddressbegin+0]<<0 + ); + } + else if ((getControl() >> Header::SOURCE_ADDRESSING_MODE & Header::SHORT_ADDRESS) == Header::SHORT_ADDRESS) + { + return ((uint16_t)(frame[SourceAddressbegin+0] ) | frame[SourceAddressbegin+1] << 8); + } + else + { + return 0; + } +} + +void +Frame802154::setPayload(int size,uint8_t data[]) +{ + cleanPayload(); + addPayload(size,data); +} + + + +void +Frame802154::getPayload(int size, uint8_t data[]) +{ + int i; + for (i=0;i> Header::DEST_ADDRESSING_MODE) % 4) == NOPAN_AND_NOADRESSFIELD) + { + SourcePANAddressbegin = 3; + } + else if (((control >> Header::DEST_ADDRESSING_MODE) % 4 ) == Header::SHORT_ADDRESS) + { + SourcePANAddressbegin = 7; + } + else if (((control >> Header::DEST_ADDRESSING_MODE) % 4) == Header::LONG_ADDRESS) + { + SourcePANAddressbegin = 13; + } + //Set SourceAddressBegin + if (control & (PAN_ID_COMPRESSION)) + { + SourceAddressbegin = SourcePANAddressbegin; + } + else + { + SourceAddressbegin = SourcePANAddressbegin + 2; + } + //SetPayloadBegin --- SECURITY BEGIN - PLS CHANGE WHEN SECU COMES IN + if (((control >> Header::SOURCE_ADDRESSING_MODE) % 4) == NOPAN_AND_NOADRESSFIELD) + { + if (control & (PAN_ID_COMPRESSION)) + { + SourceAddressbegin -= 2; + } + payloadbegin = SourcePANAddressbegin; + } + else if (((control >> Header::SOURCE_ADDRESSING_MODE) % 4) == Header::SHORT_ADDRESS) + { + payloadbegin = SourceAddressbegin + 2; + } + else + { + payloadbegin = SourceAddressbegin + 8; + } + payloadend = payloadbegin; + payloadlength = 0; + length = payloadbegin + 2; +} diff --git a/src/xpcc/communication/Frame802154/Frame802154.hpp b/src/xpcc/communication/Frame802154/Frame802154.hpp new file mode 100644 index 000000000..c4e8a3f91 --- /dev/null +++ b/src/xpcc/communication/Frame802154/Frame802154.hpp @@ -0,0 +1,339 @@ +/** +* Copyright (c) 2018, Marten Junga (Github.com/Maju-Ketchup) +* All Rights Reserved. +* +* The file is part of the xpcc library and is released under the 3-clause BSD +* license. See the file `LICENSE` for the full license governing this code. +* +* +* The headder contains the class implementation of the IEEE standart 802.15.4-2011 Frame +* current max size is 255 bytes but some devices are able to send 1023 bytes +* Set always control first +* +*/ +#ifndef XPCC_Frame802154_HPP +#define XPCC_Frame802154_HPP + +#include + +namespace xpcc { + +struct frame802154{ +public: + enum + Header : uint8_t + { + //--------------FRAME CONTROL FIELD CONSTANTS-------------------------- + //OFFSETS + /** 3 Bits see FrameTypes for Types */ + FRAME_TYPE = 0x0, + /** 2 Bits see AddressModes for Modes */ + DEST_ADDRESSING_MODE = 0xA, + /** 2 Bits mostly 00 */ + FRAME_VERSION = 0xC, + /** 2 bits see AddressModes for Modes */ + SOURCE_ADDRESSING_MODE = 0xE, + + //FLAGS + /**if enabled security is present ---- not implanted*/ + SECURITY_ENABLED = 0x8, + /** if sender has more data to send turn this flag to 1 */ + FRAME_PENDING = 0x10, + /** Acknowledgment Request Flag */ + AR = 0x20, + /** IF set to 1 only destination PAN-address is available */ + PAN_ID_COMPRESSION = 0x40, + + + //FRAME TYPES + BEACON = 0b000, + DATA = 0b001, + ACKNOWLEDGMENT = 0b010, + MAC_COMMAND = 0b011, + + //ADRESSING MODES + + NOPAN_AND_NOADRESSFIELD = 0b00, + SHORT_ADDRESS = 0b10, + LONG_ADDRESS = 0b11 + + + }; + /** 16Bit Broadcastaddress */ + static constexpr uint16_t broadcast16bitAddress = 0xFFFF; + /** 64Bit Broadcastaddress */ + static constexpr uint64_t broadcast64bitAddress = 0xFFFFFFFFFFFFFFFF; +}; + + + + +/** + * + * @ingroup 802154Frame + * @brief + * Copyright (c) 2017, Marten Junga (Github.com/Maju-Ketchup) + * All Rights Reserved. + * + * The file is part of the xpcc library and is released under the 3-clause BSD + * license. See the file `LICENSE` for the full license governing this code. + * + * + * The headder contains the class implementation of the IEEE standart 802.15.4-2011 MAC-frame \n + * Current max size is 255 bytes \n + * !The security header is not implemented at the moment! \n + * + * Set always control first \n + * + * Frame build up (First Byte on the left): + * For all fields applies LSB (Least Significant Byte) first! + * + * | 2 Octets | 1 Octet | 0/2 Octets | 0/2/8 Octets | 0/2 Octets | 0/2/8 Octets | 0/5/6/10/14 Octets | variable | 2 Octets | + * |------------------|-----------------|-----------------|---------------------|-----------------|---------------------|---------------------|---------------------|-----------------| + * | Control Header | Sequence Number | Destination PAN | Destination Address | Source PAN | Source Address | Security Header | Payload | FSC | + * + * + * Control Header: + * + * |Bit: | 0 + 1 + 2 | 3 | 4 |5 | 6 | 7 + 8 + 9 | 10 + 11 | 12 - 13 | 14 - 15 | + * |--------|-------------------------|-----------------|-------------|--|------------------|------------------------|----------------------------|----------------|-------------------| + * |Content:| Frame Type |Security Enabled |Frame Pending|AR|PAN ID Compression| Reserved | Destination Address Mode | Frame Version | Sorce Address Mode| + * + * + * + * Frame Types: \n + * 0b000 Beacon \n + * 0b001 Data \n + * 0b010 Acknowledgement \n + * 0b011 MAC command \n + * 0b100-111 Reserved \n \n + * + * Addressing Modes:\n + * 0b00 PAN identifier and address fields are not present\n + * 0b01 Reserved\n + * 0b10 Address field contains a short address (16 bit)\n + * 0b11 Address field contains an extended address (64 bit)\n + * + * + * EXAMPLE USAGE:\n + * Frame802154 sendframe;\n + * uint8_t payload = 0xE0;\n + * static uint8_t buffer[256];\n + * + * control = (frame802154::DATA< + +class Frame802154Test : public unittest::TestSuite +{ +public: + void + testConstruction(); + + void + testGetterandSetter(); + + void + testPayload(); +}; diff --git a/src/xpcc/driver/radio/dw1000/deca_regs.hpp b/src/xpcc/driver/radio/dw1000/deca_regs.hpp new file mode 100644 index 000000000..a05d35e4e --- /dev/null +++ b/src/xpcc/driver/radio/dw1000/deca_regs.hpp @@ -0,0 +1,1341 @@ +/*<------------------------------------------------------------------------------------------------------------------ + * @file deca_regs.hpp + * @brief DW1000 Register Definitions + * This file supports C++ development for DW1000 enabled devices + * + * @attention + * + * Copyright 2013 =(c); Decawave Ltd, Dublin, Ireland. + * c++ Conversion: Marten Junga (2018) + * + * All rights reserved. + * + */ + +#ifndef _DECA_REGS_H_ +#define _DECA_REGS_H_ + +#include + + +/****************************************************************************//** + * @brief Bit definitions for register DEV_ID +**/ +static constexpr uint32_t DEV_ID_ID =0x00; /* translating to C++, easy-to-use functions without register calls + * WIP --> move static constexpr in class part?! -> create enums + * TODO --> XPCC integration, IRQ, Cleanup +*/ +#ifndef _DW1000_ +#define _DW1000_ +#ifndef XPCC__LOGGER_HPP +#include +#ifndef XPCC_LOG_LEVEL + #define XPCC_LOG_LEVEL xpcc::log::DISABLED +#endif +#endif +#include "./params.hpp" +namespace xpcc +{ +//-----Without those at this place there will be unfixable bugs (cannot put them into params.hpp) - if anyone knows why pls change +//----------------------------------------- + + +//------------------------------------------------------- CLASS BEGIN ----------------------------------------------------------------------- + +/** + *@brief + *This class is a driver class for a Decawave Dw1000 Ultra-Wide-Band transmitter/receiver + * + *It needs: \n + *SPI for communication with the host Microcontroler \n + *Chipselect GPIO \n + *Reset GPIO \n + *Interrupt Request Line GPIO \n + *@ingroup dw1000 + *@author Marten Junga + */ +template < typename Spi, typename Cs, typename Reset, typename Irq > +class + Dw1000 : public xpcc::dw1000 +{ +public: + + static dw1000::local_data_t dw1000local; + + static uint8_t frame_seq_nb; /**< The pending Framenumber (init value 0) */ + + static uint64_t hostaddress; /**< The Hostaddress (init value 0xFFFFFFFFFFFFFFFF) */ + + static uint32_t TX_ANT_DLY; /**< Sending Antenna Delay (init to standart: 16436) */ + + static uint32_t RX_ANT_DLY; /**< Receiving Antenna Delay (init to standart: 16436) */ + //----------Boot---------------------------------------------------------------------------------- + + /** + *This function initializes the controller with an config_t returns true if startup was successful (include configure) + *@param conf + */ + static bool + init(config_t &conf); + + static void + /** + *@brief + *This function configures the DWM with a given config_t for configuration changes while running +* + *@param conf + */ + configure(config_t *conf); + static void + /** + *@brief + *This function sets the receive antenna delay standart 16436 + *@param rxDelay + */ + setRXAntennaDelay(uint16_t rxDelay); + + static void + /** + *@brief + *This function sets the transmit antenna delay 16436 + *@param rxDelay + */ + setTXAntennaDelay(uint16_t rxDelay); + + + //----------------Sending + + static bool + /** + *@brief + *This function sends 'size' bytes of the given data array + *@param size + *minimum 3 maximim 1023 + *@param data + *@return bool + *returns if the send was successfull + */ + send(int size, uint8_t data[]); + + static bool + /** + *@brief + *This function will send the given data at a specific point in time of the DW1000 clock + *@param size + *@param data + *@param time + *must be in the future of the DW1000 clock else send will not be successfull + *@return bool + *returns if the send was successfull + */ + sendAt(int size, uint8_t data[], uint32_t time); + + static bool + /** + *@brief + *This function returns true if the frame was sent + *@return bool + */ + isFrameSent(); + + static void + /** + *@brief + *This function reads the txtimestamp + *@param timestamp[] + *must has at least length 5 + */ + readTXTimestamp(uint8_t timestamp[]); // 40 bit register + + static uint64_t + /** + *@brief + * This function Reads the whole 40Bit register into an 64bit Integer +* + *@return uint64_t + */ + readTXTimestamp64(); + + static uint32_t + /** + *@brief + *This function reads the low 32bit of the tx time stamp + *@return uint32_t + */ + readTXTimestamplo32(); + + static uint32_t + /** + *@brief + *This function reads the high 32bit of the tx time stamp + *@return uint32_t + */ + readTXTimestamphi32(); + + static bool + /** + *@brief + *This function listens 1 ms to the channel if the channel is free it returns true if it is busy false + *@return bool + */ + isChannelFree(); + + + + + //----------------------Receiving + static void + /** + *@brief + *This function enables the receiver. -- Note that sending must be completed first + */ + rxEnable(); + + static bool + /** + *@brief + *This function enables the receiver at the given time of the DWM1000 + *@param time + *@return bool + */ + rxEnableAt(uint32_t time); + + + + static bool + /** + *@brief + *This function returns true if a frame was received + *@return bool + */ + checkForRX(); + + static bool + /** + *@brief + *This function returns true if there was an RX-Error + *@return bool + */ + checkForRXError(); + + static uint16_t + /** + *@brief + *This function returns the length of the received frame + *@return uint16_t + */ + rxlength(); + + static void + /** + *@brief + *This function fills the buffer with [size] bytes of the received frame + *@param size + *@param buffer + */ + readrx(uint32_t size,uint8_t buffer[]); + + static void + /** + *@brief + *This function turns off the receiver and soft-resets the receiver register + */ + rxdisable(); + + static void + /** + *@brief + *This function reads the 40 bit register into the given buffer - length needs to be at least 5 + *@param timestamp[] + */ + readRXTimestamp(uint8_t timestamp[]); + + static uint64_t + /** + *@brief + *This function reads 40 bit register into 64Bit Integer + *@return uint64_t + */ + readRXTimestamp64(); + + static uint32_t + /** + *@brief + *This function reads the low 32bits of the rxtimestamp + *@return uint32_t + */ + readRXTimestamplo32(void); + + static uint32_t + /** + *@brief + *This function reads the high 32bits of the rxtimestamp + *@return uint32_t + */ + readRXTimestamphi32(void); + + static void + /** + *@brief + *This function sets an timeout for how long the DWM will try to receive data. + *If 0 only rxdisable() will stop the dwm from receiving + *@param time + */ + setrxtimeout(uint16_t time); + + static void + /** + *@brief + *This function resets the receiver + */ + rxreset(void); + + + //-----------------------Common receive and transmit functions + + static void + /** + *@brief +* + */ + enableExternalSync(); + + static void + /** + *@brief + *This function stops any transmit and receive + */ + trxdisable(); + + static void + /** + *@brief + * This function set starttime for the next transmit/receive + *@param starttime + */ + setdelayedtrxtime(uint32_t starttime); + + //IRQ Handling + + /** + *@brief + *This function turns on and off IRQs when Frames were received + */ + static void toggleReceiveIRQ(); + + /** + *@brief + *This function turns on and off IRQs when Frames were send + */ + static void toggleSendIRQ(); + + /** + *@brief + *This function turns on and off IRQs when a Sync signal was received + */ + static void toggleSyncIRQ(); + + /** + *@brief + *Returns the following: + *RX_Complete = 1, + *TX_Complete = 2, + *RX_Error = 3, + *Sync = 4, + *Unknown = 0 + *@return IRQreason + */ + static IRQreason getIRQReason(); + + + + //Other Stuff + /** + *@brief + * + *@return uint32_t + */ + static uint32_t readStatusRegister(); + + /** + *@brief + * + *@return uint32_t + */ + static uint32_t readStatusMRegister(); + + /** + *@brief + * + *@return uint32_t + */ + static void setCrystaltrim(uint8_t trim); + + + + //Debug + /** + *@brief + * This function displays the statusregister for Debugging in the logstream + */ + static void displayStatusRegister(); + + /** + *@brief + *This function displays the mask statusregister for Debugging in the logstream -- for IRQ + */ + static void displayMaskRegister(); + + + static void + /** + *@brief +* + *@param timestamp[] + */ + readsystime(uint8_t timestamp[]); + + static uint32_t + /** + *@brief +* + *@return uint32_t + */ + readsystimestamphi32(void); + + static uint32_t + /** + *@brief +* + *@return uint32_t + */ + readsystimestamplo32(void); + + //config stuff + static constexpr uint32_t DEVICE_ID = 0xDECA0130; + static constexpr int FORCE_SYS_XTI = 0; + static constexpr int XTRIM_ADDRESS = 0x1E; + static constexpr int LDOTUNE_ADDRESS= 0x04; + static constexpr int PARTID_ADDRESS = 0x06; + static constexpr int LOTID_ADDRESS = 0x07; + static constexpr int ENABLE_ALL_SEQ = 1; + //init stuff + static constexpr int FORCE_LDE = 14; + static constexpr int FORCE_SYS_PLL = 2; + static constexpr int READ_ACC_ON = 7; + static constexpr int READ_ACC_OFF = 8; + static constexpr int FORCE_OTP_ON = 11; + static constexpr int FORCE_OTP_OFF = 12; + static constexpr int FORCE_TX_PLL = 13; + + //----------------------------------------PRIVATE-STUFF------------------------------------------------------------------------------------------------------------ +private: + + + static int + /** + *@brief +* + *@param mode + *@return int + */ + rxenable(RX_Mode mode); + + static int + initialise(uint16_t conf); + + static int + /** + *@brief +* + *@param headerLength + *@param headerBuffer + *@param bodylength + *@param bodyBuffer + *@return int + */ + writetospi(uint16_t headerLength, const uint8_t *headerBuffer, uint32_t bodylength, const uint8_t *bodyBuffer); + + static int + /** + *@brief + * + *@param headerLength + *@param headerBuffer + *@param readlength + *@param readBuffer + *@return int + */ + readfromspi(uint16_t headerLength, const uint8_t *headerBuffer, uint32_t readlength, uint8_t *readBuffer); + + static uint32_t + /** + *@brief + * + *@return uint32_t + */ + readdevid(); + + static void + /** + *@brief + * + *@param value + */ + setxtaltrim(uint8_t value); + + static void + /** + *@brief + * + */ + softreset(); + + static int + /** + *@brief + * + *@param txFrameLength + *@param txFrameBytes + *@param txBufferOffset + *@return int + */ + writetxdata(uint16_t txFrameLength, uint8_t *txFrameBytes, uint16_t txBufferOffset); + + static void + /** + *@brief + * + *@param txFrameLength + *@param txBufferOffset + *@param ranging + */ + writetxfctrl(uint16_t txFrameLength, uint16_t txBufferOffset, int ranging); + + static void + /** + *@brief + * + *@param regFileID + *@param regOffset + *@param regval + */ + write8bitoffsetreg(int regFileID, int regOffset, uint8_t regval); + + static int + /** + *@brief + * + *@param mode + *@return int + */ + starttx(TX_Mode mode); + + static void + /** + *@brief + * + *@param buffer + *@param length + *@param rxBufferOffset + */ + readrxdata(uint8_t *buffer, uint16_t length, uint16_t rxBufferOffset); + + static uint32_t + /** + *@brief + * + *@param addr + *@return uint32_t + */ + read32bitreg(uint16_t addr); + + static uint16_t + /** + *@brief + * + *@param regFileID + *@param regOffset + *@return uint16_t + */ + read16bitoffsetreg(int regFileID,int regOffset); + + static uint32_t + /** + *@brief + * + *@param regFileID + *@param regOffset + *@return uint32_t + */ + read32bitoffsetreg(int regFileID, int regOffset); + + static void + /** + *@brief + * + *@param recordNumber + *@param index + *@param length + *@param buffer + */ + writetodevice(uint16_t recordNumber, uint16_t index, uint32_t length , const uint8_t *buffer); + + static void + /** + *@brief + * + *@param regFileID + *@param regOffset + *@param regval + */ + write16bitoffsetreg(int regFileID,int regOffset,uint16_t regval); + + static void + /** + *@brief + * + *@param regFileID + *@param regOffset + *@param regval + */ + write32bitoffsetreg(int regFileID, int regOffset, uint32_t regval); + + static void + /** + *@brief + * + *@param x + *@param y + */ + write32bitreg(int x, uint32_t y); + + static void + /** + *@brief +* + *@param recordNumber + *@param index + *@param length + *@param buffer + */ + readfromdevice(uint16_t recordNumber, uint16_t index, uint32_t length, uint8_t *buffer); + + static uint64_t + /** + *@brief + * + *@return uint64_t + */ + get_rx_timestamp_u64(void); + + static uint8_t + /** + *@brief + * + *@param regFileID + *@param regOffset + *@return uint8_t + */ + read8bitoffsetreg(int regFileID, int regOffset); + + + + static void + /** + *@brief + * + *@param rxDelayTime + */ + setrxaftertxdelay(uint32_t rxDelayTime); + + static void + /** + *@brief + * + *@param registermask + */ + setIRQ(uint32_t registermask); + + static void + /** + *@brief + * + */ + syncrxbufptrs(void); + + static void + /** + *@brief + * + */ + forcetrxoff(void); + + static void + /** + *@brief + * + */ + _disablesequencing(); + + static void + /** + *@brief + * + */ + _aonarrayupload(); + + static void + /** + *@brief + * + */ + _loaducodefromrom(); + + static uint32_t + /** + *@brief + * + *@param address + *@return uint32_t + */ + _otpread(uint32_t address); + + static void + /** + *@brief + * + *@param clocks + */ + _enableclocks(int clocks); + + static void + /** + *@brief + * + *@param prfIndex + */ + _configlde(int prfIndex); + + + /** + *@brief + * + *@return decaIrqStatus_t + */ + static decaIrqStatus_t decamutexon(); + + static void + decamutexoff(decaIrqStatus_t s); +}; +} +#include "dw1000_impl.hpp" + +#endif + + diff --git a/src/xpcc/driver/radio/dw1000/dw1000_impl.hpp b/src/xpcc/driver/radio/dw1000/dw1000_impl.hpp new file mode 100644 index 000000000..ee0ca3182 --- /dev/null +++ b/src/xpcc/driver/radio/dw1000/dw1000_impl.hpp @@ -0,0 +1,1294 @@ +/** + * Copyright (c) 2017, Marten Junga (Github.com/Maju-Ketchup) + * All Rights Reserved. + * + * The file is part of the xpcc library and is released under the 3-clause BSD + * license. See the file `LICENSE` for the full license governing this code. + * + * + * The headder contains the class implementation of the IEEE standart 802.15.4-2011 Frame + * current max size is 255 bytes + * Set always control first + * + */ + + +//Lot of Parts copied out of the DWM1000 API added some 'easier to use functions' *mja* +//"Don't include this file directly, use 'dw1000.hpp' instead!" + + + +//--------------------------------------------------------------XPCC-Functions-Puplic------------------------------------------------------------------------ + + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint8_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::frame_seq_nb = 0; + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint64_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::hostaddress = 0xFFFFFFFFFFFFFFFF; + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::TX_ANT_DLY = 16436; + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::RX_ANT_DLY = 16436; + +template < typename Spi, typename Cs, typename Reset, typename Irq > +bool +xpcc::Dw1000< Spi, Cs, Reset, Irq >::isChannelFree() +{ + const uint32_t rxEvent = (SYS_STATUS_RXPRD | + SYS_STATUS_RXSFDD | + SYS_STATUS_LDEDONE | + SYS_STATUS_RXPHD | + SYS_STATUS_RXPHE | + SYS_STATUS_RXDFR | + SYS_STATUS_RXFCG | + SYS_STATUS_RXFCE); + write32bitreg(SYS_STATUS_ID,rxEvent); + rxEnable(); + xpcc::delayMicroseconds(1807); // time of two transmissions + trxdisable(); + if (readStatusRegister()&rxEvent) + { + rxreset(); + XPCC_LOG_INFO.printf("\n\nChannel busy\n\n\n"); + return false; + + } + else + { + rxreset(); + return true; + } +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +xpcc::dw1000::IRQreason +xpcc::Dw1000< Spi, Cs, Reset, Irq >::getIRQReason() // TODO +{ + uint32_t systatus = readStatusRegister(); + systatus &= read32bitreg(SYS_MASK_ID); + + if (systatus & SYS_STATUS_RXDFR) + { + write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXDFR); + return xpcc::dw1000::IRQreason::RX_Complete; //Frame received + + } + else if (systatus & SYS_STATUS_TXFRS ) + { + write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS); + return xpcc::dw1000::IRQreason::TX_Complete; //Frame send + } + else if (systatus & SYS_STATUS_ALL_RX_ERR) + {write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); + return xpcc::dw1000::IRQreason::RX_Error; //Receive Error + } + else if (systatus & SYS_MASK_MESYNCR ) + { + write32bitreg(SYS_STATUS_ID, SYS_MASK_MESYNCR); + return xpcc::dw1000::IRQreason::Sync; //Sync + } + else{ + return xpcc::dw1000::IRQreason::Unknown; + } +} + + + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::toggleReceiveIRQ() +{ + setIRQ( readStatusMRegister() ^ (SYS_STATUS_RXDFR | SYS_STATUS_ALL_RX_ERR)); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::toggleSendIRQ() +{ + setIRQ(readStatusMRegister() ^ SYS_MASK_MTXFRS); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::toggleSyncIRQ() +{ + setIRQ(readStatusMRegister() ^ SYS_MASK_MESYNCR); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readStatusRegister() +{ + return read32bitreg(SYS_STATUS_ID); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readStatusMRegister() +{ + return read32bitreg(SYS_MASK_ID); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::setIRQ(uint32_t registermask) +{ + write32bitreg(SYS_MASK_ID, registermask); +} + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint64_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readTXTimestamp64() +{ + uint8_t ts_tab[5]; + uint64_t ts = 0; + readTXTimestamp(ts_tab); + for (int i = 0; i < 5; i++) + { + ts = ts << 8; + ts |= ts_tab[4-i]; + } + return ts; +} + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint64_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readRXTimestamp64() +{ + uint8_t ts_tab[5]; + uint64_t ts = 0; + readRXTimestamp(ts_tab); + for (int i = 0; i < 5; i++) + { + ts = ts << 8; + ts |= ts_tab[4-i]; + } + return ts; +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::enableExternalSync() +{ + write32bitreg(EXT_SYNC_ID,EC_CTRL_OSTRM| 33 << 3); +} +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::setCrystaltrim(uint8_t trim) +{ + if (trim < 0b100000){ + write8bitoffsetreg(0x2B,0x0E,0b01100000 | trim);} +} + + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +bool +xpcc::Dw1000< Spi, Cs, Reset, Irq >::sendAt(int size, uint8_t data[], uint32_t time) +{ + setdelayedtrxtime(time); + write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS); + writetxdata(size, data, 0); // writes TX DATA to DWM1000 + writetxfctrl(size, 0, 1); //write control + if (starttx(START_TX_DELAYED) == SUCCESS)//send data direct + { + //XPCC_LOG_DEBUG << "DATA WAS SENT DELAYED" << xpcc::endl; + return true; + } + else + { + XPCC_LOG_ERROR << "DWM1000: Could not send because delayed time < systemtime" << xpcc::endl; + XPCC_LOG_DEBUG << int (readsystimestamphi32() - time) << xpcc::endl; + return false; + } +} + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +bool +xpcc::Dw1000< Spi, Cs, Reset, Irq >::send(int size, uint8_t data[]) +{ + if (size < 3 || size > 1023) + {return false;} + write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS); + writetxdata(size, data, 0); // writes TX DATA to DWM1000 + writetxfctrl(size, 0, 0); //write control + if (starttx(START_TX_IMMEDIATE) == SUCCESS)//send data direct + { + return true; + } + else + { + return false; + } +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +bool +xpcc::Dw1000< Spi, Cs, Reset, Irq >::checkForRX() +{ + uint32_t status_reg = read32bitreg(SYS_STATUS_ID); + if (status_reg & SYS_STATUS_ALL_RX_TO) + {XPCC_LOG_ERROR << "!!!DWM1000: RECEIVE TIME OUT!!!" << xpcc::endl;} + if (status_reg & SYS_STATUS_ALL_RX_ERR) + {XPCC_LOG_ERROR << "!!!DWM1000: RECEIVE ERROR OCCURRED!!!" << xpcc::endl;} + return (status_reg & (SYS_STATUS_RXFCG)); +} + + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readrx(uint32_t size, uint8_t buffer[]) +{ + if (read32bitreg(SYS_STATUS_ID & SYS_STATUS_RXFCG)) { + write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG); + readrxdata(buffer, size, 0); + } else { + XPCC_LOG_ERROR << "!!!DWM1000: ERROR NO FRAME TO READ!!!" << xpcc::endl; + } +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint16_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::rxlength() +{ + return((uint16_t)read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::displayMaskRegister () +{ + uint32_t maskregister; + maskregister = read32bitreg(SYS_MASK_ID); + XPCC_LOG_DEBUG.printf ("DWM1000 StatusMaskregister: %lx \n",maskregister); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::displayStatusRegister () +{ + uint32_t statusregister; + statusregister = read32bitreg(SYS_STATUS_ID); + XPCC_LOG_DEBUG.printf ("DWM1000 Statusregister: %lx \n", statusregister); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +bool +xpcc::Dw1000< Spi, Cs, Reset, Irq >::isFrameSent() +{ + return (read32bitreg(SYS_STATUS_ID)&SYS_STATUS_TXFRS); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::trxdisable() +{ + write32bitreg(SYS_CTRL_ID,SYS_CTRL_TRXOFF); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +bool +xpcc::Dw1000< Spi, Cs, Reset, Irq >::checkForRXError() +{ + return (read32bitreg(SYS_STATUS_ID) & SYS_STATUS_ALL_RX_ERR); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::rxdisable() +{ + trxdisable(); + rxreset(); +} + +//--------------------------------------------------------DWM1000-API---------------------------------------------------------------------------------- + +template < typename Spi, typename Cs, typename Reset, typename Irq > +xpcc::dw1000::local_data_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::dw1000local; + +template < typename Spi, typename Cs, typename Reset, typename Irq > +int +xpcc::Dw1000< Spi, Cs, Reset, Irq >::writetospi( + uint16_t headerLength, + const uint8_t *headerBuffer, + uint32_t bodylength, + const uint8_t *bodyBuffer) +{ + // XPCC_LOG_DEBUG.printf("write\n"); + Cs::reset(); + + uint8_t r_buffer[256] = {0}; + + Spi::transferBlocking( + (uint8_t*)headerBuffer /* w_buffer */, + r_buffer /* r_buffer */, + headerLength/* length */); + Spi::transferBlocking((uint8_t*)bodyBuffer /* w_buffer */, + r_buffer /* r_buffer */, bodylength/* length */); + + Cs::set(); + + return SUCCESS; +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +int +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readfromspi(uint16_t headerLength, const uint8_t *headerBuffer, uint32_t readlength, uint8_t *readBuffer) +{ + // XPCC_LOG_DEBUG.printf("read\n"); + Cs::reset(); + + uint8_t r_buffer[256] = {0}; + uint8_t w_buffer[256] = {0}; + + Spi::transferBlocking( + (uint8_t*)headerBuffer /* w_buffer */, + r_buffer /* r_buffer */, + headerLength/* length */); + + Spi::transferBlocking( + (uint8_t*)w_buffer /* w_buffer = dummy write*/, + readBuffer /* r_buffer */, readlength/* length */); + + Cs::set(); + + return SUCCESS; +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readdevid() +{ + return read32bitoffsetreg(DEV_ID_ID,0); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::setxtaltrim(uint8_t value) +{ + uint8_t reg_val = (3 << 5) | (value & FS_XTALT_MASK); + write8bitoffsetreg(FS_CTRL_ID, FS_XTALT_OFFSET, reg_val); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::_disablesequencing() +{ + _enableclocks(FORCE_SYS_XTI); // Set system clock to XTI + + write16bitoffsetreg(PMSC_ID, PMSC_CTRL1_OFFSET, PMSC_CTRL1_PKTSEQ_DISABLE); // Disable PMSC ctrl of RF and RX clk blocks +} + + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::_aonarrayupload() +{ + write8bitoffsetreg(AON_ID, AON_CTRL_OFFSET, 0x00); // Clear the register + write8bitoffsetreg(AON_ID, AON_CTRL_OFFSET, AON_CTRL_SAVE); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::softreset() +{ + _disablesequencing(); + + // Clear any AON auto download bits (as reset will trigger AON download) + write16bitoffsetreg(AON_ID, AON_WCFG_OFFSET, 0x00); + // Clear the wake-up configuration + write8bitoffsetreg(AON_ID, AON_CFG0_OFFSET, 0x00); + // Upload the new configuration + _aonarrayupload(); + + // Reset HIF, TX, RX and PMSC + write8bitoffsetreg(PMSC_ID, PMSC_CTRL0_SOFTRESET_OFFSET, PMSC_CTRL0_RESET_ALL); + + // DW1000 needs a 10us sleep to let clk PLL lock after reset - the PLL will automatically lock after the reset + // Could also have polled the PLL lock flag, but then the SPI needs to be < 3MHz !! So a simple delay is easier + //deca_sleep(1); + xpcc::delayMilliseconds(1); + + // Clear reset + write8bitoffsetreg(PMSC_ID, PMSC_CTRL0_SOFTRESET_OFFSET, PMSC_CTRL0_RESET_CLEAR); + + dw1000local.wait4resp = 0; +} + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::_loaducodefromrom() +{ + _enableclocks(FORCE_LDE); + + // Kick off the LDE load + write16bitoffsetreg(OTP_IF_ID, OTP_CTRL, OTP_CTRL_LDELOAD); // Set load LDE kick bit + + //deca_sleep(1); // Allow time for code to upload (should take up to 120 us) + xpcc::delayMilliseconds(1); + // Default clocks (ENABLE_ALL_SEQ) + _enableclocks(ENABLE_ALL_SEQ); // Enable clocks for sequencing +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::_otpread(uint32_t address) +{ + uint32_t ret_data; + + // Write the address + write16bitoffsetreg(OTP_IF_ID, OTP_ADDR, address); + + // Perform OTP Read - Manual read mode has to be set + write8bitoffsetreg(OTP_IF_ID, OTP_CTRL, OTP_CTRL_OTPREAD | OTP_CTRL_OTPRDEN); + write8bitoffsetreg(OTP_IF_ID, OTP_CTRL, 0x00); // OTPREAD is self clearing but OTPRDEN is not + + // Read read data, available 40ns after rising edge of OTP_READ + ret_data = read32bitoffsetreg(OTP_IF_ID, OTP_RDAT); + + // Return the 32bit of read data + return ret_data; +} + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::_enableclocks(int clocks) +{ + uint8_t reg[2]; + + readfromdevice(PMSC_ID, PMSC_CTRL0_OFFSET, 2, reg); + switch(clocks) + { + case ENABLE_ALL_SEQ: + { + reg[0] = 0x00 ; + reg[1] = reg[1] & 0xfe; + } + break; + case FORCE_SYS_XTI: + { + // System and RX + reg[0] = 0x01 | (reg[0] & 0xfc); + } + break; + case FORCE_SYS_PLL: + { + // System + reg[0] = 0x02 | (reg[0] & 0xfc); + } + break; + case READ_ACC_ON: + { + reg[0] = 0x48 | (reg[0] & 0xb3); + reg[1] = 0x80 | reg[1]; + } + break; + case READ_ACC_OFF: + { + reg[0] = reg[0] & 0xb3; + reg[1] = 0x7f & reg[1]; + } + break; + case FORCE_OTP_ON: + { + reg[1] = 0x02 | reg[1]; + } + break; + case FORCE_OTP_OFF: + { + reg[1] = reg[1] & 0xfd; + } + break; + case FORCE_TX_PLL: + { + reg[0] = 0x20 | (reg[0] & 0xcf); + } + break; + case FORCE_LDE: + { + reg[0] = 0x01; + reg[1] = 0x03; + } + break; + default: + break; + } + + // Need to write lower byte separately before setting the higher byte(s) + writetodevice(PMSC_ID, PMSC_CTRL0_OFFSET, 1, ®[0]); + writetodevice(PMSC_ID, 0x1, 1, ®[1]); +} + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::_configlde(int prfIndex) +{ + write8bitoffsetreg(LDE_IF_ID, LDE_CFG1_OFFSET, LDE_PARAM1); // 8-bit configuration register + + if(prfIndex) + { + write16bitoffsetreg( LDE_IF_ID, LDE_CFG2_OFFSET, (uint16_t) LDE_PARAM3_64); // 16-bit LDE configuration tuning register + } + else + { + write16bitoffsetreg( LDE_IF_ID, LDE_CFG2_OFFSET, (uint16_t) LDE_PARAM3_16); + } +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::configure(config_t *conf) +{ + + uint8_t nsSfd_result = 0; + uint8_t useDWnsSFD = 0; + uint8_t chan = conf->chan ; + uint32_t regval ; + uint16_t reg16 = lde_replicaCoeff[conf->rxCode]; + uint8_t prfIndex = conf->prf - PRF_16M; + uint8_t bw = ((chan == 4) || (chan == 7)) ? 1 : 0 ; // Select wide or narrow band + + // For 110 kbps we need a special setup + if(BR_110K == conf->dataRate) + { + dw1000local.sysCFGreg |= SYS_CFG_RXM110K ; + reg16 >>= 3; // lde_replicaCoeff must be divided by 8 + } + else + { + dw1000local.sysCFGreg &= (~SYS_CFG_RXM110K) ; + } + + dw1000local.longFrames = conf->phrMode ; + + dw1000local.sysCFGreg &= ~SYS_CFG_PHR_MODE_11; + dw1000local.sysCFGreg |= (SYS_CFG_PHR_MODE_11 & (conf->phrMode << SYS_CFG_PHR_MODE_SHFT)); + + write32bitreg(SYS_CFG_ID,dw1000local.sysCFGreg) ; + // Set the lde_replicaCoeff + write16bitoffsetreg(LDE_IF_ID, LDE_REPC_OFFSET, reg16) ; + + _configlde(prfIndex); + + // Configure PLL2/RF PLL block CFG/TUNE (for a given channel) + write32bitoffsetreg(FS_CTRL_ID, FS_PLLCFG_OFFSET, fs_pll_cfg[chan_idx[chan]]); + write8bitoffsetreg(FS_CTRL_ID, FS_PLLTUNE_OFFSET, fs_pll_tune[chan_idx[chan]]); + + // Configure RF RX blocks (for specified channel/bandwidth) + write8bitoffsetreg(RF_CONF_ID, RF_RXCTRLH_OFFSET, rx_config[bw]); + + // Configure RF TX blocks (for specified channel and PRF) + // Configure RF TX control + write32bitoffsetreg(RF_CONF_ID, RF_TXCTRL_OFFSET, tx_config[chan_idx[chan]]); + + // Configure the baseband parameters (for specified PRF, bit rate, PAC, and SFD settings) + // DTUNE0 + write16bitoffsetreg(DRX_CONF_ID, DRX_TUNE0b_OFFSET, sftsh[conf->dataRate][conf->nsSFD]); + + // DTUNE1 + write16bitoffsetreg(DRX_CONF_ID, DRX_TUNE1a_OFFSET, dtune1[prfIndex]); + + if(conf->dataRate == BR_110K) + { + write16bitoffsetreg(DRX_CONF_ID, DRX_TUNE1b_OFFSET, DRX_TUNE1b_110K); + } + else + { + if(conf->txPreambLength == PLEN_64) + { + write16bitoffsetreg(DRX_CONF_ID, DRX_TUNE1b_OFFSET, DRX_TUNE1b_6M8_PRE64); + write8bitoffsetreg(DRX_CONF_ID, DRX_TUNE4H_OFFSET, DRX_TUNE4H_PRE64); + } + else + { + write16bitoffsetreg(DRX_CONF_ID, DRX_TUNE1b_OFFSET, DRX_TUNE1b_850K_6M8); + write8bitoffsetreg(DRX_CONF_ID, DRX_TUNE4H_OFFSET, DRX_TUNE4H_PRE128PLUS); + } + } + + // DTUNE2 + write32bitoffsetreg(DRX_CONF_ID, DRX_TUNE2_OFFSET, digital_bb_config[prfIndex][conf->rxPAC]); + + // DTUNE3 (SFD timeout) + // Don't allow 0 - SFD timeout will always be enabled + if(conf->sfdTO == 0) + { + conf->sfdTO = SFDTOC_DEF; + } + write16bitoffsetreg(DRX_CONF_ID, DRX_SFDTOC_OFFSET, conf->sfdTO); + + // Configure AGC parameters + write32bitoffsetreg( AGC_CFG_STS_ID, 0xC, agc_config.lo32); + write16bitoffsetreg( AGC_CFG_STS_ID, 0x4, agc_config.target[prfIndex]); + + // Set (non-standard) user SFD for improved performance, + if(conf->nsSFD) + { + // Write non standard (DW) SFD length + write8bitoffsetreg(USR_SFD_ID, 0x00, dwnsSFDlen[conf->dataRate]); + nsSfd_result = 3 ; + useDWnsSFD = 1 ; + } + regval = (CHAN_CTRL_TX_CHAN_MASK & (chan << CHAN_CTRL_TX_CHAN_SHIFT)) | // Transmit Channel + (CHAN_CTRL_RX_CHAN_MASK & (chan << CHAN_CTRL_RX_CHAN_SHIFT)) | // Receive Channel + (CHAN_CTRL_RXFPRF_MASK & (conf->prf << CHAN_CTRL_RXFPRF_SHIFT)) | // RX PRF + ((CHAN_CTRL_TNSSFD|CHAN_CTRL_RNSSFD) & (nsSfd_result << CHAN_CTRL_TNSSFD_SHIFT)) | // nsSFD enable RX&TX + (CHAN_CTRL_DWSFD & (useDWnsSFD << CHAN_CTRL_DWSFD_SHIFT)) | // Use DW nsSFD + (CHAN_CTRL_TX_PCOD_MASK & (conf->txCode << CHAN_CTRL_TX_PCOD_SHIFT)) | // TX Preamble Code + (CHAN_CTRL_RX_PCOD_MASK & (conf->rxCode << CHAN_CTRL_RX_PCOD_SHIFT)) ; // RX Preamble Code + + write32bitreg(CHAN_CTRL_ID,regval) ; + + // Set up TX Preamble Size, PRF and Data Rate + dw1000local.txFCTRL = ((conf->txPreambLength | conf->prf) << TX_FCTRL_TXPRF_SHFT) | (conf->dataRate << TX_FCTRL_TXBR_SHFT); + write32bitreg(TX_FCTRL_ID, dw1000local.txFCTRL); + + // The SFD transmit pattern is initialised by the DW1000 upon a user TX request, but (due to an IC issue) it is not done for an auto-ACK TX. The + // SYS_CTRL write below works around this issue, by simultaneously initiating and aborting a transmission, which correctly initialises the SFD + // after its configuration or reconfiguration. + // This issue is not documented at the time of writing this code. It should be in next release of DW1000 User Manual (v2.09, from July 2016). + write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, SYS_CTRL_TXSTRT | SYS_CTRL_TRXOFF); // Request TX start and TRX off at the same time +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +int +xpcc::Dw1000< Spi, Cs, Reset, Irq >::initialise(uint16_t conf) +{ + uint16_t otp_addr = 0; + uint32_t ldo_tune = 0; + + dw1000local.dblbuffon = 0; // Double buffer mode off by default + dw1000local.wait4resp = 0; + dw1000local.sleep_mode = 0; + + dw1000local.cbTxDone = NULL; + dw1000local.cbRxOk = NULL; + dw1000local.cbRxTo = NULL; + dw1000local.cbRxErr = NULL; + + // Read and validate device ID return -1 if not recognised + if (DEVICE_ID != readdevid()) // MP IC ONLY (i.e. DW1000) FOR THIS CODE + { + return ERROR ; + } + + // Make sure the device is completely reset before starting initialisation + softreset(); + //.................................... + _enableclocks(FORCE_SYS_XTI); //searchno: 18 // NOTE: set system clock to XTI - this is necessary to make sure the values read by _otpread are reliable + + // Configure the CPLL lock detect + write8bitoffsetreg(EXT_SYNC_ID, EC_CTRL_OFFSET, EC_CTRL_PLLLCK); + + // Read OTP revision number + otp_addr = _otpread(XTRIM_ADDRESS) & 0xffff; // Read 32 bit value, XTAL trim val is in low octet-0 (5 bits) + dw1000local.otprev = (otp_addr >> 8) & 0xff; // OTP revision is next byte + + // Load LDO tune from OTP and kick it if there is a value actually programmed. + ldo_tune = _otpread(LDOTUNE_ADDRESS); + if((ldo_tune & 0xFF) != 0) + { + // Kick LDO tune + write8bitoffsetreg(OTP_IF_ID, OTP_SF, OTP_SF_LDO_KICK); // Set load LDE kick bit + dw1000local.sleep_mode |= AON_WCFG_ONW_LLDO; // LDO tune must be kicked at wake-up + } + + // Load Part and Lot ID from OTP + dw1000local.partID = _otpread(PARTID_ADDRESS); + dw1000local.lotID = _otpread(LOTID_ADDRESS); + + // XTAL trim value is set in OTP for DW1000 module and EVK/TREK boards but that might not be the case in a custom design + dw1000local.init_xtrim = otp_addr & 0x1F; + if (!dw1000local.init_xtrim) // A value of 0 means that the crystal has not been trimmed + { + dw1000local.init_xtrim = FS_XTALT_MIDRANGE ; // Set to mid-range if no calibration value inside + } + // Configure XTAL trim + setxtaltrim(dw1000local.init_xtrim); + + // Load leading edge detect code + if(conf & LOADUCODE) + { + _loaducodefromrom(); + dw1000local.sleep_mode |= AON_WCFG_ONW_LLDE; // microcode must be loaded at wake-up + } + else // Should disable the LDERUN enable bit in 0x36, 0x4 + { + uint16_t rega = read16bitoffsetreg(PMSC_ID, PMSC_CTRL1_OFFSET+1) ; + rega &= 0xFDFF ; // Clear LDERUN bit + write16bitoffsetreg(PMSC_ID, PMSC_CTRL1_OFFSET+1, rega) ; + } + + _enableclocks(ENABLE_ALL_SEQ); // Enable clocks for sequencing + + // The 3 bits in AON CFG1 register must be cleared to ensure proper operation of the DW1000 in DEEPSLEEP mode. + write8bitoffsetreg(AON_ID, AON_CFG1_OFFSET, 0x00); + + // Read system register / store local copy + dw1000local.sysCFGreg = read32bitreg(SYS_CFG_ID) ; // Read sysconfig register + + return SUCCESS ; +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +int +xpcc::Dw1000< Spi, Cs, Reset, Irq >::writetxdata(uint16_t txFrameLength, uint8_t *txFrameBytes, uint16_t txBufferOffset) +{ + if ((txBufferOffset + txFrameLength) <= 1024) + { + // Write the data to the IC TX buffer, (-2 bytes for auto generated CRC) + writetodevice( TX_BUFFER_ID, txBufferOffset, txFrameLength-2, txFrameBytes); + return SUCCESS; + } + else + { + return ERROR; + } +} + + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::writetxfctrl(uint16_t txFrameLength, uint16_t txBufferOffset, int ranging) +{ + uint32_t reg32 = dw1000local.txFCTRL | txFrameLength | (txBufferOffset << TX_FCTRL_TXBOFFS_SHFT) | (ranging << TX_FCTRL_TR_SHFT); + write32bitreg(TX_FCTRL_ID, reg32); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::write8bitoffsetreg(int regFileID, int regOffset, uint8_t regval) +{ + writetodevice(regFileID, regOffset, 1, ®val); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +int +xpcc::Dw1000< Spi, Cs, Reset, Irq >::starttx(TX_Mode mode) +{ + /*int retval = SUCCESS ; + uint8_t temp = 0x00; + + temp |= (uint8_t)SYS_CTRL_TXSTRT ; + write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, temp); + + + return retval;*/ + int retval = SUCCESS ; + uint8_t temp = 0x00; + uint16_t checkTxOK = 0 ; + + if(mode & RESPONSE_EXPECTED) + { + temp = (uint8_t)SYS_CTRL_WAIT4RESP ; // Set wait4response bit + write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, temp); + //dw1000local.wait4resp = 1; + } + + if (mode & START_TX_DELAYED) + { + // Both SYS_CTRL_TXSTRT and SYS_CTRL_TXDLYS to correctly enable TX + temp |= (uint8_t)(SYS_CTRL_TXDLYS | SYS_CTRL_TXSTRT) ; + write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, temp); + checkTxOK = read16bitoffsetreg(SYS_STATUS_ID, 3); // Read at offset 3 to get the upper 2 bytes out of 5 + if ((checkTxOK & SYS_STATUS_TXERR) == 0) // Transmit Delayed Send set over Half a Period away or Power Up error (there is enough time to send but not to power up individual blocks). + { + retval = SUCCESS ; // All okay + } + else + { + // I am taking DSHP set to Indicate that the TXDLYS was set too late for the specified DX_TIME. + // Remedial Action - (a) cancel delayed send + temp = (uint8_t)SYS_CTRL_TRXOFF; // This assumes the bit is in the lowest byte + write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, temp); + // Note event Delayed TX Time too Late + // Could fall through to start a normal send (below) just sending late..... + // ... instead return and assume return value of 1 will be used to detect and recover from the issue. + //dw1000local.wait4resp = 0; + retval = ERROR ; // Failed ! + } + } + else + { + temp |= (uint8_t)SYS_CTRL_TXSTRT ; + write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, temp); + } + + return retval; +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::rxEnable() +{ + rxenable(START_RX_IMMEDIATE); +} + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +bool +xpcc::Dw1000< Spi, Cs, Reset, Irq >::rxEnableAt(uint32_t time) +{ + setdelayedtrxtime(time); + int result; + result = rxenable(RX_Mode::START_RX_DELAYED); + if (result == SUCCESS){ + return true; + } + else { + return false; + } + +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +int +xpcc::Dw1000< Spi, Cs, Reset, Irq >::rxenable(RX_Mode mode) +{ + uint16_t temp ; + uint8_t temp1 ; + + if ((mode & NO_SYNC_PTRS) == 0) + { + syncrxbufptrs(); + } + + temp = (uint16_t)SYS_CTRL_RXENAB ; + + if (mode & START_RX_DELAYED) + { + temp |= (uint16_t)SYS_CTRL_RXDLYE ; + } + + write16bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, temp); + + if (mode & START_RX_DELAYED) // check for errors + { + temp1 = read8bitoffsetreg(SYS_STATUS_ID, 3); // Read 1 byte at offset 3 to get the 4th byte out of 5 + if ((temp1 & (SYS_STATUS_HPDWARN >> 24)) != 0) // if delay has passed do immediate RX on unless IDLE_ON_DLY_ERR is true + { + forcetrxoff(); // turn the delayed receive off + + if((mode & IDLE_ON_DLY_ERR) == 0) // if IDLE_ON_DLY_ERR not set then re-enable receiver + { + write16bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, SYS_CTRL_RXENAB); + } + return ERROR; // return warning indication + } + } + + return SUCCESS; +} // end rxenable() + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readrxdata(uint8_t *buffer, uint16_t length, uint16_t rxBufferOffset) +{ + readfromdevice(RX_BUFFER_ID,rxBufferOffset,length,buffer) ; +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::read32bitreg(uint16_t addr) +{ + return read32bitoffsetreg(addr, 0); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint16_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::read16bitoffsetreg(int regFileID,int regOffset) +{ + uint16_t regval = 0 ; + uint8_t buffer[2] ; + + readfromdevice(regFileID,regOffset,2,buffer); // Read 2 bytes (16-bits) register into buffer + + regval = (buffer[1] << 8) + buffer[0] ; + return regval ; +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::read32bitoffsetreg(int regFileID, int regOffset) +{ + uint32_t regval = 0 ; + int j ; + uint8_t buffer[4] ; + + readfromdevice(regFileID,regOffset,4,buffer); // Read 4 bytes (32-bits) register into buffer + + for (j = 3 ; j >= 0 ; j --) + { + regval = (regval << 8) + buffer[j] ; + } + return regval ; + +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::writetodevice(uint16_t recordNumber, uint16_t index, uint32_t length , const uint8_t *buffer) +{ + uint8_t header[3]; + int cnt = 0; + + if (index == 0) + { + header[cnt++] = 0x80 | recordNumber ; + } + else + { + header[cnt++] = 0xC0 | recordNumber ; + if (index <= 127) + { + header[cnt++] = (uint8_t) index; + } + else + { + header[cnt++] = 0x80 | (uint8_t)(index); + header[cnt++] = (uint8_t) (index >> 7); + } + } + writetospi(cnt, header, length, buffer); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::write16bitoffsetreg(int regFileID,int regOffset,uint16_t regval) +{ + uint8_t buffer[2]; + buffer[0] = regval & 0xFF; + buffer[1] = regval >> 8; + writetodevice(regFileID, regOffset, 2, buffer); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::write32bitoffsetreg(int regFileID, int regOffset, uint32_t regval) +{ + int j; + uint8_t buffer[4]; + + for( j = 0 ; j < 4 ; j++) + { + buffer[j] = regval & 0xff ; + regval >>= 8; + } + + writetodevice(regFileID, regOffset,4, buffer); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::write32bitreg(int x, uint32_t y) +{ + write32bitoffsetreg(x,0,y); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readfromdevice(uint16_t recordNumber, uint16_t index, uint32_t length, uint8_t buffer[]) +{ + uint8_t header[3] ; // Buffer to compose header in + uint16_t cnt = 0; // Counter for length of header + + // Write message header selecting READ operation and addresses as appropriate (this is one to three bytes long) + if (index == 0) // For index of 0, no sub-index is required + { + header[cnt++] = (uint8_t) recordNumber ; // Bit-7 zero is READ operation, bit-6 zero=NO sub-addressing, bits 5-0 is reg file id + } + else + { + header[cnt++] = (uint8_t)(0x40 | recordNumber) ; // Bit-7 zero is READ operation, bit-6 one=sub-address follows, bits 5-0 is reg file id + + if (index <= 127) // For non-zero index < 127, just a single sub-index byte is required + { + header[cnt++] = (uint8_t) index ; // Bit-7 zero means no extension, bits 6-0 is index. + } + else + { + header[cnt++] = 0x80 | (uint8_t)(index) ; // Bit-7 one means extended index, bits 6-0 is low seven bits of index. + header[cnt++] = (uint8_t) (index >> 7) ; // 8-bit value = high eight bits of index. + } + } + + // Do the read from the SPI + readfromspi(cnt, header, length, buffer); // result is stored in the buffer +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +bool +xpcc::Dw1000< Spi, Cs, Reset, Irq >::init(config_t &conf){ + Reset::configure(Reset::OutputType::OpenDrain); + Reset::setOutput(); + Reset::reset(); + + xpcc::delayMilliseconds(5); + Reset::set(); + xpcc::delayMilliseconds(1); + + if (initialise(LOADUCODE) == ERROR) + { + XPCC_LOG_DEBUG << "INIT FAILED\n" ; + return false; + } + else{ + configure(&conf); + return true; + } + +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readTXTimestamplo32(){ + return read32bitreg(TX_TIME_ID); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readTXTimestamphi32(){ + return read32bitoffsetreg(TX_TIME_ID,1); +} + + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint8_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::read8bitoffsetreg(int regFileID, int regOffset){ + + uint8_t regval; + readfromdevice(regFileID, regOffset, 1, ®val); + return regval ; +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readsystime(uint8_t timestamp[]) +{ + readfromdevice(SYS_TIME_ID, SYS_TIME_OFFSET, SYS_TIME_LEN, timestamp); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readsystimestamphi32(void) +{ + return read32bitoffsetreg(SYS_TIME_ID, 1); // Offset is 1 to get the 4 upper bytes out of 5 +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readsystimestamplo32(void) +{ + return read32bitoffsetreg(SYS_TIME_ID, 0); +} + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readRXTimestamplo32(void) +{ + return read32bitreg(RX_TIME_ID); // Read RX TIME as a 32-bit register to get the 4 lower bytes out of 5 +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint32_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readRXTimestamphi32(void) +{ + return read32bitoffsetreg(RX_TIME_ID,1); // Read RX TIME as a 32-bit register to get the 4 higher bytes out of 5 +} + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::setTXAntennaDelay(uint16_t txDelay) +{ + TX_ANT_DLY = txDelay; + write16bitoffsetreg(TX_ANTD_ID, TX_ANTD_OFFSET, txDelay); +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::setRXAntennaDelay(uint16_t rxDelay) +{ + // Set the RX antenna delay for auto TX timestamp adjustment + RX_ANT_DLY = rxDelay; + write16bitoffsetreg(LDE_IF_ID, LDE_RXANTD_OFFSET, rxDelay); +} + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::setrxaftertxdelay(uint32_t rxDelayTime) +{ + uint32_t val = read32bitreg(ACK_RESP_T_ID) ; // Read ACK_RESP_T_ID register + + val &= ~(ACK_RESP_T_W4R_TIM_MASK) ; // Clear the timer (19:0) + + val |= (rxDelayTime & ACK_RESP_T_W4R_TIM_MASK) ; // In UWB microseconds (e.g. turn the receiver on 20uus after TX) + + write32bitreg(ACK_RESP_T_ID, val) ; +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::setrxtimeout(uint16_t time) +{ + uint8_t temp ; + + temp = read8bitoffsetreg(SYS_CFG_ID, 3); // Read at offset 3 to get the upper byte only + + if(time > 0) + { + write16bitoffsetreg(RX_FWTO_ID, RX_FWTO_OFFSET, time) ; + + temp |= (uint8_t)(SYS_CFG_RXWTOE>>24); // Shift RXWTOE mask as we read the upper byte only + // OR in 32bit value (1 bit set), I know this is in high byte. + dw1000local.sysCFGreg |= SYS_CFG_RXWTOE; + + write8bitoffsetreg(SYS_CFG_ID, 3, temp); // Write at offset 3 to write the upper byte only + } + else + { + temp &= ~((uint8_t)(SYS_CFG_RXWTOE>>24)); // Shift RXWTOE mask as we read the upper byte only + // AND in inverted 32bit value (1 bit clear), I know this is in high byte. + dw1000local.sysCFGreg &= ~(SYS_CFG_RXWTOE); + + write8bitoffsetreg(SYS_CFG_ID, 3, temp); // Write at offset 3 to write the upper byte only + } + +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::rxreset(void) +{ + // Set RX reset + write8bitoffsetreg(PMSC_ID, PMSC_CTRL0_SOFTRESET_OFFSET, PMSC_CTRL0_RESET_RX); + + // Clear RX reset + write8bitoffsetreg(PMSC_ID, PMSC_CTRL0_SOFTRESET_OFFSET, PMSC_CTRL0_RESET_CLEAR); +} + + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readRXTimestamp(uint8_t timestamp[]) +{ + readfromdevice(RX_TIME_ID, RX_TIME_RX_STAMP_OFFSET, RX_TIME_RX_STAMP_LEN, timestamp) ; // Get the adjusted time of arrival +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::readTXTimestamp(uint8_t timestamp[]) +{ + readfromdevice(TX_TIME_ID, TX_TIME_TX_STAMP_OFFSET, TX_TIME_TX_STAMP_LEN, timestamp) ; // Get the adjusted time of arrival +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::setdelayedtrxtime(uint32_t starttime) +{ + write32bitoffsetreg(DX_TIME_ID, 1, starttime); // Write at offset 1 as the lower 9 bits of this register are ignored + +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::syncrxbufptrs(void) +{ + uint8_t buff ; + // Need to make sure that the host/IC buffer pointers are aligned before starting RX + buff = read8bitoffsetreg(SYS_STATUS_ID, 3); // Read 1 byte at offset 3 to get the 4th byte out of 5 + + if((buff & (SYS_STATUS_ICRBP >> 24)) != // IC side Receive Buffer Pointer + ((buff & (SYS_STATUS_HSRBP>>24)) << 1) ) // Host Side Receive Buffer Pointer + { + write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_HRBT_OFFSET , 0x01) ; // We need to swap RX buffer status reg (write one to toggle internally) + + } +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::forcetrxoff(void) +{ + decaIrqStatus_t stat ; + uint32_t mask; + mask = read32bitreg(SYS_MASK_ID) ; // Read set interrupt mask + // Need to beware of interrupts occurring in the middle of following read modify write cycle + // We can disable the radio, but before the status is cleared an interrupt can be set (e.g. the + // event has just happened before the radio was disabled) + // thus we need to disable interrupt during this operation + stat = decamutexon() ; + + write32bitreg(SYS_MASK_ID, 0) ; // Clear interrupt mask - so we don't get any unwanted events + + write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, (uint8_t)SYS_CTRL_TRXOFF) ; // Disable the radio + + // Forcing Transceiver off - so we do not want to see any new events that may have happened + write32bitreg(SYS_STATUS_ID, (SYS_STATUS_ALL_TX | SYS_STATUS_ALL_RX_ERR | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_GOOD)); + + syncrxbufptrs(); + + write32bitreg(SYS_MASK_ID, mask) ; // Set interrupt mask to what it was + + // Enable/restore interrupts again... + decamutexoff(stat) ; + //dw1000local.wait4resp = 0; + +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +uint64_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::get_rx_timestamp_u64(void) +{ + uint8_t ts_tab[5]; + uint64_t ts = 0; + int i; + readRXTimestamp(ts_tab); + for (i = 4; i >= 0; i--) + { + ts <<= 8; + ts |= ts_tab[i]; + } + return ts; +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +xpcc::dw1000::decaIrqStatus_t +xpcc::Dw1000< Spi, Cs, Reset, Irq >::decamutexon() +{ + Irq::disableExternalInterrupt(); + return 1; +} + +template < typename Spi, typename Cs, typename Reset, typename Irq > +void +xpcc::Dw1000< Spi, Cs, Reset, Irq >::decamutexoff(decaIrqStatus_t s) +{ + if (s == 0) { + Irq::enableExternalInterrupt(); + } +} + diff --git a/src/xpcc/driver/radio/dw1000/params.hpp b/src/xpcc/driver/radio/dw1000/params.hpp new file mode 100644 index 000000000..c5e884df8 --- /dev/null +++ b/src/xpcc/driver/radio/dw1000/params.hpp @@ -0,0 +1,321 @@ +/** + * Copyright (c) 2018, Marten Junga (Github.com/Maju-Ketchup) + * All Rights Reserved. + * + * The file is part of the xpcc library and is released under the 3-clause BSD + * license. See the file `LICENSE` for the full license governing this code. + * + */ +#ifndef _DECA_PARAMS_ +#define _DECA_PARAMS_ +#include +namespace xpcc +{ +/* + * + * @ingroup driver_radio + * @defgroup dw1000 DW1000 + */ +struct dw1000{ +public: + static constexpr int SUCCESS = 0; + static constexpr int ERROR = -1; + + /* UWB microsecond (uus) to Decawave device time unit (dtu, around 15.65 ps) conversion factor. + * 1 uus = 512 / 499.2 µs and 1 µs = 499.2 * 128 dtu. */ + static constexpr uint32_t UUS_TO_TIME_UNIT = 65536; + static constexpr float UUS_TO_MICROS = (4492e-1/512e0); + static constexpr float TIME_UNIT_TO_S = (1e0/499.2e6/128e0);// *UUS_TO_MICROS; + static constexpr uint32_t standardAntennaDelay = 16436; + + + enum class IRQreason : uint8_t{ + RX_Complete = 1, + TX_Complete = 2, + RX_Error = 3, + Sync = 4, + Unknown = 0 + }; + + enum PRF : uint8_t{ + PRF_64M = 2, + PRF_16M = 1 + }; + enum PLEN : uint8_t{ + PLEN_4096 = 0x0C, //!< Standard preamble length 4096 symbols + PLEN_2048 = 0x28, //!< Non-standard preamble length 2048 symbols + PLEN_1536 = 0x18, //!< Non-standard preamble length 1536 symbols + PLEN_1024 = 0x08, //!< Standard preamble length 1024 symbols + PLEN_512 = 0x34, //!< Non-standard preamble length 512 symbols + PLEN_256 = 0x24, //!< Non-standard preamble length 256 symbols + PLEN_128 = 0x14, //!< Non-standard preamble length 128 symbols + PLEN_64 = 0x04 //!< Standard preamble length 64 symbols + }; + enum PAC: uint8_t{ + PAC8 = 0, + PAC16 = 1, + PAC32 = 2, + PAC64 = 3 + }; + enum BR: uint8_t{ + + BR_110K = 0, + BR_850K = 1, + BR_6M8 = 2 + }; + + enum PHRMODE: uint8_t{ + PHRMODE_STD = 0x0, //< standard PHR mode + PHRMODE_EXT = 0x3 //< DW proprietary extended frames PHR mode + }; + + enum Loadcode: uint8_t{ + LOADUCODE = 0x1, + LOADNONE = 0x0 + }; + + static constexpr uint16_t SFDTOC_DEF = 0x1041; + + typedef int decaIrqStatus_t; + typedef struct + { + uint8_t chan ; //!< channel number {1, 2, 3, 4, 5, 7 } + PRF prf ; //!< Pulse Repetition Frequency {PRF_16M or PRF_64M} + PLEN txPreambLength ; //!< PLEN_64..PLEN_4096 + PAC rxPAC ; //!< Acquisition Chunk Size (Relates to RX preamble length) + uint8_t txCode ; //!< TX preamble code + uint8_t rxCode ; //!< RX preamble code + uint8_t nsSFD ; //!< Boolean should we use non-standard SFD for better performance + BR dataRate ; //!< Data Rate {BR_110K, BR_850K or BR_6M8} + PHRMODE phrMode ; //!< PHR mode {0x0 - standard PHRMODE_STD, 0x3 - extended frames PHRMODE_EXT} + uint16_t sfdTO ; //!< SFD timeout value (in symbols) + } config_t ; + + typedef struct + { + uint32_t status; // (0x3 * 32) & 0x00E0 + static constexpr uint8_t N_STD_FACTOR = (13); + static constexpr uint8_t LDE_PARAM1 = (PEAK_MULTPLIER | N_STD_FACTOR); + + static constexpr uint16_t LDE_PARAM3_16 =(0x1607); + static constexpr uint16_t LDE_PARAM3_64 =(0x0607); + + + + + //----------------------------------------- + // map the channel number to the index in the configuration arrays below + // 0th element is chan 1, 1st is chan 2, 2nd is chan 3, 3rd is chan 4, 4th is chan 5, 5th is chan 7 + static constexpr uint8_t chan_idx[NUM_CH_SUPPORTED] = {0, 0, 1, 2, 3, 4, 0, 5}; + + //----------------------------------------- + static constexpr uint32_t tx_config[NUM_CH] = + { + RF_TXCTRL_CH1, + RF_TXCTRL_CH2, + RF_TXCTRL_CH3, + RF_TXCTRL_CH4, + RF_TXCTRL_CH5, + RF_TXCTRL_CH7, + }; + + //Frequency Synthesiser - PLL configuration + static constexpr uint32_t fs_pll_cfg[NUM_CH] = + { + FS_PLLCFG_CH1, + FS_PLLCFG_CH2, + FS_PLLCFG_CH3, + FS_PLLCFG_CH4, + FS_PLLCFG_CH5, + FS_PLLCFG_CH7 + }; + + //Frequency Synthesiser - PLL tuning + static constexpr uint8_t fs_pll_tune[NUM_CH] = + { + FS_PLLTUNE_CH1, + FS_PLLTUNE_CH2, + FS_PLLTUNE_CH3, + FS_PLLTUNE_CH4, + FS_PLLTUNE_CH5, + FS_PLLTUNE_CH7 + }; + + //bandwidth configuration + static constexpr uint8_t rx_config[NUM_BW] = + { + RF_RXCTRLH_NBW, + RF_RXCTRLH_WBW + }; + + + static constexpr agc_cfg_struct agc_config = + { + AGC_TUNE2_VAL, + { AGC_TUNE1_16M , AGC_TUNE1_64M } //adc target + }; + + //DW non-standard SFD length for 110k, 850k and 6.81M + static constexpr uint8_t dwnsSFDlen[NUM_BR] = + { + DW_NS_SFD_LEN_110K, + DW_NS_SFD_LEN_850K, + DW_NS_SFD_LEN_6M8 + }; + + // SFD Threshold + static constexpr uint16_t sftsh[NUM_BR][NUM_SFD] = + { + { + DRX_TUNE0b_110K_STD, + DRX_TUNE0b_110K_NSTD + }, + { + DRX_TUNE0b_850K_STD, + DRX_TUNE0b_850K_NSTD + }, + { + DRX_TUNE0b_6M8_STD, + DRX_TUNE0b_6M8_NSTD + } + }; + + static constexpr uint16_t dtune1[NUM_PRF] = + { + DRX_TUNE1a_PRF16, + DRX_TUNE1a_PRF64 + }; + + static constexpr uint32_t digital_bb_config[NUM_PRF][NUM_PACS] = + { + { + DRX_TUNE2_PRF16_PAC8, + DRX_TUNE2_PRF16_PAC16, + DRX_TUNE2_PRF16_PAC32, + DRX_TUNE2_PRF16_PAC64 + }, + { + DRX_TUNE2_PRF64_PAC8, + DRX_TUNE2_PRF64_PAC16, + DRX_TUNE2_PRF64_PAC32, + DRX_TUNE2_PRF64_PAC64 + } + }; + + static constexpr uint16_t lde_replicaCoeff[PCODES] = + { + 0, // No preamble code 0 + LDE_REPC_PCODE_1, + LDE_REPC_PCODE_2, + LDE_REPC_PCODE_3, + LDE_REPC_PCODE_4, + LDE_REPC_PCODE_5, + LDE_REPC_PCODE_6, + LDE_REPC_PCODE_7, + LDE_REPC_PCODE_8, + LDE_REPC_PCODE_9, + LDE_REPC_PCODE_10, + LDE_REPC_PCODE_11, + LDE_REPC_PCODE_12, + LDE_REPC_PCODE_13, + LDE_REPC_PCODE_14, + LDE_REPC_PCODE_15, + LDE_REPC_PCODE_16, + LDE_REPC_PCODE_17, + LDE_REPC_PCODE_18, + LDE_REPC_PCODE_19, + LDE_REPC_PCODE_20, + LDE_REPC_PCODE_21, + LDE_REPC_PCODE_22, + LDE_REPC_PCODE_23, + LDE_REPC_PCODE_24 + }; +}; +} +#endif diff --git a/src/xpcc/positioning.hpp b/src/xpcc/positioning.hpp new file mode 100644 index 000000000..889e5d725 --- /dev/null +++ b/src/xpcc/positioning.hpp @@ -0,0 +1,17 @@ +// coding: utf-8 +/* Copyright (c) 2018, Roboterclub Aachen e.V. + * All Rights Reserved. + * + * The file is part of the xpcc library and is released under the 3-clause BSD + * license. See the file `LICENSE` for the full license governing this code. + */ +// ---------------------------------------------------------------------------- + +/** + * @defgroup positioning Positioning + * @brief Positioning Algorithms + */ + + +#include "positioning/ranging.hpp" +#include "positioning/multilateration.hpp" diff --git a/src/xpcc/positioning/multilateration.cpp b/src/xpcc/positioning/multilateration.cpp new file mode 100644 index 000000000..2ad5dfd3a --- /dev/null +++ b/src/xpcc/positioning/multilateration.cpp @@ -0,0 +1,199 @@ +/** +* Copyright (c) 2018, Marten Junga (Github.com/Maju-Ketchup) +* All Rights Reserved. +* +* The file is part of the xpcc library and is released under the 3-clause BSD +* license. See the file `LICENSE` for the full license governing this code. +* +* +* The headder contains the class implementation of the IEEE standart 802.15.4-2011 Frame +* current max size is 255 bytes but some devices are able to send 1023 bytes +* Set always control first +* +*/ +#include "multilateration.hpp" + +void xpcc::multilateration::activemultilateration(Vector &output, + Vector anchor1, + Vector anchor2, + Vector anchor3, + Vector anchor4, + floatunit receiveAnchor1, + floatunit receiveAnchor2, + floatunit receiveAnchor3, + floatunit receiveAnchor4) +{ + floatunit mini; + mini = min(receiveAnchor1,receiveAnchor2); + mini = min(mini,receiveAnchor3); + mini = min(mini,receiveAnchor4); + floatunit receiveTimeAnchor1 = receiveAnchor1-mini; + floatunit receiveTimeAnchor2 = receiveAnchor2-mini; + floatunit receiveTimeAnchor3 = receiveAnchor3-mini; + floatunit receiveTimeAnchor4 = receiveAnchor4-mini; + output = Vector(); + floatunit speedoflight = 299792548.f; + const floatunit A00A[9] ={ + 2*(anchor1.x-anchor4.x), + 2*(anchor1.y-anchor4.y), + 2*(anchor1.z-anchor4.z), + 2*(anchor2.x-anchor4.x), + 2*(anchor2.y-anchor4.y), + 2*(anchor2.z-anchor4.z), + 2*(anchor3.x-anchor4.x), + 2*(anchor3.y-anchor4.y), + 2*(anchor3.z-anchor4.z) + }; + const floatunit A00ba[3] = {(powf(anchor1.x,2.0f)-powf(anchor4.x,2.0f))+(powf(anchor1.y,2.0f)-powf(anchor4.y,2.0f))+(powf(anchor1.z,2.0f)-powf(anchor4.z,2.0f))-((powf(speedoflight,2.0f)*powf(receiveTimeAnchor1,2.0f))-(powf(speedoflight,2.0f)*powf(receiveTimeAnchor4,2.0f))), + (powf(anchor2.x,2.0f)-powf(anchor4.x,2.0f))+(powf(anchor2.y,2.0f)-powf(anchor4.y,2.0f))+(powf(anchor2.z,2.0f)-powf(anchor4.z,2.0f))-((powf(speedoflight,2.0f)*powf(receiveTimeAnchor2,2.0f))-(powf(speedoflight,2.0f)*powf(receiveTimeAnchor4,2.0f))), + (powf(anchor3.x,2.0f)-powf(anchor4.x,2.0f))+(powf(anchor3.y,2.0f)-powf(anchor4.y,2.0f))+(powf(anchor3.z,2.0f)-powf(anchor4.z,2.0f))-((powf(speedoflight,2.0f)*powf(receiveTimeAnchor3,2.0f))-(powf(speedoflight,2.0f)*powf(receiveTimeAnchor4,2.0f))) + }; + const floatunit A0tba[3] = { 2*(powf(speedoflight,2)*(receiveTimeAnchor1-receiveTimeAnchor4)), + 2*(powf(speedoflight,2)*(receiveTimeAnchor2-receiveTimeAnchor4)), + 2*(powf(speedoflight,2)*(receiveTimeAnchor3-receiveTimeAnchor4)) + }; + xpcc::Matrix A0tb(A0tba); + xpcc::Matrix A00M(A00A); + xpcc::Matrix A00b(A00ba); + xpcc::LUDecomposition::solve(A00M,&A00b); + xpcc::LUDecomposition::solve(A00M,&A0tb); + + + floatunit A = powf((anchor4.x-A00b[0][0]),2.f)+powf((anchor4.y-A00b[1][0]),2.f)+powf((anchor4.z-A00b[2][0]),2.f) -powf((speedoflight*receiveTimeAnchor4),2.f); + floatunit B = ((anchor4.x-A00b[0][0])*A0tb[0][0])+((anchor4.y-A00b[1][0])*A0tb[0][1])+((anchor4.z-A00b[2][0])*A0tb[2][0])-(powf(speedoflight,2)*receiveTimeAnchor4); + floatunit C = powf(A0tb[0][0],2)+powf(A0tb[1][0],2)+powf(A0tb[2][0],2)-powf(speedoflight,2); + + floatunit t0 = (2 * B); + floatunit t1 = powf(4*B,2.f)- (4*A*C); + t0 = t0 + sqrt(t1); + t0 = t0 / (2*C); + + output.x = A00b[0][0] + (A0tb[0][0]*t0); + output.y = A00b[1][0] + (A0tb[1][0]*t0); + output.z = A00b[2][0] + (A0tb[2][0]*t0); + + + +} + +void xpcc::multilateration::passivemultilateration(Vector &output, + Vector anchor1, + Vector anchor2, + Vector anchor3, + Vector anchor4, + floatunit ReceiveTimeAnchor1, + floatunit ReceiveTimeAnchor2, + floatunit ReceiveTimeAnchor3, + floatunit ReceiveTimeAnchor4, + floatunit SendtimeAnchor1, + floatunit SendtimeAnchor2, + floatunit SendtimeAnchor3, + floatunit SendtimeAnchor4) +{ + floatunit time2 = (SendtimeAnchor2-SendtimeAnchor1); + floatunit time3 = (SendtimeAnchor3-SendtimeAnchor1); + floatunit time4 = (SendtimeAnchor4-SendtimeAnchor1); + + activemultilateration(output,anchor1,anchor2,anchor3,anchor4, + ReceiveTimeAnchor1, + (ReceiveTimeAnchor2 - time2), + (ReceiveTimeAnchor3 - time3), + (ReceiveTimeAnchor4 - time4)); + +} + + + + +void +xpcc::multilateration::trilaterationAna(Vector &output, + Vector anchor0, + Vector anchor1, + Vector anchor2, + floatunit distanceToAnchor0, + floatunit distanceToAnchor1, + floatunit distanceToAnchor2) +{ +const floatunit a0[12] = { 1,anchor0[0]*anchor0[0],anchor1[0]*anchor1[0],anchor2[0]*anchor2[0], + 1,anchor0[1]*anchor0[1],anchor1[1]*anchor1[1],anchor2[1]*anchor2[1], + 1,anchor0[2]*anchor0[2],anchor1[2]*anchor1[2],anchor2[2]*anchor2[2]}; + +//Matrix A0(a0); + + + + +} + + + + + + +void +xpcc::multilateration::trilateration(Vector &output, + Vector anchor0, + Vector anchor1, + Vector anchor2, + floatunit distanceToAnchor0, + floatunit distanceToAnchor1, + floatunit distanceToAnchor2) +{ + //Annahme Positionsarrays {x,y,z} in m und Distanz in m + //Offset berechnen + output.x = 0.0; + output.y = 0.0; + output.z = 0.0; + Vector offset = Vector(anchor0.x,anchor0.y,anchor0.z); + Vector newAnchor1 = Vector(anchor1.x-offset.x,anchor1.y-offset.y,anchor1.z-offset.z); + Vector newAnchor2 = Vector(anchor2.x-offset.x,anchor2.y-offset.y,anchor2.z-offset.z); + + //Rotation berechnen -- Die von den drei Anchorpunkten aufgespannte Ebene wird in die XY Ebene rotiert + float rotationangleXY = atan2 (newAnchor1.y,newAnchor1.x); + xpcc::multilateration::rotate (2*M_PI-rotationangleXY,newAnchor1.x,newAnchor1.y); + xpcc::multilateration::rotate (2*M_PI-rotationangleXY,newAnchor2.x,newAnchor2.y); + floatunit rotationangleXZ = atan2 (newAnchor1.z,newAnchor1.x); + xpcc::multilateration::rotate (2*M_PI-rotationangleXZ,newAnchor1.x,newAnchor1.z); + xpcc::multilateration::rotate (2*M_PI-rotationangleXZ,newAnchor2.x,newAnchor2.z); + floatunit rotationangleYZ = atan2 (newAnchor2.z,newAnchor2.y); + xpcc::multilateration::rotate (2*M_PI-rotationangleYZ,newAnchor2.y,newAnchor2.z); + + //Berechne Output nach Pablo Cotera et al 2016 [Indoor Robot Positioning using an Enhanced Trilateration Algorithm] + output.x = (powf(distanceToAnchor0,2)-powf(distanceToAnchor1,2)+powf(newAnchor1.x,2)) / (2*newAnchor1.x); + output.y = ((powf(distanceToAnchor0,2)-powf(distanceToAnchor2,2)) + +powf(newAnchor2.x,2) + powf(newAnchor2.y,2)-(2*newAnchor2.x*output.x)) + / (2*newAnchor2.y); + + floatunit zsqaured = powf(distanceToAnchor0,2.0) - powf(output.x,2.0) - powf(output.y,2.0); + + if (zsqaured < 0) + { + output.z = (-1) * sqrt(abs(zsqaured)); + } + else + { + output.z = sqrt(zsqaured); + } + + //zurückrotieren + xpcc::multilateration::rotate (rotationangleYZ,output.y,output.z); + xpcc::multilateration::rotate (rotationangleXZ,output.x,output.z); + xpcc::multilateration::rotate (rotationangleXY,output.x,output.y); + //Offset aufrechnen + output.x = output.x + offset.x; + output.y = output.y + offset.y; + output.z = output.z + offset.z; + //done +} + +void xpcc::multilateration::rotate(floatunit angle, floatunit &x, floatunit &y) +{ + floatunit xnew = floatunit(x * cos(angle) ) - floatunit(y * sin(angle)); + floatunit ynew = floatunit(y * cos(angle) ) + floatunit(x* sin(angle)); + x = xnew; + y = ynew; +} + + + + diff --git a/src/xpcc/positioning/multilateration.hpp b/src/xpcc/positioning/multilateration.hpp new file mode 100644 index 000000000..ad50ddb90 --- /dev/null +++ b/src/xpcc/positioning/multilateration.hpp @@ -0,0 +1,143 @@ +/** +* Copyright (c) 2018, Marten Junga (Github.com/Maju-Ketchup) +* All Rights Reserved. +* +* The file is part of the xpcc library and is released under the 3-clause BSD +* license. See the file `LICENSE` for the full license governing this code. +* +* +* The headder contains the class implementation of the IEEE standart 802.15.4-2011 Frame +* current max size is 255 bytes but some devices are able to send 1023 bytes +* Set always control first +* +*/ +#ifndef MULTILATERATION_HPP +#define MULTILATERATION_HPP + +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include + +typedef float floatunit; + +namespace xpcc { + +/*! +@ingroup positioning +\brief Implementation of Multilateration Algorithms + +*/ +struct multilateration +{ +public: + /*! +Computes numerically the position of a tag when the position (in m) and the distance (in m) to three Anchors are given +\param Vector &output, + Vector anchor0, + Vector anchor1, + Vector anchor2, + floatunit distanceToAnchor0, + floatunit distanceToAnchor1, + floatunit distanceToAnchor2); + /*! + Computes analytically the position of a tag when the position (in m) and the distance (in m) to three Anchors are given + \param Vector &output, + Vector anchor0, + Vector anchor1, + Vector anchor2, + floatunit distanceToAnchor0, + floatunit distanceToAnchor1, + floatunit distanceToAnchor2); + /*! +Computes the Position of a tag when the receive times (in s) and the Positions (in m) of 4 Anchors are known. All Anchors need to be synchronised! +\param Vector &output, + const Vector anchor1, + const Vector anchor2, + const Vector anchor3, + const Vector anchor4, + const floatunit receiveAnchor1, + const floatunit receiveAnchor2, + const floatunit receiveAnchor3, + const floatunit receiveAnchor4); + /*! +Computes the Position of a tag when the Sendtimes of the Tag and the receive times (in s) and the Positions (in m) of 4 Anchors are known. All Anchors need to be synchronised! + +\param Vector &output, + const Vector anchor1, + const Vector anchor2, + const Vector anchor3, + const Vector anchor4, + const floatunit ReceiveTimeAnchor1, + const floatunit ReceiveTimeAnchor2, + const floatunit ReceiveTimeAnchor3, + const floatunit ReceiveTimeAnchor4, + const floatunit SendtimeAnchor1, + const floatunit SendtimeAnchor2, + const floatunit SendtimeAnchor3, + const floatunit SendtimeAnchor4); +private: + static void rotate(floatunit angle, floatunit &x, floatunit &y); +}; +} +#endif // MULTILATERATION_HPP diff --git a/src/xpcc/positioning/ranging.hpp b/src/xpcc/positioning/ranging.hpp new file mode 100644 index 000000000..7bc8a4372 --- /dev/null +++ b/src/xpcc/positioning/ranging.hpp @@ -0,0 +1,207 @@ +/** +* Copyright (c) 2018, Marten Junga (Github.com/Maju-Ketchup) +* All Rights Reserved. +* +* The file is part of the xpcc library and is released under the 3-clause BSD +* license. See the file `LICENSE` for the full license governing this code. +* +* +* The headder contains the class implementation of the IEEE standart 802.15.4-2011 Frame +* current max size is 255 bytes but some devices are able to send 1023 bytes +* Set always control first +* +*/ + +#ifndef RANGING +#define RANGING + +#include + + +namespace xpcc { +/** +* @ingroup positioning +* @brief implements ranging Algorithms for IEEE 802.15.4 Devices +* +* IMPLEMENTS single-sided two-way ranging for UWB-transmitters \n +* IMPLEMENTS double-sided two-way ranging for UWB-transmitters \n +* +* This Class gives functions and constants for Single and Doublesided Two Way Ranging for UWB devices +* It currently supports solo the Decawave DW1000 +* +*/ + +template < typename ComDevice > +class Ranging +{ +public: + + static constexpr uint32_t + control = (frame802154::DATA< +float +xpcc::Ranging::computeDsTwrDistance(float distance,xpcc::Frame802154 receiveframe) +{ + + uint8_t buffer[5]; + receiveframe.getPayload(5,buffer); + //receiveframe.debugToString(); + if (buffer[0] == DSTWR_RESP1) + { + float tof; + tof = getTof(buffer); + //XPCC_LOG_DEBUG.printf("foreign tof %f \n",tof); + tof = tof*ComDevice::TIME_UNIT_TO_S; + + return ((computeDistance(tof,SPEED_OF_LIGHT)+ distance)/2); + } + else + { + return 0; + } +} + +template < typename ComDevice > +void +xpcc::Ranging::sendSSTWRinit() +{ + xpcc::Frame802154 sendframe; + uint8_t payload = SSTWR_INIT; + static uint8_t buffer[256]; + + sendframe.setControl(control); // 16Bit Addresses short PAN DATA + sendframe.setSequenceNumber(ComDevice::frame_seq_nb); //Set SequenceNumber + sendframe.setDestinationPANAddress(0xDECA); //Set PANAddress + sendframe.setDestinationAddress16(frame802154::broadcast16bitAddress); //Set destinationaddress + sendframe.setSourceAddress16(ComDevice::hostaddress); //Set Sourceaddress to own address + sendframe.setPayload(1,&payload); + sendframe.getFrame(buffer); + ComDevice::send(sendframe.length, buffer); + ComDevice::frame_seq_nb++; +} + +template < typename ComDevice > +void +xpcc::Ranging::sendSSTWRinitAt(uint16_t address) +{ + xpcc::Frame802154 sendframe; + uint8_t payload = SSTWR_INIT; + static uint8_t buffer[256]; + + sendframe.setControl(control); // 16Bit Addresses short PAN DATA + sendframe.setSequenceNumber(ComDevice::frame_seq_nb); //Set SequenceNumber + sendframe.setDestinationPANAddress(0xDECA); //Set PANAddress + sendframe.setDestinationAddress16(address); //Set destinationaddress + sendframe.setSourceAddress16(ComDevice::hostaddress); //Set Sourceaddress to own address + sendframe.setPayload(1,&payload); + sendframe.getFrame(buffer); + ComDevice::send(sendframe.length, buffer); + ComDevice::frame_seq_nb++; +} + +template < typename ComDevice > +void +xpcc::Ranging::sendDSTWRinit() +{ + xpcc::Frame802154 sendframe; + uint8_t payload = DSTWR_INIT0; + static uint8_t buffer[256]; + + sendframe.setControl(control); // 16Bit Addresses short PAN DATA + sendframe.setSequenceNumber(ComDevice::frame_seq_nb); //Set SequenceNumber + sendframe.setDestinationPANAddress(0xDECA); //Set PANAddress + sendframe.setDestinationAddress16(frame802154::broadcast16bitAddress); //Set destinationaddress + sendframe.setSourceAddress16(ComDevice::hostaddress); //Set Sourceaddress to own address + sendframe.setPayload(1,&payload); + sendframe.getFrame(buffer); + ComDevice::send(sendframe.length, buffer); + //sendframe.debugToString(); + ComDevice::frame_seq_nb++; +} + + + +template < typename ComDevice > +void +xpcc::Ranging::sendDSTWRinitAt(uint16_t address) +{ + xpcc::Frame802154 sendframe; + uint8_t payload = DSTWR_INIT0; + static uint8_t buffer[256]; + + sendframe.setControl(control); // 16Bit Addresses short PAN DATA + sendframe.setSequenceNumber(ComDevice::frame_seq_nb); //Set SequenceNumber + sendframe.setDestinationPANAddress(0xDECA); //Set PANAddress + sendframe.setDestinationAddress16(address); //Set destinationaddress + sendframe.setSourceAddress16(ComDevice::hostaddress); //Set Sourceaddress to own address + sendframe.setPayload(1,&payload); + sendframe.getFrame(buffer); + ComDevice::send(sendframe.length, buffer); + //sendframe.debugToString(); + ComDevice::frame_seq_nb++; +} + +template < typename ComDevice > +bool +xpcc::Ranging::sendAnswer(xpcc::Frame802154 receiveframe) +{ + + static uint8_t buffer[256]; + + receiveframe.getPayload(receiveframe.payloadlength,buffer); + if (buffer[0] == SSTWR_INIT){ + return(sendtimestamps(SSTWR_RESP,receiveframe)); + } + else if (buffer[0] == DSTWR_INIT0){ + return(sendtimestamps(DSTWR_RESP0,receiveframe)); + } + else if (buffer[0] == DSTWR_RESP0){ + return(sendtimestamps(DSTWR_INIT1,receiveframe)); + } + else if (buffer[0] == DSTWR_INIT1){ + sendtof(receiveframe); + return true; + } + return false; +} + +template < typename ComDevice > +bool +xpcc::Ranging::IsRangingFrame(xpcc::Frame802154 receiveframe) +{ + uint8_t buffer[256]; + receiveframe.getPayload(receiveframe.payloadlength,buffer); + return (buffer[0] == SSTWR_INIT || buffer[0] == SSTWR_RESP || buffer[0] == DSTWR_INIT0 || buffer[0] == DSTWR_RESP0 || buffer[0] == DSTWR_INIT1 || buffer[0] == DSTWR_RESP1); +} + + +template < typename ComDevice > +float +xpcc::Ranging::computeSsTwrDistance(xpcc::Frame802154 receiveframe) +{ + + uint64_t owntx,ownrx; + uint32_t responserx = 0; + uint32_t responsetx = 0; + float tof; + float distance = 0; + static uint8_t buffer[256]; + + receiveframe.getPayload(receiveframe.payloadlength,buffer); + if (buffer[0] == SSTWR_RESP || buffer[0] == DSTWR_RESP0) + { + //timeout.restart(0); + gettimestamps(responserx,responsetx,buffer); + owntx = ComDevice::readTXTimestamp64(); + ownrx = ComDevice::readRXTimestamp64(); + + tof = computeSsTwrTof(ownrx,owntx,responsetx,responserx)*ComDevice::TIME_UNIT_TO_S; + distance = computeDistance(tof,SPEED_OF_LIGHT); + ComDevice::frame_seq_nb = receiveframe.getSequenceNumber() + 1 ; + + } + else + { + XPCC_LOG_ERROR << "EXPECTET RANGING FRAME GOT:" << xpcc::endl ; + receiveframe.debugToString(); + XPCC_LOG_ERROR << xpcc::endl << xpcc::endl ; + + } + return (distance); +} + +template < typename ComDevice > +int +xpcc::Ranging::computeSsTwrTof(int init_rx, int init_tx, int resp_tx , int resp_rx) +{ + int returnv = (((init_rx-init_tx)-(resp_tx-resp_rx))/2); + return (returnv); +} + +template < typename ComDevice > +float +xpcc::Ranging::computeDistance(float tof, int travelspeed) +{ + return (tof*travelspeed); +} + +template < typename ComDevice > +void +xpcc::Ranging::sendtof(xpcc::Frame802154 receiveframe){ + xpcc::Frame802154 send; + + static uint8_t buffer[256]; + + receiveframe.getPayload(receiveframe.payloadlength,buffer); + + if (buffer[0] == DSTWR_INIT1) + { + int tof = 0; + tof= getTof(receiveframe); + //SET ANSWER + send.setControl(control); // 16Bit Addresses; one PAN; DATA + send.setDestinationPANAddress(receiveframe.getDestinationPANAddress()); //Set PANAddress + send.setSourceAddress16(uint16_t(ComDevice::hostaddress)); //Set Sourceaddress to own address + setanswerpayload_tof(buffer,tof); + ComDevice::frame_seq_nb = receiveframe.getSequenceNumber() + 1 ; + send.setSequenceNumber(ComDevice::frame_seq_nb); + send.setDestinationAddress16(receiveframe.getSourceAddress16()); + send.setPayload(6,buffer); + send.getFrame(buffer); + //SEND ANSWER + ComDevice::send(send.length,buffer); + //XPCC_LOG_DEBUG.printf("owntx = %llu,ownrx = %llu,responserx = %lu, responsetx= %lu, tof: %d \n", owntx, ownrx,responserx, responsetx, tof); + //receiveframe.debugToString(); + //send.debugToString(); + } +} + +template < typename ComDevice > +bool +xpcc::Ranging::sendtimestamps(uint8_t flag, xpcc::Frame802154 receiveframe) +{ + xpcc::Frame802154 send; + uint32_t sendtime; + uint64_t owntx,ownrx; + static uint8_t buffer[256]; + //PREPAIR ANSWER + send.setControl(control); // 16Bit Addresses; one PAN; DATA + send.setDestinationPANAddress(receiveframe.getDestinationPANAddress()); //Set PANAddress + send.setSourceAddress16(uint16_t(ComDevice::hostaddress)); //Set Sourceaddress to own address + ownrx = ComDevice::readRXTimestamp64(); + sendtime = ((ownrx + (RESP_RX_TIMEOUT_UUS * (ComDevice::UUS_TO_TIME_UNIT))) >> 8); + owntx = ((((uint64_t)(sendtime & 0xFFFFFFEUL) << 8) )) + ComDevice::TX_ANT_DLY; + // set starttime + setanswerpayload(buffer, flag , ownrx, owntx); + (ComDevice::frame_seq_nb) = receiveframe.getSequenceNumber() + 1 ; + //SET ANSWER + send.setSequenceNumber(ComDevice::frame_seq_nb); + send.setDestinationAddress16(receiveframe.getSourceAddress16()); + send.setPayload(9,buffer); + send.getFrame(buffer); + //SEND ANSWER + ComDevice::frame_seq_nb ++; + return(ComDevice::sendAt(send.length,buffer,sendtime)); +} + +template < typename ComDevice > +void +xpcc::Ranging::gettimestamps(uint32_t ×tamp1,uint32_t ×tamp2 , uint8_t buffer[]) +{ + int i; + timestamp1 = 0; + timestamp2 = 0; + for (i=4;i>0;i--) + { + timestamp1 = (timestamp1<<8)|buffer[i]; + } + for (i=9;i>4;i--) + { + timestamp2 = (timestamp2<<8)|buffer[i]; + } + //XPCC_LOG_DEBUG.printf("timestamp1: %lu , timestamp2: %lu \n",timestamp1,timestamp2); + return; +} + +template < typename ComDevice > +int +xpcc::Ranging::getTof(uint8_t buffer[]) +{ + + int i; + int tof = 0; + for (i=1;i<5;i++){ + tof |= (uint32_t)buffer[i]<<(i-1)*8;} + if (buffer[5] == 0xFF){ + tof = 0- tof; } + return tof; +} + +template < typename ComDevice > +void +xpcc::Ranging::setanswerpayload(uint8_t buffer [],uint8_t flag, uint64_t ownrx, uint64_t owntx) +{ + buffer[0] = flag; + int i; + for (i=1;i<5;i++) + { + buffer[i] = ownrx >> ((i-1)*8); + } + for (i=5;i<9;i++) + { + buffer[i] = owntx >> ((i-5)*8); + } +} + +template < typename ComDevice > +void +xpcc::Ranging::setanswerpayload_tof(uint8_t buffer[], int tof) +{ + buffer[0] = DSTWR_RESP1; + if (tof < 0){ + buffer[5] = 0xFF; + } + else{ + buffer[5] = 0x00; + } + if (tof < 0) { + tof = tof - (2*tof); + } + + uint32_t tof2 = tof; + int i; + for (i=1;i<5;i++) + { + buffer[i] = tof2 >> ((i-1)*8); + } +} + +template < typename ComDevice > +int +xpcc::Ranging:: getTof(xpcc::Frame802154 receiveframe) +{ + + uint64_t owntx,ownrx; + uint32_t responserx; + uint32_t responsetx = 0; + int tof; + static uint8_t buffer[256]; + + receiveframe.getPayload(receiveframe.payloadlength,buffer); + if (buffer[0] == DSTWR_RESP0 ||buffer[0] == DSTWR_INIT1) + { + //timeout.restart(0); + gettimestamps(responserx,responsetx,buffer); + owntx = ComDevice::readTXTimestamp64(); + ownrx = ComDevice::readRXTimestamp64(); + tof = computeSsTwrTof(ownrx,owntx,responsetx,responserx); + + } + else if (buffer[0] == DSTWR_RESP1) + { + tof = getTof(buffer); + } + else + { + XPCC_LOG_ERROR << "EXPECTET RANGING FRAME WITH HEADER E3|E4|E5 GOT:" << xpcc::endl ; + receiveframe.debugToString(); + XPCC_LOG_ERROR << xpcc::endl << xpcc::endl ; + tof = 0; + + } + return tof; +} + + + diff --git a/src/xpcc/positioning/test/ranging_test.cpp b/src/xpcc/positioning/test/ranging_test.cpp new file mode 100644 index 000000000..97a0b54d0 --- /dev/null +++ b/src/xpcc/positioning/test/ranging_test.cpp @@ -0,0 +1,90 @@ +// coding: utf-8 +/** +* Copyright (c) 2018, Marten Junga (Github.com/Maju-Ketchup) +* All Rights Reserved. +* +* The file is part of the xpcc library and is released under the 3-clause BSD +* license. See the file `LICENSE` for the full license governing this code. +* +* +* The headder contains the class implementation of the IEEE standart 802.15.4-2011 Frame +* current max size is 255 bytes but some devices are able to send 1023 bytes +* Set always control first +* +*/ +// ---------------------------------------------------------------------------- + +#include "ranging_test.hpp" +#include "../ranging.hpp" +#include "testdevice.hpp" +#include +using ranging = xpcc::Ranging ; + +void RangingTest::testRangingMath() +{ + TEST_ASSERT_EQUALS(ranging::computeSsTwrTof(0,0,0,0),0); + TEST_ASSERT_EQUALS(ranging::computeSsTwrTof(3,0,1,0),1); + TEST_ASSERT_EQUALS(ranging::computeSsTwrTof(3,0,5,0),-1); + TEST_ASSERT_EQUALS_FLOAT(ranging::computeDistance(0,ranging::SPEED_OF_LIGHT),0); + TEST_ASSERT_EQUALS_FLOAT(ranging::computeDistance(1,ranging::SPEED_OF_LIGHT),float(ranging::SPEED_OF_LIGHT)); + TEST_ASSERT_EQUALS_FLOAT(ranging::computeDistance(0.5,ranging::SPEED_OF_LIGHT),float(ranging::SPEED_OF_LIGHT)/2.0); + + xpcc::Frame802154 testframe; + testframe.setControl(0x8841); + uint8_t payload[] = {0xE1,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS_FLOAT(ranging::computeSsTwrDistance(testframe),1.0*(1e0/499.2e6/128e0)*ranging::SPEED_OF_LIGHT); + payload[0] = 0xE5; + payload[1] = 0x1; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS_FLOAT(ranging::computeDsTwrDistance(0.0,testframe),0.5*(1e0/499.2e6/128e0)*ranging::SPEED_OF_LIGHT); + TEST_ASSERT_EQUALS_FLOAT(ranging::computeDsTwrDistance(1.0,testframe),0.5*(1+(1e0/499.2e6/128e0)*ranging::SPEED_OF_LIGHT)); + payload[1] = 0x0; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS_FLOAT(ranging::computeDsTwrDistance(1.0,testframe),0.5*(1+0)); + TEST_ASSERT_EQUALS_FLOAT(ranging::computeDsTwrDistance(0,testframe),0); +} + +void RangingTest::testRangingLogic() +{ + xpcc::Frame802154 testframe; + testframe.setControl(0x8841); + uint8_t payload[] = {0xE0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS(ranging::IsRangingFrame(testframe),true); + payload[0] = 0xE1; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS(ranging::IsRangingFrame(testframe),true); + payload[0] = 0xE2; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS(ranging::IsRangingFrame(testframe),true); + payload[0] = 0xE3; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS(ranging::IsRangingFrame(testframe),true); + payload[0] = 0xE4; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS(ranging::IsRangingFrame(testframe),true); + payload[0] = 0xE5; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS(ranging::IsRangingFrame(testframe),true); + payload[0] = 0xE6; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS(ranging::IsRangingFrame(testframe),false); + payload[0] = 0xE3; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS(ranging::getTof(testframe),1); + payload[5] = 0x2; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS(ranging::getTof(testframe),0); + payload[1] = 0x2; + payload[5] = 0x6; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS(ranging::getTof(testframe),-1); + payload[0] = 0xE5; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS(ranging::getTof(testframe),2); + payload[5] = 0xFF; + testframe.setPayload(9,payload); + TEST_ASSERT_EQUALS(ranging::getTof(testframe),-2); + +} diff --git a/src/xpcc/positioning/test/ranging_test.hpp b/src/xpcc/positioning/test/ranging_test.hpp new file mode 100644 index 000000000..ffb8589b2 --- /dev/null +++ b/src/xpcc/positioning/test/ranging_test.hpp @@ -0,0 +1,24 @@ +// coding: utf-8 +/** +* Copyright (c) 2018, Marten Junga (Github.com/Maju-Ketchup) +* All Rights Reserved. +* +* The file is part of the xpcc library and is released under the 3-clause BSD +* license. See the file `LICENSE` for the full license governing this code. +* +* +* The headder contains the class implementation of the IEEE standart 802.15.4-2011 Frame +* current max size is 255 bytes but some devices are able to send 1023 bytes +* Set always control first +* +*/ +#include + +class RangingTest : public unittest::TestSuite +{ +public: + void + testRangingMath(); + void + testRangingLogic(); +}; diff --git a/src/xpcc/positioning/test/testdevice.hpp b/src/xpcc/positioning/test/testdevice.hpp new file mode 100644 index 000000000..d0b08d3b5 --- /dev/null +++ b/src/xpcc/positioning/test/testdevice.hpp @@ -0,0 +1,38 @@ +/** +* Copyright (c) 2018, Marten Junga (Github.com/Maju-Ketchup) +* All Rights Reserved. +* +* The file is part of the xpcc library and is released under the 3-clause BSD +* license. See the file `LICENSE` for the full license governing this code. +* +* +* The headder contains the class implementation of the IEEE standart 802.15.4-2011 Frame +* current max size is 255 bytes but some devices are able to send 1023 bytes +* Set always control first +* +*/ + +class testdevice{ +public: + static constexpr uint32_t UUS_TO_TIME_UNIT = 65536; + static constexpr float UUS_TO_MICROS = (4492e-1/512e0); + static constexpr float TIME_UNIT_TO_S = (1e0/499.2e6/128e0); + static uint8_t frame_seq_nb; + static uint64_t hostaddress; + static bool send(int length,uint16_t buffer[]) { + int a = length; + a = a; + buffer[0] = buffer[0]; + return true;} + static bool sendAt(int length,uint16_t buffer[], uint32_t time) { + int a = length; + uint32_t b = time; + a=a; + b=b; + buffer[0] = buffer[0]; + return true;} + static uint64_t readTXTimestamp64(){return 1;} + static uint64_t readRXTimestamp64(){return 3;} + +}; +#include "testdevice_impl.hpp" diff --git a/src/xpcc/positioning/test/testdevice_impl.hpp b/src/xpcc/positioning/test/testdevice_impl.hpp new file mode 100644 index 000000000..d3b64f3c9 --- /dev/null +++ b/src/xpcc/positioning/test/testdevice_impl.hpp @@ -0,0 +1,16 @@ +/** +* Copyright (c) 2018, Marten Junga (Github.com/Maju-Ketchup) +* All Rights Reserved. +* +* The file is part of the xpcc library and is released under the 3-clause BSD +* license. See the file `LICENSE` for the full license governing this code. +* +* +* The headder contains the class implementation of the IEEE standart 802.15.4-2011 Frame +* current max size is 255 bytes but some devices are able to send 1023 bytes +* Set always control first +* +*/ +#include +uint8_t testdevice::frame_seq_nb = 0; +uint64_t testdevice::hostaddress = 0x1234567890ABCDEF;