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@33:6ea4c3dd504d, 2014-12-02 (annotated)

Committer:
manumaet
Date:
Tue Dec 02 13:55:01 2014 +0000
Revision:
33:6ea4c3dd504d
Parent:
32:041dd02e0e3b
Child:
34:f56962030c5c
first simple measurement works (gives 1.0362 us)

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 33:6ea4c3dd504d 29 TX.dummy = 1;
manumaet 33:6ea4c3dd504d 30 dw.sendFrame((uint8_t*)&TX, 12);
manumaet 31:6f76f3d518ac 31 break;
manumaet 31:6f76f3d518ac 32 case 2:
manumaet 32:041dd02e0e3b 33 rangingtimingsSender[RX.sequence_number][1] = dw.getRXTimestamp();
manumaet 31:6f76f3d518ac 34 break;
manumaet 31:6f76f3d518ac 35 case 3:
manumaet 31:6f76f3d518ac 36 TX.type = 4;
manumaet 33:6ea4c3dd504d 37 TX.time_difference_receiver = rangingtimingsReceiver[TX.sequence_number][1] - rangingtimingsReceiver[TX.sequence_number][0];
manumaet 33:6ea4c3dd504d 38 TX.dummy = 1;
manumaet 33:6ea4c3dd504d 39 dw.sendFrame((uint8_t*)&TX, 12);
manumaet 31:6f76f3d518ac 40 break;
manumaet 31:6f76f3d518ac 41 case 4:
manumaet 33:6ea4c3dd504d 42 tofs[RX.sequence_number % 10] = rangingtimingsSender[RX.sequence_number][1] - rangingtimingsSender[RX.sequence_number][0] - (uint32_t)RX.time_difference_receiver;
manumaet 31:6f76f3d518ac 43 //tofs[RX->sequence_number] = (rangingtimingsSender[RX->sequence_number][1] - rangingtimingsSender[RX->sequence_number][0] - RX->time_difference_receiver) >> 1;
manumaet 31:6f76f3d518ac 44 break;
manumaet 31:6f76f3d518ac 45 default : break;
manumaet 28:a830131560e8 46 }
manumaet 31:6f76f3d518ac 47
manumaet 28:a830131560e8 48 eventtimes[event_i] = RX_timestamp - TX_timestamp; // TODO: can give some wrong values because of timer reset after 17 seconds
manumaet 28:a830131560e8 49 event[event_i][0] = '!';
manumaet 28:a830131560e8 50 event[event_i][1] = 'R';
manumaet 28:a830131560e8 51 event[event_i][2] = ' ';
manumaet 32:041dd02e0e3b 52 sprintf(&event[event_i][3], "%d %d", RX.sequence_number, RX.type);
manumaet 28:a830131560e8 53 if (event_i == 8)
manumaet 28:a830131560e8 54 event_i = 0;
manumaet 28:a830131560e8 55 else
manumaet 28:a830131560e8 56 event_i++;
manumaet 31:6f76f3d518ac 57
manumaet 28:a830131560e8 58 dw.startRX();
manumaet 28:a830131560e8 59 }
manumaet 28:a830131560e8 60
manumaet 28:a830131560e8 61 void MMRanging::callbackTX() {
manumaet 28:a830131560e8 62 TX_timestamp = dw.getTXTimestamp();
manumaet 31:6f76f3d518ac 63
manumaet 31:6f76f3d518ac 64 switch (TX.type) {
manumaet 31:6f76f3d518ac 65 case 1:
manumaet 31:6f76f3d518ac 66 rangingtimingsSender[TX.sequence_number][0] = dw.getTXTimestamp();
manumaet 31:6f76f3d518ac 67 break;
manumaet 31:6f76f3d518ac 68 case 2:
manumaet 31:6f76f3d518ac 69 rangingtimingsReceiver[TX.sequence_number][1] = dw.getTXTimestamp();
manumaet 31:6f76f3d518ac 70 break;
manumaet 31:6f76f3d518ac 71 default: break;
manumaet 31:6f76f3d518ac 72 }
manumaet 31:6f76f3d518ac 73
manumaet 28:a830131560e8 74 eventtimes[event_i] = 0;
manumaet 28:a830131560e8 75 event[event_i][0] = '!';
manumaet 28:a830131560e8 76 event[event_i][1] = 'S';
manumaet 28:a830131560e8 77 event[event_i][2] = ' ';
manumaet 31:6f76f3d518ac 78 sprintf(&event[event_i][3], "%d %d", TX.sequence_number, TX.type);
manumaet 28:a830131560e8 79 if (event_i == 8)
manumaet 28:a830131560e8 80 event_i = 0;
manumaet 28:a830131560e8 81 else
manumaet 28:a830131560e8 82 event_i++;
manumaet 28:a830131560e8 83 }
manumaet 28:a830131560e8 84
manumaet 28:a830131560e8 85 void MMRanging::requestRanging() {
manumaet 31:6f76f3d518ac 86 TX.type = 1;
manumaet 31:6f76f3d518ac 87 TX.sequence_number = counter;
manumaet 31:6f76f3d518ac 88 TX.time_difference_receiver = 0;
manumaet 33:6ea4c3dd504d 89 TX.dummy = 1;
manumaet 33:6ea4c3dd504d 90 dw.sendFrame((uint8_t*)&TX, 12);
manumaet 31:6f76f3d518ac 91 }
manumaet 31:6f76f3d518ac 92
manumaet 31:6f76f3d518ac 93 void MMRanging::requestTime() {
manumaet 31:6f76f3d518ac 94 TX.type = 3;
manumaet 31:6f76f3d518ac 95 TX.sequence_number = counter;
manumaet 31:6f76f3d518ac 96 TX.time_difference_receiver = 0;
manumaet 33:6ea4c3dd504d 97 TX.dummy = 1;
manumaet 33:6ea4c3dd504d 98 dw.sendFrame((uint8_t*)&TX, 12);
manumaet 28:a830131560e8 99 counter++;
manumaet 28:a830131560e8 100 }