aleks/AirsoftTracker
1
0
Fork 0
mirror of https://github.com/ddv2005/AirsoftTracker.git synced 2026-04-01 04:54:01 +00:00
Code Issues Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
unknown
2023-08-11 00:29:02 -04:00
commit 5ab7512417
629 changed files with 77781 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2153
src/dwm1000/DW1000Ng.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

608
src/dwm1000/DW1000Ng.hpp Normal file
View File

@@ -0,0 +1,608 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
/*
* Copyright (c) 2015 by Thomas Trojer <thomas@trojer.net>
* Decawave DW1000Ng library for arduino.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* @file DW1000Ng.h
* Arduino driver library (header file) for the Decawave DW1000Ng UWB transceiver Module.
*/
#pragma once
#include <stdlib.h>
#include <string.h>
#include <Arduino.h>
#include "Adafruit_MCP23017.h"
#include "driver/spi_master.h"
#include "DW1000NgConstants.hpp"
#include "DW1000NgConfiguration.hpp"
#include "DW1000NgCompileOptions.hpp"
namespace DW1000Ng {
/**
Initiates and starts a sessions with a DW1000. If rst is not set or value 0xff, a soft resets (i.e. command
triggered) are used and it is assumed that no reset line is wired.
@param[in] ss The SPI Selection pin used to identify the specific connection
@param[in] irq The interrupt line/pin that connects the Arduino.
@param[in] rst The reset line/pin for hard resets of ICs that connect to the Arduino. Value 0xff means soft reset.
*/
void initialize(uint8_t ss, uint8_t irq, uint8_t rst, spi_host_device_t spihost,Adafruit_MCP23017 *expander);
/**
Enable debounce Clock, used to clock the LED blinking
*/
void enableDebounceClock();
/**
Enable led blinking feature
*/
void enableLedBlinking();
/**
Set DW1000's GPIO pins mode
*/
void setGPIOMode(uint8_t msgp, uint8_t mode);
/**
Applies the common sleep configuration and on-wake mode to the DW1000 for both DEEP_SLEEP and SLEEP modes.
ONW_LLDO_BIT and ONW_LLDE_BIT are 1 to default.
@param [in] config struct The sleep/deepsleep configuration to apply to the DW1000
*/
void applySleepConfiguration(sleep_configuration_t sleep_config);
/**
Enter in DeepSleep. applySleepConfiguration must be called first.
Either spi wakeup or pin wakeup must be enabled.
-- In case of future implementation of Sleep mode, you must reset proper antenna delay with setTxAntennaDelay() after wakeUp event. --
*/
void deepSleep();
/**
Wake-up from deep sleep by toggle chip select pin
*/
void spiWakeup();
/**
Resets all connected or the currently selected DW1000 chip.
Uses hardware reset or in case the reset pin is not wired it falls back to software Reset.
*/
void reset();
/**
Resets the currently selected DW1000 chip programmatically (via corresponding commands).
*/
void softwareReset();
/**
(Re-)set the network identifier which the selected chip should be associated with. This
setting is important for certain MAC address filtering rules.
This is also referred as PanId
@param[in] val An arbitrary numeric network identifier.
*/
void setNetworkId(uint16_t val);
/**
Gets the network identifier (a.k.a PAN id) set for the device
@param[out] id the bytes that represent the PAN id (2 bytes)
*/
void getNetworkId(byte id[]);
/**
(Re-)set the device address (i.e. short address) for the currently selected chip. This
setting is important for certain MAC address filtering rules.
@param[in] val An arbitrary numeric device address.
*/
void setDeviceAddress(uint16_t val);
/**
Gets the short address identifier set for the device
@param[out] address the bytes that represent the short address of the device(2 bytes)
*/
void getDeviceAddress(byte address[]);
/**
Sets the device Extended Unique Identifier.
This is a long identifier of the device.
@param[in] eui A string containing the eui in its normal notation using columns.
*/
void setEUI(char eui[]);
/**
Sets the device Extended Unique Identifier.
This is a long identifier of the device.
@param[in] eui The raw bytes of the eui.
*/
void setEUI(byte eui[]);
/**
Gets the device Extended Unique Identifier.
@param[out] eui The 8 bytes of the EUI.
*/
void getEUI(byte eui[]);
/**
Sets the transmission power of the device.
Be careful to respect your current country limitations.
@param[in] power Bytes that represent the power
*/
void setTXPower(byte power[]);
/**
Sets the transmission power of the device.
Be careful to respect your current country limitations.
@param[in] power Bytes (written as a 32-bit number) that represent the power
*/
void setTXPower(int32_t power);
/**
Sets the transmission power of the device.
Be careful to respect your current country limitations.
@param[in] driver_amplifier Base power amplifier
@param[in] mixer Mixer power
*/
void setTXPower(DriverAmplifierValue driver_amplifier, TransmitMixerValue mixer);
/**
Automatically sets power in respect to the current device settings.
This should be guaranteed to set power under -41.3 dBm / MHz (legal limit in most countries).
*/
void setTXPowerAuto();
/**
Sets the pulse generator delay value.
You should use the setTCPGDelayAuto() function.
*/
void setTCPGDelay(byte tcpg_delay);
/**
Automatically sets pulse generator delay value
*/
void setTCPGDelayAuto();
/**
Enables transmit power spectrum test mode that is used for Transmit Power regulatory testing
@param [in] repeat_interval the interval to repeat the transmission
*/
void enableTransmitPowerSpectrumTestMode(int32_t repeat_interval);
/**
Sets a delay for transmission and receive
@param [in] futureTimeBytes the timestamp in bytes of the time of the transmission (in UWB time)
*/
void setDelayedTRX(byte futureTimeBytes[]);
/**
Sets the transmission bytes inside the tx buffer of the DW1000
@param [in] data the bytes to transmit
@param [in] n the length of the array of bytes
*/
void setTransmitData(byte data[], uint16_t n);
/**
Sets the transmission bytes inside the tx buffer of the DW1000 based on the input string
@param [in] data the string to transmit
*/
void setTransmitData(const String& data);
/**
Gets the received bytes and stores them in a byte array
@param [out] data The array of byte to store the data
@param [out] n The length of the byte array
*/
void getReceivedData(byte data[], uint16_t n);
/**
Stores the received data inside a string
param [out] data the string that will contain the data
*/
void getReceivedData(String& data);
/**
Calculates the length of the received data
returns the length of the data
*/
uint16_t getReceivedDataLength();
/**
Calculates the latest transmission timestamp
return the last transmission timestamp
*/
uint64_t getTransmitTimestamp();
/**
Calculates the latest receive timestamp
return the last receive timestamp
*/
uint64_t getReceiveTimestamp();
/**
Calculates the current system timestamp inside the DW1000
return the system timestamp
*/
uint64_t getSystemTimestamp();
/* receive quality information. (RX_FSQUAL) - reg:0x12 */
/**
Gets the receive power of the device (last receive)
returns the last receive power of the device
*/
float getReceivePower();
/**
Gets the power of the first path
returns the first path power
*/
float getFirstPathPower();
/**
Gets the last receive quality
returns last receive quality
*/
float getReceiveQuality();
/**
Sets both tx and rx antenna delay value
@param [in] value the delay in UWB time
*/
void setAntennaDelay(uint16_t value);
#if defined(__AVR__)
/**
Sets both tx and rx antenna delay value, and saves it in the EEPROM for future use
@param [in] value the delay in UWB time
@param [in] the EEPROM offset at which the delay is saved
*/
void setAndSaveAntennaDelay(uint16_t delay, uint8_t eeAddress = 0);
/**
Gets the saved antenna delay value from EEPROM
returns the value of the delay saved in the EEPROM in UWB time
@param [in] the EEPROM offset at which the delay is saved
*/
uint16_t getSavedAntennaDelay(uint8_t eeAddress = 0);
/**
Sets the saved antenna delay value from EEPROM as the configured delay
@param [in] the EEPROM offset at which the delay is saved
*/
uint16_t setAntennaDelayFromEEPROM(uint8_t eeAddress = 0);
#endif
/**
Sets the tx antenna delay value
@param [in] value the delay in UWB time
*/
void setTxAntennaDelay(uint16_t value);
/**
Sets the rx antenna delay value
@param [in] value the delay in UWB time
*/
void setRxAntennaDelay(uint16_t value);
/**
Gets the tx antenna delay value
returns the value of the delay in UWB time
*/
uint16_t getTxAntennaDelay();
/**
Gets the rx antenna delay value
returns the value of the delay in UWB time
*/
uint16_t getRxAntennaDelay();
/**
Sets the function for error event handling
@param [in] handleError the target function
*/
void attachErrorHandler(void (* handleError)(void));
/**
Sets the function for end of transission event handling
@param [in] handleSent the target function
*/
void attachSentHandler(void (* handleSent)(void));
/**
Sets the function for end of receive event handling
@param [in] handleReceived the target function
*/
void attachReceivedHandler(void (* handleReceived)(void));
/**
Sets the function for receive error event handling
@param [in] handleReceiveFailed the target function
*/
void attachReceiveFailedHandler(void (* handleReceiveFailed)(void));
/**
Sets the function for receive timeout event handling
@param [in] handleReceiveTimeout the target function
*/
void attachReceiveTimeoutHandler(void (* handleReceiveTimeout)(void));
/**
Sets the function for receive timestamp availabe event handling
@param [in] handleReceiveTimestampAvailable the target function
*/
void attachReceiveTimestampAvailableHandler(void (* handleReceiveTimestampAvailable)(void));
/**
Handles dw1000 events triggered by interrupt
By default this is attached to the interrupt pin callback
*/
void interruptServiceRoutine();
boolean isTransmitDone();
void clearTransmitStatus();
boolean isReceiveDone();
void clearReceiveStatus();
boolean isReceiveFailed();
void clearReceiveFailedStatus();
boolean isReceiveTimeout();
void clearReceiveTimeoutStatus();
/**
Stops the transceiver immediately, this actually sets the device in Idle mode.
*/
void forceTRxOff();
/**
Sets the interrupt polarity
By default this is set to true by the DW1000
@param [in] val True here means active high
*/
void setInterruptPolarity(boolean val);
/**
Applies the target configuration to the DW1000
@param [in] config the configuration to apply to the DW1000
*/
void applyConfiguration(device_configuration_t config);
/**
Enables the interrupts for the target events
@param [in] interrupt_config the interrupt map to use
*/
void applyInterruptConfiguration(interrupt_configuration_t interrupt_config);
/**
Gets the current channel in use
returns the current channel
*/
Channel getChannel();
/**
Gets the current PRF of the device
returns the current PRF
*/
PulseFrequency getPulseFrequency();
/**
Sets the timeout for Raceive Frame.
@param[in] Pac size based on current preamble lenght - 1
*/
void setPreambleDetectionTimeout(uint16_t pacSize);
/**
Sets the timeout for SFD detection.
The recommended value is: PreambleLenght + SFD + 1.
The default value is 4096+64+1
@param[in] the sfd detection timeout
*/
void setSfdDetectionTimeout(uint16_t preambleSymbols);
/**
Sets the timeout for Raceive Frame. Must be sets in idle mode.
Allow the external microprocessor to enter a low power state awaiting a valid receive frame.
@param[in] time in μs. units = ~1μs(1.026μs). 0 to disable
*/
void setReceiveFrameWaitTimeoutPeriod(uint16_t timeMicroSeconds);
/**
Sets the device in receive mode
@param [in] mode IMMEDIATE or DELAYED receive
*/
void startReceive(ReceiveMode mode = ReceiveMode::IMMEDIATE);
/**
Sets the device in transmission mode
@param [in] mode IMMEDIATE or DELAYED transmission
*/
void startTransmit(TransmitMode mode = TransmitMode::IMMEDIATE);
/**
Gets the temperature inside the DW1000 Device
returns The temperature
*/
float getTemperature();
/**
Gets the voltage in input of the DW1000
returns The input voltage
*/
float getBatteryVoltage();
/**
Gets both temperature and voltage with a single read
@param [out] temp the temperature
@param [out] vbat the input voltage
*/
void getTemperatureAndBatteryVoltage(float& temp, float& vbat);
/**
Enables the frame filtering functionality using the provided configuration.
Messages must be formatted using 802.15.4-2011 format.
@param [in] config frame filtering configuration
*/
void enableFrameFiltering(frame_filtering_configuration_t config);
/**
Disables the frame filtering functionality
*/
void disableFrameFiltering();
/**
WARNING: this just sets the relative bits inside the register.
You must refer to the DW1000 User manual to activate it properly.
*/
void setDoubleBuffering(boolean val);
/**
Enables frames up to 1023 byte length
@param [in] val true or false
*/
void useExtendedFrameLength(boolean val);
/**
Sets the time before the device enters receive after a transmission.
Use 0 here to deactivate it.
@param[in] time in μs. units = ~1μs(1.026μs)
*/
void setWait4Response(uint32_t timeMicroSeconds);
#if DW1000NG_PRINTABLE
/* ##### Print device id, address, etc. ###################################### */
/**
Generates a String representation of the device identifier of the chip. That usually
are the letters "DECA" plus the version and revision numbers of the chip.
@param[out] msgBuffer The String buffer to be filled with printable device information.
Provide 128 bytes, this should be sufficient.
*/
void getPrintableDeviceIdentifier(char msgBuffer[]);
/**
Generates a String representation of the extended unique identifier (EUI) of the chip.
@param[out] msgBuffer The String buffer to be filled with printable device information.
Provide 128 bytes, this should be sufficient.
*/
void getPrintableExtendedUniqueIdentifier(char msgBuffer[]);
/**
Generates a String representation of the short address and network identifier currently
defined for the respective chip.
@param[out] msgBuffer The String buffer to be filled with printable device information.
Provide 128 bytes, this should be sufficient.
*/
void getPrintableNetworkIdAndShortAddress(char msgBuffer[]);
/**
Generates a String representation of the main operational settings of the chip. This
includes data rate, pulse repetition frequency, preamble and channel settings.
@param[out] msgBuffer The String buffer to be filled with printable device information.
Provide 128 bytes, this should be sufficient.
*/
void getPrintableDeviceMode(char msgBuffer[]);
#endif
#if DW1000NG_DEBUG
void getPrettyBytes(byte data[], char msgBuffer[], uint16_t n);
void getPrettyBytes(byte cmd, uint16_t offset, char msgBuffer[], uint16_t n);
#endif
};

