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@32:041dd02e0e3b, 2014-12-02 (annotated)

Committer:
manumaet
Date:
Tue Dec 02 13:32:34 2014 +0000
Revision:
32:041dd02e0e3b
Parent:
31:6f76f3d518ac
Child:
33:6ea4c3dd504d
transmitting of the time fails

Who changed what in which revision?

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