Wiselib
|
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_TOPOLOGY_CBTC_TOPOLOGY_H__ 00020 #define __ALGORITHMS_TOPOLOGY_CBTC_TOPOLOGY_H__ 00021 00022 #include "algorithms/topology/cbtc/cbtc_topology_message.h" 00023 #include "algorithms/topology/cbtc/cbtc_topology_neighbours.h" 00024 #include "algorithms/topology/topology_control_base.h" 00025 #include "internal_interface/position/position.h" 00026 #include "util/pstl/vector_static.h" 00027 #include <math.h> // NOTE: required for atan 00028 00029 #define PI 3.1415926535 00030 00031 namespace wiselib 00032 { 00033 00039 template<typename OsModel_P, 00040 typename Localization_P, 00041 typename Radio_P, 00042 uint16_t MAX_NODES = 32> 00043 class CbtcTopology: public TopologyBase<OsModel_P> 00044 { 00045 public: 00046 typedef OsModel_P OsModel; 00047 typedef Radio_P Radio; 00048 00049 #ifdef DEBUG 00050 typedef typename OsModel::Debug Debug; 00051 #endif 00052 typedef Localization_P Localization; 00053 00054 typedef typename OsModel_P::Timer Timer; 00055 00056 typedef CbtcTopology<OsModel, Localization, Radio, MAX_NODES> self_type; 00057 typedef CbtcTopologyMessage<OsModel, Radio> TopologyMessage; 00058 typedef CbtcTopologyNeighbours<OsModel, Radio, MAX_NODES> Neighbours; 00059 00060 typedef typename OsModel_P::size_t size_type; 00061 00062 typedef typename Radio::node_id_t node_id_t; 00063 typedef typename Radio::size_t size_t; 00064 typedef typename Radio::block_data_t block_data_t; 00065 00066 typedef typename Radio::TxPower Power; 00067 00068 typedef typename Timer::millis_t millis_t; 00069 00070 typedef typename Localization::position_t position_t; 00071 typedef vector_static<OsModel, node_id_t, MAX_NODES> Neighbors; 00072 00075 CbtcTopology(); 00076 ~CbtcTopology(); 00078 00081 void enable( void ); 00082 void disable( void ); 00084 00087 void timer_elapsed_first_phase( void *userdata ); 00088 void timer_elapsed_second_phase( void *userdata ); 00090 00093 void receive( node_id_t from, size_t len, block_data_t *data ); 00095 00096 inline void set_startup_time( millis_t startup_time ) 00097 { startup_time_ = startup_time; }; 00098 00099 inline void set_work_period_1( millis_t work_period ) 00100 { work_period_1 = work_period; }; 00101 00102 inline void set_work_period_2( millis_t work_period ) 00103 { work_period_2 = work_period; }; 00104 00105 inline void set_alpha(double angle) 00106 { alpha = angle; }; 00107 00108 Neighbors &topology(){ 00109 N.clear(); 00110 size_type i; 00111 for ( i = 0; i < neighbours.size(); ++i ){ 00112 N.push_back(neighbours[i].id); 00113 } 00114 return N; 00115 } 00116 00117 #ifdef DEBUG 00118 void init( Localization &loc, Radio& radio, Timer& timer, Debug& debug ) { 00119 loc_=&loc; 00120 radio_ = &radio; 00121 timer_ = &timer; 00122 debug_ = &debug; 00123 } 00124 #else 00125 void init( Localization &loc, Radio& radio, Timer& timer) { 00126 loc_=&loc; 00127 radio_ = &radio; 00128 timer_ = &timer; 00129 } 00130 #endif 00131 00132 void destruct() { 00133 } 00134 00135 private: 00136 00137 Radio& radio() 00138 { return *radio_; } 00139 00140 Timer& timer() 00141 { return *timer_; } 00142 00143 #ifdef DEBUG 00144 Debug& debug() 00145 { return *debug_; } 00146 #endif 00147 Localization * loc_; 00148 Radio * radio_; 00149 Timer * timer_; 00150 #ifdef DEBUG 00151 Debug * debug_; 00152 #endif 00153 00156 enum CbtcTopologyMsgIds { 00157 CbtcMsgIdHello = 200, 00158 CbtcMsgIdAck = 201, 00159 CbtcMsgIdAsymmetric = 202, 00160 CbtcMsgIdNDP = 203, 00161 }; 00162 00163 millis_t startup_time_; 00164 millis_t work_period_1; 00165 millis_t work_period_2; 00166 00167 double alpha; 00168 00169 TopologyMessage helloMessage; 00170 TopologyMessage ackMessage; 00171 TopologyMessage asymmetricMessage; 00172 TopologyMessage NDPMessage; 00173 00174 Neighbours neighbours; 00175 00176 Power selfpower; 00177 position_t selfposition; 00178 00179 Neighbors N; // Topology 00180 00181 bool just_started; 00182 bool first_phase_done; 00183 bool boundary_node; 00184 bool enabled; 00185 00186 void generate_topology(); 00187 inline bool check_gap(); 00188 inline void shrinkback(); 00189 inline void pairwise(); 00190 inline void pairwise_one_node(size_type index, int power); 00191 }; 00192 00193 // ----------------------------------------------------------------------- 00194 // ----------------------------------------------------------------------- 00195 // ----------------------------------------------------------------------- 00196 template<typename OsModel_P, 00197 typename Localization_P, 00198 typename Radio_P, 00199 uint16_t MAX_NODES> 00200 CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>:: 00201 CbtcTopology() 00202 : startup_time_ ( 2000 ), 00203 work_period_1 ( 5000 ), 00204 work_period_2 (15000), 00205 alpha(2.617993), // (5/6) * pi radians 00206 helloMessage( CbtcMsgIdHello ), 00207 ackMessage( CbtcMsgIdAck ), 00208 asymmetricMessage( CbtcMsgIdAsymmetric ), 00209 NDPMessage(CbtcMsgIdNDP), 00210 enabled(false) 00211 { 00212 } 00213 00214 // ----------------------------------------------------------------------- 00215 template<typename OsModel_P, 00216 typename Localization_P, 00217 typename Radio_P, 00218 uint16_t MAX_NODES> 00219 CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>:: 00220 ~CbtcTopology() 00221 { 00222 #ifdef DEBUG 00223 debug().debug( "CbtcTopology Destroyed\n"); 00224 #endif 00225 } 00226 00227 // ----------------------------------------------------------------------- 00228 template<typename OsModel_P, 00229 typename Localization_P, 00230 typename Radio_P, 00231 uint16_t MAX_NODES> 00232 void 00233 CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>:: 00234 enable( void ) 00235 { 00236 enabled=true; 00237 just_started = true; 00238 selfpower = Power::MIN; 00239 first_phase_done = false; 00240 boundary_node = false; 00241 00242 neighbours.set_id( radio().id() ); 00243 00244 radio().enable_radio(); 00245 #ifdef DEBUG 00246 debug().debug( "%i: CbtcTopology Boots\n", radio().id() ); 00247 #endif 00248 radio().template reg_recv_callback<self_type, &self_type::receive>( 00249 this ); 00250 timer().template set_timer<self_type, 00251 &self_type::timer_elapsed_first_phase>(startup_time_, this, 0 ); 00252 00253 selfposition = loc_->position(); 00254 } 00255 00256 // ----------------------------------------------------------------------- 00257 template<typename OsModel_P, 00258 typename Localization_P, 00259 typename Radio_P, 00260 uint16_t MAX_NODES> 00261 void 00262 CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>:: 00263 disable( void ) 00264 { 00265 enabled=false; 00266 #ifdef DEBUG 00267 debug().debug( "%i: Called CbtcTopology::disable\n", radio().id() ); 00268 #endif 00269 } 00270 00271 // ----------------------------------------------------------------------- 00272 template<typename OsModel_P, 00273 typename Localization_P, 00274 typename Radio_P, 00275 uint16_t MAX_NODES> 00276 void 00277 CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>:: 00278 timer_elapsed_first_phase( void* userdata ) 00279 { 00280 if(!enabled) 00281 return; 00282 position_t pos; 00283 #ifdef DEBUG 00284 debug().debug( "%i: Executing TimerElapsed 'CbtcTopology' 1st Phase\n", 00285 radio().id() ); 00286 #endif 00287 00288 if(first_phase_done) 00289 return; 00290 00291 if(selfpower == Power::MAX) { 00292 generate_topology(); 00293 return; 00294 } 00295 00296 if(just_started) 00297 just_started = false; 00298 else 00299 selfpower++; 00300 00301 helloMessage.set_power(selfpower.to_ratio()); 00302 pos = loc_->position(); 00303 helloMessage.set_position(pos.x, pos.y); 00304 00305 radio().set_power(selfpower); 00306 radio().send( 00307 radio().BROADCAST_ADDRESS, 00308 TopologyMessage::HELLO_SIZE, 00309 (uint8_t*)&helloMessage 00310 ); 00311 00312 timer().template set_timer<self_type, 00313 &self_type::timer_elapsed_first_phase>( work_period_1, this, 0 ); 00314 } 00315 00316 // ----------------------------------------------------------------------- 00317 template<typename OsModel_P, 00318 typename Localization_P, 00319 typename Radio_P, 00320 uint16_t MAX_NODES> 00321 void 00322 CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>:: 00323 timer_elapsed_second_phase( void* userdata ) 00324 { 00325 if(!enabled) 00326 return; 00327 position_t pos; 00328 #ifdef DEBUG 00329 debug().debug( "%i: Executing TimerElapsed 'CbtcTopology' 2nd Phase\n", 00330 radio().id() ); 00331 #endif 00332 if(!first_phase_done) 00333 return; 00334 00335 00336 if(neighbours.ndp_update() && check_gap()) { 00337 first_phase_done = false; 00338 timer().template set_timer<self_type, 00339 &self_type::timer_elapsed_first_phase>( work_period_1, this, 0); 00340 return; 00341 } 00342 00343 00344 NDPMessage.set_power(selfpower.to_ratio()); 00345 pos = loc_->position(); 00346 NDPMessage.set_position(pos.x, pos.y); 00347 00348 radio().send( 00349 radio().BROADCAST_ADDRESS, 00350 TopologyMessage::NDP_SIZE, 00351 (uint8_t*)&NDPMessage 00352 ); 00353 00354 timer().template set_timer<self_type, 00355 &self_type::timer_elapsed_second_phase>( work_period_2, this, 0 ); 00356 } 00357 00358 // ----------------------------------------------------------------------- 00359 template<typename OsModel_P, 00360 typename Localization_P, 00361 typename Radio_P, 00362 uint16_t MAX_NODES> 00363 void 00364 CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>:: 00365 receive( node_id_t from, size_t len, block_data_t *data ) 00366 { 00367 if(!enabled) 00368 return; 00369 TopologyMessage *msg = (TopologyMessage *)data; 00370 uint8_t msg_id = msg->msg_id(); 00371 00372 int p = msg->power(); 00373 Power power; 00374 power.from_ratio(p); 00375 00376 position_t pos; 00377 double angle; 00378 00379 bool check_for_gap; 00380 00381 if ( from == radio().id() ) 00382 return; 00383 00384 angle = atan2( msg->position_y() - selfposition.y, msg->position_x() - selfposition.x ); 00385 00386 if ( msg_id == CbtcMsgIdHello ) { 00387 #ifdef DEBUG 00388 debug().debug( "%i: Received HELLO from %i with power: %i\n", 00389 radio().id(), from, p ); 00390 #endif 00391 neighbours.add_update_neighbour(from, p, angle, true); 00392 ackMessage.set_power(p); 00393 pos = loc_->position(); 00394 ackMessage.set_position(pos.x, pos.y); 00395 radio().set_power( power ); 00396 radio().send( from, TopologyMessage::ACK_SIZE, (uint8_t*)&ackMessage ); 00397 radio().set_power( selfpower ); 00398 } 00399 else if ( msg_id == CbtcMsgIdAck ) { 00400 #ifdef DEBUG 00401 debug().debug( "%i: Received ACK from %i x: %f y: %f\n", 00402 radio().id(), from, msg->position_x(), msg->position_y() ); 00403 00404 #endif 00405 neighbours.add_update_neighbour(from, p, angle, false); 00406 if(!first_phase_done) 00407 generate_topology(); 00408 } 00409 else if( msg_id == CbtcMsgIdAsymmetric ) { 00410 #ifdef DEBUG 00411 debug().debug( "%i: Received ASYMMETRIC from %i\n", radio().id(), from ); 00412 #endif 00413 if(!first_phase_done) 00414 neighbours.add_asymmetric_to_remove(from); 00415 } else if( msg_id == CbtcMsgIdNDP) { 00416 #ifdef DEBUG 00417 debug().debug( "%i: Received NDP from %i\n", radio().id(), from ); 00418 #endif 00419 if(!first_phase_done) 00420 return; 00421 00422 check_for_gap = neighbours.add_update_ndp(from, p, angle); 00423 if(check_for_gap && check_gap()) { 00424 first_phase_done = false; 00425 timer().template set_timer<self_type, 00426 &self_type::timer_elapsed_first_phase>( work_period_1, this, 0 ); 00427 return; 00428 } 00429 00430 shrinkback(); 00431 } 00432 } 00433 00434 // ----------------------------------------------------------------------- 00435 template<typename OsModel_P, 00436 typename Localization_P, 00437 typename Radio_P, 00438 uint16_t MAX_NODES> 00439 void 00440 CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>:: 00441 generate_topology() 00442 { 00443 bool gap_exists; 00444 size_type i; 00445 int power; 00446 00447 if(first_phase_done) 00448 return; 00449 00450 gap_exists = check_gap(); 00451 00452 if(selfpower < Power::MAX && gap_exists) 00453 return; 00454 00455 first_phase_done = true; 00456 00457 if(gap_exists) 00458 boundary_node = true; 00459 00460 neighbours.first_phase_done = true; 00461 neighbours.copy_to_NDP(); 00462 00463 #ifdef DEBUG 00464 debug().debug( "%i: First phase done. %soundary node\n", 00465 radio().id(), boundary_node ?"B":"Not b" ); 00466 neighbours.print_basic(); 00467 #endif 00468 00469 //#Optimizations 00470 00471 //Shrink-back operation 00472 if(boundary_node) 00473 shrinkback(); 00474 00475 #ifdef DEBUG 00476 neighbours.print_optimization("Nodes[Shrinkback]:"); 00477 #endif 00478 00479 //Asymmetric edge removal 00480 if(alpha <= 2 * PI / 3) { 00481 //Nodes told to be deleted 00482 for( i = 0; i < neighbours.ATR.size(); ++i){ 00483 neighbours.delete_by_id(neighbours.ATR[i]); 00484 } 00485 neighbours.ATR.clear(); 00486 00487 //Tell nodes to delete self 00488 for( i = 0; i < neighbours.size(); ++i ){ 00489 if(neighbours[i].asymmetric){ 00490 radio().send( 00491 neighbours[i].id, 00492 TopologyMessage::ASYMMETRIC_SIZE, 00493 (uint8_t*)&asymmetricMessage 00494 ); 00495 neighbours.delete_by_index(i); 00496 i--; 00497 } 00498 } 00499 } else { 00500 for( i = 0; i < neighbours.size(); ++i ){ 00501 neighbours[i].asymmetric = false; 00502 } 00503 } 00504 00505 #ifdef DEBUG 00506 neighbours.print_optimization("Nodes[Asymmetric]:"); 00507 #endif 00508 00509 //Calculate the power to beacon 00510 if(!boundary_node) { 00511 power = (Power::MIN).to_ratio(); 00512 for( i = 0; i < neighbours.size(); ++i ){ 00513 if(neighbours[i].power > power) 00514 power = neighbours[i].power; 00515 } 00516 selfpower = Power::from_ratio(power); 00517 } 00518 00519 //Pair-wise edge removal 00520 pairwise(); 00521 00522 #ifdef DEBUG 00523 neighbours.print_optimization("Nodes[Pairwise]: "); 00524 #endif 00525 00526 TopologyBase<OsModel>::notify_listeners(); 00527 00528 timer().template set_timer<self_type, 00529 &self_type::timer_elapsed_second_phase>( work_period_2, this, 0 ); 00530 } 00531 00532 // ----------------------------------------------------------------------- 00533 template<typename OsModel_P, 00534 typename Localization_P, 00535 typename Radio_P, 00536 uint16_t MAX_NODES> 00537 inline bool 00538 CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>:: 00539 check_gap(){ 00540 size_type first, i, j; 00541 00542 if(neighbours.size() < 2) 00543 return true; 00544 00545 for ( first = 0; first < neighbours.size(); ++first ) 00546 if (!neighbours[first].asymmetric) 00547 break; 00548 00549 if(first >= neighbours.size() - 1) 00550 return true; 00551 00552 j = first; 00553 for ( i = first + 1; i < neighbours.size(); ++i ){ 00554 if (!neighbours[i].asymmetric) { 00555 if(neighbours[i].angle - neighbours[j].angle > alpha) 00556 return true; 00557 j = i; 00558 } 00559 } 00560 00561 return PI * 2.0 + neighbours[first].angle - neighbours[j].angle > alpha; 00562 } 00563 00564 00565 // ----------------------------------------------------------------------- 00566 template<typename OsModel_P, 00567 typename Localization_P, 00568 typename Radio_P, 00569 uint16_t MAX_NODES> 00570 inline void 00571 CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>:: 00572 shrinkback() 00573 { 00574 if(neighbours.size() < 3) 00575 return; 00576 00577 int p_threshold; 00578 size_type i, j, k; 00579 00580 for (p_threshold = (Power::MAX).to_ratio(); 00581 p_threshold > (Power::MIN).to_ratio(); 00582 p_threshold--) { 00583 00584 for ( i = 0; i < neighbours.size(); ++i ){ 00585 if (neighbours[i].power < p_threshold) 00586 break; 00587 } 00588 00589 //There is no one or only one 00590 if (i >= neighbours.size() - 1) 00591 return; 00592 00593 k = i; 00594 j = i + 1; 00595 00596 for (; j < neighbours.size(); ++j ){ 00597 if (neighbours[j].power < p_threshold) { 00598 if (j - k > 1 && neighbours[j].angle - neighbours[k].angle > alpha) 00599 return; 00600 k = j; 00601 } 00602 } 00603 00604 //There is only one 00605 if (k == i) 00606 return; 00607 00608 if( !(i == 0 && k == neighbours.size() - 1) && 00609 PI * 2 + neighbours[i].angle - neighbours[k].angle > alpha){ 00610 return; 00611 } 00612 00613 //delete same level 00614 neighbours.delete_by_power(p_threshold); 00615 } 00616 00617 } 00618 00619 // ----------------------------------------------------------------------- 00620 template<typename OsModel_P, 00621 typename Localization_P, 00622 typename Radio_P, 00623 uint16_t MAX_NODES> 00624 inline void 00625 CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>:: 00626 pairwise() 00627 { 00628 int i, j, fst_below, last_current, last_below; 00629 int p_threshold; 00630 double angle; 00631 00632 for (p_threshold = (Power::MAX).to_ratio(); 00633 p_threshold > (Power::MIN).to_ratio(); 00634 p_threshold--) { 00635 00636 for ( fst_below = 0; fst_below < (int)neighbours.size(); ++fst_below ) 00637 if (neighbours[fst_below].power < p_threshold) 00638 break; 00639 00640 if(fst_below == (int)neighbours.size()) 00641 return; 00642 00643 i = fst_below + 1; 00644 last_current = -1; 00645 last_below = fst_below; 00646 00647 for (; i < (int)neighbours.size(); ++i ){ 00648 //Node on the power level to be deleted 00649 if(neighbours[i].power == p_threshold) { 00650 if(last_current == -1) { 00651 if(neighbours[i].angle - neighbours[last_below].angle > PI / 3) 00652 last_current = i; 00653 } 00654 } 00655 //Otherwise node with a power below power to be deleted 00656 else { 00657 if(last_current != -1) { 00658 for(j = last_current; j < i; j++) { 00659 if(neighbours[i].angle - neighbours[j].angle > PI / 3) 00660 break; 00661 } 00662 if (j < i) 00663 return; 00664 else 00665 last_current = -1; 00666 } 00667 last_below = i; 00668 } 00669 } 00670 00671 //There is just one nodes with power below power to be deleted 00672 if(last_below == fst_below){ 00673 pairwise_one_node(fst_below, p_threshold); 00674 return; 00675 } 00676 00677 //If all nodes at the end of the vector with power to be deleted 00678 //can be deleted 00679 if(last_current == -1) { 00680 angle = neighbours[last_below].angle - 2 * PI; 00681 for(i = 0; i < fst_below; i++){ 00682 if(neighbours[i].angle - angle > PI / 3) 00683 break; 00684 } 00685 } 00686 //Otherwise, if there are some nodes with power to be deleted 00687 // after the last node with power below 00688 else { 00689 angle = neighbours[fst_below].angle + 2 * PI; 00690 for(i = last_current; i < (int)neighbours.size(); i++){ 00691 if(angle - neighbours[i].angle > PI / 3) 00692 return; 00693 } 00694 i = 0; 00695 } 00696 00697 for(; i < fst_below; i++){ 00698 if(neighbours[fst_below].angle - neighbours[i].angle > PI / 3) 00699 return; 00700 } 00701 00702 neighbours.delete_by_power(p_threshold); 00703 } 00704 } 00705 00706 // ----------------------------------------------------------------------- 00707 template<typename OsModel_P, 00708 typename Localization_P, 00709 typename Radio_P, 00710 uint16_t MAX_NODES> 00711 inline void 00712 CbtcTopology<OsModel_P, Localization_P, Radio_P, MAX_NODES>:: 00713 pairwise_one_node(size_type index, int power) 00714 { 00715 size_type i; 00716 00717 double angle1 = neighbours[index].angle; 00718 double angle2 = neighbours[index].angle - PI * 2; 00719 for ( i = 0; i < index; ++i ){ 00720 if(angle1 - neighbours[i].angle < PI / 3) 00721 continue; 00722 else if (neighbours[i].angle - angle2 < PI / 3) 00723 continue; 00724 else 00725 return; 00726 } 00727 00728 angle1 = neighbours[index].angle; 00729 angle2 = neighbours[index].angle + PI * 2; 00730 for (i = index + 1; i < neighbours.size(); ++i ){ 00731 if(neighbours[i].angle - angle1 < PI / 3) 00732 continue; 00733 else if (angle2 - neighbours[i].angle < PI / 3) 00734 continue; 00735 else 00736 return; 00737 } 00738 00739 neighbours.delete_by_power(power); 00740 } 00741 } 00742 00743 #endif