70
src/dwm1000/DW1000NgCompileOptions.hpp Normal file
View File

@@ -0,0 +1,70 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
/**
* Copyright (c) 2016 by Ludwig Grill (www.rotzbua.de)
* Decawave DW1000 library for arduino.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* @file DW1000CompileOptions.h
* Here are some options to optimize code and save some ram and rom
*
*/
#pragma once
/**
* Adds printing functions to base driver
*
*/
#define DW1000NG_PRINTABLE true
/**
* Adds debug functionalities
*
*/
#define DW1000NG_DEBUG false
/**
* Optimizes code for the DWM1000 Module
*/
#define DWM1000_OPTIMIZED false
/**
* Printable DW1000NgDeviceConfiguration about: rom:2494 byte ; ram 256 byte
* This option is needed because compiler can not optimize unused codes from inheritanced methods
* Some examples or debug code use this
* Set false if you do not need it and have to save some space
*/
#define DW1000NGCONFIGURATION_H_PRINTABLE false

73
src/dwm1000/DW1000NgConfiguration.hpp Normal file
View File

@@ -0,0 +1,73 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <Arduino.h>
#include "DW1000NgConstants.hpp"
typedef struct device_configuration_t {
boolean extendedFrameLength;
boolean receiverAutoReenable;
boolean smartPower;
boolean frameCheck;
boolean nlos;
SFDMode sfd;
Channel channel;
DataRate dataRate;
PulseFrequency pulseFreq;
PreambleLength preambleLen;
PreambleCode preaCode;
} device_configuration_t;
typedef struct interrupt_configuration_t {
boolean interruptOnSent;
boolean interruptOnReceived;
boolean interruptOnReceiveFailed;
boolean interruptOnReceiveTimeout;
boolean interruptOnReceiveTimestampAvailable;
boolean interruptOnAutomaticAcknowledgeTrigger;
} interrupt_configuration_t;
typedef struct frame_filtering_configuration_t {
boolean behaveAsCoordinator;
boolean allowBeacon;
boolean allowData;
boolean allowAcknowledgement;
boolean allowMacCommand;
boolean allowAllReserved;
boolean allowReservedFour;
boolean allowReservedFive;
} frame_filtering_configuration_t;
typedef struct sleep_configuration_t {
boolean onWakeUpRunADC;
boolean onWakeUpReceive;
boolean onWakeUpLoadEUI;
boolean onWakeUpLoadL64Param;
boolean preserveSleep;
boolean enableSLP;
boolean enableWakePIN;
boolean enableWakeSPI;
} sleep_configuration_t;

235
src/dwm1000/DW1000NgConstants.hpp Normal file
View File

