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@31:6f76f3d518ac, 2014-12-01 (annotated)

Committer:
manumaet
Date:
Mon Dec 01 13:04:18 2014 +0000
Revision:
31:6f76f3d518ac
Parent:
29:019ff388ed76
Child:
32:041dd02e0e3b
ranging draft implemented, not working yet

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 28:a830131560e8 20 dw.receiveString(messageRX);
manumaet 31:6f76f3d518ac 21
manumaet 31:6f76f3d518ac 22 rangingframe* RX = (rangingframe*)messageRX;
manumaet 31:6f76f3d518ac 23 switch (RX->type) {
manumaet 31:6f76f3d518ac 24 case 1:
manumaet 31:6f76f3d518ac 25 rangingtimingsReceiver[RX->sequence_number][0] = dw.getRXTimestamp();
manumaet 31:6f76f3d518ac 26 TX.type = 2;
manumaet 31:6f76f3d518ac 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 31:6f76f3d518ac 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 31:6f76f3d518ac 36 TX.time_difference_receiver = 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 31:6f76f3d518ac 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 28:a830131560e8 50 for(int i = 0; i < 10; i++)
manumaet 28:a830131560e8 51 event[event_i][i+3] = messageRX[i];
manumaet 28:a830131560e8 52 if (event_i == 8)
manumaet 28:a830131560e8 53 event_i = 0;
manumaet 28:a830131560e8 54 else
manumaet 28:a830131560e8 55 event_i++;
manumaet 31:6f76f3d518ac 56
manumaet 28:a830131560e8 57 dw.startRX();
manumaet 28:a830131560e8 58 }
manumaet 28:a830131560e8 59
manumaet 28:a830131560e8 60 void MMRanging::callbackTX() {
manumaet 28:a830131560e8 61 TX_timestamp = dw.getTXTimestamp();
manumaet 31:6f76f3d518ac 62
manumaet 31:6f76f3d518ac 63 switch (TX.type) {
manumaet 31:6f76f3d518ac 64 case 1:
manumaet 31:6f76f3d518ac 65 rangingtimingsSender[TX.sequence_number][0] = dw.getTXTimestamp();
manumaet 31:6f76f3d518ac 66 break;
manumaet 31:6f76f3d518ac 67 case 2:
manumaet 31:6f76f3d518ac 68 rangingtimingsReceiver[TX.sequence_number][1] = dw.getTXTimestamp();
manumaet 31:6f76f3d518ac 69 break;
manumaet 31:6f76f3d518ac 70 default: break;
manumaet 31:6f76f3d518ac 71 }
manumaet 31:6f76f3d518ac 72
manumaet 28:a830131560e8 73 eventtimes[event_i] = 0;
manumaet 28:a830131560e8 74 event[event_i][0] = '!';
manumaet 28:a830131560e8 75 event[event_i][1] = 'S';
manumaet 28:a830131560e8 76 event[event_i][2] = ' ';
manumaet 31:6f76f3d518ac 77 sprintf(&event[event_i][3], "%d %d", TX.sequence_number, TX.type);
manumaet 31:6f76f3d518ac 78 //for(int i = 0; i < 10; i++)
manumaet 31:6f76f3d518ac 79 // event[event_i][i+3] = message[i];
manumaet 28:a830131560e8 80 if (event_i == 8)
manumaet 28:a830131560e8 81 event_i = 0;
manumaet 28:a830131560e8 82 else
manumaet 28:a830131560e8 83 event_i++;
manumaet 28:a830131560e8 84 }
manumaet 28:a830131560e8 85
manumaet 28:a830131560e8 86 void MMRanging::requestRanging() {
manumaet 31:6f76f3d518ac 87 TX.type = 1;
manumaet 31:6f76f3d518ac 88 TX.sequence_number = counter;
manumaet 31:6f76f3d518ac 89 TX.time_difference_receiver = 0;
manumaet 31:6f76f3d518ac 90 dw.sendFrame((uint8_t*)&TX, 10);
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 31:6f76f3d518ac 97 dw.sendFrame((uint8_t*)&TX, 10);
manumaet 28:a830131560e8 98 counter++;
manumaet 28:a830131560e8 99 }