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_LOCALIZATION_DISTANCE_BASED_LOCALIZATION_MIMMAX_MODULE_H 00020 #define __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_LOCALIZATION_MIMMAX_MODULE_H 00021 00022 #include "algorithms/localization/distance_based/modules/localization_module.h" 00023 #include "algorithms/localization/distance_based/util/localization_defutils.h" 00024 #include "algorithms/localization/distance_based/math/localization_triangulation.h" 00025 #include "algorithms/localization/distance_based/math/vec.h" 00026 #include "config_testing.h" 00027 00028 namespace wiselib 00029 { 00030 00032 00034 template<typename OsModel_P, 00035 typename Radio_P, 00036 typename Debug_P, 00037 typename SharedData_P, 00038 typename Arithmatic_P > 00039 class LocalizationMinMaxModule 00040 : public LocalizationModule<OsModel_P, Radio_P, SharedData_P> 00041 { 00042 00043 public: 00044 typedef OsModel_P OsModel; 00045 typedef Radio_P Radio; 00046 typedef Debug_P Debug; 00047 typedef SharedData_P SharedData; 00048 typedef Arithmatic_P Arithmatic; 00049 00050 typedef LocalizationMinMaxModule<OsModel, Radio, Debug, SharedData, Arithmatic> self_type; 00051 typedef LocalizationModule<OsModel, Radio, SharedData> base_type; 00052 00053 typedef typename Radio::size_t size_t; 00054 typedef typename Radio::node_id_t node_id_t; 00055 typedef typename Radio::block_data_t block_data_t; 00056 00057 typedef typename SharedData::NeighborInfoList NeighborInfoList; 00058 typedef typename SharedData::Neighborhood Neighborhood; 00059 00063 LocalizationMinMaxModule(); 00065 ~LocalizationMinMaxModule(); 00067 00070 00072 void receive( node_id_t from, size_t len, block_data_t *data ) 00073 {} 00076 void work( void ); 00078 00081 00083 bool finished( void ); 00085 void rollback( void ); 00086 00087 void init( Radio& radio, Debug& debug, SharedData& shared_data ) 00088 { 00089 radio_ = &radio; 00090 debug_ = &debug; 00091 //shared_data_ = &shared_data; 00092 this->shared_data().is_anchor(); 00093 } 00094 00095 private: 00096 00097 enum MinMaxState 00098 { 00099 minmax_wait, 00100 minmax_work, 00101 minmax_finished 00102 }; 00103 00104 MinMaxState state_; 00105 00106 Radio* radio_; 00107 Debug* debug_; 00108 //SharedData* shared_data_; 00109 }; 00110 // ---------------------------------------------------------------------- 00111 // ---------------------------------------------------------------------- 00112 // ---------------------------------------------------------------------- 00113 template<typename OsModel_P, 00114 typename Radio_P, 00115 typename Debug_P, 00116 typename SharedData_P, 00117 typename Arithmatic_P> 00118 LocalizationMinMaxModule<OsModel_P, Radio_P, Debug_P, SharedData_P, Arithmatic_P>:: 00119 LocalizationMinMaxModule() 00120 : state_( minmax_wait ) 00121 {} 00122 // ---------------------------------------------------------------------- 00123 template<typename OsModel_P, 00124 typename Radio_P, 00125 typename Debug_P, 00126 typename SharedData_P, 00127 typename Arithmatic_P> 00128 LocalizationMinMaxModule<OsModel_P, Radio_P, Debug_P, SharedData_P,Arithmatic_P>:: 00129 ~LocalizationMinMaxModule() 00130 {} 00131 // ---------------------------------------------------------------------- 00132 template<typename OsModel_P, 00133 typename Radio_P, 00134 typename Debug_P, 00135 typename SharedData_P, 00136 typename Arithmatic_P> 00137 void 00138 LocalizationMinMaxModule<OsModel_P, Radio_P, Debug_P, SharedData_P,Arithmatic_P>:: 00139 work( void ) 00140 { 00141 00142 #ifdef LOCALIZATION_DISTANCEBASED_MINMAX_DEBUG 00143 debug_->debug( "MINMAX working" ); 00144 #endif 00145 // if anchor, do not change position 00146 if ( this->shared_data().is_anchor() ) 00147 state_ = minmax_finished; 00148 00149 if ( state_ == minmax_finished ) 00150 return; 00151 00152 00153 00154 if ( state_ == minmax_wait ) 00155 state_ = minmax_work; 00156 00157 Vec<Arithmatic> est_pos; 00158 NeighborInfoList neighbors; 00159 collect_neighbors<OsModel, Neighborhood, NeighborInfoList, Arithmatic>( 00160 this->neighborhood(), lat_anchors, neighbors ); 00161 00162 /* if ( neighbors.empty() ) 00163 { 00164 //#ifdef LOCALIZATION_DISTANCEBASED_LATERATION_DEBUG 00165 debug_->debug( "LocalizationMINMAXModule: No neighbors found\n" ); 00166 //#endif 00167 return; 00168 }*/ 00169 00170 if ( neighbors.empty() ) 00171 { 00172 //#ifdef LOCALIZATION_DISTANCEBASED_LATERATION_DEBUG 00173 if((radio_->id()==0x9999)||(radio_->id()==0x0c)) 00174 debug_->debug( "LocalizationMINMAXModule: No neighbors found\n" ); 00175 //#endif 00176 return; 00177 } 00178 //#ifdef LOCALIZATION_DISTANCEBASED_LATERATION_DEBUG 00179 if((radio_->id()==0x9999)||(radio_->id()==0x0c)) 00180 debug_->debug( "LocalizationMINMAXModule: %d neigbous found\n",neighbors.size() ); 00181 00182 //#endif 00183 00184 if ( est_pos_min_max<OsModel, NeighborInfoList, Arithmatic>( neighbors, est_pos ) ) 00185 { 00186 if ( !this->shared_data().check_residue() || 00187 check_residue<OsModel, NeighborInfoList, Arithmatic>( 00188 neighbors, est_pos, lat_anchors, 00189 this->shared_data().communication_range() ) ) 00190 { 00191 this->shared_data().set_position( est_pos ); 00192 #ifdef LOCALIZATION_DISTANCEBASED_MINMAX_DEBUG 00193 debug_->debug( "set pos to (%d, %d)\n", (uint8_t)est_pos.x(), (uint8_t)est_pos.y() ); 00194 #endif 00195 } 00196 } 00197 00198 state_ = minmax_finished; 00199 } 00200 // ---------------------------------------------------------------------- 00201 template<typename OsModel_P, 00202 typename Radio_P, 00203 typename Debug_P, 00204 typename SharedData_P, 00205 typename Arithmatic_P> 00206 bool 00207 LocalizationMinMaxModule<OsModel_P, Radio_P, Debug_P, SharedData_P,Arithmatic_P>:: 00208 finished( void ) 00209 { 00210 return state_ == minmax_finished; 00211 } 00212 // ---------------------------------------------------------------------- 00213 template<typename OsModel_P, 00214 typename Radio_P, 00215 typename Debug_P, 00216 typename SharedData_P, 00217 typename Arithmatic_P> 00218 void 00219 LocalizationMinMaxModule<OsModel_P, Radio_P, Debug_P, SharedData_P,Arithmatic_P>:: 00220 rollback( void ) 00221 { 00222 state_ = minmax_wait; 00223 //shared_data_->reset_neighborhood_(); 00224 } 00225 00226 }// namespace wiselib 00227 #endif