@@ -0,0 +1,235 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <Arduino.h>
#define GPIO_MODE 0
#define LED_MODE 1
constexpr float TIME_RES = 0.000015650040064103f;
constexpr float TIME_RES_INV = 63897.6f;
/* Speed of radio waves (light) [m/s] * timestamp resolution [~15.65ps] of DW1000Ng */
constexpr float DISTANCE_OF_RADIO = 0.0046917639786159f;
constexpr float DISTANCE_OF_RADIO_INV = 213.139451293f;
// timestamp byte length - 40 bit -> 5 byte
constexpr uint8_t LENGTH_TIMESTAMP = 5;
// timer/counter overflow (40 bits) -> 4overflow approx. every 17.2 seconds
constexpr int64_t TIME_OVERFLOW = 0x10000000000; //1099511627776LL
constexpr int64_t TIME_MAX = 0xffffffffff;
// time factors (relative to [us]) for setting delayed transceive
// TODO use non float
constexpr float SECONDS = 1e6;
constexpr float MILLISECONDS = 1e3;
constexpr float MICROSECONDS = 1;
constexpr float NANOSECONDS = 1e-3;
/* preamble codes (CHAN_CTRL - RX & TX _CODE) - reg:0x1F, bits:31-27,26-22 */
enum class PreambleCode : byte {
CODE_1 = 1,
CODE_2,
CODE_3,
CODE_4,
CODE_5,
CODE_6,
CODE_7,
CODE_8,
CODE_9,
CODE_10,
CODE_11,
CODE_12,
CODE_17 = 17,
CODE_18,
CODE_19,
CODE_20
};
/* Validity matrix for 16 MHz PRF preamble codes */
constexpr byte preamble_validity_matrix_PRF16[8][2] = {
{0,0}, /* Channel 0 doesn't exist */
{1, 2},
{3, 4},
{5, 6},
{7, 8},
{3, 4},
{0,0}, /* Channel 5 doesn't exist */
{7, 8}
};
/* Validity matrix for 64 MHz PRF preamble codes */
constexpr byte preamble_validity_matrix_PRF64[8][4] = {
{0,0,0,0}, /* Channel 0 doesn't exist */
{9, 10, 11, 12},
{9, 10, 11, 12},
{9, 10, 11, 12},
{17, 18, 19, 20},
{9, 10, 11, 12},
{0,0,0,0}, /* Channel 5 doesn't exist */
{17, 18, 19, 20}
};
/* transmission/reception bit rate (TXBR) - reg:0x08, bits:14,13 */
enum class DataRate : byte {
RATE_110KBPS,
RATE_850KBPS,
RATE_6800KBPS
};
/* transmission pulse frequency (TXPRF) - reg:0x08, bits:17,16
* 0x00 is 4MHZ, but receiver in DW1000Ng does not support it (!??) */
enum class PulseFrequency : byte {
FREQ_16MHZ = 0x01,
FREQ_64MHZ
};
/* preamble length (PE + TXPSR) - reg:0x08, bits:21,20,19,18 - table 16 */
enum class PreambleLength : byte {
LEN_64 = 0x01,
LEN_128 = 0x05,
LEN_256 = 0x09,
LEN_512 = 0x0D,
LEN_1024 = 0x02,
LEN_1536 = 0x06,
LEN_2048 = 0x0A,
LEN_4096 = 0x03
};
/* PAC size (DRX_TUNE2) - reg:0x08, sub-reg:0x27, bits:26,25 - table 33
* The value to program the sub-register changes in based of RXPRF */
enum class PacSize : byte {
SIZE_8 = 8,
SIZE_16 = 16,
SIZE_32 = 32,
SIZE_64 = 64
};
/* channel of operation (CHAN_CTRL - TX & RX _CHAN) - reg:0x1F, bits:3-0,7-4 */
enum class Channel : byte {
CHANNEL_1 = 1,
CHANNEL_2,
CHANNEL_3,
CHANNEL_4,
CHANNEL_5,
CHANNEL_7 = 7
};
/* Register is 6 bit, 7 = write, 6 = sub-adressing, 5-0 = register value
* Total header with sub-adressing can be 15 bit. */
constexpr byte WRITE = 0x80; // regular write
constexpr byte WRITE_SUB = 0xC0; // write with sub address
constexpr byte READ = 0x00; // regular read
constexpr byte READ_SUB = 0x40; // read with sub address
constexpr byte RW_SUB_EXT = 0x80; // R/W with sub address extension
/* clocks available. */
constexpr byte SYS_AUTO_CLOCK = 0x00;
constexpr byte SYS_XTI_CLOCK = 0x01;
constexpr byte SYS_PLL_CLOCK = 0x02;
constexpr byte TX_PLL_CLOCK = 0x20;
constexpr byte LDE_CLOCK = 0x03;
/* range bias tables - APS011*/
constexpr double BIAS_TABLE[18][5] = {
{61, -198, -110, -275, -295},
{63, -187, -105, -244, -266},
{65, -179, -100, -210, -235},
{67, -163, -93, -176, -199},
{69, -143, -82, -138, -150},
{71, -127, -69, -95, -100},
{73, -109, -51, -51, -58},
{75, -84, -27, 0, 0},
{77, -59, 0, 42, 49},
{79, -31, 21, 97, 91},
{81, 0, 35, 158, 127},
{83, 36, 42, 210, 153},
{85, 65, 49, 254, 175},
{87, 84, 62, 294, 197},
{89, 97, 71, 321, 233},
{91, 106, 76, 339, 245},
{93, 110, 81, 356, 264},
{95, 112, 86, 394, 284}
};
enum class DriverAmplifierValue : byte {
dB_18,
dB_15,
dB_12,
dB_9,
dB_6,
dB_3,
dB_0,
OFF
};
enum class TransmitMixerValue : byte {
dB_0,
dB_0_5,
dB_1,
dB_1_5,
dB_2,
dB_2_5,
dB_3,
dB_3_5,
dB_4,
dB_4_5,
dB_5,
dB_5_5,
dB_6,
dB_6_5,
dB_7,
dB_7_5,
dB_8,
dB_8_5,
dB_9,
dB_9_5,
dB_10,
dB_10_5,
dB_11,
dB_11_5,
dB_12,
dB_12_5,
dB_13,
dB_13_5,
dB_14,
dB_14_5,
dB_15,
dB_15_5
};
enum class SFDMode {STANDARD_SFD, DECAWAVE_SFD};
enum class TransmitMode {IMMEDIATE, DELAYED};
enum class ReceiveMode {IMMEDIATE, DELAYED};
enum class SPIClock {SLOW, FAST};

349
src/dwm1000/DW1000NgRTLS.cpp Normal file
View File

