41#include <clmsg-discovery.h>
42#include <clmsg-stats.h>
54#define DEBUG(level, text) \
56 if (debug_tdoa >= level) { \
57 std::cout << NOW << " UwRangingTDOA(" << node_id << "): " << text \
72 : TclClass(
"Module/UW/RANGING_TDOA")
95 , scheduler_active(false)
104 , range_bytes_sent(0)
111 , mac2phy_delay(1e-6)
123 , entries_timestamps()
128 bind(
"n_nodes_", (
int *) &
n_nodes);
131 bind(
"max_tt", (
double *) &
max_tt);
138 if (n_nodes < 0 || n_nodes > std::numeric_limits<uwrange_node_t>::max()) {
139 DEBUG(0,
"Negative or too big number of nodes, setting default to 0")
145 std::vector<std::vector<tdoa_entry>>(
157 for (
int ni = 0; ni <
n_nodes; ni++) {
158 for (
int nj = ni + 1; nj <
n_nodes; nj++) {
168 hdr_cmn *ch = HDR_CMN(p);
170 if (ch->direction() == hdr_cmn::UP) {
175 }
else if (ch->direction() == hdr_cmn::DOWN) {
193 Packet *p = msg->
pkt;
196 hdr_cmn *ch = HDR_CMN(p);
200 "recvSyncClMsg::updating deltas on outgoing packet")
207 const Packet *p = msg->
pkt;
210 hdr_cmn *ch = HDR_CMN(p);
217 ClMsgStats stats_clmsg = ClMsgStats(
phy_id, UNICAST);
219 sendSyncClMsg(&stats_clmsg);
221 if (stats_clmsg.getStats()->type_id ==
225 stats_clmsg.getStats());
234 " RX ERRORED ping with pkt_token_id "
235 << pkt_id <<
" from node "
244 "recvSyncClMsg::RX ping with pkt_token_id "
245 << pkt_id <<
" from node "
260 return Module::recvSyncClMsg(m);
270 weight = (pow(1 -
ber[entry], pkt_size));
273 "entry " << entry <<
" PER " << pow(1 -
ber[entry], pkt_size)
275 <<
" weight: " << weight)
277 }
else if (mode == -1)
282 "entry " << entry <<
" PER " << pow(1 -
ber[entry], pkt_size)
284 <<
" weight: " << weight)
286 }
else if (mode == -2)
291 "entry " << entry <<
" PER " << pow(1 -
ber[entry], pkt_size)
293 <<
" weight: " << weight)
306 std::multimap<double, int, std::greater<double>> sorted_pairs;
307 double max_weight = 0.0;
309 for (
int n_entries = 1; n_entries <
n_nodes; n_entries++) {
310 sorted_pairs.clear();
314 for (
int entry = 0; entry <
n_nodes; entry++) {
320 sorted_pairs.insert(std::pair<double, int>(w, entry));
322 "entryWeight(" << entry <<
"," << n_entries
327 std::multimap<double, int>::iterator iter = sorted_pairs.begin();
329 for (
int i = 0; i < n_entries; i++)
331 weight += 1 * (iter->first);
333 if (iter == sorted_pairs.end())
337 if (weight > max_weight) {
339 opt_n_entries = n_entries;
343 "N entries: " << n_entries <<
" weight: " << weight <<
"")
349 std::multimap<double, int, std::greater<double>> sorted_pairs;
351 for (
int entry = 0; entry <
n_nodes; entry++) {
358 std::multimap<double, int>::iterator iter = sorted_pairs.begin();
360 if (sorted_entries) {
361 for (
int i = 0; i < opt_n_entries; i++) {
362 sorted_entries->push_back(iter->second);
367 DEBUG(2,
"calcOptEntries::opt_n_entries = " << opt_n_entries)
369 return opt_n_entries;
376 Packet *p = Packet::alloc();
377 hdr_cmn *ch = HDR_CMN(p);
378 hdr_mac *mach = HDR_MAC(p);
381 mach->macDA() = MAC_BROADCAST;
383 mach->ftype() = MF_DATA;
390 std::vector<int> sorted_entries;
393 DEBUG(2,
"rangeTX::calling calcOptEntries rangeID: " <<
packet_id)
399 "rangeTX::OPT SIZE: " << sorted_entries.size()
402 for (
auto i : sorted_entries) {
404 "rangeTX::times_mat[" << i <<
"][" <<
last_ids[i]
410 (*rangh).times_[j] = new_entry;
415 "rangeTX::entries_mat["
421 "rangeTX::entry not valid for node "
427 ch->size() += rangh->
getSize();
429 "rangeTX::queueing range packet with pkt_id: "
431 <<
" with # entries: " << (
int) rangh->
times_size_);
447 <<
" but must be >0!!!")
456 DEBUG(1,
"ERROR! updateHoldoverTime() called with NULL packet!")
466 "updateHoldoverTime::sending range packet with pkt_id: "
468 for (
int i = 0; i < rangh->
times_size(); i++) {
469 DEBUG(2,
"updateHoldoverTime::elem[" << i <<
"]")
470 auto &elem = (*rangh).times_[i];
472 DEBUG(2,
"updateHoldoverTime::elem.time = " << elem.time)
478 DEBUG(2,
"updateHoldoverTime::tx_duration = " << tx_duration)
481 "updateHoldoverTime::tx_timestamp = "
484 "updateHoldoverTime::entries_mat["
486 << (
int) elem.node <<
"].time = " << elem.time)
494 "updateHoldoverTime::times_mat["
513 DEBUG(1,
"ERROR! invalid pkt_id: "
514 << pkt_id <<
" from node " << pkt_node_id)
516 for (
size_t i = 0; i <
entries_mat[pkt_node_id][pkt_id].size(); ++i) {
521 double spherical_tof = -1.;
522 double spherical_timestamp = -1.;
523 for (
int i = 0; i < rangh->
times_size(); i++) {
524 auto &elem = (*rangh).times_[i];
526 entries_mat[pkt_node_id][pkt_id][elem.node] = elem;
528 DEBUG(2,
"rangeRX::elem[" << i <<
"]")
530 "rangeRX::entries_mat[" << pkt_node_id <<
"][" << pkt_id <<
"]["
532 <<
"].time = " << elem.time)
542 "rangeRX::times_mat["
543 <<
node_id <<
"][" << (
int) elem.id
545 DEBUG(2,
"rangeRX::elem.time = " << elem.time)
546 DEBUG(2,
"rangeRX::spherical_tof = " << spherical_tof)
548 if (spherical_tof >= -1e-3 && spherical_tof <
max_tt) {
550 std::abs(spherical_tof);
554 DEBUG(1,
"Negative spherical range tt!!! ");
560 for (
int i = 0; i < rangh->
times_size(); i++) {
561 auto &elem = (*rangh).times_[i];
567 DEBUG(2,
"rangeRX::elem[" << i <<
"]")
569 double hyperbolic_tof = (NOW -
577 "rangeRX::times_mat[" << (
int) elem.node <<
"]["
578 << (
int) elem.id <<
"] "
580 DEBUG(2,
"rangeRX::spherical_tof = " << spherical_tof)
582 "rangeRX::time_of_flights["
585 DEBUG(2,
"rangeRX::elem.time = " << elem.time)
586 DEBUG(2,
"rangeRX::hyperbolic_tof = " << hyperbolic_tof)
588 if (hyperbolic_tof >= -1e-3) {
590 std::abs(hyperbolic_tof);
592 std::min( spherical_timestamp,
595 DEBUG(1,
"Negative hyp range from node "
596 << (
int) elem.node <<
" to " << pkt_node_id);
611 return (entry.
id >= 0 && entry.
node >= 0 && entry.
time >= 0);
617 Tcl &tcl = Tcl::instance();
619 if (strcasecmp(argv[1],
"start") == 0) {
623 if (strcasecmp(argv[1],
"stop") == 0) {
627 if (strcasecmp(argv[1],
"sendRange") == 0) {
631 if (strcasecmp(argv[1],
"getRangePktsTx") == 0) {
635 if (strcasecmp(argv[1],
"getRangePktsRx") == 0) {
639 if (strcasecmp(argv[1],
"getRangeBytesTx") == 0) {
643 if (strcasecmp(argv[1],
"getRangePktsErr") == 0) {
647 if (strcasecmp(argv[1],
"getFullPktTime") == 0) {
651 if (strcasecmp(argv[1],
"getSavedEntries") == 0) {
655 if (strcasecmp(argv[1],
"calcOptEntries") == 0) {
661 if (strcasecmp(argv[1],
"setId") == 0) {
664 if (node_id < 0 || node_id > std::numeric_limits<uwrange_node_t>::max())
669 if (strcasecmp(argv[1],
"setPHYId") == 0) {
679 if (strcasecmp(argv[1],
"get_distance") == 0) {
680 int n1 = atoi(argv[2]);
681 int n2 = atoi(argv[3]);
683 if (n1 >= 0 && n1 < n_nodes && n2 >= 0 && n2 <
n_nodes) {
685 tcl.resultf(
"%.17f", 0.);
694 return Module::command(argc, argv);
718 double u = RNG::defaultrng()->uniform_double();
720 nextTX = (-log(u) / lambda);
packet_t PT_UWRANGING_TDOA
ClMessage_t CLMSG_TAP_PKT
ClMessage_t CLMSG_TRIGGER_STATS
clmsg_tap_type clmsg_type
packet to send via clmsg to other layers.
Packet * pkt
Verbosity level.
double tx_duration
rx or tx timestamp
double last_ber
SINR of the last received packet.
virtual void expire(Event *e)
Class that represent the binding of the protocol with tcl.
UwRangingTDOAModuleClass()
Constructor of the TDOAGenericModule class.
TclObject * create(int, const char *const *)
Creates the TCL object needed for the tcl language interpretation.
Class that represents a UwRangingTDOA Node.
double delay_start
Starting delay in seconds.
int dist_num
Number of distances each node computes.
virtual void updateHoldoverTime(Packet *p, double tx_duration)
Update the holdover time with the actual time of transmission.
double full_pkt_tx_dur
TX duration for a full range packet.
virtual void recv(Packet *) override
Performs the reception of packets from upper and lower layers.
virtual int calcOptEntries(std::vector< int > *sorted_entries=nullptr) const
Compute the optimal number of entries for the ranging message.
std::vector< double > entries_timestamps
int phy_id
Id of the PHY module.
virtual void rangeRX(const Packet *p)
int range_pkts_recv
Counter of received ranging packets.
double max_tt
Floating numbers within this value won't be discarded by NNLS.
virtual void stop()
Stop to send packets.
virtual int recvSyncClMsg(ClMessage *m) override
Retrieve packets from other modules.
int range_entries
Number of entries per ranging packet, if <= 0 enable adaptive payload size.
int n_nodes
Number of nodes.
virtual void rangeTX()
Send a ranging message down to the MAC layer.
UwRangingTDOA()
Constructor of the UwRangingTDOA class.
UwRangeTimer send_timer
Timer that schedules ranging packet transmissions.
int range_pkts_sent
Counter of sent ranging packets.
int packet_id
id of the last ranging packet sent (= node_id + k*n_nodes)
const size_t PKTIDMAX
Max number of ranging packets that can be saved.
std::vector< double > entry_last_tx
Vector of the last (actual) TX times of each entry.
std::vector< std::vector< int > > tof_map
std::vector< double > time_of_flights
bool scheduler_active
True if the ranging scheduler is been activated with Tcl "start" command.
int saved_entries
Total number of entries not sent in the ranging packets.
int range_bytes_sent
Counter of sent ranging bytes.
double max_age
Max age of a ranging packet in seconds.
virtual int command(int argc, const char *const *argv) override
TCL command interpreter.
std::vector< int > last_ids
int range_pkts_err
Counter of lost ranging packets.
double range_period
Ranging period in seconds.
std::vector< std::vector< double > > times_mat
virtual double entryWeight(int entry, int pkt_size, int mode) const
Compute the weight of the entry given the packet size in bits.
virtual bool isValid(double value) const
Check if value is not negative.
int poisson_traffic
If true the ranging packets are sent according to a poisson process with rate range_period.
std::vector< std::vector< std::vector< tdoa_entry > > > entries_mat
virtual void start()
Start to send packets.
int node_id
Id of the current node.
std::vector< uwrange_time_t > tx_timestamp
Vector of the TX timestamp for each packet to be sent.
double soundspeed
Speed of sound in m/s.
virtual void throttleTX()
Schedule the transmission of a rangig message.
int queue_size
Counts the packet sent to the MAC waiting to be transmitted.
std::vector< double > ber
Header of the token bus protocol.
size_t getSize() const
Returns the size of a ranging header with all the entries.
uwrange_node_t times_size_
uwrange_pkt_t source_pkt_id
Ranging packet id.
uwrange_node_t source_node_id
Source node id.
Define the struct of a single tdoa payload entry.
uwrange_pkt_t id
Id of the packet.
uwrange_time_t time
Holdover time.
uwrange_node_t node
Id of the source node.
Definition of UwPhysical class.
UwRangingTDOAModuleClass class_module_uwrangingtdoa
Provides the definition of the class UwRangingTDOA.
#define HDR_RANGING_TDOA(p)
alias defined to access the hdr_ranging_tdoa HEADER
Provides the definition of the class UWSMPosition.
Utilities to manage stats.
#define DEBUG(level, text)