Tobi's ubw test branch
Dependencies: mavlink_bridge mbed
Fork of AIT_UWB_Range by
main.cpp@41:0a3bb028d4ba, 2015-03-01 (annotated)
- Committer:
- manumaet
- Date:
- Sun Mar 01 20:19:38 2015 +0000
- Revision:
- 41:0a3bb028d4ba
- Parent:
- 40:5ce51b7e3118
- Child:
- 42:83931678c4de
before following exactly manual initializing
Who changed what in which revision?
User | Revision | Line number | New 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 | 41:0a3bb028d4ba | 11 | float rangings[1000]; |
manumaet | 41:0a3bb028d4ba | 12 | int index = 0; |
manumaet | 6:d5864a1b9e17 | 13 | |
manumaet | 0:f50e671ffff7 | 14 | int main() { |
manumaet | 41:0a3bb028d4ba | 15 | pc.printf("\r\nDecaWave 1.0 up and running!\r\n"); |
manumaet | 30:4ecc69d3cf8d | 16 | dw.setEUI(0xFAEDCD01FAEDCD01); // basic methods called to check if we have a working SPI connection |
manumaet | 28:a830131560e8 | 17 | pc.printf("DEVICE_ID register: 0x%X\r\n", dw.getDeviceID()); |
manumaet | 28:a830131560e8 | 18 | pc.printf("EUI register: %016llX\r\n", dw.getEUI()); |
manumaet | 38:8ef3b8d8b908 | 19 | pc.printf("Voltage: %fV\r\n", dw.getVoltage()); |
manumaet | 38:8ef3b8d8b908 | 20 | pc.printf("System Configuration: %llX\r\n", dw.readRegister40(DW1000_SYS_CFG, 0)); |
manumaet | 38:8ef3b8d8b908 | 21 | pc.printf("Size of Rangingframe: %d\r\n", sizeof(r.TX)); |
manumaet | 34:f56962030c5c | 22 | |
manumaet | 41:0a3bb028d4ba | 23 | uint16_t setdelay = 32953;//462; // TODO: = 2^15 |
manumaet | 34:f56962030c5c | 24 | dw.writeRegister16(DW1000_TX_ANTD, 0, setdelay); |
manumaet | 41:0a3bb028d4ba | 25 | //dw.writeRegister16(DW1000_LDE_CTRL, 0x1804, setdelay / 2); |
manumaet | 34:f56962030c5c | 26 | pc.printf("Antenna Delay TX: %d\r\n", dw.readRegister16(DW1000_TX_ANTD, 0)); |
manumaet | 34:f56962030c5c | 27 | pc.printf("Antenna Delay RX: %d\r\n", dw.readRegister16(DW1000_LDE_CTRL, 0x1804)); |
manumaet | 41:0a3bb028d4ba | 28 | pc.printf("TX Power: %llX\r\n", dw.readRegister40(DW1000_TX_POWER, 0)); |
manumaet | 39:bb57aa77b015 | 29 | //r.receiver = true; |
manumaet | 36:883de6f9a73b | 30 | if (r.receiver) |
manumaet | 40:5ce51b7e3118 | 31 | r.address = 1; |
manumaet | 36:883de6f9a73b | 32 | else |
manumaet | 36:883de6f9a73b | 33 | r.address = 0; // sender node has address 0 |
manumaet | 36:883de6f9a73b | 34 | pc.printf("Address: %d\r\n", r.address); |
manumaet | 41:0a3bb028d4ba | 35 | //wait(1); |
manumaet | 32:041dd02e0e3b | 36 | |
manumaet | 0:f50e671ffff7 | 37 | while(1) { |
manumaet | 41:0a3bb028d4ba | 38 | if (!r.receiver) { |
manumaet | 41:0a3bb028d4ba | 39 | #if 1 // normal operation |
manumaet | 41:0a3bb028d4ba | 40 | r.requestRangingAll(); |
manumaet | 41:0a3bb028d4ba | 41 | for (int i = 1; i <= 4; i++) { // Request ranging to all anchors |
manumaet | 41:0a3bb028d4ba | 42 | pc.printf("%f, ", r.distances[i]); |
manumaet | 41:0a3bb028d4ba | 43 | //pc.printf("%f s\r\n", r.roundtriptimes[i]); |
manumaet | 41:0a3bb028d4ba | 44 | } |
manumaet | 41:0a3bb028d4ba | 45 | pc.printf("\r\n"); |
manumaet | 41:0a3bb028d4ba | 46 | #endif |
manumaet | 41:0a3bb028d4ba | 47 | #if 0 // calibration of antennadelay |
manumaet | 41:0a3bb028d4ba | 48 | r.requestRanging(1); |
manumaet | 41:0a3bb028d4ba | 49 | if (r.distances[1] < 100) { |
manumaet | 41:0a3bb028d4ba | 50 | rangings[index] = r.distances[1]; |
manumaet | 41:0a3bb028d4ba | 51 | //pc.printf("%f, ", r.distances[1]); |
manumaet | 41:0a3bb028d4ba | 52 | index++; |
manumaet | 41:0a3bb028d4ba | 53 | } |
manumaet | 41:0a3bb028d4ba | 54 | if (index == 1000) { |
manumaet | 41:0a3bb028d4ba | 55 | for(int i = 0; i < 999; i++) |
manumaet | 41:0a3bb028d4ba | 56 | rangings[999] += rangings[i]; |
manumaet | 41:0a3bb028d4ba | 57 | float mean = rangings[999] / 1000; |
manumaet | 41:0a3bb028d4ba | 58 | pc.printf("\r\n\r\nMean: %f Delay: %d\r\n\r\n", mean, setdelay); |
manumaet | 41:0a3bb028d4ba | 59 | wait(2); |
manumaet | 41:0a3bb028d4ba | 60 | if(mean > 5) |
manumaet | 41:0a3bb028d4ba | 61 | setdelay += 10; |
manumaet | 41:0a3bb028d4ba | 62 | else |
manumaet | 41:0a3bb028d4ba | 63 | setdelay--; |
manumaet | 41:0a3bb028d4ba | 64 | if(abs(mean-5) < 0.003f) { |
manumaet | 41:0a3bb028d4ba | 65 | pc.printf("\r\n\r\nDELAY: %d\r\n\r\n", setdelay); |
manumaet | 41:0a3bb028d4ba | 66 | return 0; |
manumaet | 41:0a3bb028d4ba | 67 | } |
manumaet | 41:0a3bb028d4ba | 68 | dw.writeRegister16(DW1000_TX_ANTD, 0, setdelay / 2); |
manumaet | 41:0a3bb028d4ba | 69 | dw.writeRegister16(DW1000_LDE_CTRL, 0x1804, setdelay / 2); |
manumaet | 41:0a3bb028d4ba | 70 | index = 0; |
manumaet | 41:0a3bb028d4ba | 71 | } |
manumaet | 41:0a3bb028d4ba | 72 | #endif |
manumaet | 39:bb57aa77b015 | 73 | } else { |
manumaet | 41:0a3bb028d4ba | 74 | //pc.printf("%lld\r\n", r.timeDifference40Bit(r.rangingtimingsReceiver[0][0], r.rangingtimingsReceiver[0][1])); // debuging output to find 40Bit overflows on receiver side |
manumaet | 36:883de6f9a73b | 75 | } |
manumaet | 36:883de6f9a73b | 76 | |
manumaet | 41:0a3bb028d4ba | 77 | #if 0 // averaging the distance with a ringbuffer |
manumaet | 41:0a3bb028d4ba | 78 | int average[50]; |
manumaet | 41:0a3bb028d4ba | 79 | int average_index = 0; |
manumaet | 41:0a3bb028d4ba | 80 | float averaged; |
manumaet | 41:0a3bb028d4ba | 81 | |
manumaet | 40:5ce51b7e3118 | 82 | pc.printf("Distance: %f\r\n", (r.tofs[2]*300/MMRANGING_TIMEUNIT_US / 2) - 0.5); |
manumaet | 40:5ce51b7e3118 | 83 | average[average_index] = r.tofs[2]; |
manumaet | 40:5ce51b7e3118 | 84 | average_index++; |
manumaet | 40:5ce51b7e3118 | 85 | if(average_index == 50) |
manumaet | 40:5ce51b7e3118 | 86 | average_index = 0; |
manumaet | 40:5ce51b7e3118 | 87 | for(int i = 0; i < 50; i++) |
manumaet | 40:5ce51b7e3118 | 88 | averaged += average[i]; |
manumaet | 40:5ce51b7e3118 | 89 | averaged /= 50; |
manumaet | 40:5ce51b7e3118 | 90 | pc.printf("Distance: %f\r\n", (averaged*300/MMRANGING_TIMEUNIT_US / 2) - 0.5); |
manumaet | 40:5ce51b7e3118 | 91 | #endif |
manumaet | 38:8ef3b8d8b908 | 92 | |
manumaet | 39:bb57aa77b015 | 93 | #if 0 // Output bars on console |
manumaet | 38:8ef3b8d8b908 | 94 | for(int i = 1; i < 3; i++) { |
manumaet | 38:8ef3b8d8b908 | 95 | //pc.printf("%f ", r.tofs[j]*MMRANGING_TIMEUNIT_NS); |
manumaet | 38:8ef3b8d8b908 | 96 | pc.printf("%lld [", r.tofs[i]); |
manumaet | 38:8ef3b8d8b908 | 97 | int dots = r.tofs[i]*70/1400; |
manumaet | 38:8ef3b8d8b908 | 98 | if (abs(dots) < 10000) |
manumaet | 38:8ef3b8d8b908 | 99 | for(int j = 0; j < dots; j++) |
manumaet | 38:8ef3b8d8b908 | 100 | pc.printf("="); |
manumaet | 38:8ef3b8d8b908 | 101 | pc.printf("]\r\n"); |
manumaet | 38:8ef3b8d8b908 | 102 | } |
manumaet | 38:8ef3b8d8b908 | 103 | #endif |
manumaet | 36:883de6f9a73b | 104 | |
manumaet | 40:5ce51b7e3118 | 105 | #ifdef EVENTS // Output interrupt callback events for debugging (defined in MMRanging.h) |
manumaet | 36:883de6f9a73b | 106 | for(int j = 0; j < 10; j++) |
manumaet | 36:883de6f9a73b | 107 | if(r.event[j][0] == '!') { |
manumaet | 36:883de6f9a73b | 108 | pc.printf("%s\r\n", r.event[j]); |
manumaet | 36:883de6f9a73b | 109 | r.event[j][0] = 'X'; |
manumaet | 36:883de6f9a73b | 110 | } |
manumaet | 36:883de6f9a73b | 111 | r.event_i = 0; |
manumaet | 36:883de6f9a73b | 112 | #endif |
manumaet | 39:bb57aa77b015 | 113 | //pc.printf("Status: %llX\r\n", dw.getStatus()); |
manumaet | 39:bb57aa77b015 | 114 | //pc.printf("TX Control: %llX\r\n", dw.readRegister40(DW1000_TX_FCTRL, 0)); |
manumaet | 38:8ef3b8d8b908 | 115 | //pc.printf("\r\n"); |
manumaet | 41:0a3bb028d4ba | 116 | //wait(0.002); |
manumaet | 41:0a3bb028d4ba | 117 | //wait(0.2); |
manumaet | 0:f50e671ffff7 | 118 | } |
manumaet | 0:f50e671ffff7 | 119 | } |