@@ -0,0 +1,349 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <Arduino.h>
#include "DW1000NgRTLS.hpp"
#include "DW1000Ng.hpp"
#include "DW1000NgUtils.hpp"
#include "DW1000NgTime.hpp"
#include "DW1000NgRanging.hpp"
static byte SEQ_NUMBER = 0;
namespace DW1000NgRTLS {
byte increaseSequenceNumber(){
return ++SEQ_NUMBER;
}
void transmitTwrShortBlink() {
byte Blink[] = {BLINK, SEQ_NUMBER++, 0,0,0,0,0,0,0,0, NO_BATTERY_STATUS | NO_EX_ID, TAG_LISTENING_NOW};
DW1000Ng::getEUI(&Blink[2]);
DW1000Ng::setTransmitData(Blink, sizeof(Blink));
DW1000Ng::startTransmit();
}
void transmitRangingInitiation(byte tag_eui[], byte tag_short_address[]) {
byte RangingInitiation[] = {DATA, SHORT_SRC_LONG_DEST, SEQ_NUMBER++, 0,0, 0,0,0,0,0,0,0,0, 0,0, RANGING_INITIATION, 0,0};
DW1000Ng::getNetworkId(&RangingInitiation[3]);
memcpy(&RangingInitiation[5], tag_eui, 8);
DW1000Ng::getDeviceAddress(&RangingInitiation[13]);
memcpy(&RangingInitiation[16], tag_short_address, 2);
DW1000Ng::setTransmitData(RangingInitiation, sizeof(RangingInitiation));
DW1000Ng::startTransmit();
}
void transmitPoll(byte anchor_address[]){
byte Poll[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0 , RANGING_TAG_POLL};
DW1000Ng::getNetworkId(&Poll[3]);
memcpy(&Poll[5], anchor_address, 2);
DW1000Ng::getDeviceAddress(&Poll[7]);
DW1000Ng::setTransmitData(Poll, sizeof(Poll));
DW1000Ng::startTransmit();
}
void transmitResponseToPoll(byte tag_short_address[]) {
byte pollAck[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, RANGING_CONTINUE, 0, 0};
DW1000Ng::getNetworkId(&pollAck[3]);
memcpy(&pollAck[5], tag_short_address, 2);
DW1000Ng::getDeviceAddress(&pollAck[7]);
DW1000Ng::setTransmitData(pollAck, sizeof(pollAck));
DW1000Ng::startTransmit();
}
void transmitFinalMessage(byte anchor_address[], uint16_t reply_delay, uint64_t timePollSent, uint64_t timeResponseToPollReceived) {
/* Calculation of future time */
byte futureTimeBytes[LENGTH_TIMESTAMP];
uint64_t timeFinalMessageSent = DW1000Ng::getSystemTimestamp();
timeFinalMessageSent += DW1000NgTime::microsecondsToUWBTime(reply_delay);
DW1000NgUtils::writeValueToBytes(futureTimeBytes, timeFinalMessageSent, LENGTH_TIMESTAMP);
DW1000Ng::setDelayedTRX(futureTimeBytes);
timeFinalMessageSent += DW1000Ng::getTxAntennaDelay();
byte finalMessage[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, RANGING_TAG_FINAL_RESPONSE_EMBEDDED,
0,0,0,0,0,0,0,0,0,0,0,0
};
DW1000Ng::getNetworkId(&finalMessage[3]);
memcpy(&finalMessage[5], anchor_address, 2);
DW1000Ng::getDeviceAddress(&finalMessage[7]);
DW1000NgUtils::writeValueToBytes(finalMessage + 10, (uint32_t) timePollSent, 4);
DW1000NgUtils::writeValueToBytes(finalMessage + 14, (uint32_t) timeResponseToPollReceived, 4);
DW1000NgUtils::writeValueToBytes(finalMessage + 18, (uint32_t) timeFinalMessageSent, 4);
DW1000Ng::setTransmitData(finalMessage, sizeof(finalMessage));
DW1000Ng::startTransmit(TransmitMode::DELAYED);
}
void transmitRangingConfirm(byte tag_short_address[], byte next_anchor[]) {
byte rangingConfirm[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, RANGING_CONFIRM, next_anchor[0], next_anchor[1]};
DW1000Ng::getNetworkId(&rangingConfirm[3]);
memcpy(&rangingConfirm[5], tag_short_address, 2);
DW1000Ng::getDeviceAddress(&rangingConfirm[7]);
DW1000Ng::setTransmitData(rangingConfirm, sizeof(rangingConfirm));
DW1000Ng::startTransmit();
}
void transmitActivityFinished(byte tag_short_address[], byte blink_rate[]) {
/* I send the new blink rate to the tag */
byte rangingConfirm[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, ACTIVITY_FINISHED, blink_rate[0], blink_rate[1]};
DW1000Ng::getNetworkId(&rangingConfirm[3]);
memcpy(&rangingConfirm[5], tag_short_address, 2);
DW1000Ng::getDeviceAddress(&rangingConfirm[7]);
DW1000Ng::setTransmitData(rangingConfirm, sizeof(rangingConfirm));
DW1000Ng::startTransmit();
}
static uint32_t calculateNewBlinkRate(byte frame[]) {
uint32_t blinkRate = frame[11] + static_cast<uint32_t>(((frame[12] & 0x3F) << 8));
byte multiplier = ((frame[12] & 0xC0) >> 6);
if(multiplier == 0x01) {
blinkRate *= 25;
} else if(multiplier == 0x02) {
blinkRate *= 1000;
}
return blinkRate;
}
void waitForTransmission() {
while(!DW1000Ng::isTransmitDone()) {
#if defined(ESP8266)
yield();
#endif
}
DW1000Ng::clearTransmitStatus();
}
boolean receiveFrame() {
DW1000Ng::startReceive();
while(!DW1000Ng::isReceiveDone()) {
if(DW1000Ng::isReceiveTimeout() ) {
DW1000Ng::clearReceiveTimeoutStatus();
return false;
}
#if defined(ESP8266)
yield();
#endif
}
DW1000Ng::clearReceiveStatus();
return true;
}
static boolean waitForNextRangingStep() {
DW1000NgRTLS::waitForTransmission();
if(!DW1000NgRTLS::receiveFrame()) return false;
return true;
}
RangeRequestResult tagRangeRequest() {
DW1000NgRTLS::transmitTwrShortBlink();
if(!DW1000NgRTLS::waitForNextRangingStep()) return {false, 0};
size_t init_len = DW1000Ng::getReceivedDataLength();
byte init_recv[init_len];
DW1000Ng::getReceivedData(init_recv, init_len);
if(!(init_len > 17 && init_recv[15] == RANGING_INITIATION)) {
return { false, 0};
}
DW1000Ng::setDeviceAddress(DW1000NgUtils::bytesAsValue(&init_recv[16], 2));
return { true, static_cast<uint16_t>(DW1000NgUtils::bytesAsValue(&init_recv[13], 2)) };
}
static RangeResult tagFinishRange(uint16_t anchor, uint16_t replyDelayUs) {
RangeResult returnValue;
byte target_anchor[2];
DW1000NgUtils::writeValueToBytes(target_anchor, anchor, 2);
DW1000NgRTLS::transmitPoll(target_anchor);
/* Start of poll control for range */
if(!DW1000NgRTLS::waitForNextRangingStep()) {
returnValue = {false, false, 0, 0};
} else {
size_t cont_len = DW1000Ng::getReceivedDataLength();
byte cont_recv[cont_len];
DW1000Ng::getReceivedData(cont_recv, cont_len);
if (cont_len > 10 && cont_recv[9] == ACTIVITY_CONTROL && cont_recv[10] == RANGING_CONTINUE) {
/* Received Response to poll */
DW1000NgRTLS::transmitFinalMessage(
&cont_recv[7],
replyDelayUs,
DW1000Ng::getTransmitTimestamp(), // Poll transmit time
DW1000Ng::getReceiveTimestamp() // Response to poll receive time
);
if(!DW1000NgRTLS::waitForNextRangingStep()) {
returnValue = {false, false, 0, 0};
} else {
size_t act_len = DW1000Ng::getReceivedDataLength();
byte act_recv[act_len];
DW1000Ng::getReceivedData(act_recv, act_len);
if(act_len > 10 && act_recv[9] == ACTIVITY_CONTROL) {
if (act_len > 12 && act_recv[10] == RANGING_CONFIRM) {
returnValue = {true, true, static_cast<uint16_t>(DW1000NgUtils::bytesAsValue(&act_recv[11], 2)), 0};
} else if(act_len > 12 && act_recv[10] == ACTIVITY_FINISHED) {
returnValue = {true, false, 0, calculateNewBlinkRate(act_recv)};
}
} else {
returnValue = {false, false, 0, 0};
}
}
} else {
returnValue = {false, false, 0, 0};
}
}
return returnValue;
}
RangeInfrastructureResult tagRangeInfrastructure(uint16_t target_anchor, uint16_t finalMessageDelay) {
RangeInfrastructureResult returnValue;
RangeResult result = tagFinishRange(target_anchor, finalMessageDelay);
byte keep_going = 1;
if(!result.success) {
keep_going = 0;
returnValue = {false , 0};
} else {
while(result.success && result.next) {
result = tagFinishRange(result.next_anchor, finalMessageDelay);
if(!result.success) {
keep_going = 0;
returnValue = {false , 0};
break;
}
#if defined(ESP8266)
if (keep_going == 1) {
yield();
}
#endif
}
if (keep_going == 1) {
if(result.success && result.new_blink_rate != 0) {
keep_going = 0;
returnValue = { true, static_cast<uint16_t>(result.new_blink_rate) };
} else {
if(!result.success) {
keep_going = 0;
returnValue = { false , 0 };
} else {
// TODO. Handle this condition?
}
}
}
}
return returnValue;
}
RangeInfrastructureResult tagTwrLocalize(uint16_t finalMessageDelay) {
RangeRequestResult request_result = DW1000NgRTLS::tagRangeRequest();
if(request_result.success) {
RangeInfrastructureResult result = DW1000NgRTLS::tagRangeInfrastructure(request_result.target_anchor, finalMessageDelay);
if(result.success)
return result;
}
return {false, 0};
}
RangeAcceptResult anchorRangeAccept(NextActivity next, uint16_t value) {
RangeAcceptResult returnValue;
double range;
if(!DW1000NgRTLS::receiveFrame()) {
returnValue = {false, 0};
} else {
size_t poll_len = DW1000Ng::getReceivedDataLength();
byte poll_data[poll_len];
DW1000Ng::getReceivedData(poll_data, poll_len);
if(poll_len > 9 && poll_data[9] == RANGING_TAG_POLL) {
uint64_t timePollReceived = DW1000Ng::getReceiveTimestamp();
DW1000NgRTLS::transmitResponseToPoll(&poll_data[7]);
DW1000NgRTLS::waitForTransmission();
uint64_t timeResponseToPoll = DW1000Ng::getTransmitTimestamp();
delayMicroseconds(1500);
if(!DW1000NgRTLS::receiveFrame()) {
returnValue = {false, 0};
} else {
size_t rfinal_len = DW1000Ng::getReceivedDataLength();
byte rfinal_data[rfinal_len];
DW1000Ng::getReceivedData(rfinal_data, rfinal_len);
if(rfinal_len > 18 && rfinal_data[9] == RANGING_TAG_FINAL_RESPONSE_EMBEDDED) {
uint64_t timeFinalMessageReceive = DW1000Ng::getReceiveTimestamp();
byte finishValue[2];
DW1000NgUtils::writeValueToBytes(finishValue, value, 2);
if(next == NextActivity::RANGING_CONFIRM) {
DW1000NgRTLS::transmitRangingConfirm(&rfinal_data[7], finishValue);
} else {
DW1000NgRTLS::transmitActivityFinished(&rfinal_data[7], finishValue);
}
DW1000NgRTLS::waitForTransmission();
range = DW1000NgRanging::computeRangeAsymmetric(
DW1000NgUtils::bytesAsValue(rfinal_data + 10, LENGTH_TIMESTAMP), // Poll send time
timePollReceived,
timeResponseToPoll, // Response to poll sent time
DW1000NgUtils::bytesAsValue(rfinal_data + 14, LENGTH_TIMESTAMP), // Response to Poll Received
DW1000NgUtils::bytesAsValue(rfinal_data + 18, LENGTH_TIMESTAMP), // Final Message send time
timeFinalMessageReceive // Final message receive time
);
range = DW1000NgRanging::correctRange(range);
/* In case of wrong read due to bad device calibration */
if(range <= 0)
range = 0.000001;
returnValue = {true, range};
}
}
}
}
return returnValue;
}
}

133
src/dwm1000/DW1000NgRTLS.hpp Normal file
View File

