Wiselib
|
00001 /* 00002 * File: echo.h 00003 * Author: Koninis, Amaxilatis 00004 * 00005 * Created on August 27, 2010, 12:32 PM 00006 */ 00007 00008 #ifndef ECHO_H 00009 #define ECHO_H 00010 00011 //wiselib includes 00012 #include "util/delegates/delegate.hpp" 00013 #include "util/pstl/vector_static.h" 00014 #include "util/pstl/pair.h" 00015 #include "pgb_payloads_ids.h" 00016 00017 #include "echomsg.h" 00018 00019 /* 00020 * DEBUG MESSAGES TEMPLATE 00021 * Echo::<task> [ type= ...] 00022 */ 00023 00024 //#define DEBUG_ECHO 00025 //#define DEBUG_ECHO_EXTRA 00026 //#define DEBUG_PIGGYBACKING 00027 #define MAX_PG_PAYLOAD 30 00028 #define ECHO_MAX_NODES 11 00029 00034 #define ENABLE_LQI_THRESHOLDS 00035 00040 //#define ENABLE_STABILITY_THRESHOLDS 00041 00042 //#define CALCULATE_INVERSE_STABILITY 00043 00048 #define ECHO_TIMES_ACC_NEARBY 2 00049 00050 #define SUNSPOT_TEST 00051 00052 namespace wiselib { 00061 template<typename OsModel_P, typename Radio_P, typename Timer_P, 00062 typename Debug_P> 00063 class Echo { 00064 public: 00065 // Type definitions 00066 typedef OsModel_P OsModel; 00067 00068 typedef Radio_P Radio; 00069 typedef Timer_P Timer; 00070 typedef Debug_P Debug; 00071 typedef typename OsModel_P::Clock Clock; 00072 00073 typedef typename Radio::node_id_t node_id_t; 00074 typedef typename Radio::size_t size_t; 00075 typedef typename Radio::block_data_t block_data_t; 00076 typedef typename Radio::message_id_t message_id_t; 00077 typedef typename Clock::time_t time_t; 00078 00079 typedef typename Radio::ExtendedData ExData; 00080 typedef typename Radio::TxPower TxPower; 00081 00082 typedef EchoMsg<OsModel, Radio> EchoMsg_t; 00083 typedef Echo<OsModel_P, Radio_P, Timer_P, Debug_P> self_t; 00084 TxPower power; 00085 00086 typedef delegate4<void, uint8_t, node_id_t, uint8_t, uint8_t*> 00087 event_notifier_delegate_t; 00088 // typedef status_delegate_t radio_delegate_t; 00089 00090 struct neighbor_entry { 00091 time_t last_echo; 00092 time_t timeout; 00093 time_t first_beacon; 00094 uint16_t last_lqi; 00095 uint16_t avg_lqi; 00096 uint8_t beacons_in_row; 00097 node_id_t id; 00098 uint32_t total_beacons; 00099 uint8_t inverse_link_assoc; 00100 uint16_t stability; 00101 bool active; 00102 bool stable; 00103 bool bidi; 00104 }; 00105 00106 struct reg_alg_entry { 00107 uint8_t alg_id; 00108 uint8_t data[MAX_PG_PAYLOAD]; 00109 uint8_t size; 00110 event_notifier_delegate_t event_notifier_callback; 00111 uint8_t events_flag; 00112 }; 00113 00114 // -------------------------------------------------------------------- 00115 typedef struct reg_alg_entry reg_alg_entry_t; 00116 typedef wiselib::vector_static<OsModel, reg_alg_entry_t, TOTAL_REG_ALG> 00117 reg_alg_vector_t; 00118 typedef typename reg_alg_vector_t::iterator reg_alg_iterator_t; 00119 00123 reg_alg_vector_t registered_apps; 00124 00128 typedef struct neighbor_entry neighbor_entry_t; 00129 00133 typedef wiselib::vector_static<OsModel, neighbor_entry_t, ECHO_MAX_NODES> 00134 node_info_vector_t; 00135 typedef typename node_info_vector_t::iterator iterator_t; 00136 00140 node_info_vector_t neighborhood; 00141 // -------------------------------------------------------------------- 00142 00143 enum error_codes { 00144 SUCCESS = OsModel::SUCCESS, 00145 RGD_NUM_INUSE = 1, 00146 RGD_LIST_FULL = 2, 00147 INV_ALG_ID = 3 00149 }; 00150 // -------------------------------------------------------------------- 00151 enum event_codes { 00152 NEW_NB = 1, 00153 NEW_NB_BIDI = 2, 00154 DROPPED_NB = 4, 00155 NEW_PAYLOAD = 8, 00156 NEW_PAYLOAD_BIDI = 16, 00157 LOST_NB_BIDI = 32, 00158 NB_READY = 64, 00161 DEFAULT = 5 00163 }; 00164 // -------------------------------------------------------------------- 00165 00171 Echo() : 00172 status_(WAITING) { 00173 } 00174 ; 00175 00180 ~Echo() { 00181 } 00182 ; 00183 00191 void enable() { 00192 00196 radio().enable_radio(); 00197 recv_callback_id_ = radio().template reg_recv_callback<self_t, 00198 &self_t::receive> (this); 00199 00203 init_echo(); 00204 00208 // power.set_dB( -30 ); 00209 // radio_->set_power( power ); 00210 00214 set_status(SEARCHING); 00215 00216 // send first beacon 00217 say_hello(0); 00218 00219 #ifdef DEBUG_ECHO 00220 debug().debug("Neighborhood discovery enabled in node %d\n",radio().id()); 00221 #endif 00222 } 00223 ; 00224 00225 // -------------------------------------------------------------------- 00226 00239 void disable() { 00240 set_status(WAITING); 00241 radio().template unreg_recv_callback(recv_callback_id_); 00242 }; 00243 00253 void init_echo() { 00254 neighborhood.clear(); 00255 node_stability = 0; 00256 } 00257 ; 00258 00259 uint16_t msgs_count() { 00260 return msgs_stats.echo_msg_count; 00261 } 00262 ; 00263 00264 uint32_t msgs_size() { 00265 return msgs_stats.echo_msg_size; 00266 } 00267 ; 00268 00269 uint16_t get_node_stability() { 00270 return node_stability; 00271 } 00272 ; 00273 00278 uint8_t register_payload_space(uint8_t payload_id) { 00279 00280 if (registered_apps.empty()) { 00281 reg_alg_entry_t entry;// = {payload_id, 0, 0, event_notifier_delegate_t(), 0}; 00282 entry.alg_id = payload_id; 00283 entry.size = 0; 00284 entry.events_flag = 0; 00285 entry.event_notifier_callback = event_notifier_delegate_t(); 00286 00287 // entry.events_flag = events_flag; 00288 registered_apps.push_back(entry); 00289 } else if (registered_apps.max_size() == registered_apps.size()) { 00290 return RGD_LIST_FULL; 00291 } else { 00292 for (size_t i = 0; i < registered_apps.size(); i++) 00293 if (registered_apps.at(i).alg_id == payload_id) 00294 return RGD_NUM_INUSE; 00295 00296 reg_alg_entry_t entry;// = {payload_id, 0, 0, event_notifier_delegate_t(), 0}; 00297 entry.alg_id = payload_id; 00298 entry.size = 0; 00299 entry.events_flag = 0; 00300 entry.event_notifier_callback = event_notifier_delegate_t(); 00301 00302 /* entry.alg_id = payload_id; 00303 entry.size = 0; 00304 entry.status_notifier_callback = status_notifier_delegate_t(); 00305 */ 00306 // entry.events_flag = events_flag; 00307 registered_apps.push_back(entry); 00308 } 00309 00310 return 0; 00311 } 00312 00316 uint8_t unregister_payload_space(uint8_t payload_id) { 00317 00318 for (reg_alg_iterator_t it = registered_apps.begin(); it 00319 != registered_apps.end(); it++) { 00320 if (it->alg_id == payload_id) { 00321 registered_apps.erase(it); 00322 return SUCCESS; 00323 } 00324 } 00325 00326 return INV_ALG_ID; 00327 } 00328 00333 uint8_t set_payload(uint8_t payload_id, uint8_t *data, uint8_t len) { 00334 00335 for (reg_alg_iterator_t it = registered_apps.begin(); it 00336 != registered_apps.end(); it++) { 00337 if (it->alg_id == payload_id) { 00338 memcpy(it->data, data, len); 00339 it->size = len; 00340 return 0; 00341 } 00342 } 00343 00344 return INV_ALG_ID; 00345 } 00346 00347 bool is_neighbor(node_id_t id) { 00348 for (iterator_t it = neighborhood.begin(); it != neighborhood.end(); ++it) { 00349 if (it->stable && it->id == id) 00350 return true; 00351 } 00352 return false; 00353 } 00354 00355 bool is_neighbor_bidi(node_id_t id) { 00356 for (iterator_t it = neighborhood.begin(); it != neighborhood.end(); ++it) { 00357 if (it->bidi && it->id == id) 00358 return true; 00359 } 00360 return false; 00361 } 00362 00363 uint8_t nb_size(void) { 00364 uint8_t size = 0; 00365 for (iterator_t it = neighborhood.begin(); it != neighborhood.end(); ++it) { 00366 if (it->active) 00367 size++; 00368 } 00369 00370 return size; 00371 } 00372 00373 uint8_t stable_nb_size(void) { 00374 uint8_t size = 0; 00375 for (iterator_t it = neighborhood.begin(); it != neighborhood.end(); ++it) { 00376 if (it->stable) 00377 size++; 00378 } 00379 00380 return size; 00381 } 00382 00383 uint8_t bidi_nb_size(void) { 00384 uint8_t size = 0; 00385 for (iterator_t it = neighborhood.begin(); it != neighborhood.end(); ++it) { 00386 if (it->bidi) 00387 size++; 00388 } 00389 00390 return size; 00391 } 00392 00393 uint8_t get_link_assoc(node_id_t neighbor_id) { 00394 for (iterator_t it = neighborhood.begin(); it != neighborhood.end(); ++it) { 00395 if (it->id == neighbor_id) { 00396 return it->beacons_in_row; 00397 } 00398 } 00399 return 0; 00400 } 00401 00402 uint8_t get_ilink_assoc(node_id_t neighbor_id) { 00403 for (iterator_t it = neighborhood.begin(); it != neighborhood.end(); ++it) { 00404 if (it->id == neighbor_id) { 00405 return it->inverse_link_assoc; 00406 } 00407 } 00408 return 0; 00409 } 00410 00411 uint8_t get_nb_stability(node_id_t id) { 00412 for (iterator_t it = neighborhood.begin(); it != neighborhood.end(); ++it) { 00413 if (it->id == id) { 00414 return it->stability; 00415 } 00416 } 00417 return 0; 00418 } 00419 00420 uint8_t get_nb_receive_stability(node_id_t id) { 00421 uint8_t stability = 0; 00422 for (iterator_t it = neighborhood.begin(); it != neighborhood.end(); ++it) { 00423 if (it->id == id) { 00424 uint32_t millis = (uint32_t) clock().milliseconds( 00425 clock().time()) + clock().seconds(clock().time()) 00426 * 1000 - (uint32_t) clock().milliseconds( 00427 it->first_beacon) - (uint32_t) clock().seconds( 00428 it->first_beacon) * 1000; 00429 uint32_t beacons_send = (millis / beacon_period) + 1; 00430 00431 #ifdef DEBUG_ECHO 00432 if ( beacons_send < it->total_beacons ) 00433 debug().debug( "WARNING beacons_send %d total_beacons %d\n",beacons_send,it->total_beacons); 00434 #endif 00435 00436 stability = (it->total_beacons * 100) / beacons_send; 00437 #ifdef DEBUG_ECHO 00438 if ( stability > 100 ) { 00439 debug().debug( "stability of %x is %d\n",it->id,stability); 00440 } 00441 #endif 00442 00443 break; 00444 } 00445 } 00446 00447 return stability; 00448 } 00449 00453 void init(Radio& radio, Clock& clock, Timer& timer, Debug& debug) { 00454 radio_ = &radio; 00455 clock_ = &clock; 00456 timer_ = &timer; 00457 debug_ = &debug; 00458 beacon_period = 1000; 00459 timeout_period = 15000; 00460 min_lqi_threshold = 160; 00461 max_lqi_threshold = 200; 00462 max_stability_threshold = 40; 00463 min_stability_threshold = 20; 00464 }; 00465 00469 void init(Radio& radio, Clock& clock, Timer& timer, Debug& debug, 00470 uint16_t beacon_pd, uint16_t timeout_pd, uint8_t min_theshold, 00471 uint8_t max_threshold) { 00472 radio_ = &radio; 00473 clock_ = &clock; 00474 timer_ = &timer; 00475 debug_ = &debug; 00476 beacon_period = beacon_pd; 00477 timeout_period = timeout_pd; 00478 min_lqi_threshold = min_theshold; 00479 max_lqi_threshold = max_threshold; 00480 min_stability_threshold = min_theshold; 00481 max_stability_threshold = max_threshold; 00482 }; 00483 00484 void set_beacon_period(uint16_t beacon_pd) { 00485 beacon_period = beacon_pd; 00486 } 00487 ; 00488 00489 void set_timeout_perdio(uint16_t timeout_pd) { 00490 timeout_period = timeout_pd; 00491 } 00492 ; 00493 // -------------------------------------------------------------------- 00494 template<class T, void(T::*TMethod)(uint8_t, node_id_t, uint8_t, uint8_t*)> 00495 uint8_t reg_event_callback(uint8_t alg_id, uint8_t events_flag, T *obj_pnt) { 00496 00497 for (reg_alg_iterator_t it = registered_apps.begin(); it 00498 != registered_apps.end(); it++) { 00499 if (it->alg_id == alg_id) { 00500 it->event_notifier_callback 00501 = event_notifier_delegate_t::template from_method<T, 00502 TMethod>(obj_pnt); 00503 it->events_flag = events_flag; 00504 return 0; 00505 } 00506 } 00507 00508 reg_alg_entry_t entry; 00509 entry.alg_id = alg_id; 00510 entry.size = 0; 00511 entry.event_notifier_callback 00512 = event_notifier_delegate_t::template from_method<T, TMethod>( 00513 obj_pnt); 00514 entry.events_flag = events_flag; 00515 registered_apps.push_back(entry); 00516 00517 return 0; 00518 // return INV_ALG_ID; 00519 } 00520 // -------------------------------------------------------------------- 00521 void unreg_event_callback(uint8_t alg_id) { 00522 for (reg_alg_iterator_t it = registered_apps.begin(); it 00523 != registered_apps.end(); it++) { 00524 if (it->alg_id == alg_id) { 00525 it->event_notifier_callback = event_notifier_delegate_t(); 00526 return; 00527 } 00528 } 00529 } 00530 00535 void register_debug_callback(uint8_t flags) { 00536 if (flags == 0) { 00537 flags = self_t::NEW_NB | self_t::NEW_NB_BIDI | self_t::DROPPED_NB 00538 | self_t::NEW_PAYLOAD_BIDI | self_t::LOST_NB_BIDI; 00539 } 00540 reg_event_callback<self_t, &self_t::debug_callback> (7, flags, this); 00541 } 00542 00543 private: 00544 00550 void say_hello(void * a) { 00551 00552 // Check for Neighbors that do not exist and need to be dropped 00553 cleanup_nearby(); 00554 00555 // if in searching mode send a new beacon 00556 if (status() == SEARCHING) { 00557 00558 // prepare the echo message 00559 EchoMsg_t echomsg; 00560 00561 // neighbor_entry_t new_nb_entry; 00562 // new_nb_entry.id=66; 00563 // new_nb_entry.beacons_in_row=1; 00564 00565 // if (radio().id()==9) 00566 // neighborhood.push_back(new_nb_entry); 00567 00568 // create the list based on local stable neighbors 00569 add_list_to_beacon(&echomsg); 00570 /* 00571 if (radio().id()==9) 00572 debug().debug("TEST: id: %d stability: %d size of list of neighbors: %d\n",read<OsModel, block_data_t, node_id_t> ( 00573 echomsg.payload()),read<OsModel, block_data_t, uint8_t> ( 00574 echomsg.payload() + sizeof(node_id_t)) 00575 ,echomsg.nb_list_size());*/ 00576 #ifdef ENABLE_STABILITY_THRESHOLDS 00577 echomsg.add_nb_entry(radio().id()); 00578 echomsg.add(get_node_stability()); 00579 00580 /* if (radio().id()==9) 00581 debug().debug("TEST: id: %d stability: %d size of list of neighbors: %d\n",read<OsModel, block_data_t, node_id_t> ( 00582 echomsg.payload() + sizeof(node_id_t)+sizeof(uint8_t) ),read<OsModel, block_data_t, uint16_t> ( 00583 echomsg.payload() + sizeof(node_id_t)*2 + sizeof(uint8_t)) 00584 ,echomsg.nb_list_size());*/ 00585 #endif 00586 add_pg_payload(&echomsg); 00587 00588 00589 //send the Beacon 00590 00591 #ifdef SUNSPOT_TEST 00592 block_data_t buffer[Radio::MAX_MESSAGE_LENGTH]; // buffer for the message data 00593 00594 buffer[0] = 0x7f; 00595 buffer[1] = 0x69; 00596 buffer[2] = 110; 00597 00598 memcpy( buffer+3, (uint8_t *) &echomsg, echomsg.buffer_size() ); 00599 radio().send(Radio::BROADCAST_ADDRESS, echomsg.buffer_size()+3, (uint8_t *) buffer); 00600 #else 00601 radio().send(Radio::BROADCAST_ADDRESS, echomsg.buffer_size(), 00602 (uint8_t *) &echomsg); 00603 #endif 00604 00605 msgs_stats.echo_msg_count++; 00606 msgs_stats.echo_msg_size += echomsg.buffer_size(); 00607 00608 #ifdef DEBUG_ECHO_EXTRA 00609 debug().debug( "Debug::echo::say_hello(%d) msg size= %d\n", radio().id(), echomsg.buffer_size()); 00610 debug().debug( "HELLOMSG[ %d | %d | ", echomsg.data()[0], echomsg.data()[1]); 00611 for (uint16_t i = 0; i < echomsg.buffer_size(); i++) 00612 debug().debug( "%d ", echomsg.data()[i] ); 00613 // debug().debug( "%d ", hellomsg.data()[2 + 2 * i] + hellomsg.data()[2 + 2 * i + 1]*256); 00614 debug().debug( "]\n"); 00615 #endif 00616 #ifdef DEBUG_ECHO_EXTRA 00617 show_nearby(); 00618 #endif 00619 } 00620 00621 //Reset the timoout for the next beacon 00622 timer().template set_timer<self_t, &self_t::say_hello> (beacon_period, 00623 this, (void*) 0); 00624 } 00625 ; 00626 00633 void receive(node_id_t from, size_t len, block_data_t * msg, 00634 ExData const &ex) { 00635 // void receive(node_id_t from, size_t len, block_data_t * msg) { 00636 00637 #ifdef SUNSPOT_TEST 00638 len-=3; 00639 msg=msg+3; 00640 #endif 00641 00642 #ifdef ENABLE_LQI_THRESHOLDS 00643 #ifndef SHAWN 00644 if ( ex.link_metric() > max_lqi_threshold ) { 00645 return; 00646 } 00647 #endif 00648 #endif 00649 // if in waiting status do not process messages 00650 if (status() == WAITING) 00651 return; 00652 00653 // if own return 00654 if (from == radio().id()) 00655 return; 00656 00657 00658 // if it is a beacon 00659 if (*msg == EchoMsg_t::HELLO_MESSAGE) { 00660 00661 EchoMsg_t *recvmsg; 00662 recvmsg = (EchoMsg_t *)msg; 00663 // memcpy(&recvmsg, msg, len); 00664 00665 #ifdef DEBUG_ECHO_EXTRA 00666 debug().debug( "Debug::echo::receive node %d got beacon from %d length %d \n", radio().id(), from, len); 00667 #endif 00668 00669 // check the beacons sender status 00670 received_beacon(from, ex); 00671 00672 for (iterator_t 00673 it = neighborhood.begin(); 00674 it != neighborhood.end(); 00675 ++it) { 00676 00677 bool contains_my_id = false; 00678 if (!it->active) { 00679 continue; 00680 } 00681 00682 if (it->id == from ) { 00683 00684 uint8_t nb_size_bytes = recvmsg->nb_list_size(); 00685 uint8_t bytes_read = 0; 00686 00687 00688 while (nb_size_bytes != bytes_read) { 00689 00690 node_id_t neighbor_id = read<OsModel, block_data_t, node_id_t> ( 00691 recvmsg->payload() + bytes_read); 00692 bytes_read += sizeof(node_id_t); 00693 // debug().debug( "Debug::echo::receive %d got beacon from %d bytes_read= %d \n", radio().id(), from, bytes_read); 00694 00695 /* if (radio().id()==4 && from==9) { 00696 debug().debug( "Debug::echo::receive %d got beacon from %d bytes_read= %d \n", radio().id(), from, bytes_read); 00697 debug().debug("TEST2: id: %d stability: %d size of list of neighbors: %d\n",read<OsModel, block_data_t, node_id_t> ( 00698 recvmsg->payload()),read<OsModel, block_data_t, uint8_t> ( 00699 recvmsg->payload() + sizeof(node_id_t)) 00700 ,recvmsg->nb_list_size()); 00701 }*/ 00702 00703 if ( neighbor_id == radio().id()) { 00704 #ifndef ENABLE_STABILITY_THRESHOLDS 00705 contains_my_id = true; 00706 #endif 00707 // debug().debug( "Debug::echo::NO %d got beacon from %d size= %d \n", radio().id(), bytes_read, nb_size_bytes); 00708 00709 #ifdef CALCULATE_INVERSE_STABILITY 00710 it->inverse_link_assoc 00711 = read<OsModel, block_data_t, uint8_t> ( 00712 recvmsg->payload() + bytes_read ); 00713 // debug().debug( "Debug::echo::XXXXXX %d from %d it->inverse_link_assoc %d\n", 00714 // radio().id(), from, it->inverse_link_assoc); 00715 00716 00717 bytes_read += sizeof(uint8_t); 00718 #endif 00719 #ifndef ENABLE_STABILITY_THRESHOLDS 00720 break; 00721 #endif 00722 } 00723 #ifdef ENABLE_STABILITY_THRESHOLDS 00724 else if (neighbor_id == from) { 00725 00726 00727 it->stability = read<OsModel, block_data_t, uint16_t > (recvmsg->payload()+bytes_read); 00728 // debug().debug( "Debug::echo::received_beacon::%d stability %d threshold %d\n", radio().id(), it->stability, max_stability_threshold); 00729 /* 00730 if (radio().id()==4&& from==9) 00731 debug().debug( "Debug::echo::XXXXXX %d from %d stability %d iLinkAssoc %d linkAssoc %d\n", 00732 radio().id(), bytes_read, nb_size_bytes , get_ilink_assoc(from), it->inverse_link_assoc);*/ 00733 00734 bytes_read+=sizeof(uint16_t); 00735 if ( 00736 //((6 * node_stability > 5 * it->stability) 00737 //&& ( 4 * node_stability < 5 * it->stability)) 00738 //&& 00739 (it->stability > max_stability_threshold) && 00740 (node_stability > max_stability_threshold) 00741 ) { 00742 contains_my_id = true; 00743 } 00744 00745 /* if (radio().id()==4 && from==9) { 00746 debug().debug( "Debug::echo::YES %d got beacon from %d size= %d \n", radio().id(), bytes_read, nb_size_bytes); 00747 // exit(1); 00748 }*/ 00749 } 00750 #endif 00751 #ifdef CALCULATE_INVERSE_STABILITY 00752 else { 00753 bytes_read+=sizeof(uint8_t); 00754 } 00755 #endif 00756 } 00757 00758 if (!it->stable) { 00759 continue; 00760 } 00761 #ifdef DEBUG_ECHO 00762 #ifdef ISENSE 00763 debug().debug( "Debug::echo NODE %x has bidirectional communication with %x", radio().id(), from); 00764 #else 00765 #endif 00766 debug().debug( "Debug::echo NODE %d has bidirectional communication with %d\n", radio().id(), from); 00767 #endif 00768 00769 if (contains_my_id) { 00770 if (!it->bidi) { 00771 it->bidi = true; 00772 notify_listeners(NEW_NB_BIDI, from, 0, 0); 00773 } 00774 00775 } 00776 else { 00777 if (it->bidi) { 00778 it->bidi = false; 00779 notify_listeners(LOST_NB_BIDI, from, 0, 0); 00780 } 00781 } 00782 00783 uint8_t * alg_pl = recvmsg->payload() 00784 + recvmsg->nb_list_size(); 00785 for (int i = 0; i < recvmsg->get_pg_payloads_num(); i++) { 00786 00787 #ifdef DEBUG_PIGGYBACKING 00788 debug().debug( "Debug::echo NODE %d: new payload from %d with alg_id %d and size %d ", 00789 radio().id(), from, *alg_pl, *(alg_pl+1) ); 00790 00791 debug().debug( " ["); 00792 for (uint8_t j = 1; j<= *(alg_pl + 1); j++) { 00793 debug().debug( "%d ", *(alg_pl + j + 1) ); 00794 } 00795 debug().debug( "]\n"); 00796 #endif 00797 00798 for (reg_alg_iterator_t it = registered_apps.begin(); it 00799 != registered_apps.end(); it++) { 00800 00801 if ((it->alg_id == *alg_pl) 00802 && (it->event_notifier_callback != 0)) { 00803 if ((it->events_flag & (uint8_t) NEW_PAYLOAD) 00804 == (uint8_t) NEW_PAYLOAD) { 00805 it->event_notifier_callback(NEW_PAYLOAD, 00806 from, *(alg_pl + 1), alg_pl + 2); 00807 } else if (((it->events_flag 00808 & (uint8_t) NEW_PAYLOAD_BIDI) 00809 == (uint8_t) NEW_PAYLOAD_BIDI) 00810 && is_neighbor_bidi(from)) { 00811 it->event_notifier_callback( 00812 NEW_PAYLOAD_BIDI, from, *(alg_pl 00813 + 1), alg_pl + 2); 00814 } 00815 } 00816 } 00817 00818 alg_pl += *(alg_pl + 1) + 2; 00819 00820 #ifdef DEBUG_ECHO 00821 #ifdef ISENSE 00822 debug().debug( "Debug::echo NODE %x has bidirectional communication with %x", radio().id(), from); 00823 #else 00824 debug().debug( "Debug::echo NODE %d has bidirectional communication with %d\n", radio().id(), from); 00825 #endif 00826 #endif 00827 } 00828 break; 00829 } 00830 00831 } 00832 } 00833 00834 } 00835 ; 00836 00841 #ifdef DEBUG_ECHO_EXTRA 00842 void show_nearby() { 00843 debug().debug( "Debug::echo::show_nearby(%d)\n", radio().id()); 00844 00845 for (iterator_t 00846 it = neighborhood.begin(); 00847 it != neighborhood.end(); 00848 ++it) { 00849 if (it->bidi) 00850 debug().debug( " (%dbidi %f %f)", it->id, it->last_echo, clock().time()); 00851 else 00852 debug().debug( " (%duni %f %f)", it->id, it->last_echo, clock().time()); 00853 } 00854 00855 debug().debug( "\n"); 00856 }; 00857 #endif 00858 00866 void received_beacon(node_id_t from, ExData ex) { 00867 // known is true if node from was contacted before 00868 bool known = false; 00869 iterator_t it = neighborhood.begin(); 00870 // iterate nearby nodes to see if from is known 00871 for (; it != neighborhood.end(); ++it) { 00872 // if known 00873 if (it->id == from) { 00874 00875 // debug().debug( "Debug::echo::received_beacon::%d new neighbor %d stability %d iLinkAssoc %d linkAssoc %d\n", 00876 // radio().id(), from, get_nb_stability(from) , get_ilink_assoc(from), get_link_assoc(from)); 00877 00878 it->total_beacons++; 00879 00880 if (!it->active) { 00881 break; 00882 } 00883 00884 #ifdef ENABLE_LQI_THRESHOLDS 00885 #ifndef SHAWN 00886 if (!it->stable) { 00887 if ( ex.link_metric() > min_lqi_threshold ) { 00888 return; 00889 } 00890 } 00891 #endif 00892 #endif 00893 00894 // set the latest beacon received to now 00895 it->last_echo = clock().time(); 00896 // increase the beacons received so far by one 00897 if (it->beacons_in_row != 255) { 00898 it->beacons_in_row++; 00899 } 00900 00901 it->last_lqi = ex.link_metric(); 00902 // it->timeout = it->last_echo + timeout_period; 00903 00904 // set as a known source node 00905 known = true; 00906 00907 #ifdef ENABLE_STABILITY_THRESHOLDS 00908 // debug().debug( "Debug::echo::received_beacon2::%d stability %d threshold %d\n", radio().id(), it->stability, max_stability_threshold); 00909 if ( 00910 // ((6 * it->stability > 5 * node_stability) 00911 // && ( 4 * it->stability < 5 * node_stability)) 00912 // && (!it->stable) 00913 // && 00914 it->stability > max_stability_threshold) 00915 // && (it->stability * ((255 + it->inverse_link_assoc)/510) > max_stability_threshold)) 00916 { 00917 it->stable = true; 00918 notify_listeners(NEW_NB, from, 0, 0); 00919 } 00920 #else 00921 //if heard ECHO_TIMES_ACC_NEARBY or more beacons in a row add to listen_only 00922 if ((it->beacons_in_row == ECHO_TIMES_ACC_NEARBY) 00923 && (!it->stable)) { 00924 // add to the listen only vector 00925 it->stable = true; 00926 notify_listeners(NEW_NB, from, 0, 0); 00927 #ifdef DEBUG_ECHO 00928 #ifdef ISENSE_APP 00929 debug().debug( "Debug::echo NODE %x can listen messages of %x", radio().id(), from); 00930 #else 00931 debug().debug( "Debug::echo NODE %d can listen messages of %d\n", radio().id(), from); 00932 #endif 00933 #endif 00934 } 00935 #endif 00936 break; 00937 } 00938 } 00939 00940 // if not known so far add to the vector if space available 00941 if (!known) { 00942 00943 #ifdef ENABLE_LQI_THRESHOLDS 00944 #ifndef SHAWN 00945 if ( ex.link_metric() > min_lqi_threshold ) { 00946 return; 00947 } 00948 #endif 00949 #endif 00950 if (neighborhood.size() < neighborhood.max_size()) { 00951 00952 if (it == neighborhood.end()) { 00953 // create a new struct entry for the vector 00954 neighbor_entry_t new_nb_entry; 00955 new_nb_entry.id = from; 00956 new_nb_entry.first_beacon = clock().time(); 00957 new_nb_entry.last_echo = clock().time(); 00958 // new_nb_entry.timeout = new_nb_entry.last_echo + timeout_period; 00959 new_nb_entry.beacons_in_row = 1; 00960 new_nb_entry.stability = 0; 00961 new_nb_entry.inverse_link_assoc = 0; 00962 new_nb_entry.total_beacons = 1; 00963 new_nb_entry.active = true; 00964 new_nb_entry.stable = false; 00965 new_nb_entry.bidi = false; 00966 00967 // a.uptime = ((double)a.time_known-(double)a.beacons_missed)/(double)a.time_known; 00968 //add the struct to the vector 00969 neighborhood.push_back(new_nb_entry); 00970 } else { 00971 it->active = true; 00972 it->last_echo = clock().time(); 00973 it->beacons_in_row = 1; 00974 it->stable = false; 00975 it->bidi = false; 00976 it->total_beacons++; 00977 } 00978 00979 //debug().debug("Added new neighbor %d %d\n",radio().id(),from); 00980 00981 } 00982 } 00983 00984 }; 00985 00992 void debug_callback(uint8_t event, node_id_t from, uint8_t len, 00993 uint8_t* data) { 00994 /* 00995 if (self_t::NEW_PAYLOAD == event) { 00996 debug_->debug("event NEW_PAYLOAD!! \n"); 00997 debug_->debug("NODE %d: new payload from %d with size %d ", 00998 radio_->id(), from, len); 00999 01000 //print payload 01001 debug_->debug(" ["); 01002 for (uint8_t j = 0; j < len; j++) { 01003 debug_->debug("%d ", *(data + j)); 01004 } 01005 debug_->debug("]\n"); 01006 } else if (self_t::NEW_PAYLOAD_BIDI == event) { 01007 debug_->debug("event NEW_PAYLOAD_BIDI!! \n"); 01008 debug_->debug("NODE %d: new payload from %d (bidi) with size %d ", 01009 radio_->id(), from, len); 01010 01011 //print payload 01012 debug_->debug(" ["); 01013 for (uint8_t j = 0; j < len; j++) { 01014 debug_->debug("%d ", *(data + j)); 01015 } 01016 debug_->debug("]\n"); 01017 } else */ 01018 01019 if (self_t::NEW_NB == event) { 01020 #ifdef SHAWN 01021 debug_->debug( 01022 "NEW_NB;%x;Time;%d; Node ;%x; has ;%d; neighbors;stability;%d\n", 01023 from, clock_->seconds(clock_->time()), radio_->id(), 01024 stable_nb_size(), node_stability); 01025 #else 01026 debug_->debug( "NB;%x;%x" , from, radio_->id()); 01027 #endif 01028 } else if (self_t::NEW_NB_BIDI == event) { 01029 #ifdef SHAWN 01030 debug_->debug( 01031 "NEW_NB_BIDI;%x;Time;%d; Node ;%x; has ;%d; neighbors;stability;%d\n", 01032 from, clock_->seconds(clock_->time()), radio_->id(), 01033 stable_nb_size(), node_stability); 01034 #else 01035 debug_->debug( "NBB;%x;%x" , from, radio_->id()); 01036 #endif 01037 } else if (self_t::DROPPED_NB == event) { 01038 #ifdef SHAWN 01039 debug_->debug( 01040 "DROPPED_NB;%x;Time;%d; Node ;%x; has ;%d; neighbors;stability;%d\n", 01041 from, clock_->seconds(clock_->time()), radio_->id(), 01042 stable_nb_size(), node_stability); 01043 #else 01044 debug_->debug( "NBD;%x;%x" , from, radio_->id()); 01045 #endif 01046 } else if (self_t::LOST_NB_BIDI == event) { 01047 #ifdef SHAWN 01048 debug_->debug( 01049 "LOST_NB_BIDI;%x;Time;%d; Node ;%x; has ;%d; neighbors;stability;%d\n", 01050 from, clock_->seconds(clock_->time()), radio_->id(), 01051 stable_nb_size(), node_stability); 01052 #else 01053 debug_->debug( "NBL;%x;%x" , from, radio_->id()); 01054 #endif 01055 } 01056 01057 } 01058 01059 /* 01060 * Search the list of the nearby nodes 01061 * for nodes that have missed too many 01062 * beacons and remove them from the neighborhood 01063 */ 01064 void cleanup_nearby() { 01065 01066 uint32_t current_millisec = (uint32_t) clock().milliseconds( 01067 clock().time()) + clock().seconds(clock().time()) * 1000; 01068 01069 if (clock().seconds(clock().time()) == 10) { 01070 notify_listeners(NB_READY, 0, 0, 0); 01071 01072 } 01073 // iterate the nearby_nodes vector for the missing nodes 01074 for (iterator_t 01075 it = neighborhood.begin(); 01076 it != neighborhood.end(); 01077 ++it) { 01078 01079 if (!it->active) 01080 continue; 01081 01082 uint32_t last_echo_millisec = clock().seconds(it->last_echo) * 1000 01083 + (uint32_t) clock().milliseconds(it->last_echo); 01084 01085 // debug().debug( "Debug::echo NODE %d cleanup %d %d\n", 01086 // radio().id(), 01087 // last_echo_millisec , 01088 // current_millisec ); 01089 01090 if ((last_echo_millisec + beacon_period + 40) < current_millisec) { 01091 it->beacons_in_row = 0; 01092 } 01093 //TODO: Add a delta to last_echo_millisec 01094 // if last echo was too long before 01095 if ((last_echo_millisec + (uint32_t) timeout_period) 01096 < current_millisec) { 01097 01098 // remove the node from the neighborhood 01099 // remove_from_neighborhood(it); 01100 // neighborhood.erase(it); 01101 if (it->stable) { 01102 // debug().debug( "::timout NODE %x dropped from neighbors %x", it->id, radio().id(),it->stability); 01103 notify_listeners(DROPPED_NB, it->id, 0, 0); 01104 } 01105 it->active = false; 01106 it->stable = false; 01107 it->bidi = false; 01108 it->beacons_in_row = 0; 01109 it->stability = 0; 01110 01111 #ifdef DEBUG_ECHO 01112 #ifdef ISENSE 01113 debug().debug( "Debug::echo NODE %x dropped from neighbors %x", radio().id(), it->id); 01114 #else 01115 debug().debug( "Debug::echo NODE %d droped from neighbors %d\n", radio().id(), it->id); 01116 #endif 01117 #endif 01118 cleanup_nearby(); 01119 break; 01120 } 01121 /* 01122 #ifdef ENABLE_STABILITY_THRESHOLDSX 01123 else if (( it->stability < min_stability_threshold) && (it->stable)) { 01124 // debug().debug( "Debug::echo NODE %d dpd %d \n", radio().id(), it->id); 01125 01126 // debug().debug( "::ss NODE %x dropped from neighbors %x", it->id, radio().id(),it->stability); 01127 notify_listeners(DROPPED_NB, it->id, 0, 0); 01128 // it->beacons_in_row = 0; 01129 it->active = false; 01130 it->stable = false; 01131 it->bidi = false; 01132 cleanup_nearby(); 01133 break; 01134 } 01135 #endif 01136 */ 01137 } 01138 01142 uint16_t new_node_stability = 0; 01143 uint16_t nodes_counted = 0; 01144 for (iterator_t 01145 it = neighborhood.begin(); 01146 it != neighborhood.end(); 01147 ++it) { 01148 #ifdef CALCULATE_INVERSE_STABILITY 01149 if (it->inverse_link_assoc > 0) { 01150 new_node_stability += it->inverse_link_assoc; 01151 nodes_counted++; 01152 } 01153 /*else if (it->inverse_link_assoc > 1) { 01154 nodes_counted++; 01155 }*/ 01156 01157 // debug().debug( "Debug::echo %d Adding [%d] with iAssoc %d\n", radio().id(), it->id, it->inverse_link_assoc); 01158 #else 01159 if ( it->beacons_in_row > 0 ) { 01160 new_node_stability += it->beacons_in_row; 01161 nodes_counted++; 01162 } 01163 // debug().debug( "Debug::echo %d Adding [%d] with Assoc %d\n", radio().id(), it->id, it->beacons_in_row); 01164 #endif 01165 } 01166 01170 if (nodes_counted != 0) { 01171 new_node_stability = new_node_stability / nodes_counted; 01172 } 01173 01177 node_stability = (node_stability * 3 + new_node_stability) / 4; 01178 // node_stability = weight1 * node_stability + weight2 * node_stability_prv; 01179 01180 // debug_->debug( "new;%x;Time;%d; Node ;%x; has ;%d; neighbors;stability;%d" , from, clock_->seconds(clock_->time()), radio_->id(), stable_nb_size(),node_stability); 01181 }; 01182 01189 void set_status(int status) { 01190 #ifdef DEBUG_ECHO_EXTRA 01191 if (status == SEARCHING) 01192 debug().debug( "Debug::echo::set_status SEARCHING\n"); 01193 else 01194 debug().debug( "Debug::echo::set_status WAITING\n"); 01195 #endif 01196 status_ = status; 01197 } 01198 ; 01199 01203 int status() { 01204 return status_; 01205 } 01206 ; 01207 01208 // -------------------------------------------------------------------- 01209 01210 void notify_listeners(uint8_t event, node_id_t from, uint8_t len, 01211 uint8_t *data) { 01212 01213 for (reg_alg_iterator_t ait = registered_apps.begin(); ait 01214 != registered_apps.end(); ++ait) { 01215 01216 if ((ait->event_notifier_callback != 0) && ((ait->events_flag 01217 & (uint8_t) event) == (uint8_t) event)) { 01218 01219 ait->event_notifier_callback(event, from, len, data); 01220 01221 } 01222 } 01223 // debug_callback(event, from, len, data); 01224 } 01229 void add_pg_payload(EchoMsg_t * msg) { 01230 01231 for (reg_alg_iterator_t ait = registered_apps.begin(); ait 01232 != registered_apps.end(); ++ait) { 01233 if (ait->size != 0) { 01234 msg->append_payload(ait->alg_id, ait->data, ait->size); 01235 } 01236 } 01237 } 01238 01243 void add_list_to_beacon(EchoMsg_t * msg) { 01244 01245 // add only the stable neighbor nodes to the array 01246 for (iterator_t 01247 it = neighborhood.begin(); 01248 it != neighborhood.end(); 01249 ++it) { 01250 #ifdef CALCULATE_INVERSE_STABILITY 01251 if (it->active) { 01252 msg->add_nb_entry(it->id); 01253 msg->add(it->beacons_in_row); 01254 } 01255 #else 01256 if (it->stable) { 01257 msg->add_nb_entry(it->id); 01258 } 01259 #endif 01260 } 01261 01262 // uint8_t nb_size_bytes = msg->nb_list_size(); 01263 // uint8_t nb_size = nb_size_bytes/(sizeof(node_id_t)+1); 01264 //debug().debug( "Debug::echo NODE %d has %d %d\n", 01265 // radio().id(), 01266 // nb_size_bytes, 01267 // nb_size); 01268 01269 //debug().debug( "Debug::echo NODEX %d \n",radio().id()); 01270 // 01271 // for (int i = 0; i < nb_size_bytes; i++ ) { 01272 //debug().debug( "%d ",read<OsModel, block_data_t, uint8_t > (msg->payload()+i)); 01273 01274 //debug().debug( "Debug::echo NODEX %d has bidirectional communication with %d with assoc %d\n", 01275 // radio().id(), 01276 // read<OsModel, block_data_t, node_id_t > (msg->payload()+i), 01277 // read<OsModel, block_data_t, uint8_t > (msg->payload()+(sizeof(node_id_t)+sizeof(uint8_t)))); 01278 // } 01279 //debug().debug( "]\n"); 01280 // 01281 //debug().debug( "Debug::echo NODEX %d has bidirectional communication with %d with assoc %d\n", 01282 // radio().id(), 01283 // read<OsModel, block_data_t, node_id_t > (msg->payload()), 01284 // read<OsModel, block_data_t, uint8_t > (msg->payload()+(sizeof(node_id_t)))); 01285 // 01286 } 01287 01288 enum NODE_ECHO_STATUS { 01289 SEARCHING = 1, WAITING = 0 01290 }; 01291 01295 int recv_callback_id_; 01299 uint8_t status_; 01300 uint16_t node_stability; 01301 uint16_t node_stability_prv; 01302 01307 uint16_t timeout_period; 01312 uint16_t beacon_period; 01318 uint16_t min_lqi_threshold; 01324 uint16_t max_lqi_threshold; 01330 uint16_t min_stability_threshold; 01336 uint16_t max_stability_threshold; 01337 01338 01339 struct messages_statistics { 01340 uint16_t echo_msg_count; 01341 uint32_t echo_msg_size; 01342 } msgs_stats; 01343 01344 Radio * radio_; 01345 Clock * clock_; 01346 Timer * timer_; 01347 Debug * debug_; 01348 01349 Radio& radio() { 01350 return *radio_; 01351 } 01352 01353 Clock& clock() { 01354 return *clock_; 01355 } 01356 01357 Timer& timer() { 01358 return *timer_; 01359 } 01360 01361 Debug& debug() { 01362 return *debug_; 01363 } 01364 01365 }; 01366 } 01367 01368 #endif /* ECHO_H */ 01369 //Nullum magnum ingenium sine mixtura dementiae fuit.