void
_sched(void)
{
Proc *p;
Thread *t;
Resched:
p = _threadgetproc();
if((t = p->thread) != nil){
needstack(128);
_threaddebug(DBGSCHED, "pausing, state=%s", psstate(t->state));
if(setjmp(t->sched)==0)
longjmp(p->sched, 1);
return;
}else{
t = runthread(p);
if(t == nil){
_threaddebug(DBGSCHED, "all threads gone; exiting");
_schedexit(p);
}
_threaddebug(DBGSCHED, "running %d.%d", t->proc->pid, t->id);
p->thread = t;
if(t->moribund){
_threaddebug(DBGSCHED, "%d.%d marked to die");
goto Resched;
}
t->state = Running;
t->nextstate = Ready;
longjmp(t->sched, 1);
}
}
void NavigationController::start(double n) {
if(running) {
ROS_WARN("Already in navigation loop");
return;
}
NumOfWPs = n;
getrosparams();
if (drivercontroller.AssignSW(true, "CH", true, false)){
ROS_INFO("Navigation started...");
running = true;
boost::thread runthread(boost::bind(&NavigationController::controlloop,this));
}else{
ROS_ERROR("Failed enabling navigation");
return;
}
}
void* THThread_main(void *arg)
{
THThreadState* state = arg;
state->status = runthread(state->data);
return NULL;
}