@@ -0,0 +1,133 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <Arduino.h>
/* Frame control */
constexpr byte BLINK = 0xC5;
constexpr byte DATA = 0x41;
constexpr byte SHORT_SRC_AND_DEST = 0x88;
constexpr byte LONG_SRC_AND_DEST = 0xCC;
constexpr byte SHORT_SRC_LONG_DEST = 0x8C;
constexpr byte LONG_SRC_SHORT_DEST = 0xC8;
/* Application ID */
constexpr byte RTLS_APP_ID_LOW = 0x9A;
constexpr byte RTLS_APP_ID_HIGH = 0x60;
constexpr uint16_t RTLS_APP_ID = RTLS_APP_ID_LOW | ((uint16_t) (RTLS_APP_ID_HIGH << 8));
/* Function code */
constexpr byte ACTIVITY_CONTROL = 0x10;
constexpr byte RANGING_INITIATION = 0x20;
constexpr byte RANGING_TAG_POLL = 0x21;
constexpr byte RANGING_TAG_FINAL_RESPONSE_EMBEDDED = 0x23;
constexpr byte RANGING_TAG_FINAL_RESPONSE_NO_EMBEDDED = 0x25;
constexpr byte RANGING_TAG_FINAL_SEND_TIME = 0x27;
/* Activity code */
constexpr byte ACTIVITY_FINISHED = 0x00;
constexpr byte RANGING_CONFIRM = 0x01;
constexpr byte RANGING_CONTINUE = 0x02;
/* BLINK Encoding Header */
constexpr byte BATTERY_GOOD = 0x00;
constexpr byte BATTERY_10_30_PERCENT = 0x02;
constexpr byte BATTERY_0_10_PERCENT = 0x01;
constexpr byte NO_BATTERY_STATUS = 0x03;
constexpr byte TEMPERATURE_DATA = 0x20;
constexpr byte EX_ID = 0x80;
constexpr byte NO_EX_ID = 0x40;
/* BLINK Ext Header */
constexpr byte BLINK_RATE_AND_LISTENING = 0x01;
constexpr byte TAG_LISTENING_NOW = 0x02;
enum class NextActivity {
ACTIVITY_FINISHED,
RANGING_CONFIRM
};
typedef struct RangeRequestResult {
boolean success;
uint16_t target_anchor;
} RangeRequestResult;
typedef struct RangeResult {
boolean success;
boolean next;
uint16_t next_anchor;
uint32_t new_blink_rate;
} RangeResult;
typedef struct RangeInfrastructureResult {
boolean success;
uint16_t new_blink_rate;
} RangeInfrastructureResult;
typedef struct RangeAcceptResult {
boolean success;
double range;
} RangeAcceptResult;
namespace DW1000NgRTLS {
/*** TWR functions used in ISO/IEC 24730-62:2013, refer to the standard or the decawave manual for details about TWR ***/
byte increaseSequenceNumber();
void transmitTwrShortBlink();
void transmitRangingInitiation(byte tag_eui[], byte tag_short_address[]);
void transmitPoll(byte anchor_address[]);
void transmitResponseToPoll(byte tag_short_address[]);
void transmitFinalMessage(byte anchor_address[], uint16_t reply_delay, uint64_t timePollSent, uint64_t timeResponseToPollReceived);
void transmitRangingConfirm(byte tag_short_address[], byte next_anchor[]);
void transmitActivityFinished(byte tag_short_address[], byte blink_rate[]);
boolean receiveFrame();
void waitForTransmission();
/*** End of TWR functions ***/
/* Send a request range from tag to the rtls infrastructure */
RangeRequestResult tagRangeRequest();
/* Used by an anchor to accept an incoming tagRangeRequest by means of the infrastructure
NextActivity is used to indicate the tag what to do next after the ranging process (Activity finished is to return to blink (range request),
Continue range is to tell the tag to range a new anchor)
value is the value relative to the next activity (Activity finished = new blink rante, continue range = new anchor address)
*/
RangeAcceptResult anchorRangeAccept(NextActivity next, uint16_t value);
/* Used by tag to range after range request accept of the infrastructure
Target anchor is given after a range request success
Finalmessagedelay is used in the process of TWR, a value of 1500 works on 8mhz-80mhz range devices,
you could try to decrease it to improve system performance.
*/
RangeInfrastructureResult tagRangeInfrastructure(uint16_t target_anchor, uint16_t finalMessageDelay);
/* Can be used as a single function start the localization process from the tag.
Finalmessagedelay is the same as in function tagRangeInfrastructure
*/
RangeInfrastructureResult tagTwrLocalize(uint16_t finalMessageDelay);
}

88
src/dwm1000/DW1000NgRanging.cpp Normal file
View File

@@ -0,0 +1,88 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <Arduino.h>
#include "DW1000Ng.hpp"
#include "DW1000NgConstants.hpp"
#include "DW1000NgRanging.hpp"
#include "DW1000NgConstants.hpp"
#include "DW1000NgRTLS.hpp"
namespace DW1000NgRanging {
/* asymmetric two-way ranging (more computation intense, less error prone) */
double computeRangeAsymmetric(
uint64_t timePollSent,
uint64_t timePollReceived,
uint64_t timePollAckSent,
uint64_t timePollAckReceived,
uint64_t timeRangeSent,
uint64_t timeRangeReceived
)
{
uint32_t timePollSent_32 = static_cast<uint32_t>(timePollSent);
uint32_t timePollReceived_32 = static_cast<uint32_t>(timePollReceived);
uint32_t timePollAckSent_32 = static_cast<uint32_t>(timePollAckSent);
uint32_t timePollAckReceived_32 = static_cast<uint32_t>(timePollAckReceived);
uint32_t timeRangeSent_32 = static_cast<uint32_t>(timeRangeSent);
uint32_t timeRangeReceived_32 = static_cast<uint32_t>(timeRangeReceived);
double round1 = static_cast<double>(timePollAckReceived_32 - timePollSent_32);
double reply1 = static_cast<double>(timePollAckSent_32 - timePollReceived_32);
double round2 = static_cast<double>(timeRangeReceived_32 - timePollAckSent_32);
double reply2 = static_cast<double>(timeRangeSent_32 - timePollAckReceived_32);
int64_t tof_uwb = static_cast<int64_t>((round1 * round2 - reply1 * reply2) / (round1 + round2 + reply1 + reply2));
double distance = tof_uwb * DISTANCE_OF_RADIO;
return distance;
}
double correctRange(double range) {
double result = 0;
Channel currentChannel = DW1000Ng::getChannel();
double rxPower = -(static_cast<double>(DW1000Ng::getReceivePower()));
size_t index = DW1000Ng::getPulseFrequency() == PulseFrequency::FREQ_16MHZ ? 1 : 2;
if(currentChannel == Channel::CHANNEL_4 || currentChannel == Channel::CHANNEL_7)
index+=2;
if (rxPower < BIAS_TABLE[0][0]) {
result = range += BIAS_TABLE[0][index]*0.001;
} else if (rxPower >= BIAS_TABLE[17][0]) {
result = range += BIAS_TABLE[17][index]*0.001;
} else {
for(auto i=0; i < 17; i++) {
if (rxPower >= BIAS_TABLE[i][0] && rxPower < BIAS_TABLE[i+1][0]){
result = range += BIAS_TABLE[i][index]*0.001;
break;
}
}
}
return result;
}
}

59
src/dwm1000/DW1000NgRanging.hpp Normal file
View File

@@ -0,0 +1,59 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <Arduino.h>
namespace DW1000NgRanging {
/**
Asymmetric two-way ranging algorithm (more computation intense, less error prone)
@param [in] timePollSent timestamp of poll transmission
@param [in] timePollReceived timestamp of poll receive
@param [in] timePollAckSent timestamp of response to poll transmission
@param [in] timePollAckReceived timestamp of response to poll receive
@param [in] timeRangeSent timestamp of final message transmission
@param [in] timeRangeReceived timestamp of final message receive
returns the range in meters
*/
double computeRangeAsymmetric(
uint64_t timePollSent,
uint64_t timePollReceived,
uint64_t timePollAckSent,
uint64_t timePollAckReceived,
uint64_t timeRangeSent,
uint64_t timeRangeReceived
);
//TODO Symmetric
/**
Removes bias from the target range
returns the unbiased range
*/
double correctRange(double range);
}

361
src/dwm1000/DW1000NgRegisters.hpp Normal file
View File

