|
Wiselib
|
wiselib.testing/algorithms/localization/distance_based/modules/distance/localization_dv_hop_module.h
Go to the documentation of this file.
00001 /*************************************************************************** 00002 ** This file is part of the generic algorithm library Wiselib. ** 00003 ** Copyright (C) 2008,2009 by the Wisebed (www.wisebed.eu) project. ** 00004 ** ** 00005 ** The Wiselib is free software: you can redistribute it and/or modify ** 00006 ** it under the terms of the GNU Lesser General Public License as ** 00007 ** published by the Free Software Foundation, either version 3 of the ** 00008 ** License, or (at your option) any later version. ** 00009 ** ** 00010 ** The Wiselib is distributed in the hope that it will be useful, ** 00011 ** but WITHOUT ANY WARRANTY; without even the implied warranty of ** 00012 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 00013 ** GNU Lesser General Public License for more details. ** 00014 ** ** 00015 ** You should have received a copy of the GNU Lesser General Public ** 00016 ** License along with the Wiselib. ** 00017 ** If not, see <http://www.gnu.org/licenses/>. ** 00018 ***************************************************************************/ 00019 #ifndef __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_LOCALIZATION_DVHOP_MODULE_H 00020 #define __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_LOCALIZATION_DVHOP_MODULE_H 00021 00022 #include "algorithms/localization/distance_based/modules/localization_module.h" 00023 #include "algorithms/localization/distance_based/modules/distance/localization_dv_hop_messages.h" 00024 #include "algorithms/localization/distance_based/util/localization_defutils.h" 00025 #include "config_testing.h" 00026 #include "algorithms/localization/distance_based/math/vec.h" 00027 00028 namespace wiselib 00029 { 00030 00032 00039 template<typename OsModel_P, 00040 typename Radio_P, 00041 typename Clock_P, 00042 typename Debug_P, 00043 typename SharedData_P, 00044 typename NodeSet_P, 00045 typename Arithmatic_P > 00046 class LocalizationDvHopModule 00047 : public LocalizationModule<OsModel_P, Radio_P, SharedData_P> 00048 { 00049 00050 public: 00051 typedef OsModel_P OsModel; 00052 typedef Radio_P Radio; 00053 typedef Clock_P Clock; 00054 typedef Debug_P Debug; 00055 typedef SharedData_P SharedData; 00056 typedef NodeSet_P NodeSet; 00057 typedef Arithmatic_P Arithmatic; 00058 00059 typedef LocalizationDvHopModule<OsModel, Radio, Clock, Debug, SharedData, NodeSet,Arithmatic> self_type; 00060 typedef LocalizationModule<OsModel, Radio, SharedData> base_type; 00061 00062 typedef typename Radio::size_t size_t; 00063 typedef typename Radio::node_id_t node_id_t; 00064 typedef typename Radio::block_data_t block_data_t; 00065 00066 typedef typename Clock_P::time_t time_t; 00067 00068 typedef LocalizationDvHopMessage<OsModel, Radio, Arithmatic> DvHopMessage; 00069 typedef LocalizationDvCalMessage<OsModel, Radio, Arithmatic> DvCalMessage; 00070 00071 typedef typename SharedData::Neighborhood::NeighborhoodIterator NeighborhoodIterator; 00072 00076 LocalizationDvHopModule(); 00078 ~LocalizationDvHopModule(); 00080 00081 00084 00088 void receive( node_id_t from, size_t len, block_data_t *data ); 00094 void work( void ); 00096 00097 00100 00103 bool finished( void ); 00105 00106 void rollback( void ); 00107 00108 00109 void init( Radio& radio, Clock& clock, Debug& debug, SharedData& shared_data ) { 00110 radio_ = &radio; 00111 clock_ = &clock; 00112 debug_ = &debug; 00113 this->set_shared_data( shared_data ); 00114 00115 message_counter1 = 0; 00116 //message_counter2 = 0; 00117 00118 //#define MAX_MESSAGE_COUNTER 5 00119 last_useful_msg_ = clock_->time(); 00120 00121 00122 //this->state_ = dvh_init ; 00123 // avg_hop_dist_ = UNKNOWN_AVG_HOP_DIST ; 00124 // hop_sum_ = 0 ; 00125 // hop_cnt_ = 0 ; 00126 } 00127 00128 protected: 00129 00132 00142 bool process_dv_hop_message_anchor( node_id_t from, size_t len, block_data_t *data ); 00152 bool process_dv_hop_message_unknown( node_id_t from, size_t len, block_data_t *data ); 00159 bool process_dv_cal_message( node_id_t from, size_t len, block_data_t *data ); 00161 00162 00163 private: 00164 00165 enum MessagesIds 00166 { 00167 DV_HOP_MESSAGE = 200, 00168 DV_CAL_MESSAGE = 201 00169 }; 00170 00171 enum DvHopState 00172 { 00173 dvh_init, 00174 dvh_work, 00175 dvh_calibrated, 00176 dvh_finished 00177 }; 00178 00179 DvHopState state_; 00180 Arithmatic avg_hop_dist_; 00181 Arithmatic hop_sum_; 00182 int hop_cnt_; 00183 00184 time_t last_useful_msg_; 00185 NodeSet known_anchors_; 00186 00187 Radio* radio_; 00188 Clock* clock_; 00189 Debug* debug_; 00190 uint8_t message_counter1; 00191 // uint8_t message_counter2; 00192 //uint8_t message_counter3; 00193 00194 }; 00195 // ---------------------------------------------------------------------- 00196 // ---------------------------------------------------------------------- 00197 // ---------------------------------------------------------------------- 00198 template<typename OsModel_P, 00199 typename Radio_P, 00200 typename Clock_P, 00201 typename Debug_P, 00202 typename SharedData_P, 00203 typename NodeSet_P, 00204 typename Arithmatic_P> 00205 LocalizationDvHopModule<OsModel_P, Radio_P, Clock_P, Debug_P, SharedData_P, NodeSet_P, Arithmatic_P>:: 00206 LocalizationDvHopModule() 00207 : state_ ( dvh_init ), 00208 avg_hop_dist_ ( UNKNOWN_AVG_HOP_DIST ), 00209 hop_sum_ ( 0 ), 00210 hop_cnt_ ( 0 ) 00211 { 00212 00213 00214 00215 } 00216 // ---------------------------------------------------------------------- 00217 template<typename OsModel_P, 00218 typename Radio_P, 00219 typename Clock_P, 00220 typename Debug_P, 00221 typename SharedData_P, 00222 typename NodeSet_P, 00223 typename Arithmatic_P> 00224 LocalizationDvHopModule<OsModel_P, Radio_P, Clock_P, Debug_P, SharedData_P, NodeSet_P, Arithmatic_P>:: 00225 ~LocalizationDvHopModule() 00226 {} 00227 // ---------------------------------------------------------------------- 00228 template<typename OsModel_P, 00229 typename Radio_P, 00230 typename Clock_P, 00231 typename Debug_P, 00232 typename SharedData_P, 00233 typename NodeSet_P, 00234 typename Arithmatic_P> 00235 void 00236 LocalizationDvHopModule<OsModel_P, Radio_P, Clock_P, Debug_P, SharedData_P, NodeSet_P, Arithmatic_P>:: 00237 receive( node_id_t from, size_t len, block_data_t *data ) 00238 { 00239 00240 00241 switch ( data[0] ) 00242 { 00243 case DV_HOP_MESSAGE: 00244 if ( this->shared_data().is_anchor() ) 00245 process_dv_hop_message_anchor( from, len, data ); 00246 else 00247 process_dv_hop_message_unknown( from, len, data ); 00248 break; 00249 case DV_CAL_MESSAGE: 00250 process_dv_cal_message( from, len, data ); 00251 break; 00252 } 00253 } 00254 // ---------------------------------------------------------------------- 00255 template<typename OsModel_P, 00256 typename Radio_P, 00257 typename Clock_P, 00258 typename Debug_P, 00259 typename SharedData_P, 00260 typename NodeSet_P, 00261 typename Arithmatic_P> 00262 void 00263 LocalizationDvHopModule<OsModel_P, Radio_P, Clock_P, Debug_P, SharedData_P, NodeSet_P, Arithmatic_P>:: 00264 work( void ) 00265 { 00266 #ifdef LOCALIZATION_DISTANCEBASED_DVHOP_DEBUG 00267 if( this->shared_data().is_anchor()){ 00268 00269 //debug_->debug( "DVHOP:MODUL::Anchor %x is working in state %d ",radio_->id(), this->state_ ); 00270 } 00271 #endif 00272 00273 // do initial stuff and set state to 'working' 00274 if ( state_ == dvh_init ) 00275 { 00276 if ( this->shared_data().is_anchor() ) 00277 { 00278 DvHopMessage message; 00279 message.set_msg_id( DV_HOP_MESSAGE ); 00280 message.set_anchor( radio_->id( ) ); 00281 message.set_anchor_position( this->shared_data().position() ); 00282 //debug_->debug( "DVHOP:MODUL::Anchor %x sending DV_HOP_MESSAGE ",radio_->id() ); 00283 00284 radio_->send( Radio::BROADCAST_ADDRESS, message.buffer_size(), (block_data_t*)&message ); 00285 00286 if(message_counter1>1){ 00287 state_ = dvh_work; 00288 //debug_->debug( "DVHOP:MODUL::Anchor %x date is work ",radio_->id() ); 00289 } 00290 message_counter1++; 00291 //}else{ 00292 00293 // if(message_counter1>MAX_MESSAGE_COUNTER) 00294 state_ = dvh_work; 00295 // message_counter1++; 00296 } 00297 //state_ = dvh_work; 00298 } 00299 00300 // check, whether state can set to be finished or not 00301 // - unknown node is set finished, if given idle-time is passed ( no 00302 // more useful messages came in ) and hops were converted into distances 00303 // - anchor is finished, if given idle-time is passed and there are 00304 // at least one more anchors known, so that the average hop distance 00305 // could estimated 00306 if ( (state_ != dvh_finished) && (clock_->time() - last_useful_msg_ > this->shared_data().idle_time()) ) 00307 { 00308 if ( !this->shared_data().is_anchor() && state_ == dvh_calibrated ) 00309 { 00310 00311 //if(message_counter2>MAX_MESSAGE_COUNTER){ 00312 state_ = dvh_finished; 00313 #ifdef LOCALIZATION_DISTANCEBASED_DVHOP_DEBUG 00314 if(radio_->id()==0x1c72) 00315 debug_->debug( "DVHOP:MODUL:: timeout finish" ); 00316 #endif 00317 //} 00318 //message_counter2++; 00319 } 00320 else if ( this->shared_data().is_anchor() && state_ == dvh_calibrated ) 00321 { 00322 DvCalMessage message; 00323 message.set_msg_id( DV_CAL_MESSAGE ); 00324 message.set_avg_hop_dist( hop_sum_ / hop_cnt_ ); 00325 00326 // debug_->debug( "DVHOP:MODUL::Anchor %x sending DV_CAL_MESSAGE ",radio_->id() ); 00327 00328 00329 radio_->send( Radio::BROADCAST_ADDRESS, message.buffer_size(), (block_data_t*)&message ); 00330 00331 //if(message_counter2>MAX_MESSAGE_COUNTER){ 00332 state_ = dvh_finished; 00333 00334 //} 00335 //message_counter2++; 00336 } 00337 } 00338 00339 } 00340 // ---------------------------------------------------------------------- 00341 template<typename OsModel_P, 00342 typename Radio_P, 00343 typename Clock_P, 00344 typename Debug_P, 00345 typename SharedData_P, 00346 typename NodeSet_P, 00347 typename Arithmatic_P> 00348 bool 00349 LocalizationDvHopModule<OsModel_P, Radio_P, Clock_P, Debug_P, SharedData_P, NodeSet_P, Arithmatic_P>:: 00350 process_dv_hop_message_anchor( node_id_t from, size_t len, block_data_t *data ) 00351 { 00352 00353 00354 if( (this->shared_data().is_anchor()) && (state_ == 0)) 00355 return false; 00356 00357 if ( state_ == dvh_finished ) return true; 00358 00359 DvHopMessage* msg = (DvHopMessage*)data; 00360 node_id_t anchor = msg->anchor(); 00361 00362 // if the anchor is already known, 'floodlimit' is reached or 00363 // the message is from the anchor itself 00364 if ( ( known_anchors_.find( anchor ) != known_anchors_.end() ) || 00365 ( known_anchors_.size() >= this->shared_data().floodlimit() ) || 00366 ( radio_->id() == anchor ) ) 00367 return true; 00368 00369 // add anchor to known ones and set time of last useful message to 00370 // actual 00371 known_anchors_.insert( anchor ); 00372 last_useful_msg_ = clock_->time(); 00373 00374 Arithmatic distance= Vec<Arithmatic>::euclidean_distance( this->shared_data().position(), msg->anchor_position() ); 00375 Arithmatic avg_hop_dist = distance / (msg->hop_count() + 1); 00376 00377 // build average hop distance of all known anchors 00378 ++hop_cnt_; 00379 hop_sum_ += avg_hop_dist; 00380 00381 #ifdef LOCALIZATION_DISTANCEBASED_DVHOP_DEBUG 00382 if(radio_->id()==0x9999) 00383 debug_->debug( "Hop calc at %d: %f / %d = %f\n", 00384 radio_->id(), 00385 hop_sum_, hop_cnt_, hop_sum_ / hop_cnt_ ); 00386 #endif 00387 00388 state_ = dvh_calibrated; 00389 00390 return true; 00391 } 00392 // ---------------------------------------------------------------------- 00393 template<typename OsModel_P, 00394 typename Radio_P, 00395 typename Clock_P, 00396 typename Debug_P, 00397 typename SharedData_P, 00398 typename NodeSet_P, 00399 typename Arithmatic_P> 00400 bool 00401 LocalizationDvHopModule<OsModel_P, Radio_P, Clock_P, Debug_P, SharedData_P, NodeSet_P, Arithmatic_P>:: 00402 process_dv_hop_message_unknown( node_id_t from, size_t len, block_data_t *data ) 00403 { 00404 if ( state_ == dvh_finished ) return true; 00405 00406 DvHopMessage* msg = (DvHopMessage*)data; 00407 node_id_t source = from; 00408 node_id_t anchor = msg->anchor(); 00409 00410 Vec<Arithmatic> anchor_pos = msg->anchor_position(); 00411 int rcvd_hops = msg->hop_count() + 1; 00412 00413 // if anchor is new and floodlimit not reached, insert new anchor; 00414 // if anchor is known and new hop-distance is smaller than old one, 00415 // update info; 00416 // otherwise return 00417 NeighborhoodIterator it = this->shared_data().neighborhood().find( anchor ); 00418 00419 if ( it == this->neighborhood().end_neighborhood() ) 00420 { 00421 if ( this->neighborhood().anchor_cnt() >= (int)this->shared_data().floodlimit() ){ 00422 #ifdef LOCALIZATION_DISTANCEBASED_DVHOP_DEBUG 00423 if(radio_->id()==0x1c72) 00424 debug_->debug( "exceeding floodlimit %d " , 00425 this->neighborhood().anchor_cnt() 00426 ); 00427 #endif 00428 return true; 00429 } 00430 00431 this->neighborhood().update_anchor( anchor, anchor_pos ); 00432 #ifdef LOCALIZATION_DISTANCEBASED_DVHOP_DEBUG 00433 if(radio_->id()==0x1c72) 00434 debug_->debug( "adding anchor %d %x" , 00435 this->neighborhood().anchor_cnt(), anchor 00436 ); 00437 #endif 00438 00439 // set iterator to the new inserted anchor 00440 it = this->neighborhood().find( anchor ); 00441 } 00442 else 00443 { 00444 #ifdef LOCALIZATION_DISTANCEBASED_DVHOP_DEBUG 00445 if(radio_->id()==0x1c72) 00446 debug_->debug( "Anchor %x is already known " , 00447 anchor 00448 ); 00449 #endif 00450 if ( it->second.hops() < rcvd_hops ) 00451 return true; 00452 else if ( it->second.hops() == rcvd_hops ) 00453 { 00454 this->neighborhood().update_ref_node( anchor, source ); 00455 return true; 00456 } 00457 } 00458 it->second.set_hops( rcvd_hops ); 00459 if ( avg_hop_dist_ != UNKNOWN_AVG_HOP_DIST ) 00460 it->second.convert_hops( avg_hop_dist_ ); 00461 00462 this->neighborhood().set_ref_node( anchor, source ); 00463 00464 // set time of last useful message to actual and broadcast new info 00465 last_useful_msg_ = clock_->time(); 00466 DvHopMessage message; 00467 message.set_msg_id( DV_HOP_MESSAGE ); 00468 message.set_anchor( msg->anchor() ); 00469 message.set_anchor_position( msg->anchor_position() ); 00470 message.set_hop_count( rcvd_hops ); 00471 //TODO than one message 00472 //for(uint8_t i =0 ; i<MAX_MESSAGE_COUNTER ; i++) 00473 radio_->send( Radio::BROADCAST_ADDRESS, message.buffer_size(), (block_data_t*)&message ); 00474 00475 00476 00477 return true; 00478 } 00479 // ---------------------------------------------------------------------- 00480 template<typename OsModel_P, 00481 typename Radio_P, 00482 typename Clock_P, 00483 typename Debug_P, 00484 typename SharedData_P, 00485 typename NodeSet_P, 00486 typename Arithmatic_P> 00487 bool 00488 LocalizationDvHopModule<OsModel_P, Radio_P, Clock_P, Debug_P, SharedData_P, NodeSet_P, Arithmatic_P>:: 00489 process_dv_cal_message( node_id_t from, size_t len, block_data_t *data ) 00490 { 00491 00492 if( (this->shared_data().is_anchor()) && (state_ == dvh_init)) 00493 return false; 00494 00495 //#ifdef LOCALIZATION_DISTANCEBASED_DVHOP_DEBUG 00496 if(radio_->id()==0x9999) 00497 debug_->debug( "dv_cal_message\n" 00498 ); 00499 //#endif 00500 00501 if ( state_ == dvh_finished ) return true; 00502 00503 // if there is already an average hop distance known, return 00504 if ( avg_hop_dist_ != UNKNOWN_AVG_HOP_DIST ) 00505 return true; 00506 00507 DvCalMessage* msg = (DvCalMessage*)data; 00508 00509 // set received info and convert hop-count into real distances 00510 avg_hop_dist_ = msg->avg_hop_dist(); 00511 for ( NeighborhoodIterator 00512 it = this->neighborhood().begin_neighborhood(); 00513 it != this->neighborhood().end_neighborhood(); 00514 ++it ) 00515 it->second.convert_hops( avg_hop_dist_ ); 00516 00517 // set state to calibrated and broadcast info 00518 //if(known_anchors_.size()>2) 00519 state_ = dvh_calibrated; 00520 //else return false; 00521 //TODO more than one message 00522 //for(uint8_t i =0 ; i<MAX_MESSAGE_COUNTER ; i++) 00523 radio_->send( Radio::BROADCAST_ADDRESS, len, data ); 00524 00525 return true; 00526 } 00527 // ---------------------------------------------------------------------- 00528 template<typename OsModel_P, 00529 typename Radio_P, 00530 typename Clock_P, 00531 typename Debug_P, 00532 typename SharedData_P, 00533 typename NodeSet_P, 00534 typename Arithmatic_P> 00535 bool 00536 LocalizationDvHopModule<OsModel_P, Radio_P, Clock_P, Debug_P, SharedData_P, NodeSet_P, Arithmatic_P>:: 00537 finished( void ) 00538 { 00539 return state_ == dvh_finished; 00540 } 00541 // ---------------------------------------------------------------------- 00542 template<typename OsModel_P, 00543 typename Radio_P, 00544 typename Clock_P, 00545 typename Debug_P, 00546 typename SharedData_P, 00547 typename NodeSet_P, 00548 typename Arithmatic_P> 00549 void 00550 LocalizationDvHopModule<OsModel_P, Radio_P, Clock_P, Debug_P, SharedData_P, NodeSet_P, Arithmatic_P>:: 00551 rollback( void ) 00552 { 00553 state_ = dvh_init; 00554 last_useful_msg_ = clock_->time(); 00555 avg_hop_dist_ = UNKNOWN_AVG_HOP_DIST; 00556 hop_sum_ = 0; 00557 hop_cnt_ = 0; 00558 this->shared_data().reset_neighborhood_(); 00559 } 00560 00561 }// namespace wiselib 00562 #endif
