Tobias Naegeli / Mbed 2 deprecated AIT_UWB_Range

Tobi's ubw test branch

Dependencies:   mavlink_bridge mbed

Fork of AIT_UWB_Range by Benjamin Hepp

main.cpp@46:6398237672a0, 2015-03-10 (annotated)

Committer:
manumaet
Date:
Tue Mar 10 12:11:13 2015 +0000
Revision:
46:6398237672a0
Parent:
45:01a33363bc21
Child:
47:b6120c152ad1
Final commit with altCallbackRX for the testcase and improved power.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manumaet 24:6f25ba679490 1 // by Matthias Grob & Manuel Stalder - ETH Zürich - 2015
manumaet 0:f50e671ffff7 2 #include "mbed.h"
manumaet 26:a65c6f26c458 3 #include "PC.h" // Serial Port via USB for debugging with Terminal
manumaet 27:71178fdb78e1 4 #include "DW1000.h" // our DW1000 device driver
manumaet 44:2e0045042a59 5 #include "MM2WayRanging.h" // our self developed ranging application
manumaet 44:2e0045042a59 6
manumaet 44:2e0045042a59 7 #define myprintf pc.printf // to make the code adaptable to other outputs that support printf
manumaet 44:2e0045042a59 8
manumaet 44:2e0045042a59 9 PC pc(USBTX, USBRX, 921600); // USB UART Terminal
manumaet 44:2e0045042a59 10 DW1000 dw(PA_7, PA_6, PA_5, PB_6, PB_9); // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ)
manumaet 44:2e0045042a59 11 MM2WayRanging node(dw); // Instance of the two way ranging algorithm
manumaet 0:f50e671ffff7 12
manumaet 44:2e0045042a59 13 void rangeAndDisplayAll(){
manumaet 44:2e0045042a59 14 node.requestRangingAll(); // Request ranging to all anchors
manumaet 45:01a33363bc21 15 for (int i = 1; i <= 5; i++) { // Output Results
manumaet 45:01a33363bc21 16 myprintf("%f, ", node.distances[i]);
manumaet 45:01a33363bc21 17 //myprintf("T:%f", node.roundtriptimes[i]);
manumaet 45:01a33363bc21 18 //myprintf("\r\n");
manumaet 44:2e0045042a59 19 }
manumaet 45:01a33363bc21 20 myprintf("\r\n");
manumaet 44:2e0045042a59 21 }
manumaet 6:d5864a1b9e17 22
manumaet 45:01a33363bc21 23 void calibrationRanging(int destination){
manumaet 45:01a33363bc21 24
manumaet 45:01a33363bc21 25 const int numberOfRangings = 100;
manumaet 45:01a33363bc21 26 float rangings[numberOfRangings];
manumaet 45:01a33363bc21 27 int index = 0;
manumaet 45:01a33363bc21 28 float mean = 0;
manumaet 45:01a33363bc21 29 float start, stop;
manumaet 45:01a33363bc21 30
manumaet 45:01a33363bc21 31 Timer localTimer;
manumaet 45:01a33363bc21 32 localTimer.start();
manumaet 45:01a33363bc21 33
manumaet 45:01a33363bc21 34 start = localTimer.read();
manumaet 45:01a33363bc21 35
manumaet 45:01a33363bc21 36 while (1) {
manumaet 45:01a33363bc21 37
manumaet 45:01a33363bc21 38 node.requestRanging(destination);
manumaet 45:01a33363bc21 39 if(node.overflow){
manumaet 45:01a33363bc21 40 myprintf("Overflow! Measured: %f\r\n", node.distances[destination]);
manumaet 45:01a33363bc21 41 }
manumaet 45:01a33363bc21 42
manumaet 45:01a33363bc21 43 if (node.distances[destination] == -1) {
manumaet 45:01a33363bc21 44 myprintf("Measurement timed out\r\n");
manumaet 45:01a33363bc21 45 wait(0.001);
manumaet 45:01a33363bc21 46 continue;
manumaet 45:01a33363bc21 47 }
manumaet 45:01a33363bc21 48
manumaet 45:01a33363bc21 49 if (node.distances[destination] < 100) {
manumaet 45:01a33363bc21 50 rangings[index] = node.distances[destination];
manumaet 45:01a33363bc21 51 //myprintf("%f\r\n", node.distances[destination]);
manumaet 45:01a33363bc21 52 index++;
manumaet 45:01a33363bc21 53
manumaet 45:01a33363bc21 54 if (index == numberOfRangings) {
manumaet 45:01a33363bc21 55 stop = localTimer.read();
manumaet 45:01a33363bc21 56
manumaet 45:01a33363bc21 57 for (int i = 0; i < numberOfRangings - 1; i++)
manumaet 45:01a33363bc21 58 rangings[numberOfRangings - 1] += rangings[i];
manumaet 45:01a33363bc21 59
manumaet 45:01a33363bc21 60 mean = rangings[numberOfRangings - 1] / numberOfRangings;
manumaet 45:01a33363bc21 61 myprintf("\r\n\r\nMean %i: %f\r\n", destination, mean);
manumaet 45:01a33363bc21 62 myprintf("Elapsed Time for %i: %f\r\n", numberOfRangings, stop - start);
manumaet 45:01a33363bc21 63
manumaet 45:01a33363bc21 64 mean = 0;
manumaet 45:01a33363bc21 65 index = 0;
manumaet 45:01a33363bc21 66
manumaet 45:01a33363bc21 67 //wait(2);
manumaet 45:01a33363bc21 68
manumaet 45:01a33363bc21 69 start = localTimer.read();
manumaet 45:01a33363bc21 70 }
manumaet 45:01a33363bc21 71 }
manumaet 45:01a33363bc21 72
manumaet 45:01a33363bc21 73 else myprintf("%f\r\n", node.distances[destination]);
manumaet 45:01a33363bc21 74
manumaet 46:6398237672a0 75 }
manumaet 46:6398237672a0 76
manumaet 46:6398237672a0 77 }
manumaet 45:01a33363bc21 78
manumaet 46:6398237672a0 79 struct __attribute__((packed, aligned(1))) DistancesFrame {
manumaet 46:6398237672a0 80 uint8_t source;
manumaet 46:6398237672a0 81 uint8_t destination;
manumaet 46:6398237672a0 82 uint8_t type;
manumaet 46:6398237672a0 83 float dist[4];
manumaet 46:6398237672a0 84 };
manumaet 46:6398237672a0 85
manumaet 45:01a33363bc21 86
manumaet 46:6398237672a0 87
manumaet 46:6398237672a0 88 void altCallbackRX();
manumaet 45:01a33363bc21 89 // -----------------------------------------------------------------------------------------------
manumaet 45:01a33363bc21 90
manumaet 0:f50e671ffff7 91 int main() {
manumaet 44:2e0045042a59 92 pc.printf("\r\nDecaWave 1.0 up and running!\r\n"); // Splashscreen
manumaet 30:4ecc69d3cf8d 93 dw.setEUI(0xFAEDCD01FAEDCD01); // basic methods called to check if we have a working SPI connection
manumaet 28:a830131560e8 94 pc.printf("DEVICE_ID register: 0x%X\r\n", dw.getDeviceID());
manumaet 28:a830131560e8 95 pc.printf("EUI register: %016llX\r\n", dw.getEUI());
manumaet 38:8ef3b8d8b908 96 pc.printf("Voltage: %fV\r\n", dw.getVoltage());
manumaet 34:f56962030c5c 97
manumaet 46:6398237672a0 98 node.isAnchor = true;
manumaet 45:01a33363bc21 99 // declare as anchor or beacon
manumaet 44:2e0045042a59 100 if (node.isAnchor) {
manumaet 46:6398237672a0 101 node.address = 5;
manumaet 44:2e0045042a59 102 myprintf("This node is Anchor node %d \r\n", node.address);
manumaet 44:2e0045042a59 103 } else {
manumaet 44:2e0045042a59 104 node.address = 0;
manumaet 44:2e0045042a59 105 myprintf("This node is a Beacon. ");
manumaet 44:2e0045042a59 106 }
manumaet 32:041dd02e0e3b 107
manumaet 46:6398237672a0 108 if (node.address == 5){
manumaet 46:6398237672a0 109 dw.setCallbacks(&altCallbackRX, NULL);
manumaet 46:6398237672a0 110 }
manumaet 46:6398237672a0 111
manumaet 0:f50e671ffff7 112 while(1) {
manumaet 45:01a33363bc21 113 if (!node.isAnchor){
manumaet 44:2e0045042a59 114 rangeAndDisplayAll();
manumaet 45:01a33363bc21 115 //calibrationRanging(4);
manumaet 45:01a33363bc21 116
manumaet 45:01a33363bc21 117
manumaet 45:01a33363bc21 118
manumaet 46:6398237672a0 119
manumaet 46:6398237672a0 120
manumaet 45:01a33363bc21 121 }else
manumaet 46:6398237672a0 122 //myprintf(".\r");
manumaet 45:01a33363bc21 123
manumaet 46:6398237672a0 124 wait(0.5);
manumaet 0:f50e671ffff7 125 }
manumaet 46:6398237672a0 126 }
manumaet 46:6398237672a0 127
manumaet 46:6398237672a0 128
manumaet 46:6398237672a0 129 void altCallbackRX(){
manumaet 46:6398237672a0 130
manumaet 46:6398237672a0 131 DistancesFrame distFrame;
manumaet 46:6398237672a0 132 float distances[4];
manumaet 46:6398237672a0 133 dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&distFrame, dw.getFramelength());
manumaet 46:6398237672a0 134
manumaet 46:6398237672a0 135 if (distFrame.destination == 5){
manumaet 46:6398237672a0 136 for (int i = 0; i<4; i++){
manumaet 46:6398237672a0 137 distances[i] = distFrame.dist[i];
manumaet 46:6398237672a0 138 }
manumaet 46:6398237672a0 139
manumaet 46:6398237672a0 140 for (int i = 0; i < 4; i++) { // Output Results
manumaet 46:6398237672a0 141 myprintf("%f, ", distances[i]);
manumaet 46:6398237672a0 142
manumaet 46:6398237672a0 143 }
manumaet 46:6398237672a0 144
manumaet 46:6398237672a0 145 myprintf("\r\n");
manumaet 46:6398237672a0 146 }
manumaet 46:6398237672a0 147 dw.startRX();
manumaet 46:6398237672a0 148 }