// Function: cmptThread // Access: Private // Description: All core component functionality is contained in this thread. // All of the JAUS component state machine code can be found here. void *cmptThread(void *threadData) { JausMessage rxMessage; double time, prevTime, nextExcecuteTime = 0.0; struct timespec sleepTime; cmptThreadRunning = TRUE; sleepTime.tv_sec = 0; sleepTime.tv_nsec = 1000; time = getTimeSeconds(); cmpt->state = JAUS_INITIALIZE_STATE; // Set JAUS state to INITIALIZE cmptStartupState(); while(cmptRun) // Execute state machine code while not in the SHUTDOWN state { do { if(nodeManagerReceive(cmptNmi, &rxMessage)) { cDebug(4, "CMPT: Got message: %s from %d.%d.%d.%d\n", jausMessageCommandCodeString(rxMessage), rxMessage->source->subsystem, rxMessage->source->node, rxMessage->source->component, rxMessage->source->instance); cmptProcessMessage(rxMessage); } else { if(getTimeSeconds() > nextExcecuteTime) { break; } else { nanosleep(&sleepTime, NULL); } } }while(getTimeSeconds() < nextExcecuteTime); prevTime = time; time = getTimeSeconds(); cmptThreadHz = 1.0/(time-prevTime); // Compute the update rate of this thread switch(cmpt->state) // Switch component behavior based on which state the machine is in { case JAUS_INITIALIZE_STATE: cmptInitState(); break; case JAUS_STANDBY_STATE: cmptStandbyState(); break; case JAUS_READY_STATE: cmptReadyState(); break; case JAUS_EMERGENCY_STATE: cmptEmergencyState(); break; case JAUS_FAILURE_STATE: cmptFailureState(); break; case JAUS_SHUTDOWN_STATE: cmptRun = FALSE; break; default: cmpt->state = JAUS_FAILURE_STATE; // The default case JAUS_is undefined, therefore go into Failure State break; } cmptAllState(); nodeManagerSendCoreServiceConnections(cmptNmi); nextExcecuteTime = 2.0 * time + 1.0/CMPT_THREAD_DESIRED_RATE_HZ - getTimeSeconds(); } cmptShutdownState(); usleep(50000); // Sleep for 50 milliseconds and then exit cmptThreadRunning = FALSE; return NULL; }
void* ojCmptThread(void *threadData) { int i = 0; OjCmpt ojCmpt; JausMessage rxMessage; double prevTime = 0; double time = ojGetTimeSec(); double nextStateTime = ojGetTimeSec(); // Get handle to OpenJausComponent that was created ojCmpt = (OjCmpt)threadData; while(ojCmpt->run) // Execute state machine code while not in the SHUTDOWN state { switch(nodeManagerTimedReceive(ojCmpt->nmi, &rxMessage, nextStateTime)) { case NMI_MESSAGE_RECEIVED: // If we were sent a message if(rxMessage) { ojCmptProcessMessage(ojCmpt, rxMessage); } // Always check for messages on incoming SC queues for(i=0; i<OJ_CMPT_MAX_INCOMING_SC_COUNT; i++) { if(ojCmpt->inConnection[i] && ojCmpt->inConnection[i]->isActive) // Attempt to process incoming message { if(scManagerReceiveServiceConnection(ojCmpt->nmi, ojCmpt->inConnection[i], &rxMessage)) { ojCmptProcessMessage(ojCmpt, rxMessage); } } } break; case NMI_RECEIVE_TIMED_OUT: prevTime = time; time = ojGetTimeSec(); ojCmpt->rateHz = 1.0/(time-prevTime); // Compute the update rate of this thread nextStateTime = time + 1.0/ojCmpt->frequencyHz; if(ojCmpt->mainCallback) { ojCmpt->mainCallback(ojCmpt); } if(ojCmpt->state != JAUS_UNDEFINED_STATE && ojCmpt->stateCallback[ojCmpt->state]) { ojCmpt->stateCallback[ojCmpt->state](ojCmpt); } ojCmptManageServiceConnections(ojCmpt); nodeManagerSendCoreServiceConnections(ojCmpt->nmi); break; case NMI_CONDITIONAL_WAIT_ERROR: printf("ojCmpt.c : Conditional Wait Error in nodeManagerTimedReceive, Exiting ojCmpt Thread\n"); ojCmpt->run = FALSE; break; case NMI_CLOSED_ERROR: printf("ojCmpt.c : Node Manager Interface closed when calling nodeManagerTimedReceive, Exiting ojCmpt Thread\n"); ojCmpt->run = FALSE; break; default: printf("ojCmpt.c : Error in nodeManagerTimedReceive, Exiting ojCmpt Thread\n"); ojCmpt->run = FALSE; break; } } return NULL; }