@@ -0,0 +1,361 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <Arduino.h>
// no sub-address for register write
constexpr uint16_t NO_SUB = 0xFF;
// device id register
constexpr uint16_t DEV_ID = 0x00;
constexpr uint16_t LEN_DEV_ID = 4;
// extended unique identifier register
constexpr uint16_t EUI = 0x01;
constexpr uint16_t LEN_EUI = 8;
// PAN identifier, short address register
constexpr uint16_t PANADR = 0x03;
constexpr uint16_t LEN_PANADR = 4;
// device configuration register
constexpr uint16_t SYS_CFG = 0x04;
constexpr uint16_t FFEN_BIT = 0;
constexpr uint16_t FFBC_BIT = 1;
constexpr uint16_t FFAB_BIT = 2;
constexpr uint16_t FFAD_BIT = 3;
constexpr uint16_t FFAA_BIT = 4;
constexpr uint16_t FFAM_BIT = 5;
constexpr uint16_t FFAR_BIT = 6;
constexpr uint16_t FFA4_BIT = 7;
constexpr uint16_t FFA5_BIT = 8;
constexpr uint16_t HIRQ_POL_BIT = 9;
constexpr uint16_t SPI_EDGE_BIT = 10;
constexpr uint16_t DIS_FCE_BIT = 11;
constexpr uint16_t DIS_DRXB_BIT = 12;
constexpr uint16_t DIS_PHE_BIT = 13;
constexpr uint16_t DIS_RSDE_BIT = 14;
constexpr uint16_t FCS_INIT2F_BIT = 15;
constexpr uint16_t PHR_MODE_0_BIT = 16;
constexpr uint16_t PHR_MODE_1_BIT = 17;
constexpr uint16_t DIS_STXP_BIT = 18;
constexpr uint16_t RXM110K_BIT = 22;
constexpr uint16_t RXWTOE_BIT = 28;
constexpr uint16_t RXAUTR_BIT = 29;
constexpr uint16_t AUTOACK_BIT = 30;
constexpr uint16_t AACKPEND_BIT = 31;
constexpr uint16_t LEN_SYS_CFG = 4;
// device control register
constexpr uint16_t SYS_CTRL = 0x0D;
constexpr uint16_t LEN_SYS_CTRL = 4;
constexpr uint16_t SFCST_BIT = 0;
constexpr uint16_t TXSTRT_BIT = 1;
constexpr uint16_t TXDLYS_BIT = 2;
constexpr uint16_t TRXOFF_BIT = 6;
constexpr uint16_t WAIT4RESP_BIT = 7;
constexpr uint16_t RXENAB_BIT = 8;
constexpr uint16_t RXDLYS_BIT = 9;
// system event status register
constexpr uint16_t SYS_STATUS = 0x0F;
constexpr uint16_t SYS_STATUS_SUB = 0x04;
constexpr uint16_t IRQS_BIT = 0;
constexpr uint16_t CPLOCK_BIT = 1;
constexpr uint16_t ESYNCR_BIT = 2;
constexpr uint16_t AAT_BIT = 3;
constexpr uint16_t TXFRB_BIT = 4;
constexpr uint16_t TXPRS_BIT = 5;
constexpr uint16_t TXPHS_BIT = 6;
constexpr uint16_t TXFRS_BIT = 7;
constexpr uint16_t RXPRD_BIT = 8;
constexpr uint16_t RXSFDD_BIT = 9;
constexpr uint16_t LDEDONE_BIT = 10;
constexpr uint16_t RXPHD_BIT = 11;
constexpr uint16_t RXPHE_BIT = 12;
constexpr uint16_t RXDFR_BIT = 13;
constexpr uint16_t RXFCG_BIT = 14;
constexpr uint16_t RXFCE_BIT = 15;
constexpr uint16_t RXRFSL_BIT = 16;
constexpr uint16_t RXRFTO_BIT = 17;
constexpr uint16_t LDEERR_BIT = 18;
constexpr uint16_t RXOVRR_BIT = 20;
constexpr uint16_t RXPTO_BIT = 21;
constexpr uint16_t GPIOIRQ_BIT = 22;
constexpr uint16_t SLP2INIT_BIT = 23;
constexpr uint16_t RFPLL_LL_BIT = 24;
constexpr uint16_t CLKPLL_LL_BIT = 25;
constexpr uint16_t RXSFDTO_BIT = 26;
constexpr uint16_t HPDWARN_BIT = 27;
constexpr uint16_t TXBERR_BIT = 28;
constexpr uint16_t AFFREJ_BIT = 29;
constexpr uint16_t HSRBP_BIT = 30;
constexpr uint16_t ICRBP_BIT = 31;
constexpr uint16_t RXRSCS_BIT = 0;
constexpr uint16_t RXPREJ_BIT = 1;
constexpr uint16_t TXPUTE_BIT = 2;
constexpr uint16_t LEN_SYS_STATUS = 4;
constexpr uint16_t LEN_SYS_STATUS_SUB = 1;
// system event mask register
// NOTE: uses the bit definitions of SYS_STATUS (below 32)
constexpr uint16_t SYS_MASK = 0x0E;
constexpr uint16_t LEN_SYS_MASK = 4;
// system time counter
constexpr uint16_t SYS_TIME = 0x06;
constexpr uint16_t LEN_SYS_TIME = 5;
// RX timestamp register
constexpr uint16_t RX_TIME = 0x15;
constexpr uint16_t LEN_RX_TIME = 14;
constexpr uint16_t RX_STAMP_SUB = 0x00;
constexpr uint16_t FP_AMPL1_SUB = 0x07;
constexpr uint16_t LEN_RX_STAMP = 5;
constexpr uint16_t LEN_FP_AMPL1 = 2;
// RX frame quality
constexpr uint16_t RX_FQUAL = 0x12;
constexpr uint16_t LEN_RX_FQUAL = 8;
constexpr uint16_t STD_NOISE_SUB = 0x00;
constexpr uint16_t FP_AMPL2_SUB = 0x02;
constexpr uint16_t FP_AMPL3_SUB = 0x04;
constexpr uint16_t CIR_PWR_SUB = 0x06;
constexpr uint16_t LEN_STD_NOISE = 2;
constexpr uint16_t LEN_FP_AMPL2 = 2;
constexpr uint16_t LEN_FP_AMPL3 = 2;
constexpr uint16_t LEN_CIR_PWR = 2;
// TX timestamp register
constexpr uint16_t TX_TIME = 0x17;
constexpr uint16_t LEN_TX_TIME = 10;
constexpr uint16_t TX_STAMP_SUB = 0;
constexpr uint16_t LEN_TX_STAMP = 5;
// timing register (for delayed RX/TX)
constexpr uint16_t DX_TIME = 0x0A;
constexpr uint16_t LEN_DX_TIME = 5;
// Receive Frame Wait Timeout Period
constexpr uint16_t RX_WFTO = 0x0C;
constexpr uint16_t LEN_RX_WFTO = 2;
// transmit data buffer
constexpr uint16_t TX_BUFFER = 0x09;
constexpr uint16_t LEN_TX_BUFFER = 1024;
constexpr uint16_t LEN_UWB_FRAMES = 127;
constexpr uint16_t LEN_EXT_UWB_FRAMES = 1023;
// RX frame info
constexpr uint16_t RX_FINFO = 0x10;
constexpr uint16_t LEN_RX_FINFO = 4;
// receive data buffer
constexpr uint16_t RX_BUFFER = 0x11;
constexpr uint16_t LEN_RX_BUFFER = 1024;
// transmit control
constexpr uint16_t TX_FCTRL = 0x08;
constexpr uint16_t LEN_TX_FCTRL = 5;
// channel control
constexpr uint16_t CHAN_CTRL = 0x1F;
constexpr uint16_t LEN_CHAN_CTRL = 4;
constexpr uint16_t DWSFD_BIT = 17;
constexpr uint16_t TNSSFD_BIT = 20;
constexpr uint16_t RNSSFD_BIT = 21;
// user-defined SFD
constexpr uint16_t USR_SFD = 0x21;
constexpr uint16_t LEN_USR_SFD = 41;
constexpr uint16_t SFD_LENGTH_SUB = 0x00;
constexpr uint16_t LEN_SFD_LENGTH = 1;
// OTP control (for LDE micro code loading only)
constexpr uint16_t OTP_IF = 0x2D;
constexpr uint16_t OTP_ADDR_SUB = 0x04;
constexpr uint16_t OTP_CTRL_SUB = 0x06;
constexpr uint16_t OTP_RDAT_SUB = 0x0A;
constexpr uint16_t LEN_OTP_ADDR = 2;
constexpr uint16_t LEN_OTP_CTRL = 2;
constexpr uint16_t LEN_OTP_RDAT = 4;
// AGC_TUNE1/2/3 (for re-tuning only)
constexpr uint16_t AGC_TUNE = 0x23;
constexpr uint16_t AGC_TUNE1_SUB = 0x04;
constexpr uint16_t AGC_TUNE2_SUB = 0x0C;
constexpr uint16_t AGC_TUNE3_SUB = 0x12;
constexpr uint16_t LEN_AGC_TUNE1 = 2;
constexpr uint16_t LEN_AGC_TUNE2 = 4;
constexpr uint16_t LEN_AGC_TUNE3 = 2;
// EXT_SYNC (External Synchronization Control)
constexpr uint16_t EXT_SYNC = 0x24;
constexpr uint16_t EC_CTRL_SUB = 0x00;
constexpr uint16_t PLLLDT_BIT = 2;
constexpr uint16_t EC_RXTC_SUB = 0x04;
constexpr uint16_t EC_GOLP_SUB = 0x08;
constexpr uint16_t LEN_EC_CTRL = 4;
constexpr uint16_t LEN_EC_RXTC = 4;
constexpr uint16_t LEN_EC_GOLP = 4;
// DRX_TUNE2 (for re-tuning only)
constexpr uint16_t DRX_TUNE = 0x27;
constexpr uint16_t DRX_TUNE0b_SUB = 0x02;
constexpr uint16_t DRX_TUNE1a_SUB = 0x04;
constexpr uint16_t DRX_TUNE1b_SUB = 0x06;
constexpr uint16_t DRX_TUNE2_SUB = 0x08;
constexpr uint16_t DRX_SFDTOC_SUB = 0x20;
constexpr uint16_t DRX_PRETOC_SUB = 0x24;
constexpr uint16_t DRX_TUNE4H_SUB = 0x26;
constexpr uint16_t DRX_CAR_INT_SUB = 0x28;
constexpr uint16_t RXPACC_NOSAT_SUB = 0x2C;
constexpr uint16_t LEN_DRX_TUNE0b = 2;
constexpr uint16_t LEN_DRX_TUNE1a = 2;
constexpr uint16_t LEN_DRX_TUNE1b = 2;
constexpr uint16_t LEN_DRX_TUNE2 = 4;
constexpr uint16_t LEN_DRX_SFDTOC = 2;
constexpr uint16_t LEN_DRX_PRETOC = 2;
constexpr uint16_t LEN_DRX_TUNE4H = 2;
constexpr uint16_t LEN_DRX_CAR_INT = 3;
constexpr uint16_t LEN_RXPACC_NOSAT = 2;
// LDE_CFG1 (for re-tuning only)
constexpr uint16_t LDE_IF = 0x2E;
constexpr uint16_t LDE_CFG1_SUB = 0x0806;
constexpr uint16_t LDE_RXANTD_SUB = 0x1804;
constexpr uint16_t LDE_CFG2_SUB = 0x1806;
constexpr uint16_t LDE_REPC_SUB = 0x2804;
constexpr uint16_t LEN_LDE_CFG1 = 1;
constexpr uint16_t LEN_LDE_CFG2 = 2;
constexpr uint16_t LEN_LDE_REPC = 2;
constexpr uint16_t LEN_LDE_RXANTD = 2;
// DIG_DIAG (Digital Diagnostics Interface)
constexpr uint16_t DIG_DIAG = 0x2F;
constexpr uint16_t EVC_CTRL_SUB = 0x00;
constexpr uint16_t EVC_STO_SUB = 0x10;
constexpr uint16_t EVC_PTO_SUB = 0x12;
constexpr uint16_t EVC_FWTO_SUB = 0x14;
constexpr uint16_t DIAG_TMC_SUB = 0x24;
constexpr uint16_t LEN_EVC_CTRL = 4;
constexpr uint16_t LEN_EVC_STO = 2;
constexpr uint16_t LEN_EVC_PTO = 2;
constexpr uint16_t LEN_EVC_FWTO = 2;
constexpr uint16_t LEN_DIAG_TMC = 2;
// TX_POWER (for re-tuning only)
constexpr uint16_t TX_POWER = 0x1E;
constexpr uint16_t LEN_TX_POWER = 4;
// RF_CONF (for re-tuning only)
constexpr uint16_t RF_CONF = 0x28;
constexpr uint16_t RF_CONF_SUB = 0x00;
constexpr uint16_t RF_RXCTRLH_SUB = 0x0B;
constexpr uint16_t RF_TXCTRL_SUB = 0x0C;
constexpr uint16_t LEN_RX_CONF_SUB = 4;
constexpr uint16_t LEN_RF_RXCTRLH = 1;
constexpr uint16_t LEN_RF_TXCTRL = 4;
// TX_CAL (for re-tuning only)
constexpr uint16_t TX_CAL = 0x2A;
constexpr uint16_t TC_PGDELAY_SUB = 0x0B;
constexpr uint16_t LEN_TC_PGDELAY = 1;
constexpr uint16_t TC_SARC = 0x00;
constexpr uint16_t TC_SARL = 0x03;
// FS_CTRL (for re-tuning only)
constexpr uint16_t FS_CTRL = 0x2B;
constexpr uint16_t FS_PLLCFG_SUB = 0x07;
constexpr uint16_t FS_PLLTUNE_SUB = 0x0B;
constexpr uint16_t FS_XTALT_SUB = 0x0E;
constexpr uint16_t LEN_FS_PLLCFG = 4;
constexpr uint16_t LEN_FS_PLLTUNE = 1;
constexpr uint16_t LEN_FS_XTALT = 1;
// AON
constexpr uint16_t AON = 0x2C;
constexpr uint16_t AON_WCFG_SUB = 0x00;
constexpr uint16_t ONW_RADC_BIT = 0;
constexpr uint16_t ONW_RX_BIT = 1;
constexpr uint16_t ONW_LEUI_BIT = 3;
constexpr uint16_t ONW_LDC_BIT = 6;
constexpr uint16_t ONW_L64P_BIT = 7;
constexpr uint16_t ONW_PRES_SLEEP_BIT = 8;
constexpr uint16_t ONW_LLDE_BIT = 11;
constexpr uint16_t ONW_LLDO_BIT = 12;
constexpr uint16_t LEN_AON_WCFG = 2;
constexpr uint16_t AON_CTRL_SUB = 0x02;
constexpr uint16_t RESTORE_BIT = 0;
constexpr uint16_t SAVE_BIT = 1;
constexpr uint16_t UPL_CFG_BIT = 2;
constexpr uint16_t LEN_AON_CTRL = 1;
constexpr uint16_t AON_CFG0_SUB = 0x06;
constexpr uint16_t SLEEP_EN_BIT = 0;
constexpr uint16_t WAKE_PIN_BIT = 1;
constexpr uint16_t WAKE_SPI_BIT = 2;
constexpr uint16_t WAKE_CNT_BIT = 3;
constexpr uint16_t LPDIV_EN_BIT = 4;
constexpr uint16_t LEN_AON_CFG0 = 4;
constexpr uint16_t AON_CFG1_SUB = 0x0A;
constexpr uint16_t SLEEP_CEN_BIT = 0;
constexpr uint16_t SMXX_BIT = 1;
constexpr uint16_t LPOSC_CAL_BIT = 2;
constexpr uint16_t LEN_AON_CFG1 = 2;
// PMSC
constexpr uint16_t PMSC = 0x36;
constexpr uint16_t PMSC_CTRL0_SUB = 0x00;
constexpr uint16_t GPDCE_BIT = 18;
constexpr uint16_t KHZCLKEN_BIT = 23;
constexpr uint16_t PMSC_SOFTRESET_SUB = 0x03;
constexpr uint16_t PMSC_CTRL1_SUB = 0x04;
constexpr uint16_t ATXSLP_BIT = 11;
constexpr uint16_t ARXSLP_BIT = 12;
constexpr uint16_t PMSC_LEDC_SUB = 0x28;
constexpr uint16_t BLNKEN = 8;
constexpr uint16_t LEN_PMSC_CTRL0 = 4;
constexpr uint16_t LEN_PMSC_SOFTRESET = 1;
constexpr uint16_t LEN_PMSC_CTRL1 = 4;
constexpr uint16_t LEN_PMSC_LEDC = 4;
// TX_ANTD Antenna delays
constexpr uint16_t TX_ANTD = 0x18;
constexpr uint16_t LEN_TX_ANTD = 2;
// Acknowledgement time and response time
constexpr uint16_t ACK_RESP_T = 0x1A;
constexpr uint16_t ACK_RESP_T_W4R_TIME_SUB = 0x00;
constexpr uint16_t LEN_ACK_RESP_T_W4R_TIME_SUB = 3;
constexpr uint16_t LEN_ACK_RESP_T = 4;
// GPIO
constexpr uint16_t GPIO_CTRL = 0x26;
constexpr uint16_t GPIO_MODE_SUB = 0x00;
constexpr uint16_t LEN_GPIO_MODE = 4;

