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@34:f56962030c5c, 2014-12-27 (annotated)

Committer:
manumaet
Date:
Sat Dec 27 14:37:12 2014 +0000
Revision:
34:f56962030c5c
Parent:
33:6ea4c3dd504d
Child:
35:95d838d33bf5
when merging to eclipse & svn

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 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 34:f56962030c5c 22 uint8_t Index = RX.sequence_number % 10;
manumaet 34:f56962030c5c 23
manumaet 32:041dd02e0e3b 24 switch (RX.type) {
manumaet 31:6f76f3d518ac 25 case 1:
manumaet 34:f56962030c5c 26 rangingtimingsReceiver[Index][0] = dw.getRXTimestamp();
manumaet 31:6f76f3d518ac 27 TX.type = 2;
manumaet 32:041dd02e0e3b 28 TX.sequence_number = RX.sequence_number;
manumaet 31:6f76f3d518ac 29 TX.time_difference_receiver = 0;
manumaet 34:f56962030c5c 30 dw.sendFrame((uint8_t*)&TX, sizeof(TX));
manumaet 31:6f76f3d518ac 31 break;
manumaet 31:6f76f3d518ac 32 case 2:
manumaet 34:f56962030c5c 33 rangingtimingsSender[Index][1] = dw.getRXTimestamp();
manumaet 31:6f76f3d518ac 34 break;
manumaet 31:6f76f3d518ac 35 case 3:
manumaet 31:6f76f3d518ac 36 TX.type = 4;
manumaet 34:f56962030c5c 37 TX.time_difference_receiver = rangingtimingsReceiver[Index][1] - rangingtimingsReceiver[Index][0];
manumaet 34:f56962030c5c 38 dw.sendFrame((uint8_t*)&TX, sizeof(TX));
manumaet 31:6f76f3d518ac 39 break;
manumaet 31:6f76f3d518ac 40 case 4:
manumaet 34:f56962030c5c 41 //tofs[Index] = RX.time_difference_receiver;
manumaet 34:f56962030c5c 42 tofs[Index] = (rangingtimingsSender[Index][1] - rangingtimingsSender[Index][0] - RX.time_difference_receiver);// - 65536;
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 34:f56962030c5c 63 uint8_t Index = TX.sequence_number % 10;
manumaet 31:6f76f3d518ac 64 switch (TX.type) {
manumaet 31:6f76f3d518ac 65 case 1:
manumaet 34:f56962030c5c 66 rangingtimingsSender[Index][0] = dw.getTXTimestamp();
manumaet 31:6f76f3d518ac 67 break;
manumaet 31:6f76f3d518ac 68 case 2:
manumaet 34:f56962030c5c 69 rangingtimingsReceiver[Index][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 34:f56962030c5c 89 dw.sendFrame((uint8_t*)&TX, sizeof(TX));
manumaet 31:6f76f3d518ac 90 }
manumaet 31:6f76f3d518ac 91
manumaet 31:6f76f3d518ac 92 void MMRanging::requestTime() {
manumaet 31:6f76f3d518ac 93 TX.type = 3;
manumaet 31:6f76f3d518ac 94 TX.sequence_number = counter;
manumaet 31:6f76f3d518ac 95 TX.time_difference_receiver = 0;
manumaet 34:f56962030c5c 96 dw.sendFrame((uint8_t*)&TX, sizeof(TX));
manumaet 28:a830131560e8 97 counter++;
manumaet 34:f56962030c5c 98 }
manumaet 34:f56962030c5c 99
manumaet 34:f56962030c5c 100 void MMRanging::sendRangingframe(uint8_t type, uint8_t sequence_number, uint64_t time_difference_receiver, uint16_t dummy) {
manumaet 34:f56962030c5c 101 TX.type = type;
manumaet 34:f56962030c5c 102 TX.sequence_number = sequence_number;
manumaet 34:f56962030c5c 103 TX.time_difference_receiver = time_difference_receiver;
manumaet 34:f56962030c5c 104 dw.sendFrame((uint8_t*)&TX, sizeof(TX));
manumaet 28:a830131560e8 105 }