Tobias Naegeli
Tobi's ubw test branch
Dependencies: mavlink_bridge mbed
Fork of
AIT_UWB_Range
by 
Benjamin Hepp
Diff: DW1000/DW1000.cpp
- Revision:
- 67:bd0f0580af5a
- Parent:
- 65:4c3bd79b57d2
--- a/DW1000/DW1000.cpp Tue Jan 05 10:16:21 2016 +0000
+++ b/DW1000/DW1000.cpp Tue Jan 05 17:06:19 2016 +0000
@@ -6,14 +6,15 @@
//#include "PC.h"
//static PC pc(USBTX, USBRX, 115200); // USB UART Terminal
-DW1000::DW1000(SPI& spi, InterruptIn& irq, PinName CS, PinName RESET) : spi(spi), cs(CS), irq(irq), reset(RESET) {
+DW1000::DW1000(SPI& spi, InterruptIn& irq, PinName CS, PinName RESET) : spi(spi), cs(CS), irq(irq), reset(RESET)
+{
setCallbacks(NULL, NULL);
deselect();
//wait(2);
- select();
-
+ select();
+
//wait(2);
- deselect();
+ deselect();
//wait(2); // Chip must be deselected first
resetAll(); // we do a soft reset of the DW1000 everytime the driver starts
@@ -56,7 +57,7 @@
writeRegister8 (DW1000_FS_CTRL, 0x0B, 0xA6); //FS_PLLTUNE for channel 5
loadLDE(); // important everytime DW1000 initialises/awakes otherwise the LDE algorithm must be turned off or there's receiving malfunction see User Manual LDELOAD on p22 & p158
-
+
// 110kbps CAUTION: a lot of other registers have to be set for an optimized operation on 110kbps
writeRegister16(DW1000_TX_FCTRL, 1, 0x0800 | 0x0100 | 0x0080); // use 1024 symbols preamble (0x0800) (previously 2048 - 0x2800), 16MHz pulse repetition frequency (0x0100), 110kbps bit rate (0x0080) see p.69 of DW1000 User Manual
writeRegister8(DW1000_SYS_CFG, 2, 0x44); // enable special receiving option for 110kbps (disable smartTxPower)!! (0x44) see p.64 of DW1000 User Manual [DO NOT enable 1024 byte frames (0x03) becuase it generates disturbance of ranging don't know why...]
@@ -67,7 +68,8 @@
writeRegister8(DW1000_SYS_CFG, 3, 0x20); // enable auto reenabling receiver after error
}
-void DW1000::setCallbacks(void (*callbackRX)(void), void (*callbackTX)(void)) {
+void DW1000::setCallbacks(void (*callbackRX)(void), void (*callbackTX)(void))
+{
bool RX = false;
bool TX = false;
if (callbackRX) {
@@ -81,23 +83,27 @@
setInterrupt(RX, TX);
}
-uint32_t DW1000::getDeviceID() {
+uint32_t DW1000::getDeviceID()
+{
uint32_t result;
readRegister(DW1000_DEV_ID, 0, (uint8_t*)&result, 4);
return result;
}
-uint64_t DW1000::getEUI() {
+uint64_t DW1000::getEUI()
+{
uint64_t result;
readRegister(DW1000_EUI, 0, (uint8_t*)&result, 8);
return result;
}
-void DW1000::setEUI(uint64_t EUI) {
+void DW1000::setEUI(uint64_t EUI)
+{
writeRegister(DW1000_EUI, 0, (uint8_t*)&EUI, 8);
}
-float DW1000::getVoltage() {
+float DW1000::getVoltage()
+{
uint8_t buffer[7] = {0x80, 0x0A, 0x0F, 0x01, 0x00}; // algorithm form User Manual p57
writeRegister(DW1000_RF_CONF, 0x11, buffer, 2);
writeRegister(DW1000_RF_CONF, 0x12, &buffer[2], 1);
@@ -109,49 +115,60 @@
return Voltage;
}
-uint64_t DW1000::getStatus() {
+uint64_t DW1000::getStatus()
+{
return readRegister40(DW1000_SYS_STATUS, 0);
}
-uint64_t DW1000::getRXTimestamp() {
+uint64_t DW1000::getRXTimestamp()
+{
return readRegister40(DW1000_RX_TIME, 0);
}
-uint64_t DW1000::getTXTimestamp() {
+uint64_t DW1000::getTXTimestamp()
+{
return readRegister40(DW1000_TX_TIME, 0);
}
-uint16_t DW1000::getStdNoise() {
+uint16_t DW1000::getStdNoise()
+{
return readRegister16(DW1000_RX_FQUAL, 0x00);
}
-uint16_t DW1000::getPACC() {
+uint16_t DW1000::getPACC()
+{
uint32_t v = readRegister32(DW1000_RX_FINFO, 0x00);
v >>= 20;
return static_cast<uint16_t>(v);
}
-uint16_t DW1000::getFPINDEX() {
+uint16_t DW1000::getFPINDEX()
+{
return readRegister16(DW1000_RX_TIME, 0x05);
}
-uint16_t DW1000::getFPAMPL1() {
+uint16_t DW1000::getFPAMPL1()
+{
return readRegister16(DW1000_RX_TIME, 0x07);
}
-uint16_t DW1000::getFPAMPL2() {
+uint16_t DW1000::getFPAMPL2()
+{
return readRegister16(DW1000_RX_FQUAL, 0x02);
}
-uint16_t DW1000::getFPAMPL3() {
+uint16_t DW1000::getFPAMPL3()
+{
return readRegister16(DW1000_RX_FQUAL, 0x04);
}
-uint16_t DW1000::getCIRPWR() {
+uint16_t DW1000::getCIRPWR()
+{
return readRegister16(DW1000_RX_FQUAL, 0x06);
}
-uint8_t DW1000::getPRF() {
+uint8_t DW1000::getPRF()
+{
uint16_t prf_mask = (0x1 << 19) | (0x1 << 18);
uint16_t prf = readRegister16(DW1000_CHAN_CTRL, 0x00);
prf >> 18;
@@ -159,15 +176,18 @@
return static_cast<uint8_t>(prf);
}
-void DW1000::sendString(char* message) {
+void DW1000::sendString(char* message)
+{
sendFrame((uint8_t*)message, strlen(message)+1);
}
-void DW1000::receiveString(char* message) {
+void DW1000::receiveString(char* message)
+{
readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)message, getFramelength()); // get data from buffer
}
-void DW1000::sendFrame(uint8_t* message, uint16_t length) {
+void DW1000::sendFrame(uint8_t* message, uint16_t length)
+{
//if (length >= 1021) length = 1021; // check for maximim length a frame can have with 1024 Byte frames [not used, see constructor]
if (length >= 125) length = 125; // check for maximim length a frame can have with 127 Byte frames
writeRegister(DW1000_TX_BUFFER, 0, message, length); // fill buffer
@@ -176,12 +196,34 @@
length = ((backup & 0xFC) << 8) | (length & 0x03FF);
writeRegister16(DW1000_TX_FCTRL, 0, length);
stopTRX(); // stop receiving
- writeRegister8(DW1000_SYS_CTRL, 0, 0x02); // trigger sending process by setting the TXSTRT bit
+ writeRegister8(DW1000_SYS_CTRL, 0, 0x02); // trigger sending process by setting the TXSTRT bit // send ping 1.
//wait(0.1);
- startRX(); // enable receiver again
+ startRX();
+ int status1 = 0;
+ int status2 = 0;
+ for (int i=1; i<1000; i++) {
+ uint64_t status = getStatus();
+ if ((status & 0x4000)&&(status1==0)) {// a frame was received
+ //printf("bingo :-) %d \n\r",i) ;
+ callbackRX.call();
+ writeRegister16(DW1000_SYS_STATUS, 0, 0x6F00);
+ //status1=1;
+ //break;
+ //callbackRX.call();
+ //writeRegister16(DW1000_SYS_STATUS, 0, 0x6F00); // clearing of receiving status bits
+ }
+ if ((status & 0x80)&&(status2==0)) { // sending complete
+ callbackTX.call();
+ writeRegister8(DW1000_SYS_STATUS, 0, 0xF8); // clearing of sending status bits
+ //status2=2;
+ //break;
+ }
+ //printf("irq_index: %d \n\r",this->irq_index);
+ } // enable receiver again
}
-void DW1000::sendDelayedFrame(uint8_t* message, uint16_t length, uint64_t TxTimestamp) {
+void DW1000::sendDelayedFrame(uint8_t* message, uint16_t length, uint64_t TxTimestamp)
+{
//if (length >= 1021) length = 1021; // check for maximim length a frame can have with 1024 Byte frames [not used, see constructor]
if (length >= 125) length = 125; // check for maximim length a frame can have with 127 Byte frames
writeRegister(DW1000_TX_BUFFER, 0, message, length); // fill buffer
@@ -198,28 +240,33 @@
startRX(); // enable receiver again
}
-void DW1000::startRX() {
+void DW1000::startRX()
+{
writeRegister8(DW1000_SYS_CTRL, 0x01, 0x01); // start listening for preamble by setting the RXENAB bit
}
-void DW1000::stopTRX() {
+void DW1000::stopTRX()
+{
writeRegister8(DW1000_SYS_CTRL, 0, 0x40); // disable tranceiver go back to idle mode
}
// PRIVATE Methods ------------------------------------------------------------------------------------
-void DW1000::loadLDE() { // initialise LDE algorithm LDELOAD User Manual p22
+void DW1000::loadLDE() // initialise LDE algorithm LDELOAD User Manual p22
+{
writeRegister16(DW1000_PMSC, 0, 0x0301); // set clock to XTAL so OTP is reliable
writeRegister16(DW1000_OTP_IF, 0x06, 0x8000); // set LDELOAD bit in OTP
wait_us(150);
writeRegister16(DW1000_PMSC, 0, 0x0200); // recover to PLL clock
}
-void DW1000::resetRX() {
+void DW1000::resetRX()
+{
writeRegister8(DW1000_PMSC, 3, 0xE0); // set RX reset
writeRegister8(DW1000_PMSC, 3, 0xF0); // clear RX reset
}
-void DW1000::hardwareReset(PinName reset_pin) {
+void DW1000::hardwareReset(PinName reset_pin)
+{
// DWM1000 RESET logic.
if (DWM1000_DAMAGED) {
// The following code works for damaged DWM1000 modules.
@@ -242,7 +289,8 @@
}
}
-void DW1000::resetAll() {
+void DW1000::resetAll()
+{
if (reset.is_connected()) {
reset = 1;
wait_ms(100);
@@ -259,11 +307,13 @@
}
-void DW1000::setInterrupt(bool RX, bool TX) {
+void DW1000::setInterrupt(bool RX, bool TX)
+{
writeRegister16(DW1000_SYS_MASK, 0, RX*0x4000 | TX*0x0080); // RX good frame 0x4000, TX done 0x0080
}
-void DW1000::resetInterruptFlags() {
+void DW1000::resetInterruptFlags()
+{
uint64_t status = getStatus();
if (status & 0x4000) { // a frame was received
writeRegister16(DW1000_SYS_STATUS, 0, 0x6F00); // clearing of receiving status bits
@@ -273,88 +323,101 @@
}
}
-void DW1000::ISR() {
- uint64_t status = getStatus();
- if (status & 0x4000) { // a frame was received
- callbackRX.call();
- writeRegister16(DW1000_SYS_STATUS, 0, 0x6F00); // clearing of receiving status bits
- }
- if (status & 0x80) { // sending complete
- callbackTX.call();
- writeRegister8(DW1000_SYS_STATUS, 0, 0xF8); // clearing of sending status bits
- }
- //printf("irq_index: %d \n\r",this->irq_index);
+void DW1000::ISR()
+{
+ //uint64_t status = getStatus();
+// if (status & 0x4000) { // a frame was received
+// callbackRX.call();
+// writeRegister16(DW1000_SYS_STATUS, 0, 0x6F00); // clearing of receiving status bits
+// }
+// if (status & 0x80) { // sending complete
+// callbackTX.call();
+// writeRegister8(DW1000_SYS_STATUS, 0, 0xF8); // clearing of sending status bits
+// }
+ //printf("irq_index: %d \n\r",this->irq_index);
}
-uint16_t DW1000::getFramelength() {
+uint16_t DW1000::getFramelength()
+{
uint16_t framelength = readRegister16(DW1000_RX_FINFO, 0); // get framelength
framelength = (framelength & 0x03FF) - 2; // take only the right bits and subtract the 2 CRC Bytes
return framelength;
}
// SPI Interface ------------------------------------------------------------------------------------
-uint8_t DW1000::readRegister8(uint8_t reg, uint16_t subaddress) {
+uint8_t DW1000::readRegister8(uint8_t reg, uint16_t subaddress)
+{
uint8_t result;
readRegister(reg, subaddress, &result, 1);
return result;
}
-uint16_t DW1000::readRegister16(uint8_t reg, uint16_t subaddress) {
+uint16_t DW1000::readRegister16(uint8_t reg, uint16_t subaddress)
+{
uint16_t result;
readRegister(reg, subaddress, (uint8_t*)&result, 2);
return result;
}
-uint32_t DW1000::readRegister32(uint8_t reg, uint16_t subaddress) {
+uint32_t DW1000::readRegister32(uint8_t reg, uint16_t subaddress)
+{
uint32_t result;
readRegister(reg, subaddress, (uint8_t*)&result, 4);
return result;
}
-uint64_t DW1000::readRegister40(uint8_t reg, uint16_t subaddress) {
+uint64_t DW1000::readRegister40(uint8_t reg, uint16_t subaddress)
+{
uint64_t result;
readRegister(reg, subaddress, (uint8_t*)&result, 5);
result &= 0xFFFFFFFFFF; // only 40-Bit
return result;
}
-void DW1000::writeRegister8(uint8_t reg, uint16_t subaddress, uint8_t buffer) {
+void DW1000::writeRegister8(uint8_t reg, uint16_t subaddress, uint8_t buffer)
+{
writeRegister(reg, subaddress, &buffer, 1);
}
-void DW1000::writeRegister16(uint8_t reg, uint16_t subaddress, uint16_t buffer) {
+void DW1000::writeRegister16(uint8_t reg, uint16_t subaddress, uint16_t buffer)
+{
writeRegister(reg, subaddress, (uint8_t*)&buffer, 2);
}
-void DW1000::writeRegister32(uint8_t reg, uint16_t subaddress, uint32_t buffer) {
+void DW1000::writeRegister32(uint8_t reg, uint16_t subaddress, uint32_t buffer)
+{
writeRegister(reg, subaddress, (uint8_t*)&buffer, 4);
}
-void DW1000::writeRegister40(uint8_t reg, uint16_t subaddress, uint64_t buffer) {
+void DW1000::writeRegister40(uint8_t reg, uint16_t subaddress, uint64_t buffer)
+{
writeRegister(reg, subaddress, (uint8_t*)&buffer, 5);
}
-void DW1000::readRegister(uint8_t reg, uint16_t subaddress, uint8_t *buffer, int length) {
+void DW1000::readRegister(uint8_t reg, uint16_t subaddress, uint8_t *buffer, int length)
+{
setupTransaction(reg, subaddress, false);
for(int i=0; i<length; i++) // get data
buffer[i] = spi.write(0x00);
deselect();
}
-void DW1000::writeRegister(uint8_t reg, uint16_t subaddress, uint8_t *buffer, int length) {
+void DW1000::writeRegister(uint8_t reg, uint16_t subaddress, uint8_t *buffer, int length)
+{
setupTransaction(reg, subaddress, true);
for(int i=0; i<length; i++) // put data
spi.write(buffer[i]);
deselect();
}
-void DW1000::setupTransaction(uint8_t reg, uint16_t subaddress, bool write) {
+void DW1000::setupTransaction(uint8_t reg, uint16_t subaddress, bool write)
+{
reg |= (write * DW1000_WRITE_FLAG); // set read/write flag
select();
if (subaddress > 0) { // there's a subadress, we need to set flag and send second header byte
spi.write(reg | DW1000_SUBADDRESS_FLAG);
if (subaddress > 0x7F) { // sub address too long, we need to set flag and send third header byte
- spi.write((uint8_t)(subaddress & 0x7F) | DW1000_2_SUBADDRESS_FLAG); // and
+ spi.write((uint8_t)(subaddress & 0x7F) | DW1000_2_SUBADDRESS_FLAG); // and
spi.write((uint8_t)(subaddress >> 7));
} else {
spi.write((uint8_t)subaddress);
@@ -364,23 +427,27 @@
}
}
-void DW1000::select() { // always called to start an SPI transmission
+void DW1000::select() // always called to start an SPI transmission
+{
irq.disable_irq();
cs = 0; // set Cable Select pin low to start transmission
}
-void DW1000::deselect() { // always called to end an SPI transmission
+void DW1000::deselect() // always called to end an SPI transmission
+{
cs = 1; // set Cable Select pin high to stop transmission
irq.enable_irq();
}
-void DW1000::enable_irq() { // always called to start an SPI transmission
+void DW1000::enable_irq() // always called to start an SPI transmission
+{
//printf("Enabling irq %d\r\n", irq_index);
//irq_mp.enableCallback(irq_index);
//irq_mp.enable_irq();
}
-void DW1000::disable_irq() { // always called to end an SPI transmission
+void DW1000::disable_irq() // always called to end an SPI transmission
+{
//irq_mp.disableCallback(irq_index);
//irq_mp.disable_irq();
}
