ENSEEIHT/TP-reseaux-locaux
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity
TP-reseaux-locaux/DecaDuino-master/DecaDuino.cpp

1301 lines
29 KiB
C++
Raw Normal View History

init
2023-06-20 18:50:12 +00:00
// DecaDuino.cpp
//
// Another DecaWave DW1000 driver for Arduino
// See the README file in this directory for documentation
#include <SPI.h>
#include "DecaDuino.h"
#include <util/atomic.h>
DecaDuino* DecaDuino::_DecaDuinoInterrupt[MAX_NB_DW1000_FOR_INTERRUPTS] = {0, 0, 0};
DecaDuino::DecaDuino(uint8_t slaveSelectPin, uint8_t interruptPin) {
_slaveSelectPin = slaveSelectPin;
_interruptPin = interruptPin;
}
boolean DecaDuino::init() {
// Call init with 0xFFFF for both Short Address and PanId (no address/panid identification: Promiscuous mode)
return init(0xFFFFFFFF);
}
boolean DecaDuino::init ( uint32_t shortAddressAndPanId ) {
uint8_t buf[8];
uint16_t ui16t;
uint32_t ui32t;
// Initialise the IRQ and Slave Select pin
pinMode(_interruptPin, INPUT);
pinMode(_slaveSelectPin, OUTPUT);
digitalWrite(_slaveSelectPin, HIGH);
SPI.begin();
// Initialise the RX pointers
rxDataAvailable = false;
rxData = NULL;
rxDataLen = NULL;
// Wait for DW1000 POR (up to 5msec)
delay(5);
#ifdef DECADUINO_DEBUG
delay(3000); // delay to see next messages on console for debug
Serial.println("DecaDuino Debug is active!");
#endif
// Reset the DW1000 now
resetDW1000();
// Check the device type
if ( readSpiUint32(DW1000_REGISTER_DEV_ID) != 0xdeca0130 ) return false;
// Load Extended Unique Identifier – the 64-bit IEEE device address - in memory
euid = getEuid();
// Attach interrupt handler
if (_interruptPin == DW1000_IRQ0_PIN) {
_DecaDuinoInterrupt[DW1000_IRQ0_PIN] = this;
attachInterrupt(_interruptPin, DecaDuino::isr0, HIGH);
} else if (_interruptPin == DW1000_IRQ1_PIN) {
_DecaDuinoInterrupt[DW1000_IRQ1_PIN] = this;
attachInterrupt(_interruptPin, DecaDuino::isr1, HIGH);
} else if (_interruptPin == DW1000_IRQ2_PIN) {
_DecaDuinoInterrupt[DW1000_IRQ2_PIN] = this;
attachInterrupt(_interruptPin, DecaDuino::isr2, HIGH);
} else return false;
// --- Configure DW1000 -----------------------------------------------------------------------------------------
// System Configuration Register
ui32t = readSpiUint32(DW1000_REGISTER_SYS_CFG);
ui32t |= DW1000_REGISTER_SYS_CFG_RXAUTR_MASK; // RXAUTR: Receiver Auto-Re-enable after a RX failure
writeSpiUint32(DW1000_REGISTER_SYS_CFG,ui32t);
#ifdef DECADUINO_DEBUG
sprintf((char*)debugStr,"SYS_CFG=%08lx", ui32t);
Serial.println((char*)debugStr);
#endif
// System Event Mask Register
ui32t = readSpiUint32(DW1000_REGISTER_SYS_MASK);
ui32t |= DW1000_REGISTER_SYS_MASK_MRXFCG_MASK; // MRXFCG: interrupt when good frame (FCS OK) received
ui32t |= DW1000_REGISTER_SYS_MASK_MTXFRS_MASK;
writeSpiUint32(DW1000_REGISTER_SYS_MASK, ui32t);
#ifdef DECADUINO_DEBUG
sprintf((char*)debugStr,"SYS_MASK=%08lx", ui32t);
Serial.println((char*)debugStr);
#endif
// Enable frame filtering on addressing fields if init() is called with a shortAddressAndPanId != 0xFFFFFFFF
if ( shortAddressAndPanId != 0xFFFFFFFF ) {
ui32t = readSpiUint32(DW1000_REGISTER_SYS_CFG);
ui32t |= 0x0000003D;
writeSpiUint32(DW1000_REGISTER_SYS_CFG,ui32t);
setShortAddressAndPanId(shortAddressAndPanId);
}
// Set default antenna delay value
setAntennaDelay(DWM1000_DEFAULT_ANTENNA_DELAY_VALUE);
// --- End of DW1000 configuration ------------------------------------------------------------------------------
lastTxOK = false;
// Return true if everything OK
return true;
} // End of init()
void DecaDuino::resetDW1000() {
uint8_t buf[8];
uint32_t ui32t;
// Initialise the SPI port
currentSPISettings = SPISettings(500000, MSBFIRST, SPI_MODE0);
delay(100);
// Getting PMSC_CTRL0 register
ui32t = readSpiUint32(DW1000_REGISTER_PMSC_CTRL0);
#ifdef DECADUINO_DEBUG
sprintf((char*)debugStr,"PMSC_CTRL0=%08lx", ui32t);
Serial.println((char*)debugStr);
#endif
// Set SYSCLKS bits to 01
ui32t = ( ui32t & 0xFFFFFFFC ) | 1;
writeSpiUint32(DW1000_REGISTER_PMSC_CTRL0, ui32t);
delay(1);
// Clear SOFTRESET bits
ui32t &= 0x0FFFFFFF;
writeSpiUint32(DW1000_REGISTER_PMSC_CTRL0, ui32t);
delay(1);
#ifdef DECADUINO_DEBUG
sprintf((char*)debugStr,"PMSC_CTRL0=%08lx", ui32t);
Serial.println((char*)debugStr);
#endif
// Set SOFTRESET bits
ui32t |= 0xF0000000;
ui32t &= 0xFFFFFFFC;
writeSpiUint32(DW1000_REGISTER_PMSC_CTRL0, ui32t);
delay(5);
// Load the LDE algorithm microcode into LDE RAM or disable LDE execution (clear LDERUNE)
// Load the LDE algorithm microcode into LDE RAM (procedure p.22 DW1000 User Manual + comment p.21)
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
SPI.beginTransaction(currentSPISettings);
digitalWrite(_slaveSelectPin, LOW);
buf[0] = 0xF6;
buf[1] = 0x00;
buf[2] = 0x01;
buf[3] = 0x03;
spi_send(buf,4);
digitalWrite(_slaveSelectPin, HIGH);
SPI.endTransaction();
SPI.beginTransaction(currentSPISettings);
digitalWrite(_slaveSelectPin, LOW);
buf[0] = 0xED;
buf[1] = 0x06;
buf[2] = 0x00;
buf[3] = 0x80;
spi_send(buf,4);
digitalWrite(_slaveSelectPin, HIGH);
SPI.endTransaction();
delayMicroseconds(160);
SPI.beginTransaction(currentSPISettings);
digitalWrite(_slaveSelectPin, LOW);
buf[0] = 0xF6;
buf[1] = 0x00;
buf[2] = 0x00;
buf[3] = 0x02;
spi_send(buf,4);
digitalWrite(_slaveSelectPin, HIGH);
SPI.endTransaction();
}
// Initialise the SPI port
currentSPISettings = SPISettings(6000000, MSBFIRST, SPI_MODE0);
delay(1);
#ifdef DECADUINO_DEBUG
ui32t = readSpiUint32(DW1000_REGISTER_PMSC_CTRL0);
sprintf((char*)debugStr,"PMSC_CTRL0=%08lx", ui32t);
Serial.println((char*)debugStr);
#endif
trxStatus = DW1000_TRX_STATUS_IDLE;
}
void DecaDuino::isr0() {
#ifdef DECADUINO_DEBUG
//Serial.println("\n###isr0###");
#endif
if (_DecaDuinoInterrupt[DW1000_IRQ0_PIN]) _DecaDuinoInterrupt[DW1000_IRQ0_PIN]->handleInterrupt();
}
void DecaDuino::isr1() {
#ifdef DECADUINO_DEBUG
//Serial.println("\n###isr1###");
#endif
if (_DecaDuinoInterrupt[DW1000_IRQ1_PIN]) _DecaDuinoInterrupt[DW1000_IRQ1_PIN]->handleInterrupt();
}
void DecaDuino::isr2() {
#ifdef DECADUINO_DEBUG
//Serial.println("\n###isr2###");
#endif
if (_DecaDuinoInterrupt[DW1000_IRQ2_PIN]) _DecaDuinoInterrupt[DW1000_IRQ2_PIN]->handleInterrupt();
}
void DecaDuino::handleInterrupt() {
uint8_t buf[8];
uint32_t sysStatusReg, ack, ui32t;
double rxtofs, rxttcki;
ack = 0;
// Read System Event Status Register
sysStatusReg = readSpiUint32(DW1000_REGISTER_SYS_STATUS);
// If IRQS is cleared, no enabled interrupt (SYS_MASK) have assert the IRQ pin: exit
if ( ! ( sysStatusReg & DW1000_REGISTER_SYS_STATUS_IRQS_MASK ) )
return;
#ifdef DECADUINO_DEBUG
// Serial.print("\n###isr### ");
//ui32t = readSpiUint32(DW1000_REGISTER_SYS_MASK);
//sprintf((char*)debugStr,"SYS_MASK =%08x", ui32t);
//Serial.println((char*)debugStr);
//sprintf((char*)debugStr,"SYS_STATUS=%08x ", sysStatusReg);
//Serial.print((char*)debugStr);
#endif
// Checking RX frame interrupt
if ( sysStatusReg & DW1000_REGISTER_SYS_STATUS_RXDFR_MASK ) { // RXDFR
trxStatus = DW1000_TRX_STATUS_IDLE;
#ifdef DECADUINO_DEBUG
// Serial.print("RXDFR ");
#endif
// Good frame
if ( sysStatusReg & DW1000_REGISTER_SYS_STATUS_RXFCG_MASK ) { // RXFCG
#ifdef DECADUINO_DEBUG
//Serial.print("RXFCG ");
#endif
if ( rxData == NULL ) {
#ifdef DECADUINO_DEBUG
Serial.print("Error: no RX buffer set");
#endif
} else {
// get frame length
ui32t = (readSpiUint32(DW1000_REGISTER_RX_FINFO) & DW1000_REGISTER_RX_FINFO_RXFLEN_MASK) - 2; // FCS is 2-bytes long. Avoid it in the len.
*rxDataLen = (uint16_t)ui32t;
#ifdef DECADUINO_DEBUG
//sprintf((char*)debugStr,"length=%dbytes ", *rxDataLen);
// Serial.print((char*)debugStr);
#endif
// get frame data
if ( rxDataLenMax != 0 ) {
// Put frame data at the end of the buffer
readSpi(DW1000_REGISTER_RX_BUFFER, rxData+rxDataLenMax-*rxDataLen, *rxDataLen);
} else {
// Put frame data at the begin of the buffer
readSpi(DW1000_REGISTER_RX_BUFFER, rxData, *rxDataLen);
}
// rxDataAvailable = true;
if ( sysStatusReg & DW1000_REGISTER_SYS_STATUS_LDEDONE_MASK ) {
// Get RX timestamp
encodeUint64(0, buf); // init buffer the 64-bit buffer
readSpi(DW1000_REGISTER_RX_TIME, buf, 5);
lastRxTimestamp = decodeUint64(buf);
// Get transmitter-receiver skew (clock offset or crystal offset between the local receiver and the remote end transmitter device)
readSpi(DW1000_REGISTER_RX_TTCKO, buf, 3);
/* Commented by Adrien on 20150906
ui32t = decodeUint32(buf) & 0x0007FFFF; // Clock offset is a 19-bit signed integer
if ( ui32t & 0x00080000 )
ui32t |= 0xFFF80000; // negative value
ui32t = 0x01F00000/ui32t;
*/
// Drien 20150906: should we read rxttcki value in DW1000_REGISTER_RX_TTCKI?
rxttcki = 32505856;
// Turn rxtofs to a signed double value (RD032014)
if (buf[2] & 0x04 ) { // rxtofs is negative
buf[2] |= 0xF8;
buf[2] = ~buf[2];
buf[1] = ~buf[1];
buf[0] = ~buf[0];
rxtofs = buf[2] * 256*256 + buf[1] * 256 + buf[0];
rxtofs = rxtofs+1;
rxtofs = rxtofs*-1;
} else {
rxtofs = buf[2] * 256*256 + buf[1] * 256 + buf[0];
}
clkOffset = rxtofs * 1000000 / rxttcki;
rxDataAvailable = true;
// Serial.print("clock offset=");
// Serial.println(ui32t, HEX);
// Serial.println(offseti);
// Serial.print("RXTOFS=0x");
// Serial.println(ui32t, HEX);
// ui32t = 0x01F00000/ui32t;
// Serial.print("clock offset=0x");
// Serial.println(ui32t, HEX);
#ifdef DECADUINO_DEBUG
Serial.print("RX Frame timestamp=");
printUint64(lastRxTimestamp);
Serial.print(", skew=");
Serial.println(clkOffset);
#endif
}
}
// Clearing the RXFCG bit (it clears the interrupt if enabled)
ack |= DW1000_REGISTER_SYS_STATUS_RXFCG_MASK;
}
// Bad frame (FCS error)
if ( sysStatusReg & DW1000_REGISTER_SYS_STATUS_RXFCE_MASK ) { // RXFCE
#ifdef DECADUINO_DEBUG
Serial.println("RXFCG (FCS error)");
#endif
// Clearing the RXFCG bit (it clears the interrupt if enabled)
ack |= DW1000_REGISTER_SYS_STATUS_RXFCE_MASK;
}
// Clearing the RXDFR bit (it clears the interrupt if enabled)
ack |= DW1000_REGISTER_SYS_STATUS_RXDFR_MASK;
}
// Manage TX completion interrupt
if ( sysStatusReg & DW1000_REGISTER_SYS_STATUS_TXFRS_MASK ) { // TXFRS
trxStatus = DW1000_TRX_STATUS_IDLE;
// Read TX timestamp
encodeUint64(0, buf); // // init buffer the 64-bit buffer
readSpi(DW1000_REGISTER_TX_TIME, buf, 5);
lastTxTimestamp = decodeUint64(buf);
lastTxOK = true;
#ifdef DECADUINO_DEBUG
Serial.print("TX Frame OK. Tx timestamp=");
printUint64(lastTxTimestamp);
#endif
ack |= DW1000_REGISTER_SYS_STATUS_TXFRS_MASK;
}
// Acknoledge by writing '1' in all set bits in the System Event Status Register
writeSpiUint32(DW1000_REGISTER_SYS_STATUS, ack);
#ifdef DECADUINO_DEBUG
Serial.println();
#endif
}
bool DecaDuino::hasTxSucceeded() {
return lastTxOK;
}
uint64_t DecaDuino::alignDelayedTransmission ( uint64_t wantedDelay ) {
return ((getSystemTimeCounter() + wantedDelay) & 0xFFFFFFFE00) + getAntennaDelay();
}
uint8_t DecaDuino::pdDataRequest(uint8_t* buf, uint16_t len) {
return pdDataRequest(buf, len, false, 0);
}
uint8_t DecaDuino::pdDataRequest(uint8_t* buf, uint16_t len, uint8_t delayed, uint64_t time) {
uint32_t ui32t;
uint8_t tempbuf[8];
#ifdef DECADUINO_DEBUG
sprintf((char*)debugStr,"Request to send %dbyte(s): |", len);
Serial.print((char*)debugStr);
for (int i=0;i<len;i++) {
sprintf((char*)debugStr,"%02x|", buf[i]);
Serial.print((char*)debugStr);
}
Serial.println();
#endif
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
trxStatus = DW1000_TRX_STATUS_TX;
// copy PSDU in tx buffer
writeSpi(DW1000_REGISTER_TX_BUFFER, buf, len);
// read tx frame control register
ui32t = readSpiUint32(DW1000_REGISTER_TX_FCTRL);
// set frame length
ui32t = (ui32t & ~DW1000_REGISTER_TX_FCTRL_FRAME_LENGTH_MASK) | (len+2); // FCS is 2-bytes long
writeSpiUint32(DW1000_REGISTER_TX_FCTRL, ui32t);
if ( delayed ) { // if delayed transmission
// send time
encodeUint64 ( (time - getAntennaDelay() ) & 0x000000FFFFFFFE00, tempbuf); // time is 5-bytes long, 9 lsb=0
writeSpi(DW1000_REGISTER_DX_TIME, tempbuf, 5);
// set tx start bit and Transmitter Delayed Sendind bit
writeSpiUint32(DW1000_REGISTER_SYS_CTRL, DW1000_REGISTER_SYS_CTRL_TXSTRT_MASK | DW1000_REGISTER_SYS_CTRL_TXDLYS_MASK);
} else {
// set tx start bit
writeSpiUint32(DW1000_REGISTER_SYS_CTRL, DW1000_REGISTER_SYS_CTRL_TXSTRT_MASK);
}
lastTxOK = false;
}
/*
#ifdef DECADUINO_DEBUG
ui32t = readSpiUint32(DW1000_REGISTER_TX_FCTRL);
sprintf((char*)debugStr,"TX_FCTRL=%08x\n", ui32t);
Serial.print((char*)debugStr);
#endif
*/
return true;
}
uint8_t DecaDuino::send(uint8_t* buf, uint16_t len) {
return pdDataRequest(buf, len);
}
uint8_t DecaDuino::send(uint8_t* buf, uint16_t len, uint8_t delayed, uint64_t time) {
return pdDataRequest(buf, len, delayed, time);
}
void DecaDuino::setRxBuffer(uint8_t* buf, uint16_t *len) {
#ifdef DECADUINO_DEBUG
sprintf((char*)debugStr,"Setting RX buffer address to 0x%08lx", (uint32_t)buf);
Serial.println((char*)debugStr);
#endif
rxData = buf;
rxDataLen = len;
rxDataLenMax = 0;
}
void DecaDuino::setRxBuffer(uint8_t* buf, uint16_t *len, uint16_t max) {
setRxBuffer(buf, len);
rxDataLenMax = max;
}
void DecaDuino::plmeRxEnableRequest(void) {
#ifdef DECADUINO_DEBUG
sprintf((char*)debugStr,"RX enable request");
Serial.println((char*)debugStr);
#endif
// set rx enable bit in system control register
writeSpiUint32(DW1000_REGISTER_SYS_CTRL, DW1000_REGISTER_SYS_CTRL_RXENAB_MASK);
trxStatus = DW1000_TRX_STATUS_RX;
}
void DecaDuino::plmeRxEnableRequest(uint16_t max) {
plmeRxEnableRequest();
rxDataLenMax = max;
}
void DecaDuino::plmeRxEnableRequest(uint8_t* buf, uint16_t *len) {
#ifdef DECADUINO_DEBUG
sprintf((char*)debugStr,"RX enable request with address buffer");
Serial.println((char*)debugStr);
#endif
setRxBuffer(buf, len);
plmeRxEnableRequest();
}
void DecaDuino::plmeRxEnableRequest(uint8_t* buf, uint16_t *len, uint16_t max) {
plmeRxEnableRequest(buf, len);
rxDataLenMax = max;
}
void DecaDuino::plmeRxDisableRequest(void) {
#ifdef DECADUINO_DEBUG
sprintf((char*)debugStr,"RX disable request");
Serial.println((char*)debugStr);
#endif
// set transceiver off bit in system control register (go to idle mode)
writeSpiUint32(DW1000_REGISTER_SYS_CTRL, DW1000_REGISTER_SYS_CTRL_TRXOFF_MASK);
trxStatus = DW1000_TRX_STATUS_IDLE;
}
uint8_t DecaDuino::rxFrameAvailable(void) {
if ( rxDataAvailable ) {
rxDataAvailable = false;
return true;
}
else return false;
}
uint8_t DecaDuino::rxFrameAvailable(uint8_t* buf, uint16_t *len) {
return rxFrameAvailable(buf, len, 0);
}
uint8_t DecaDuino::rxFrameAvailable(uint8_t* buf, uint16_t *len, uint16_t max) {
uint16_t i;
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
if ( rxDataAvailable ) {
if ( max == 0 ) {
for (i=0; i<*rxDataLen; i++)
buf[i] = rxData[i];
} else {
for (i=0; i<*rxDataLen; i++)
buf[i+max-*rxDataLen] = rxData[i];
}
*len = *rxDataLen;
rxDataAvailable = false;
return true;
}
}
return false;
}
uint8_t DecaDuino::getTrxStatus(void) {
return trxStatus;
}
void DecaDuino::spi_send ( uint8_t u8 ) {
SPI.transfer(u8);
}
void DecaDuino::spi_send ( uint16_t u16 ) {
SPI.transfer16(u16);
}
void DecaDuino::spi_send ( uint8_t* buf, uint16_t len ) {
int i;
for (i=0; i<len; i++)
SPI.transfer(buf[i]);
}
void DecaDuino::spi_receive ( uint8_t* buf, uint16_t len ) {
int i;
for (i=0; i<len; i++)
buf[i] = SPI.transfer(0);
}
void DecaDuino::readSpi(uint8_t address, uint8_t* buf, uint16_t len) {
uint8_t addr = 0 | (address & 0x3F) ; // Mask register address (6bits) and preserve MSb at low (Read) and no subaddress
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
SPI.beginTransaction(currentSPISettings);
digitalWrite(_slaveSelectPin, LOW);
spi_send(addr);
spi_receive(buf,len);
digitalWrite(_slaveSelectPin, HIGH);
SPI.endTransaction();
}
}
void DecaDuino::readSpiSubAddress(uint8_t address, uint16_t subAddress, uint8_t* buf, uint16_t len) {
uint8_t addr, sub_addr;
addr = 0 | (address & 0x3F) | 0x40; // Mask register address (6bits), preserve MSb at low (Read) and set subaddress present bit (0x40)
if ( subAddress < 0x80 ) {
// This is a 2-bytes header SPI transaction
sub_addr = 0 | (subAddress & 0x7F); // Mask register address (6bits)
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
SPI.beginTransaction(currentSPISettings);
digitalWrite(_slaveSelectPin, LOW);
spi_send(addr);
spi_send(sub_addr);
spi_receive(buf,len);
digitalWrite(_slaveSelectPin, HIGH);
SPI.endTransaction();
}
} else {
// This is a 3-bytes header SPI transaction
/** @todo implement readSpiSubAddress in case of a 3-bytes header SPI transaction */
}
}
uint32_t DecaDuino::readSpiUint32(uint8_t address) {
uint8_t buf[4];
readSpi(address, buf, 4);
return decodeUint32(buf);
}
void DecaDuino::writeSpi(uint8_t address, uint8_t* buf, uint16_t len) {
uint8_t addr = 0 | (address & 0x3F) | 0x80; // Mask register address (6bits) and set MSb (Write) and no subaddress
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
SPI.beginTransaction(currentSPISettings);
digitalWrite(_slaveSelectPin, LOW);
spi_send(addr);
spi_send(buf,len);
digitalWrite(_slaveSelectPin, HIGH);
SPI.endTransaction();
}
}
void DecaDuino::writeSpiSubAddress(uint8_t address, uint16_t subAddress, uint8_t* buf, uint16_t len) {
uint8_t addr, sub_addr;
addr = 0 | (address & 0x3F) | 0x80 | 0x40; // Mask register address (6bits), set MSb (Write) and set subaddress present bit (0x40)
if ( subAddress < 0x80 ) {
// This is a 2-bytes header SPI transaction
sub_addr = 0 | (subAddress & 0x7F); // Mask register address (6bits)
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
SPI.beginTransaction(currentSPISettings);
digitalWrite(_slaveSelectPin, LOW);
spi_send(addr);
spi_send(sub_addr);
spi_send(buf,len);
digitalWrite(_slaveSelectPin, HIGH);
SPI.endTransaction();
}
} else {
// This is a 3-bytes header SPI transaction
/** @todo implement writeSpiSubAddress in case of a 3-bytes header SPI transaction */
}
}
void DecaDuino::writeSpiUint32(uint8_t address, uint32_t ui32t) {
uint8_t buf[4];
encodeUint32(ui32t, buf);
writeSpi(address, buf, 4);
}
uint16_t DecaDuino::decodeUint16 ( uint8_t *data ) {
return 0 | (data[1] << 8) | data[0];
}
void DecaDuino::encodeUint16 ( uint16_t from, uint8_t *to ) {
to[1] = (from & 0xFF00) >> 8;
to[0] = from & 0xFF;
}
uint32_t DecaDuino::decodeUint32 ( uint8_t *data ) {
return 0 | (data[3] << 24) | (data[2] << 16) | (data[1] << 8) | data[0];
}
void DecaDuino::encodeUint32 ( uint32_t from, uint8_t *to ) {
to[3] = (from & 0xFF000000) >> 24;
to[2] = (from & 0xFF0000) >> 16;
to[1] = (from & 0xFF00) >> 8;
to[0] = from & 0xFF;
}
uint64_t DecaDuino::decodeUint40 ( uint8_t *data )
{
uint64_t tmp = 0;
tmp = data[4];
tmp = tmp << 32;
tmp = tmp | decodeUint32(data);
return tmp;
}
void DecaDuino::encodeUint40 ( uint64_t from, uint8_t *to )
{
to[4] = (from & 0xFF00000000) >> 32;
to[3] = (from & 0xFF000000) >> 24;
to[2] = (from & 0xFF0000) >> 16;
to[1] = (from & 0xFF00) >> 8;
to[0] = from & 0xFF;
}
uint64_t DecaDuino::decodeUint64 ( uint8_t *data ) {
uint64_t tmp = 0;
tmp = decodeUint32(&data[4]); // | decodeUint32(data);
tmp = tmp << 32;
tmp = tmp | decodeUint32(data);
return tmp;
}
void DecaDuino::encodeUint64 ( uint64_t from, uint8_t *to ) {
to[7] = (from & 0xFF00000000000000) >> 56;
to[6] = (from & 0xFF000000000000) >> 48;
to[5] = (from & 0xFF0000000000) >> 40;
to[4] = (from & 0xFF00000000) >> 32;
to[3] = (from & 0xFF000000) >> 24;
to[2] = (from & 0xFF0000) >> 16;
to[1] = (from & 0xFF00) >> 8;
to[0] = from & 0xFF;
}
float DecaDuino::decodeFloat ( uint8_t *data ) {
typedef union _data {
float f;
char s[4];
} myData;
myData q;
q.s[0] = data[0];
q.s[1] = data[1];
q.s[2] = data[2];
q.s[3] = data[3];
return q.f;
}
void DecaDuino::encodeFloat ( float from, uint8_t *to ) {
typedef union _data {
float f;
char s[4];
} myData;
myData q;
q.f = from;
to[0] = q.s[0];
to[1] = q.s[1];
to[2] = q.s[2];
to[3] = q.s[3];
}
void DecaDuino::printUint64 ( uint64_t ui64 ) {
uint8_t buf[8];
encodeUint64(ui64, buf);
sprintf((char*)debugStr, "%08lx%08lx", decodeUint32(&buf[4]), decodeUint32(buf));
Serial.print((char*)debugStr);
}
void DecaDuino::getSystemTimeCounter ( uint64_t *p ) {
uint8_t buf[8];
encodeUint64(0, buf); // init buffer the 64-bit buffer
readSpi(DW1000_REGISTER_SYS_TIME, buf, 5);
*p = decodeUint64(buf);
}
void DecaDuino::setPHRMode(uint8_t mode) {
uint32_t ui32t;
ui32t = readSpiUint32(DW1000_REGISTER_SYS_CFG);
ui32t = ui32t & (~DW1000_REGISTER_SYS_CFG_PHR_MODE_MASK);
ui32t |= mode << DW1000_REGISTER_SYS_CFG_PHR_MODE_SHIFT;
writeSpiUint32(DW1000_REGISTER_SYS_CFG,ui32t);
}
uint8_t DecaDuino::getPHRMode(void) {
uint32_t ui32t;
ui32t = readSpiUint32(DW1000_REGISTER_SYS_CFG);
ui32t = (ui32t & DW1000_REGISTER_SYS_CFG_PHR_MODE_MASK) >> DW1000_REGISTER_SYS_CFG_PHR_MODE_SHIFT;
return (uint8_t)ui32t;
}
uint64_t DecaDuino::getSystemTimeCounter ( void ) {
uint64_t p;
getSystemTimeCounter(&p);
return p;
}
uint64_t DecaDuino::getLastTxTimestamp() {
return lastTxTimestamp;
}
uint64_t DecaDuino::getLastRxTimestamp() {
return lastRxTimestamp;
}
double DecaDuino::getLastRxSkew() {
return clkOffset;
}
uint16_t DecaDuino::getPanId() {
uint8_t buf[2];
readSpiSubAddress(DW1000_REGISTER_PANADR, DW1000_REGISTER_PANADR_PANID_OFFSET, buf, 2);
return decodeUint16(buf);
}
uint16_t DecaDuino::getShortAddress() {
uint8_t buf[2];
readSpiSubAddress(DW1000_REGISTER_PANADR, DW1000_REGISTER_PANADR_SHORT_ADDRESS_OFFSET, buf, 2);
return decodeUint16(buf);
}
uint64_t DecaDuino::getEuid() {
uint8_t buf[8];
readSpi(DW1000_REGISTER_EUI, buf, 8);
return decodeUint64(buf);
}
void DecaDuino::setPanId(uint16_t panId) {
uint8_t buf[2];
encodeUint16(panId, buf);
writeSpiSubAddress(DW1000_REGISTER_PANADR, DW1000_REGISTER_PANADR_PANID_OFFSET, buf, 2);
}
void DecaDuino::setShortAddress(uint16_t shortAddress) {
uint8_t buf[2];
encodeUint16(shortAddress, buf);
writeSpiSubAddress(DW1000_REGISTER_PANADR, DW1000_REGISTER_PANADR_SHORT_ADDRESS_OFFSET, buf, 2);
}
void DecaDuino::setShortAddressAndPanId(uint16_t shortAddress, uint16_t panId) {
setPanId(panId);
setShortAddress(shortAddress);
}
int DecaDuino::setShortAddressAndPanId(uint32_t shortAddressPanId) {
uint32_t ret;
writeSpiUint32(0x03, shortAddressPanId);
ret = readSpiUint32(0x03);
if ( ret != shortAddressPanId ) {
#ifdef DECADUINO_DEBUG
Serial.println("Setting Short Address and PanId OK\n");
#endif
return false;
} else {
#ifdef DECADUINO_DEBUG
Serial.println("Error while Setting Short Address and PanId\n");
#endif
return true;
}
}
uint8_t DecaDuino::getChannelRaw(void) {
uint8_t buf;
readSpiSubAddress(DW1000_REGISTER_CHAN_CTRL, 0, &buf, 1);
return buf;
}
uint8_t DecaDuino::getChannel(void) {
return getChannelRaw() & 0x0F;
}
uint8_t DecaDuino::getRxPrf(void) {
uint32_t ui32t;
ui32t = readSpiUint32(DW1000_REGISTER_CHAN_CTRL);
ui32t = ( ui32t & DW1000_REGISTER_CHAN_CTRL_RXPRF_MASK) >> 18;
return (uint8_t)ui32t;
}
uint8_t DecaDuino::getTxPcode(void) {
uint32_t ui32t;
ui32t = readSpiUint32(DW1000_REGISTER_CHAN_CTRL);
ui32t = ( ui32t & DW1000_REGISTER_CHAN_CTRL_TX_PCODE_MASK) >> 22;
return (uint8_t)ui32t;
}
uint8_t DecaDuino::getRxPcode(void) {
uint32_t ui32t;
ui32t = readSpiUint32(DW1000_REGISTER_CHAN_CTRL);
ui32t = ( ui32t & DW1000_REGISTER_CHAN_CTRL_RX_PCODE_MASK) >> 27;
return (uint8_t)ui32t;
}
bool DecaDuino::setChannel(uint8_t channel) {
uint32_t ui32t;
if ( ( channel != 6 ) && ( channel <= 7 ) && ( channel >= 1 ) ) {
channel = channel + (channel << 4);
ui32t = readSpiUint32(DW1000_REGISTER_CHAN_CTRL);
ui32t = ui32t & 0xFFFFFF00;
ui32t |= channel; // set rx and tx channel
writeSpiUint32(DW1000_REGISTER_CHAN_CTRL, ui32t);
if ( getChannelRaw() == channel )
return true;
}
return false;
}
bool DecaDuino::setRxPrf(uint8_t prf) {
uint32_t ui32t;
if ( ( prf == 1 ) || ( prf == 2 ) ) {
ui32t = readSpiUint32(DW1000_REGISTER_CHAN_CTRL);
ui32t = ui32t & (~DW1000_REGISTER_CHAN_CTRL_RXPRF_MASK);
ui32t |= prf << 18;
writeSpiUint32(DW1000_REGISTER_CHAN_CTRL, ui32t);
return true;
} else return false;
}
bool DecaDuino::setTxPcode(uint8_t pcode) {
uint32_t ui32t;
if ( ( pcode > 0 ) && ( pcode <= 20 ) ) {
ui32t = readSpiUint32(DW1000_REGISTER_CHAN_CTRL);
ui32t = ui32t & (~DW1000_REGISTER_CHAN_CTRL_TX_PCODE_MASK);
ui32t |= pcode << 22;
writeSpiUint32(DW1000_REGISTER_CHAN_CTRL, ui32t);
return true;
} else return false;
}
bool DecaDuino::setRxPcode(uint8_t pcode) {
uint32_t ui32t;
if ( ( pcode > 0 ) && ( pcode <= 20 ) ) {
ui32t = readSpiUint32(DW1000_REGISTER_CHAN_CTRL);
ui32t = ui32t & (~DW1000_REGISTER_CHAN_CTRL_RX_PCODE_MASK);
ui32t |= pcode << 27;
writeSpiUint32(DW1000_REGISTER_CHAN_CTRL, ui32t);
return true;
} else return false;
}
int DecaDuino::getPreambleLength(void) {
uint32_t ui32t;
uint32_t mask;
int plength;
mask = 0x003C0000; // bits 21, 20, 19 and 18 from DW1000_REGISTER_TX_FCTRL
ui32t = readSpiUint32(DW1000_REGISTER_TX_FCTRL);
ui32t = ui32t & mask;
ui32t = ui32t >> 18;
// Preamble length selection (Table 15, Page 73 from DW1000 User Manual)
switch(ui32t){
case 0x00000001:
plength = 64;
break;
case 0x00000005:
plength = 128;
break;
case 0x00000009:
plength = 256;
break;
case 0x0000000D:
plength = 512;
break;
case 0x00000002:
plength = 1024;
break;
case 0x00000006:
plength = 1536;
break;
case 0x0000000A:
plength = 2048;
break;
case 0x00000003:
plength = 4096;
break;
default:
plength = 128;
break;
}
#ifdef DECADUINO_DEBUG
sprintf((char*)debugStr,"TX_FCTRL=%08x\n", ui32t);
Serial.print((char*)debugStr);
#endif
return plength;
}
bool DecaDuino::setPreambleLength (int plength) {
uint32_t ui32t;
uint32_t mask;
switch(plength){
case 64:
mask = 0x00040000;
break;
case 128:
mask = 0x00140000;
break;
case 256:
mask = 0x00240000;
break;
case 512:
mask = 0x00340000;
break;
case 1024:
mask = 0x00080000;
break;
case 1536:
mask = 0x00180000;
break;
case 2048:
mask = 0x00280000;
break;
case 4096:
mask = 0x000C0000;
break;
default:
return false;
}
ui32t = readSpiUint32(DW1000_REGISTER_TX_FCTRL);
ui32t = ui32t & 0xFFC3FFFF; // bits 21, 20, 19, 18 to zero
ui32t |= mask;
writeSpiUint32(DW1000_REGISTER_TX_FCTRL, ui32t);
return true;
}
uint16_t DecaDuino::getAntennaDelay() {
return antennaDelay;
}
void DecaDuino::setAntennaDelay(uint16_t newAntennaDelay) {
setAntennaDelayReg(newAntennaDelay);
antennaDelay = newAntennaDelay;
}
uint16_t DecaDuino::getAntennaDelayReg(){
uint8_t buf[2];
readSpi(DW1000_REGISTER_TX_ANTD, buf, 2);
return decodeUint16(buf);
}
void DecaDuino::setAntennaDelayReg(uint16_t newAntennaDelay) {
uint8_t buf[2];
encodeUint16(newAntennaDelay, buf);
writeSpi(DW1000_REGISTER_TX_ANTD, buf, 2);
}
uint8_t DecaDuino::getTemperatureRaw() {
uint8_t u8t;
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
u8t = 0x80; writeSpiSubAddress(0x28, 0x11, &u8t, 1); // 1. Write Sub-Register 28:11 1byte 0x80
u8t = 0x0A; writeSpiSubAddress(0x28, 0x12, &u8t, 1); // 2. Write Sub-Register 28:12 1byte 0x0A
u8t = 0x0F; writeSpiSubAddress(0x28, 0x12, &u8t, 1); // 3. Write Sub-Register 28:12 1byte 0x0F
u8t = 0x01; writeSpiSubAddress(0x2A, 0x00, &u8t, 1); // 4. Write Register 2A:00 1byte 0x01
u8t = 0x00; writeSpiSubAddress(0x2A, 0x00, &u8t, 1); // 5. Write Register 2A:00 1byte 0x00
readSpiSubAddress(0x2A, 0x04, &u8t, 1); // 6. Read Register 2A:04 1byte 8 bit Temperature reading
}
return u8t;
}
uint8_t DecaDuino::getVoltageRaw() {
uint8_t u8t;
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
u8t = 0x80; writeSpiSubAddress(0x28, 0x11, &u8t, 1); // 1. Write Sub-Register 28:11 1byte 0x80
u8t = 0x0A; writeSpiSubAddress(0x28, 0x12, &u8t, 1); // 2. Write Sub-Register 28:12 1byte 0x0A
u8t = 0x0F; writeSpiSubAddress(0x28, 0x12, &u8t, 1); // 3. Write Sub-Register 28:12 1byte 0x0F
u8t = 0x01; writeSpiSubAddress(0x2A, 0x00, &u8t, 1); // 4. Write Register 2A:00 1byte 0x01
u8t = 0x00; writeSpiSubAddress(0x2A, 0x00, &u8t, 1); // 5. Write Register 2A:00 1byte 0x00
readSpiSubAddress(0x2A, 0x03, &u8t, 1); // 6. Read Register 2A:03 1byte 8 bit Voltage reading
}
return u8t;
}
float DecaDuino::getTemperature(void) {
// Temperature (°C )= (SAR_LTEMP - (OTP_READ(Vtemp @ 23°C )) x 1.14) + 23
// Todo: what is OTP_READ(Vtemp @ 23°C ) ?
return 0;
}
float DecaDuino::getVoltage(void) {
// Voltage (volts) = (SAR_LVBAT- (OTP_READ(Vmeas @ 3.3 V )) /173) + 3.3
// Todo: what is OTP_READ(Vmeas @ 3.3 V ) ?
return 0;
}
void DecaDuino::sleepRequest(void) {
uint8_t ui8t;
#ifdef DECADUINO_DEBUG
sprintf((char*)debugStr,"sleep request");
Serial.println((char*)debugStr);
#endif
readSpiSubAddress(DW1000_REGISTER_AON_CFG0, DW1000_REGISTER_OFFSET_AON_CFG0, &ui8t, 1);
ui8t |= DW1000_REGISTER_AON_CFG0_SLEEP_EN_MASK;
writeSpiSubAddress(DW1000_REGISTER_AON_CFG0, DW1000_REGISTER_OFFSET_AON_CFG0, &ui8t, 1);
readSpiSubAddress(DW1000_REGISTER_AON_CTRL, DW1000_REGISTER_OFFSET_AON_CTRL, &ui8t, 1);
ui8t |= DW1000_REGISTER_AON_CTRL_UPL_CFG_MASK;
writeSpiSubAddress(DW1000_REGISTER_AON_CTRL, DW1000_REGISTER_OFFSET_AON_CTRL, &ui8t, 1);
delay(1);
// The DWM1000 is now sleepping
trxStatus = DW1000_TRX_STATUS_SLEEP;
}
Reference in a new issue Copy permalink