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

MMRanging/MMRanging.cpp@35:95d838d33bf5, 2015-02-17 (annotated)

Committer:
manumaet
Date:
Tue Feb 17 10:58:58 2015 +0000
Revision:
35:95d838d33bf5
Parent:
34:f56962030c5c
Child:
36:883de6f9a73b
working console bar with two nodes after cleanup

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manumaet 27:71178fdb78e1 1 #include "MMRanging.h"
manumaet 27:71178fdb78e1 2
manumaet 34:f56962030c5c 3 MMRanging::MMRanging(DW1000& DW) : dw(DW) {
manumaet 28:a830131560e8 4 event_i = 0;
manumaet 28:a830131560e8 5 counter = 0;
manumaet 29:019ff388ed76 6 dw.setCallbacks(this, &MMRanging::callbackRX, &MMRanging::callbackTX);
manumaet 28:a830131560e8 7 dw.startRX();
manumaet 27:71178fdb78e1 8 }
manumaet 28:a830131560e8 9
manumaet 28:a830131560e8 10 void MMRanging::callbackRX() {
manumaet 28:a830131560e8 11 RX_timestamp = dw.getRXTimestamp();
manumaet 31:6f76f3d518ac 12
manumaet 32:041dd02e0e3b 13 rangingframe RX;
manumaet 32:041dd02e0e3b 14 dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&RX, dw.getFramelength()); // get data from buffer
manumaet 34:f56962030c5c 15 uint8_t Index = RX.sequence_number % 10;
manumaet 34:f56962030c5c 16
manumaet 32:041dd02e0e3b 17 switch (RX.type) {
manumaet 31:6f76f3d518ac 18 case 1:
manumaet 34:f56962030c5c 19 rangingtimingsReceiver[Index][0] = dw.getRXTimestamp();
manumaet 35:95d838d33bf5 20 sendRangingframe(2, RX.sequence_number, 0);
manumaet 31:6f76f3d518ac 21 break;
manumaet 31:6f76f3d518ac 22 case 2:
manumaet 34:f56962030c5c 23 rangingtimingsSender[Index][1] = dw.getRXTimestamp();
manumaet 31:6f76f3d518ac 24 break;
manumaet 31:6f76f3d518ac 25 case 3:
manumaet 35:95d838d33bf5 26 sendRangingframe(4, TX.sequence_number, rangingtimingsReceiver[Index][1] - rangingtimingsReceiver[Index][0]); // TODO: sequence number???
manumaet 31:6f76f3d518ac 27 break;
manumaet 31:6f76f3d518ac 28 case 4:
manumaet 35:95d838d33bf5 29 tofs = (rangingtimingsSender[Index][1] - rangingtimingsSender[Index][0] - RX.time_difference_receiver);// - 65536;
manumaet 31:6f76f3d518ac 30 break;
manumaet 31:6f76f3d518ac 31 default : break;
manumaet 28:a830131560e8 32 }
manumaet 31:6f76f3d518ac 33
manumaet 28:a830131560e8 34 eventtimes[event_i] = RX_timestamp - TX_timestamp; // TODO: can give some wrong values because of timer reset after 17 seconds
manumaet 28:a830131560e8 35 event[event_i][0] = '!';
manumaet 28:a830131560e8 36 event[event_i][1] = 'R';
manumaet 28:a830131560e8 37 event[event_i][2] = ' ';
manumaet 32:041dd02e0e3b 38 sprintf(&event[event_i][3], "%d %d", RX.sequence_number, RX.type);
manumaet 28:a830131560e8 39 if (event_i == 8)
manumaet 28:a830131560e8 40 event_i = 0;
manumaet 28:a830131560e8 41 else
manumaet 28:a830131560e8 42 event_i++;
manumaet 31:6f76f3d518ac 43
manumaet 28:a830131560e8 44 dw.startRX();
manumaet 28:a830131560e8 45 }
manumaet 28:a830131560e8 46
manumaet 28:a830131560e8 47 void MMRanging::callbackTX() {
manumaet 28:a830131560e8 48 TX_timestamp = dw.getTXTimestamp();
manumaet 34:f56962030c5c 49 uint8_t Index = TX.sequence_number % 10;
manumaet 31:6f76f3d518ac 50 switch (TX.type) {
manumaet 31:6f76f3d518ac 51 case 1:
manumaet 34:f56962030c5c 52 rangingtimingsSender[Index][0] = dw.getTXTimestamp();
manumaet 31:6f76f3d518ac 53 break;
manumaet 31:6f76f3d518ac 54 case 2:
manumaet 34:f56962030c5c 55 rangingtimingsReceiver[Index][1] = dw.getTXTimestamp();
manumaet 31:6f76f3d518ac 56 break;
manumaet 31:6f76f3d518ac 57 default: break;
manumaet 31:6f76f3d518ac 58 }
manumaet 31:6f76f3d518ac 59
manumaet 28:a830131560e8 60 eventtimes[event_i] = 0;
manumaet 28:a830131560e8 61 event[event_i][0] = '!';
manumaet 28:a830131560e8 62 event[event_i][1] = 'S';
manumaet 28:a830131560e8 63 event[event_i][2] = ' ';
manumaet 31:6f76f3d518ac 64 sprintf(&event[event_i][3], "%d %d", TX.sequence_number, TX.type);
manumaet 28:a830131560e8 65 if (event_i == 8)
manumaet 28:a830131560e8 66 event_i = 0;
manumaet 28:a830131560e8 67 else
manumaet 28:a830131560e8 68 event_i++;
manumaet 28:a830131560e8 69 }
manumaet 28:a830131560e8 70
manumaet 28:a830131560e8 71 void MMRanging::requestRanging() {
manumaet 35:95d838d33bf5 72 sendRangingframe(1, counter, 0);
manumaet 31:6f76f3d518ac 73 }
manumaet 31:6f76f3d518ac 74
manumaet 31:6f76f3d518ac 75 void MMRanging::requestTime() {
manumaet 35:95d838d33bf5 76 sendRangingframe(3, counter, 0);
manumaet 28:a830131560e8 77 counter++;
manumaet 34:f56962030c5c 78 }
manumaet 34:f56962030c5c 79
manumaet 35:95d838d33bf5 80 void MMRanging::sendRangingframe(uint8_t type, uint8_t sequence_number, uint64_t time_difference_receiver) {
manumaet 34:f56962030c5c 81 TX.type = type;
manumaet 34:f56962030c5c 82 TX.sequence_number = sequence_number;
manumaet 34:f56962030c5c 83 TX.time_difference_receiver = time_difference_receiver;
manumaet 34:f56962030c5c 84 dw.sendFrame((uint8_t*)&TX, sizeof(TX));
manumaet 28:a830131560e8 85 }