33
src/dwm1000/DW1000NgTime.cpp Normal file
View File

@@ -0,0 +1,33 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <Arduino.h>
#include "DW1000NgTime.hpp"
#include "DW1000NgConstants.hpp"
namespace DW1000NgTime {
uint64_t microsecondsToUWBTime(uint64_t microSeconds) {
return static_cast<uint64_t>(microSeconds * TIME_RES_INV);
}
}

31
src/dwm1000/DW1000NgTime.hpp Normal file
View File

@@ -0,0 +1,31 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <Arduino.h>
namespace DW1000NgTime {
uint64_t microsecondsToUWBTime(uint64_t microSeconds);
}

141
src/dwm1000/DW1000NgUtils.cpp Normal file
View File

@@ -0,0 +1,141 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
/*
* Copyright (c) 2015 by Thomas Trojer <thomas@trojer.net>
* Decawave DW1000 library for arduino.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* @file DW1000.cpp
* Helper functions.
*/
#include <Arduino.h>
#include "DW1000NgUtils.hpp"
#include "DW1000NgConstants.hpp"
#include "DW1000NgRegisters.hpp"
namespace DW1000NgUtils {
/*
* Set the value of a bit in an array of bytes that are considered
* consecutive and stored from MSB to LSB.
* @param data
* The number as byte array.
* @param n
* The number of bytes in the array.
* @param bit
* The position of the bit to be set.
* @param val
* The boolean value to be set to the given bit position.
*/
void setBit(byte data[], uint16_t n, uint16_t bit, boolean val) {
uint16_t idx;
uint8_t shift;
idx = bit/8;
if(idx >= n) {
return; // TODO proper error handling: out of bounds
}
byte* targetByte = &data[idx];
shift = bit%8;
if(val) {
bitSet(*targetByte, shift);
} else {
bitClear(*targetByte, shift);
}
}
/*
* Check the value of a bit in an array of bytes that are considered
* consecutive and stored from MSB to LSB.
* @param data
* The number as byte array.
* @param n
* The number of bytes in the array.
* @param bit
* The position of the bit to be checked.
*/
boolean getBit(byte data[], uint16_t n, uint16_t bit) {
uint16_t idx;
uint8_t shift;
idx = bit/8;
if(idx >= n) {
return false; // TODO proper error handling: out of bounds
}
byte targetByte = data[idx];
shift = bit%8;
return bitRead(targetByte, shift); // TODO wrong type returned byte instead of boolean
}
void writeValueToBytes(byte data[], uint64_t val, uint8_t n) {
for(auto i = 0; i < n; i++) {
data[i] = ((val >> (i*8)) & 0xFF);
}
}
uint64_t bytesAsValue(byte data[], uint8_t n) {
uint64_t value = 0;
for(auto i = 0; i < n; i++) {
value |= ((uint64_t)data[i] << (i*8));
}
return value;
}
uint8_t nibbleFromChar(char c) {
if(c >= '0' && c <= '9') {
return c-'0';
}
if(c >= 'a' && c <= 'f') {
return c-'a'+10;
}
if(c >= 'A' && c <= 'F') {
return c-'A'+10;
}
return 255;
}
void convertToByte(char string[], byte* bytes) {
byte eui_byte[LEN_EUI];
// we fill it with the char array under the form of "AA:FF:1C:...."
for(uint16_t i = 0; i < LEN_EUI; i++) {
eui_byte[i] = (nibbleFromChar(string[i*3]) << 4)+nibbleFromChar(string[i*3+1]);
}
memcpy(bytes, eui_byte, LEN_EUI);
}
}

