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

MM2WayRanging/MM2WayRanging.h@67:bd0f0580af5a, 2016-01-05 (annotated)

Committer:
naegelit
Date:
Tue Jan 05 17:06:19 2016 +0000
Revision:
67:bd0f0580af5a
Parent:
60:43be9228b3b9 
working with 4 odroids (hacky version)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bhepp 48:5999e510f154 1 // Adapted from Matthias Grob & Manuel Stalder - ETH Zürich - 2015
manumaet 44:2e0045042a59 2
bhepp 48:5999e510f154 3 #pragma once
manumaet 44:2e0045042a59 4
manumaet 44:2e0045042a59 5 #include "mbed.h"
manumaet 44:2e0045042a59 6 #include "DW1000.h"
manumaet 44:2e0045042a59 7
manumaet 44:2e0045042a59 8 #define TIMEUNITS_TO_US (1/(128*499.2)) // conversion between the decawave timeunits (ca 15.65ps) to microseconds.
manumaet 44:2e0045042a59 9 #define US_TO_TIMEUNITS (128*499.2) // conversion between microseconds to the decawave timeunits (ca 15.65ps).
manumaet 45:01a33363bc21 10 #define MMRANGING_2POWER40 1099511627776 // decimal value of 2^40 to correct timeroverflow between timestamps
manumaet 45:01a33363bc21 11
bhepp 48:5999e510f154 12 #define NUM_OF_UWB_ADDRESSES 256
manumaet 45:01a33363bc21 13
manumaet 44:2e0045042a59 14
manumaet 44:2e0045042a59 15 //Predefined delay for the critical answers in the ranging algorithm
manumaet 44:2e0045042a59 16 //HAS TO BE BIGGER THAN THE PROCESSING TIME OF THE FRAME ON THE NODE
manumaet 45:01a33363bc21 17 #define ANSWER_DELAY_US 2500 //2500 works for 110kbps, 900 for 6.8Mbps
manumaet 45:01a33363bc21 18 #define ANSWER_DELAY_TIMEUNITS ANSWER_DELAY_US * (128*499.2)
manumaet 44:2e0045042a59 19
manumaet 44:2e0045042a59 20 class MM2WayRanging {
manumaet 45:01a33363bc21 21
manumaet 45:01a33363bc21 22 public:
manumaet 45:01a33363bc21 23 MM2WayRanging(DW1000& DW);
manumaet 45:01a33363bc21 24
bhepp 48:5999e510f154 25 void requestRanging(uint8_t remote_address);
manumaet 45:01a33363bc21 26
bhepp 58:b824bdac7cd8 27 void setInterrupt(bool enable)
bhepp 58:b824bdac7cd8 28 {
bhepp 58:b824bdac7cd8 29 dw.setInterrupt(enable, enable);
bhepp 60:43be9228b3b9 30 if (!enable)
bhepp 60:43be9228b3b9 31 {
bhepp 60:43be9228b3b9 32 dw.resetInterruptFlags();
bhepp 60:43be9228b3b9 33 }
bhepp 58:b824bdac7cd8 34 }
bhepp 58:b824bdac7cd8 35
manumaet 45:01a33363bc21 36 //TODO: Better capsulation on those?
manumaet 45:01a33363bc21 37 bool isAnchor;
bhepp 48:5999e510f154 38 uint8_t address; // Identifies the nodes as address and remote_address in rangingframes
manumaet 44:2e0045042a59 39
manumaet 45:01a33363bc21 40 //TODO: Make those PRIVATE!
bhepp 48:5999e510f154 41 float roundtriptimes[NUM_OF_UWB_ADDRESSES]; // Array containing the round trip times to the anchors or the timeout which occured
bhepp 48:5999e510f154 42 float distances[NUM_OF_UWB_ADDRESSES]; // Array containing the finally calculated Distances to the anchors
bhepp 48:5999e510f154 43
bhepp 48:5999e510f154 44 struct __attribute__((packed, aligned(1))) ReceptionStats {
bhepp 48:5999e510f154 45 uint16_t std_noise;
bhepp 48:5999e510f154 46 uint16_t preamble_acc_count;
bhepp 48:5999e510f154 47 uint16_t first_path_index;
bhepp 48:5999e510f154 48 uint16_t first_path_amp_1;
bhepp 48:5999e510f154 49 uint16_t first_path_amp_2;
bhepp 48:5999e510f154 50 uint16_t first_path_amp_3;
bhepp 48:5999e510f154 51 uint16_t channel_impulse_response_power;
bhepp 48:5999e510f154 52 uint8_t prf;
bhepp 48:5999e510f154 53 };
bhepp 48:5999e510f154 54 ReceptionStats reception_stats[NUM_OF_UWB_ADDRESSES][3];
manumaet 45:01a33363bc21 55
manumaet 45:01a33363bc21 56 bool overflow; // TRUE if counter overflows while ranging
manumaet 45:01a33363bc21 57
manumaet 45:01a33363bc21 58 private:
manumaet 45:01a33363bc21 59
manumaet 45:01a33363bc21 60
manumaet 45:01a33363bc21 61 DW1000& dw;
manumaet 45:01a33363bc21 62 Timer LocalTimer;
manumaet 45:01a33363bc21 63
manumaet 45:01a33363bc21 64 void callbackRX();
manumaet 45:01a33363bc21 65 void callbackTX();
bhepp 48:5999e510f154 66 void sendPingFrame(uint8_t remote_address);
bhepp 48:5999e510f154 67 void sendDelayedAnswer(uint8_t remote_address, uint8_t type, uint64_t rxTimestamp);
bhepp 48:5999e510f154 68 void sendTransferFrame(uint8_t remote_address, int timestamp);
manumaet 45:01a33363bc21 69
bhepp 48:5999e510f154 70 inline float calibratedDistance(uint8_t remote_address);
manumaet 44:2e0045042a59 71
manumaet 45:01a33363bc21 72 /**
manumaet 45:01a33363bc21 73 * These two functions correct the timestamps if the counter had an overflow between measurements
manumaet 45:01a33363bc21 74 */
bhepp 48:5999e510f154 75 void correctReceiverTimestamps(uint8_t address);
bhepp 48:5999e510f154 76 void correctSenderTimestamps(uint8_t address);
manumaet 45:01a33363bc21 77
manumaet 45:01a33363bc21 78 int timediffRec;
manumaet 45:01a33363bc21 79 int timediffSend;
manumaet 45:01a33363bc21 80
manumaet 45:01a33363bc21 81 enum FrameType{
bhepp 48:5999e510f154 82 PING = 1,
manumaet 45:01a33363bc21 83 ANCHOR_RESPONSE,
manumaet 45:01a33363bc21 84 BEACON_RESPONSE,
manumaet 46:6398237672a0 85 TRANSFER_FRAME,
manumaet 46:6398237672a0 86 DISTANCES_FRAME
manumaet 45:01a33363bc21 87 };
manumaet 45:01a33363bc21 88
manumaet 47:b6120c152ad1 89 //the packed attribute makes sure the types only use their respective size in memory (8 bit for uint8_t), otherwise they would always use 32 bit
manumaet 47:b6120c152ad1 90 //IT IS A GCC SPECIFIC DIRECTIVE
manumaet 46:6398237672a0 91 struct __attribute__((packed, aligned(1))) RangingFrame {
bhepp 48:5999e510f154 92 uint8_t address;
bhepp 48:5999e510f154 93 uint8_t remote_address;
manumaet 45:01a33363bc21 94 uint8_t type;
manumaet 45:01a33363bc21 95 };
manumaet 45:01a33363bc21 96
manumaet 46:6398237672a0 97 struct __attribute__((packed, aligned(1))) ExtendedRangingFrame : RangingFrame{
manumaet 45:01a33363bc21 98 int signedTime;
bhepp 48:5999e510f154 99 ReceptionStats stats1;
bhepp 48:5999e510f154 100 ReceptionStats stats2;
manumaet 45:01a33363bc21 101 };
manumaet 45:01a33363bc21 102
manumaet 45:01a33363bc21 103
manumaet 45:01a33363bc21 104 RangingFrame rangingFrame; // buffer in class for sending a frame (not made locally because then we can recall in the interrupt what was sent)
manumaet 45:01a33363bc21 105 ExtendedRangingFrame transferFrame;
manumaet 45:01a33363bc21 106 ExtendedRangingFrame receivedFrame;
bhepp 48:5999e510f154 107
manumaet 45:01a33363bc21 108 uint64_t rxTimestamp;
bhepp 48:5999e510f154 109 uint64_t senderTimestamps[NUM_OF_UWB_ADDRESSES][3];
bhepp 48:5999e510f154 110 uint64_t receiverTimestamps[NUM_OF_UWB_ADDRESSES][3];
bhepp 48:5999e510f154 111 bool acknowledgement[NUM_OF_UWB_ADDRESSES]; // flag to indicate if ranging has succeeded
bhepp 48:5999e510f154 112 uint32_t tofs[NUM_OF_UWB_ADDRESSES]; // Array containing time of flights for each node (index is address of node)
manumaet 44:2e0045042a59 113
manumaet 44:2e0045042a59 114 };