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@40:5ce51b7e3118, 2015-02-27 (annotated)

Committer:
manumaet
Date:
Fri Feb 27 11:47:51 2015 +0000
Revision:
40:5ce51b7e3118
Parent:
39:bb57aa77b015
Child:
41:0a3bb028d4ba
distance to 4 anchors works and is visible in MATLAB, before cleanup and calibration

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 27:71178fdb78e1 5 #include "MMRanging.h" // our self developed raning application
manumaet 0:f50e671ffff7 6
manumaet 26:a65c6f26c458 7 PC pc(USBTX, USBRX, 921600); // USB UART Terminal
manumaet 30:4ecc69d3cf8d 8 DW1000 dw(PA_7, PA_6, PA_5, PB_6, PB_9); // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ)
manumaet 28:a830131560e8 9 MMRanging r(dw); // Ranging class for getting distances and later positions
manumaet 14:8041c9b68406 10
manumaet 40:5ce51b7e3118 11 Timer LocalTimer;
manumaet 40:5ce51b7e3118 12 float time_before;
manumaet 40:5ce51b7e3118 13
manumaet 40:5ce51b7e3118 14 int average[50];
manumaet 40:5ce51b7e3118 15 int average_index = 0;
manumaet 40:5ce51b7e3118 16 float averaged;
manumaet 6:d5864a1b9e17 17
manumaet 0:f50e671ffff7 18 int main() {
manumaet 40:5ce51b7e3118 19 LocalTimer.start();
manumaet 27:71178fdb78e1 20 pc.printf("DecaWave 0.2\r\nup and running!\r\n");
manumaet 30:4ecc69d3cf8d 21 dw.setEUI(0xFAEDCD01FAEDCD01); // basic methods called to check if we have a working SPI connection
manumaet 28:a830131560e8 22 pc.printf("DEVICE_ID register: 0x%X\r\n", dw.getDeviceID());
manumaet 28:a830131560e8 23 pc.printf("EUI register: %016llX\r\n", dw.getEUI());
manumaet 38:8ef3b8d8b908 24 pc.printf("Voltage: %fV\r\n", dw.getVoltage());
manumaet 38:8ef3b8d8b908 25 pc.printf("System Configuration: %llX\r\n", dw.readRegister40(DW1000_SYS_CFG, 0));
manumaet 38:8ef3b8d8b908 26 pc.printf("Size of Rangingframe: %d\r\n", sizeof(r.TX));
manumaet 34:f56962030c5c 27
manumaet 40:5ce51b7e3118 28 uint16_t setdelay = 32768; // TODO: = 2^15
manumaet 34:f56962030c5c 29 dw.writeRegister16(DW1000_TX_ANTD, 0, setdelay);
manumaet 34:f56962030c5c 30 pc.printf("Antenna Delay TX: %d\r\n", dw.readRegister16(DW1000_TX_ANTD, 0));
manumaet 34:f56962030c5c 31 pc.printf("Antenna Delay RX: %d\r\n", dw.readRegister16(DW1000_LDE_CTRL, 0x1804));
manumaet 39:bb57aa77b015 32 //r.receiver = true;
manumaet 36:883de6f9a73b 33 if (r.receiver)
manumaet 40:5ce51b7e3118 34 r.address = 1;
manumaet 36:883de6f9a73b 35 else
manumaet 36:883de6f9a73b 36 r.address = 0; // sender node has address 0
manumaet 36:883de6f9a73b 37 pc.printf("Address: %d\r\n", r.address);
manumaet 36:883de6f9a73b 38 wait(2);
manumaet 32:041dd02e0e3b 39
manumaet 0:f50e671ffff7 40 while(1) {
manumaet 36:883de6f9a73b 41 if (!r.receiver) { // Request ranging
manumaet 40:5ce51b7e3118 42 for (int i = 1; i <= 4; i++) {
manumaet 40:5ce51b7e3118 43 r.acknowledgement[i] = false;
manumaet 40:5ce51b7e3118 44 r.requestRanging(i); // TODO: ask all available nodes in MMRanging
manumaet 40:5ce51b7e3118 45 time_before = LocalTimer.read();
manumaet 40:5ce51b7e3118 46 while(!r.acknowledgement[i] && LocalTimer.read() < time_before + 0.5);
manumaet 40:5ce51b7e3118 47 pc.printf("%f, ", (r.tofs[i]*300/MMRANGING_TIMEUNIT_US / 2) - 0.5);
manumaet 40:5ce51b7e3118 48 //pc.printf("%2.6fs\r\n", LocalTimer.read() - time_before);
manumaet 40:5ce51b7e3118 49 }
manumaet 40:5ce51b7e3118 50 pc.printf("\r\n"); // logging output
manumaet 39:bb57aa77b015 51 } else {
manumaet 40:5ce51b7e3118 52 //pc.printf("%lld\r\n", r.timeDifference40Bit(r.rangingtimingsReceiver[0][0], r.rangingtimingsReceiver[0][1]));
manumaet 36:883de6f9a73b 53 }
manumaet 36:883de6f9a73b 54
manumaet 40:5ce51b7e3118 55 #if 0
manumaet 40:5ce51b7e3118 56 pc.printf("Distance: %f\r\n", (r.tofs[2]*300/MMRANGING_TIMEUNIT_US / 2) - 0.5);
manumaet 40:5ce51b7e3118 57 average[average_index] = r.tofs[2];
manumaet 40:5ce51b7e3118 58 average_index++;
manumaet 40:5ce51b7e3118 59 if(average_index == 50)
manumaet 40:5ce51b7e3118 60 average_index = 0;
manumaet 40:5ce51b7e3118 61 for(int i = 0; i < 50; i++)
manumaet 40:5ce51b7e3118 62 averaged += average[i];
manumaet 40:5ce51b7e3118 63 averaged /= 50;
manumaet 40:5ce51b7e3118 64 pc.printf("Distance: %f\r\n", (averaged*300/MMRANGING_TIMEUNIT_US / 2) - 0.5);
manumaet 40:5ce51b7e3118 65 #endif
manumaet 38:8ef3b8d8b908 66
manumaet 39:bb57aa77b015 67 #if 0 // Output bars on console
manumaet 38:8ef3b8d8b908 68 for(int i = 1; i < 3; i++) {
manumaet 38:8ef3b8d8b908 69 //pc.printf("%f ", r.tofs[j]*MMRANGING_TIMEUNIT_NS);
manumaet 38:8ef3b8d8b908 70 pc.printf("%lld [", r.tofs[i]);
manumaet 38:8ef3b8d8b908 71 int dots = r.tofs[i]*70/1400;
manumaet 38:8ef3b8d8b908 72 if (abs(dots) < 10000)
manumaet 38:8ef3b8d8b908 73 for(int j = 0; j < dots; j++)
manumaet 38:8ef3b8d8b908 74 pc.printf("=");
manumaet 38:8ef3b8d8b908 75 pc.printf("]\r\n");
manumaet 38:8ef3b8d8b908 76 }
manumaet 38:8ef3b8d8b908 77 #endif
manumaet 36:883de6f9a73b 78
manumaet 40:5ce51b7e3118 79 #ifdef EVENTS // Output interrupt callback events for debugging (defined in MMRanging.h)
manumaet 36:883de6f9a73b 80 for(int j = 0; j < 10; j++)
manumaet 36:883de6f9a73b 81 if(r.event[j][0] == '!') {
manumaet 36:883de6f9a73b 82 pc.printf("%s\r\n", r.event[j]);
manumaet 36:883de6f9a73b 83 r.event[j][0] = 'X';
manumaet 36:883de6f9a73b 84 }
manumaet 36:883de6f9a73b 85 r.event_i = 0;
manumaet 36:883de6f9a73b 86 #endif
manumaet 39:bb57aa77b015 87 //pc.printf("Status: %llX\r\n", dw.getStatus());
manumaet 39:bb57aa77b015 88 //pc.printf("TX Control: %llX\r\n", dw.readRegister40(DW1000_TX_FCTRL, 0));
manumaet 38:8ef3b8d8b908 89 //pc.printf("\r\n");
manumaet 39:bb57aa77b015 90 wait(0.02);
manumaet 40:5ce51b7e3118 91 wait(0.2);
manumaet 0:f50e671ffff7 92 }
manumaet 0:f50e671ffff7 93 }