|
Wiselib
|
wiselib.testing/algorithms/synchronization/tpsn/tpsn_synchronization.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_SYNCHRONIZATION_TPSN_SYNCHRONIZATION_H__ 00020 #define __ALGORITHMS_SYNCHRONIZATION_TPSN_SYNCHRONIZATION_H__ 00021 00022 #include "algorithms/synchronization/tpsn/tpsn_synchronization_message.h" 00023 #include "util/pstl/vector_static.h" 00024 00025 #define DEBUG_TPSN_SYNCHRONIZATION 00026 #define DEBUG_TPSN_SYNCHRONIZATION_ISENSE 00027 // #define DEBUG_TPSN_SYNCHRONIZATION_SHAWN 00028 00029 namespace wiselib 00030 { 00031 00037 template<typename OsModel_P, 00038 typename Radio_P = typename OsModel_P::Radio, 00039 typename Debug_P = typename OsModel_P::Debug, 00040 typename Clock_P = typename OsModel_P::Clock, 00041 uint16_t MAX_NODES = 32> 00042 class TpsnSynchronization 00043 { 00044 public: 00045 typedef OsModel_P OsModel; 00046 typedef Radio_P Radio; 00047 typedef Debug_P Debug; 00048 typedef Clock_P Clock; 00049 00050 typedef typename OsModel_P::Timer Timer; 00051 00052 typedef TpsnSynchronization<OsModel, Radio, Debug, Clock, MAX_NODES> self_type; 00053 typedef TpsnSynchronizationMessage<OsModel, Radio, Clock> SynchronizationMessage; 00054 00055 typedef typename Radio::node_id_t node_id_t; 00056 typedef typename Radio::size_t size_t; 00057 typedef typename Radio::block_data_t block_data_t; 00058 static const uint8_t NODE_ID_SIZE = sizeof( node_id_t ); 00059 00060 typedef typename Timer::millis_t millis_t; 00061 00062 typedef typename Clock::time_t time_t; 00063 static const uint8_t TIME_SIZE = sizeof( time_t ); 00064 00068 00071 TpsnSynchronization(); 00072 ~TpsnSynchronization(); 00074 00077 void enable( void ); 00078 void disable( void ); 00079 inline void set_root( void ) 00080 { level_ = 0; } 00082 00085 void timer_elapsed( void *userdata ); 00087 00090 void receive( node_id_t from, size_t len, block_data_t *data ); 00092 00093 inline void set_root_startup_time( millis_t root_startup_time ) 00094 { root_startup_time_ = root_startup_time; }; 00095 00096 inline void set_tree_construction_time( millis_t tree_construction_time ) 00097 { tree_construction_time_ = tree_construction_time; }; 00098 00099 inline void set_random_interval_time( millis_t random_interval_time ) 00100 { random_interval_time_ = random_interval_time; }; 00101 00102 void init( Radio& radio, Timer& timer, Debug& debug, Clock& clock ) { 00103 radio_ = &radio; 00104 timer_ = &timer; 00105 debug_ = &debug; 00106 clock_ = &clock; 00107 } 00108 00109 void destruct() { 00110 } 00111 00112 private: 00113 Radio& radio() 00114 { return *radio_; } 00115 00116 Timer& timer() 00117 { return *timer_; } 00118 00119 Debug& debug() 00120 { return *debug_; } 00121 00122 Clock& clock() 00123 { return *clock_; } 00124 00125 Radio * radio_; 00126 Timer * timer_; 00127 Debug * debug_; 00128 Clock * clock_; 00129 00130 void send_sync_pulse(); 00131 00134 // TODO: standarize msg ids 00135 enum TpsnSynchronizationMsgIds { 00136 TpsnMsgIdLevelDiscovery = 230, 00137 TpsnMsgIdLevelRequest = 231, 00138 TpsnMsgIdTimeSync = 232, 00139 TpsnMsgIdSynchronizationPulse = 233, 00140 TpsnMsgIdAcknowledgement = 234, 00141 }; 00142 00143 uint8_t levelDiscoveryMessage[2]; 00144 00145 SynchronizationMessage synchronizationMessage; 00146 00147 int8_t level_; 00148 bool built_tree_; 00149 time_t time_; 00150 millis_t root_startup_time_, tree_construction_time_, random_interval_time_, timeout_; 00151 00152 TpsnSynchronizationMsgIds levelRequestMessage; 00153 TpsnSynchronizationMsgIds timeSyncMessage; 00154 00155 node_id_t father_; 00156 int8_t new_level_; 00157 00158 bool synchronized_; 00159 bool requested_father_; 00160 uint8_t retries_; 00161 const uint8_t MAX_RETRIES; 00162 bool enabled_; 00163 }; 00164 // ----------------------------------------------------------------------- 00165 // ----------------------------------------------------------------------- 00166 // ----------------------------------------------------------------------- 00167 template<typename OsModel_P, 00168 typename Radio_P, 00169 typename Debug_P, 00170 typename Clock_P, 00171 uint16_t MAX_NODES> 00172 TpsnSynchronization<OsModel_P, Radio_P, Debug_P, Clock_P, MAX_NODES>:: 00173 TpsnSynchronization() 00174 : level_ ( -1 ), 00175 root_startup_time_ ( 1000 ), 00176 tree_construction_time_ ( 1000 ), // 3000 00177 random_interval_time_ ( 1000 ), // 2000 00178 timeout_ ( 1000 ), // 15000 00179 levelRequestMessage ( TpsnMsgIdLevelRequest ), 00180 timeSyncMessage ( TpsnMsgIdTimeSync ), 00181 MAX_RETRIES ( 4 ), 00182 enabled_(false) 00183 {}; 00184 // ----------------------------------------------------------------------- 00185 template<typename OsModel_P, 00186 typename Radio_P, 00187 typename Debug_P, 00188 typename Clock_P, 00189 uint16_t MAX_NODES> 00190 TpsnSynchronization<OsModel_P, Radio_P, Debug_P, Clock_P, MAX_NODES>:: 00191 ~TpsnSynchronization() 00192 { 00193 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00194 debug().debug( "TpsnSynchronization Destroyed\n" ); 00195 #endif 00196 }; 00197 // ----------------------------------------------------------------------- 00198 template<typename OsModel_P, 00199 typename Radio_P, 00200 typename Debug_P, 00201 typename Clock_P, 00202 uint16_t MAX_NODES> 00203 void 00204 TpsnSynchronization<OsModel_P, Radio_P, Debug_P, Clock_P, MAX_NODES>:: 00205 enable( void ) 00206 { 00207 if(!enabled_){ 00208 radio().enable_radio(); 00209 radio().template reg_recv_callback<self_type, &self_type::receive>( 00210 this ); 00211 enabled_=true; 00212 } 00213 levelDiscoveryMessage[0] = TpsnMsgIdLevelDiscovery; 00214 if ( level_ == 0 ) 00215 { 00216 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00217 debug().debug( "%i: TpsnSynchronization Boots as Root\n", radio().id() ); 00218 #endif 00219 built_tree_ = false; 00220 timer().template set_timer<self_type, &self_type::timer_elapsed>( 00221 root_startup_time_, this, 0 ); 00222 } 00223 else 00224 { 00225 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00226 debug().debug( "%i: TpsnSynchronization Boots as Normal node\n", radio().id() ); 00227 #endif 00228 synchronized_ = false; 00229 retries_ = 0; 00230 new_level_ = -1; 00231 timer().template set_timer<self_type, &self_type::timer_elapsed>( 00232 timeout_, this, 0 ); 00233 } 00234 } 00235 // ----------------------------------------------------------------------- 00236 template<typename OsModel_P, 00237 typename Radio_P, 00238 typename Debug_P, 00239 typename Clock_P, 00240 uint16_t MAX_NODES> 00241 void 00242 TpsnSynchronization<OsModel_P, Radio_P, Debug_P, Clock_P, MAX_NODES>:: 00243 disable( void ) 00244 { 00245 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00246 debug().debug( "%i: Called TpsnSynchronization::disable\n", radio().id() ); 00247 #endif 00248 } 00249 // ----------------------------------------------------------------------- 00250 template<typename OsModel_P, 00251 typename Radio_P, 00252 typename Debug_P, 00253 typename Clock_P, 00254 uint16_t MAX_NODES> 00255 void 00256 TpsnSynchronization<OsModel_P, Radio_P, Debug_P, Clock_P, MAX_NODES>:: 00257 timer_elapsed( void* userdata ) 00258 { 00259 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00260 debug().debug( "%i: Executing TimerElapsed 'TpsnSynchronization'\n", radio().id() ); 00261 #endif 00262 if ( level_ == 0 ) 00263 { 00264 if ( not built_tree_ ) 00265 { 00266 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00267 debug().debug( "%i: I'm root and I'll start the tree construction\n", radio().id() ); 00268 #endif 00269 built_tree_ = true; 00270 levelDiscoveryMessage[1] = level_; 00271 radio().send( radio().BROADCAST_ADDRESS, 2, (uint8_t*)&levelDiscoveryMessage ); 00272 timer().template set_timer<self_type, &self_type::timer_elapsed>( 00273 tree_construction_time_, this, 0 ); 00274 } 00275 else 00276 { 00277 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00278 debug().debug( "%i: I'm root and I'll start the synchronization\n", radio().id() ); 00279 #endif 00280 radio().send( radio().BROADCAST_ADDRESS, 1, (uint8_t*)&timeSyncMessage ); 00281 } 00282 } 00283 else 00284 { 00285 if ( level_ == -1 ) 00286 { 00287 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00288 debug().debug( "%i: I'm not root and I don't have a father\n", radio().id() ); 00289 #endif 00290 if ( new_level_ == -1 ) 00291 { 00292 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00293 debug().debug( "%i: I neither have found a new father yet, I request one: %d\n", radio().id(), (uint8_t*)&levelRequestMessage ); 00294 #endif 00295 requested_father_ = true; 00296 00297 radio().send( radio().BROADCAST_ADDRESS, 1, (uint8_t*)&levelRequestMessage ); 00298 timer().template set_timer<self_type, &self_type::timer_elapsed>( 00299 timeout_, this, 0 ); 00300 } 00301 else 00302 { 00303 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00304 debug().debug( "%i: I have a new father and request synchronization\n", radio().id() ); 00305 #endif 00306 level_ = new_level_; 00307 requested_father_ = false; 00308 send_sync_pulse(); 00309 } 00310 } 00311 else if ( not synchronized_ ) 00312 { 00313 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00314 debug().debug( "%i: I'm not root and I have a father but I'm not synchronized\n", radio().id() ); 00315 #endif 00316 if ( retries_ < MAX_RETRIES ) 00317 { 00318 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00319 debug().debug( "%i: I request synchronization for the %i time\n", radio().id(), retries_ ); 00320 #endif 00321 ++retries_; 00322 send_sync_pulse(); 00323 } 00324 else 00325 { 00326 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00327 debug().debug( "%i: I tried synchronizing 4 times, and now request a father\n", radio().id(), retries_ ); 00328 #endif 00329 retries_ = 0; 00330 level_ = -1; 00331 new_level_ = -1; 00332 requested_father_ = true; 00333 radio().send( radio().BROADCAST_ADDRESS, 1, (uint8_t*)&levelRequestMessage ); 00334 timer().template set_timer<self_type, &self_type::timer_elapsed>( 00335 timeout_, this, 0 ); 00336 } 00337 } 00338 } 00339 } 00340 // ----------------------------------------------------------------------- 00341 template<typename OsModel_P, 00342 typename Radio_P, 00343 typename Debug_P, 00344 typename Clock_P, 00345 uint16_t MAX_NODES> 00346 void 00347 TpsnSynchronization<OsModel_P, Radio_P, Debug_P, Clock_P, MAX_NODES>:: 00348 receive( node_id_t from, size_t len, block_data_t *data ) 00349 { 00350 if ( from == radio().id() or 00351 (radio().id() == 4610 and from == 21102) or 00352 (radio().id() == 1412 and from != 21102) or 00353 (radio().id() == 1412 and from != 2841)) 00354 return; 00355 uint8_t msg_id = *data; 00356 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00357 // debug().debug( "%i: Received message from %i and msg_id is %d\n", radio().id(), from, msg_id ); 00358 #endif 00359 if ( msg_id == TpsnMsgIdLevelDiscovery && level_ == -1 ) 00360 { 00361 ++data; // TODO: volem el segon byte 00362 uint8_t level = *data; 00363 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00364 debug().debug( "%i: It's a level_discovery message of level %i\n", radio().id(), level ); 00365 #endif 00366 if ( not requested_father_ ) 00367 { 00368 level_ = level + 1; 00369 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00370 debug().debug( "%i: This is my first level_discovery message, so I get the level %i\n", radio().id(), level_ ); 00371 #endif 00372 father_ = from; 00373 levelDiscoveryMessage[1] = level_; 00374 radio().send( radio().BROADCAST_ADDRESS, 2, (uint8_t*)&levelDiscoveryMessage ); 00375 } 00376 else 00377 { 00378 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00379 debug().debug( "%i: I was requesting a father, and my new_level_ was %i\n", radio().id(), new_level_ ); 00380 #endif 00381 if ( level < new_level_ || new_level_ == -1) 00382 { 00383 new_level_ = level + 1; 00384 father_ = from; 00385 } 00386 } 00387 } 00388 else if ( msg_id == TpsnMsgIdLevelRequest && level_ != -1 && not requested_father_ ) 00389 { 00390 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00391 debug().debug( "%i: It's a level_request message, so I sent my level: %i to %i \n", radio().id(), level_, from ); 00392 #endif 00393 levelDiscoveryMessage[1] = level_; 00394 radio().send( from, 2, (uint8_t*)&levelDiscoveryMessage ); 00395 } 00396 else if ( msg_id == TpsnMsgIdSynchronizationPulse ) 00397 { 00398 synchronizationMessage.set_t2( clock().time() ); 00399 SynchronizationMessage *msg = (SynchronizationMessage *)data; 00400 if ( msg->receiver() == radio().id() ) 00401 { 00402 synchronizationMessage.set_msg_id( TpsnMsgIdAcknowledgement ); 00403 synchronizationMessage.set_t1( msg->t1() ); 00404 synchronizationMessage.set_t3( clock().time() ); 00405 radio().send( from, 1 + 3*TIME_SIZE, (uint8_t*)&synchronizationMessage ); 00406 #ifdef DEBUG_TPSN_SYNCHRONIZATION_SHAWN 00407 debug().debug( "%i: It's a synchronization_pulse, so I send an acknowledgement, t1(%f), t2(%f), t3(%f)\n", radio().id(), 00408 synchronizationMessage.t1(), synchronizationMessage.t2(), synchronizationMessage.t3() ); 00409 #endif 00410 #ifdef DEBUG_TPSN_SYNCHRONIZATION_ISENSE 00411 debug().debug( "%i: It's a synchronization_pulse, so I send an acknowledgement, t1(%d), t2(%d), t3(%d)\n", radio().id(), 00412 synchronizationMessage.t1().sec(), synchronizationMessage.t2().sec(), synchronizationMessage.t3().sec() ); 00413 #endif 00414 } 00415 else if ( from == father_ ) 00416 { 00417 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00418 debug().debug( "%i: It's a synchronization from my father (%i), so I require synchronization to him\n", radio().id(), father_ ); 00419 #endif 00420 // TODO: wait random time 00421 send_sync_pulse(); 00422 } 00423 } 00424 else if ( msg_id == TpsnMsgIdTimeSync && level_ == 1 ) 00425 { 00426 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00427 debug().debug( "%i: I got time_sync message from root, and I'm in level 1, so I synchronize with him\n", radio().id() ); 00428 #endif 00429 // TODO: wait random time 00430 send_sync_pulse(); 00431 } 00432 else if ( msg_id == TpsnMsgIdAcknowledgement ) 00433 { 00434 time_t t4 = clock().time(); 00435 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00436 debug().debug( "%i: It's an acknowledgement message so I synchronize\n", radio().id() ); 00437 #endif 00438 SynchronizationMessage *msg = (SynchronizationMessage *)data; 00439 time_t offset = ( msg->t2() + msg->t3() - msg->t1() - t4 )/* TODO: / 2*/; 00440 #ifdef DEBUG_TPSN_SYNCHRONIZATION_ISENSE 00441 debug().debug( "%i: Offset: %d s, %d ms\n", radio().id(), offset.sec(), offset.ms() ); 00442 // TODO: divisió de mala manera 00443 bool arreglar = offset.sec() % 2; 00444 offset.sec_ = offset.sec() / 2; 00445 offset.ms_ = offset.ms() / 2; 00446 if (arreglar) 00447 offset.ms_ = offset.ms() + 500; 00448 debug().debug( "%i: Offset: %d s, %d ms\n", radio().id(), offset.sec(), offset.ms() ); 00449 debug().debug( "%i: My old time is: %d s, %d ms\n", radio().id(), clock().time().sec(), clock().time().ms() ); 00450 clock().set_time( clock().time() + offset); 00451 #endif 00452 // clock().set_time( clock().time() + offset); 00453 synchronized_ = true; 00454 #ifdef DEBUG_TPSN_SYNCHRONIZATION_SHAWN 00455 debug().debug( "%i: t1(%f), t2(%f), t3(%f), t4(%f) \n", radio().id(), msg->t1(), msg->t2(), msg->t3(), t4 ); 00456 debug().debug( "%i: My new time is %f\n", radio().id(), clock().time() + offset ); 00457 #endif 00458 #ifdef DEBUG_TPSN_SYNCHRONIZATION_ISENSE 00459 debug().debug( "%i: t1(%d s, %d ms), t2(%d s, %d ms), t3(%d s, %d ms), t4(%d s, %d ms) \n", radio().id(), msg->t1().sec(), msg->t1().ms(), msg->t2().sec(), msg->t2().ms(), msg->t3().sec(), msg->t3().ms(), t4.sec(), t4.ms() ); 00460 debug().debug( "%i: My new time is %d s, %d ms\n", radio().id(), clock().time().sec(), clock().time().ms() ); 00461 #endif 00462 00463 } 00464 } 00465 // ----------------------------------------------------------------------- 00466 template<typename OsModel_P, 00467 typename Radio_P, 00468 typename Debug_P, 00469 typename Clock_P, 00470 uint16_t MAX_NODES> 00471 void 00472 TpsnSynchronization<OsModel_P, Radio_P, Debug_P, Clock_P, MAX_NODES>:: 00473 send_sync_pulse( ) 00474 { 00475 #ifdef DEBUG_TPSN_SYNCHRONIZATION 00476 debug().debug( "%i: Sending synch_pulse to %i\n", radio().id(), father_ ); 00477 #endif 00478 timer().template set_timer<self_type, &self_type::timer_elapsed>( 00479 timeout_, this, 0 ); 00480 synchronizationMessage.set_msg_id( TpsnMsgIdSynchronizationPulse ); 00481 synchronizationMessage.set_receiver( father_ ); 00482 synchronizationMessage.set_t1( clock().time() ); 00483 #ifdef DEBUG_TPSN_SYNCHRONIZATION_ISENSE 00484 debug().debug( "%i: Set t1(%d s, %d ms)\n", radio().id(), synchronizationMessage.t1().sec(), synchronizationMessage.t1().ms() ); 00485 #endif 00486 radio().send( radio().BROADCAST_ADDRESS, 1 + TIME_SIZE + NODE_ID_SIZE, (uint8_t*)&synchronizationMessage ); 00487 } 00488 } 00489 #endif