110
src/dwm1000/DW1000NgUtils.hpp Normal file
View File

@@ -0,0 +1,110 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
/*
* Copyright (c) 2015 by Thomas Trojer <thomas@trojer.net>
* Decawave DW1000 library for arduino.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* @file DW1000.h
* Helper functions.
*/
#pragma once
#include <Arduino.h>
#include "DW1000NgConstants.hpp"
namespace DW1000NgUtils {
/**
Returns target bit value inside a byte array
@param [in] data the byte array
@param [in] n the length of the byte array
@param [in] bit the bit position
returns bit value (true = 1, false = 0)
*/
boolean getBit(byte data[], uint16_t n, uint16_t bit);
/**
Sets the target bit value inside an array of bytes
@param [in] data the byte array
@param [in] n the length of the byte array
@param [in] bit the bit position
@param [in] val the bit value
*/
void setBit(byte data[], uint16_t n, uint16_t bit, boolean val);
/**
Writes the target value inside a given byte array.
@param [in] data the byte array
@param [in] val the value to insert
@param [in] n the length of the byte array
*/
void writeValueToBytes(byte data[], uint64_t val, uint8_t n);
/**
Gets the target byte array value
@param [in] data the byte array
@param [in] n the length of the byte array
returns the byte array value
*/
uint64_t bytesAsValue(byte data[], uint8_t n);
/**
Converts from char to 4 bits (hexadecimal)
@param [in] c the character
returns target value
*/
uint8_t nibbleFromChar(char c);
/**
Converts the target string to eui bytes
@param [in] string The eui string (in format XX:XX:XX:XX:XX:XX:XX:XX)
@param [out] eui_byte The eui bytes
*/
void convertToByte(char string[], byte* eui_byte);
}

148
src/dwm1000/SPIporting.cpp Normal file
View File

@@ -0,0 +1,148 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*
* @file SPIporting.hpp
* Arduino porting for the SPI interface.
*/
#include <Arduino.h>
#include <SPI.h>
#include "SPIporting.hpp"
#include "DW1000NgConstants.hpp"
#include "DW1000NgRegisters.hpp"
uint32_t m_spiClock = 0;
spi_host_device_t m_spiHost;
spi_device_handle_t m_spiHandle = 0;
Adafruit_MCP23017 *m_expander = NULL;
namespace SPIporting {
void SPIInitWithClock(uint32_t clock)
{
if(m_spiHandle)
{
spi_bus_remove_device(m_spiHandle);
m_spiHandle = 0;
}
spi_device_interface_config_t devcfg={
.command_bits = 0,
.address_bits = 0,
.dummy_bits = 0,
.mode=0,
.clock_speed_hz=clock,
.spics_io_num=-1,
.flags = 0,
.queue_size=7,
.pre_cb=NULL,
.post_cb=NULL,
};
m_spiClock = clock;
spi_bus_add_device(m_spiHost, &devcfg, &m_spiHandle);
}
namespace {
constexpr uint32_t EspSPImaximumSpeed = 20000000; //20MHz
constexpr uint32_t SPIminimumSpeed = 2000000; //2MHz
const SPISettings _fastSPI = SPISettings(EspSPImaximumSpeed, MSBFIRST, SPI_MODE0);
const SPISettings _slowSPI = SPISettings(SPIminimumSpeed, MSBFIRST, SPI_MODE0);
const SPISettings* _currentSPI = &_fastSPI;
void _openSPI(uint8_t slaveSelectPIN) {
if(_currentSPI->_clock!=m_spiClock)
{
SPIInitWithClock(_currentSPI->_clock);
}
m_expander->digitalWrite(slaveSelectPIN, LOW);
}
void _closeSPI(uint8_t slaveSelectPIN) {
m_expander->digitalWrite(slaveSelectPIN, HIGH);
}
}
void SPIinit(spi_host_device_t spihost, Adafruit_MCP23017 *expander) {
m_spiHost = spihost;
m_expander = expander;
SPIInitWithClock(EspSPImaximumSpeed);
}
void SPIend() {
spi_bus_remove_device(m_spiHandle);
m_spiHandle = 0;
}
void SPIselect(uint8_t slaveSelectPIN, uint8_t irq) {
m_expander->pinMode(slaveSelectPIN, OUTPUT);
m_expander->digitalWrite(slaveSelectPIN, HIGH);
}
uint8_t spi_transfer(uint8_t data)
{
esp_err_t ret;
spi_transaction_t t;
memset(&t, 0, sizeof(t));
t.flags = SPI_TRANS_USE_RXDATA;
t.length=8;
t.tx_buffer=&data;
ret=spi_device_transmit(m_spiHandle, &t);
assert(ret==ESP_OK);
return t.rx_data[0];
}
void writeToSPI(uint8_t slaveSelectPIN, uint8_t headerLen, byte header[], uint16_t dataLen, byte data[]) {
_openSPI(slaveSelectPIN);
for(auto i = 0; i < headerLen; i++) {
spi_transfer(header[i]); // send header
}
for(auto i = 0; i < dataLen; i++) {
spi_transfer(data[i]); // write values
}
delayMicroseconds(5);
_closeSPI(slaveSelectPIN);
}
void readFromSPI(uint8_t slaveSelectPIN, uint8_t headerLen, byte header[], uint16_t dataLen, byte data[]){
_openSPI(slaveSelectPIN);
for(auto i = 0; i < headerLen; i++) {
spi_transfer(header[i]); // send header
}
for(auto i = 0; i < dataLen; i++) {
data[i] = spi_transfer(0x00); // read values
}
delayMicroseconds(5);
_closeSPI(slaveSelectPIN);
}
void setSPIspeed(SPIClock speed) {
if(speed == SPIClock::FAST) {
_currentSPI = &_fastSPI;
} else if(speed == SPIClock::SLOW) {
_currentSPI = &_slowSPI;
}
}
}

82
src/dwm1000/SPIporting.hpp Normal file
View File

@@ -0,0 +1,82 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*
* @file SPIporting.hpp
* Arduino porting for the SPI interface.
*/
#pragma once
#include <Arduino.h>
#include "DW1000NgConstants.hpp"
#include "Adafruit_MCP23017.h"
#include "driver/spi_master.h"
namespace SPIporting{
/**
Initializes the SPI bus.
*/
void SPIinit(spi_host_device_t spihost, Adafruit_MCP23017 *expander);
/**
Tells the driver library that no communication to a DW1000 will be required anymore.
This basically just frees SPI and the previously used pins.
*/
void SPIend();
/**
(Re-)selects a specific DW1000 chip for communication. Used in case you switched SPI to another device.
*/
void SPIselect(uint8_t slaveSelectPIN, uint8_t irq = 0xff);
/**
Arduino function to write to the SPI.
Takes two separate byte buffers for write header and write data
@param [in] Header lenght
@param [in] Header array built before
@param [in] Data lenght
@param [in] Data array
*/
void writeToSPI(uint8_t slaveSelectPIN, uint8_t headerLen, byte header[], uint16_t dataLen, byte data[]);
/**
Arduino function to read from the SPI.
Takes two separate byte buffers for write header and write data
@param [in] Header lenght
@param [in] Header array built before
@param [in] Data lenght
@param [out] Data array
*/
void readFromSPI(uint8_t slaveSelectPIN, uint8_t headerLen, byte header[], uint16_t dataLen, byte data[]);
/**
Sets speed of SPI clock, fast or slow(20MHz or 2MHz)
@param [in] SPIClock FAST or SLOW
*/
void setSPIspeed(SPIClock speed);
}

35
src/dwm1000/deprecated.hpp Normal file
View File

@@ -0,0 +1,35 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#ifdef __has_cpp_attribute
#if __has_cpp_attribute(deprecated)
#define DEPRECATED [[deprecated]]
#define DEPRECATED_MSG(msg) [[deprecated(msg)]]
#endif // __has_cpp_attribute(deprecated)
#else
#define DEPRECATED __attribute__((deprecated))
#define DEPRECATED_MSG(msg) __attribute__((deprecated(msg)))
#endif // __has_cpp_attribute

29
src/dwm1000/require_cpp11.hpp Normal file
View File

@@ -0,0 +1,29 @@
/*
* MIT License
*
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#if __cplusplus < 201103L
#error "You need Arduino IDE version >=1.6.6 to use this project as it needs C++11 support."
#endif