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@45:01a33363bc21, 2015-03-08 (annotated)

Committer:
manumaet
Date:
Sun Mar 08 15:59:14 2015 +0000
Revision:
45:01a33363bc21
Parent:
44:2e0045042a59
Child:
46:6398237672a0
Added initialization values & calibration data.

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 45:01a33363bc21 75 }
manumaet 45:01a33363bc21 76
manumaet 45:01a33363bc21 77 }
manumaet 45:01a33363bc21 78
manumaet 45:01a33363bc21 79 // -----------------------------------------------------------------------------------------------
manumaet 45:01a33363bc21 80
manumaet 0:f50e671ffff7 81 int main() {
manumaet 44:2e0045042a59 82 pc.printf("\r\nDecaWave 1.0 up and running!\r\n"); // Splashscreen
manumaet 30:4ecc69d3cf8d 83 dw.setEUI(0xFAEDCD01FAEDCD01); // basic methods called to check if we have a working SPI connection
manumaet 28:a830131560e8 84 pc.printf("DEVICE_ID register: 0x%X\r\n", dw.getDeviceID());
manumaet 28:a830131560e8 85 pc.printf("EUI register: %016llX\r\n", dw.getEUI());
manumaet 38:8ef3b8d8b908 86 pc.printf("Voltage: %fV\r\n", dw.getVoltage());
manumaet 34:f56962030c5c 87
manumaet 45:01a33363bc21 88 node.isAnchor = false;
manumaet 45:01a33363bc21 89 // declare as anchor or beacon
manumaet 44:2e0045042a59 90 if (node.isAnchor) {
manumaet 44:2e0045042a59 91 node.address = 1;
manumaet 44:2e0045042a59 92 myprintf("This node is Anchor node %d \r\n", node.address);
manumaet 44:2e0045042a59 93 } else {
manumaet 44:2e0045042a59 94 node.address = 0;
manumaet 44:2e0045042a59 95 myprintf("This node is a Beacon. ");
manumaet 44:2e0045042a59 96 }
manumaet 32:041dd02e0e3b 97
manumaet 0:f50e671ffff7 98 while(1) {
manumaet 45:01a33363bc21 99 if (!node.isAnchor){
manumaet 44:2e0045042a59 100 rangeAndDisplayAll();
manumaet 45:01a33363bc21 101 //calibrationRanging(4);
manumaet 45:01a33363bc21 102
manumaet 45:01a33363bc21 103
manumaet 45:01a33363bc21 104
manumaet 45:01a33363bc21 105 }else
manumaet 44:2e0045042a59 106 myprintf(".\r");
manumaet 45:01a33363bc21 107
manumaet 45:01a33363bc21 108 wait(0.3);
manumaet 0:f50e671ffff7 109 }
manumaet 0:f50e671ffff7 110 }