Wiselib
wiselib.testing/algorithms/neighbor_discovery/echo.h
Go to the documentation of this file.
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.
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines