monitorTask.cpp

00001 /*
00002   制御タスクの処理スレッド
00003   Satofumi KAMIMURA
00004   $Id$
00005 */
00006 
00007 #include "monitorTask.h"
00008 #include "vmonitor.h"
00009 #include "parseArgs.h"
00010 #include "screenTask.h"
00011 #include <algorithm>
00012 
00013 
00014 unsigned long MonitorTask::ticks = 0;
00015 std::vector<TaskInterface*> MonitorTask::tasks;
00016 VXV::Position3D MonitorTask::bodyPosition = VXV::Position3D();
00017 RunCtrl* MonitorTask::runObj = NULL;
00018 ScreenTask* MonitorTask::scr = NULL;
00019 
00020 
00021 int MonitorTask::taskThread(void* args) {
00022   MonitorTask* obj = (MonitorTask*)args;
00023 
00024   bool active = true;
00025   unsigned long processed_ticks = 0;
00026   obj->lock();
00027   obj->pre_ticks = VXV::GetTicks();
00028   obj->unlock();
00029   do {
00030     obj->lock();
00031 
00032     if (obj->is_pause) {
00033       // screenTask の実行
00034       if (!tasks.empty() && tasks[0]) {
00035         tasks[0]->send();
00036       }
00037     } else {
00038       unsigned long ticks_diff = VXV::GetTicks() - obj->pre_ticks;
00039       obj->ticks = obj->pre_total +
00040         static_cast<unsigned long>(ticks_diff * obj->time_magnify);
00041       unsigned long loop_times = obj->ticks - processed_ticks;
00042       processed_ticks = obj->ticks;
00043 
00044       // ロボットの位置を更新
00045       for (std::vector<TaskInterface*>::iterator it = tasks.begin();
00046            it != tasks.end(); ++it) {
00047         if (!*it) {
00048           continue;
00049         }
00050         if ((*it)->updatePosition()) {
00051           bodyPosition = (*it)->getBodyPosition(bodyPosition);
00052         }
00053       }
00054 
00055       // 登録タスクの処理
00056       for (std::vector<TaskInterface*>::iterator it = tasks.begin();
00057            it != tasks.end(); ++it) {
00058         if (*it) {
00059           (*it)->recv();
00060         }
00061       }
00062       for (unsigned long times = 0; times < loop_times; ++times) {
00063         for (std::vector<TaskInterface*>::iterator it = tasks.begin();
00064              it != tasks.end(); ++it) {
00065           if (*it) {
00066             (*it)->exec1msec(obj->ticks + times);
00067           }
00068         }
00069       }
00070       for (std::vector<TaskInterface*>::iterator it = tasks.begin();
00071            it != tasks.end(); ++it) {
00072         if (*it) {
00073           (*it)->send();
00074         }
00075       }
00076 
00077       // 待機スレッドの起床評価
00078       for (std::vector<waits_t>::iterator it = obj->waits.begin();
00079            it != obj->waits.end();) {
00080         if (obj->ticks > it->ticks) {
00081           SDL_LockMutex(it->mutex);
00082           SDL_CondSignal(it->cond);
00083           SDL_UnlockMutex(it->mutex);
00084           it = obj->waits.erase(it);
00085         } else {
00086           ++it;
00087         }
00088       }
00089     }
00090 
00091     active = obj->active;
00092     obj->unlock();
00093     SDL_Delay(1);
00094   } while (active);
00095 
00096   return 0;
00097 }
00098 
00099 
00100 MonitorTask::MonitorTask(void)
00101   : mutex(NULL), thread(NULL), active(true), is_pause(false), pre_total(0),
00102     pre_ticks(VXV::GetTicks()), time_magnify(1.0) {
00103   waits.clear();
00104   mutex = SDL_CreateMutex();
00105   thread = SDL_CreateThread(taskThread, this);
00106 }
00107 
00108 
00109 MonitorTask::~MonitorTask(void) {
00110   lock();
00111   active = false;
00112   unlock();
00113   SDL_WaitThread(thread, NULL);
00114   SDL_DestroyMutex(mutex);
00115 }
00116 
00117 
00118 void MonitorTask::lock(void) {
00119   SDL_LockMutex(mutex);
00120 }
00121 
00122 
00123 void MonitorTask::unlock(void) {
00124   SDL_UnlockMutex(mutex);
00125 }
00126 
00127 
00128 SDL_cond* MonitorTask::setWakeupTicks(unsigned long wait_ticks,
00129                                       SDL_cond* wait_cond,
00130                                       SDL_mutex* wait_mutex) {
00131   if (wait_ticks < ticks) {
00132     return false;
00133   }
00134 
00135   waits_t add;
00136   add.ticks = wait_ticks;
00137   add.cond = wait_cond;
00138   add.mutex = wait_mutex;
00139   waits.push_back(add);
00140 
00141   return add.cond;
00142 }
00143 
00144 
00145 void MonitorTask::add(TaskInterface* task) {
00146   if (task) {
00147     lock();
00148     task->init();
00149     tasks.push_back(task);
00150     unlock();
00151   }
00152 }
00153 
00154 
00155 void MonitorTask::del(TaskInterface* task) {
00156   if (task) {
00157     lock();
00158     tasks.erase(remove(tasks.begin(), tasks.end(), task));
00159     unlock();
00160   }
00161 }
00162 
00163 
00164 unsigned long MonitorTask::getTicks(void) {
00165   lock();
00166   unsigned long ret_ticks = ticks;
00167   unlock();
00168   return ret_ticks;
00169 }
00170 
00171 
00172 void MonitorTask::pause(void) {
00173   lock();
00174   is_pause = true;
00175   pre_total +=
00176     static_cast<unsigned long>((VXV::GetTicks() - pre_ticks) * time_magnify);
00177   unlock();
00178 }
00179 
00180 
00181 void MonitorTask::start(void) {
00182   lock();
00183   pre_ticks = VXV::GetTicks();
00184   is_pause = false;
00185   unlock();
00186 }
00187 
00188 
00189 void MonitorTask::setTimeMagnify(double magnify) {
00190   pause();
00191   lock();
00192   time_magnify = magnify;
00193   unlock();
00194   start();
00195 }
00196 
00197 
00198 void MonitorTask::waitToTicks(unsigned long wait_ticks,
00199                               SDL_cond* cond, SDL_mutex* wait_mutex) {
00200   SDL_LockMutex(wait_mutex);
00201 
00202   lock();
00203   SDL_cond* mustWait = setWakeupTicks(wait_ticks, cond, wait_mutex);
00204   if (!mustWait) {
00205     SDL_UnlockMutex(wait_mutex);
00206   }
00207   unlock();
00208 
00209   if (mustWait) {
00210     SDL_CondWait(cond, wait_mutex);
00211     SDL_UnlockMutex(wait_mutex);
00212   }
00213 }
00214 
00215 
00216 void MonitorTask::waitOnlyCommand(vmonitor* mon, int monitorMode,
00217                                   SDL_cond* cond, SDL_mutex* wait_mutex,
00218                                   const char* module, const char* command) {
00219 
00220   if (monitorMode == Monitor::Playback) {
00221     // 待機
00222     mon->log->lock();
00223     unsigned long ticks = mon->log->readTag(module, command);
00224     mon->log->unlock();
00225     waitToTicks(ticks, cond, wait_mutex);
00226 
00227   } else {
00228     // 記録
00229     mon->log->lock();
00230     mon->log->writeTag(module, command, mon->getTicks());
00231     mon->log->writeTagEnd();
00232     mon->log->unlock();
00233   }
00234 }
00235 
00236 
00237 void MonitorTask::setRunObject(RunCtrl* run_obj) {
00238   runObj = run_obj;
00239 }
00240 
00241 
00242 VXV::Position MonitorTask::getRunPosition(void) {
00243   if (runObj) {
00244 #if 0
00245     return runObj->getCrdPosition(runObj, runObj->local_offset + bodyPosition);
00246 #else
00247     VXV::Position3D position;
00248     convertWithAngle(position, bodyPosition, runObj->local_offset);
00249     position += VXV::Position(0, 0, bodyPosition.zt);
00250 
00251     return runObj->getCrdPosition(runObj, position);
00252 #endif
00253   } else {
00254     return bodyPosition;
00255   }
00256 }
00257 
00258 
00259 void MonitorTask::setScreenObject(ScreenTask* scr_obj) {
00260   if (tasks.empty()) {
00261     add(scr_obj);
00262     scr = scr_obj;
00263 
00264   } else {
00265     tasks[0] = scr_obj;
00266     scr = scr_obj;
00267   }
00268 }
00269 

Generated on Mon Apr 13 22:52:03 2009 by  doxygen 1.5.7.1