// Function: cmptShutdown // Access: Public // Description: This function allows the abstracted component functionality contained in this file to be stoped from an external source. // If the component is in the running state, this function will terminate all threads running in this file // This function will also close the Jms connection to the Node Manager and check out the component from the Node Manager int cmptShutdown(void) { double timeOutSec; if(cmpt->state != JAUS_SHUTDOWN_STATE) // Execute the shutdown routines only if the component is running { cmptRun = FALSE; timeOutSec = getTimeSeconds() + CMPT_THREAD_TIMEOUT_SEC; while(cmptThreadRunning) { usleep(100000); if(getTimeSeconds() >= timeOutSec) { pthread_cancel(cmptThreadId); cmptThreadRunning = FALSE; cError("cmpt: cmptThread Shutdown Improperly\n"); break; } } nodeManagerClose(cmptNmi); // Close Node Manager Connection jausSubsystemDestroy(cmpt->node->subsystem); jausNodeDestroy(cmpt->node); jausServicesDestroy(cmpt->services); cmpt->state = JAUS_SHUTDOWN_STATE; propertiesDestroy(cmptProperties); } return 0; }
// Function: wdShutdown // Access: Public // Description: This function allows the abstracted component functionality contained in this file to be stoped from an external source. // If the component is in the running state, this function will terminate all threads running in this file // This function will also close the Jms connection to the Node Manager and check out the component from the Node Manager int wdShutdown(void) { double timeOutSec; if(wd && wd->state != JAUS_SHUTDOWN_STATE) // Execute the shutdown routines only if the component is running { wdRun = FALSE; timeOutSec = getTimeSeconds() + WD_THREAD_TIMEOUT_SEC; while(wdThreadRunning) { SLEEP_MS(1000); if(getTimeSeconds() >= timeOutSec) { pthread_cancel(wdThreadId); wdThreadRunning = FALSE; //cError("wd: wdThread Shutdown Improperly\n"); break; } } nodeManagerClose(wdNmi); // Close Node Manager Connection jausSubsystemDestroy(wd->node->subsystem); jausNodeDestroy(wd->node); jausServicesDestroy(wd->services); wd->state = JAUS_SHUTDOWN_STATE; propertiesDestroy(wdProperties); } return 0; }
int scManagerUpdateServiceConnection(ServiceConnection sc, unsigned short sequenceNumber) { int returnValue = JAUS_FALSE; sc->lastSentTime = getTimeSeconds(); if(sequenceNumber == sc->sequenceNumber) { returnValue = SC_ERROR_SEQUENCE_NUMBERS_EQUAL; } else if(sequenceNumber == 0 && sc->sequenceNumber != 65535) { returnValue = SC_ERROR_SEQUENCE_NUMBER_OUT_OF_SYNC; } else if(sequenceNumber - sc->sequenceNumber != 1 && sequenceNumber !=0) { returnValue = SC_ERROR_SEQUENCE_NUMBER_OUT_OF_SYNC; } else { returnValue = JAUS_TRUE; } sc->sequenceNumber = sequenceNumber; return returnValue; }
void scManagerProcessConfirmScMessage(NodeManagerInterface nmi, ConfirmServiceConnectionMessage message) { ServiceConnection prevSc = NULL; ServiceConnection sc = nmi->scm->incommingSc; TerminateServiceConnectionMessage terminateSc; JausMessage txMessage; while(sc) { if( sc->commandCode == message->serviceConnectionCommandCode && sc->address->id == message->source->id ) { if( message->responseCode == JAUS_SC_SUCCESSFUL ) { sc->confirmedUpdateRateHz = message->confirmedPeriodicUpdateRateHertz; sc->instanceId = message->instanceId; sc->isActive = JAUS_TRUE; sc->sequenceNumber = 65535; sc->lastSentTime = getTimeSeconds(); } else { // Set SC Inactive sc->isActive = JAUS_FALSE; // Remove Service Connection if(prevSc) { prevSc->nextSc = sc->nextSc; sc->nextSc = NULL; } else { nmi->scm->incommingSc = sc->nextSc; sc->nextSc = NULL; } nmi->scm->incommingScCount--; } return; } prevSc = sc; sc = sc->nextSc; } // The SC was not found, so send a terminate to prevent streaming if( message->responseCode == JAUS_SC_SUCCESSFUL ) { terminateSc = terminateServiceConnectionMessageCreate(); terminateSc->source->id = nmi->cmpt->address->id; terminateSc->destination->id = message->source->id; terminateSc->serviceConnectionCommandCode = message->serviceConnectionCommandCode; terminateSc->instanceId = message->instanceId; txMessage = terminateServiceConnectionMessageToJausMessage(terminateSc); nodeManagerSend(nmi, txMessage); jausMessageDestroy(txMessage); terminateServiceConnectionMessageDestroy(terminateSc); } }
bool InfluenceWander::update() { if( !Parent::update() ){ return false; } //---choose new direction ever so often---------------------- if(mTimeOut<getTimeSeconds()){ onExpired(); } return true; }
JausBoolean scManagerReceiveServiceConnection(NodeManagerInterface nmi, ServiceConnection requestSc, JausMessage *message) { ServiceConnection prevSc; ServiceConnection sc = nmi->scm->incommingSc; prevSc = NULL; while(sc) { if(sc->commandCode == requestSc->commandCode && sc->address->id == requestSc->address->id ) { if(getTimeSeconds() > (sc->lastSentTime + sc->timeoutSec)) { // Connection has Timed Out sc->isActive = JAUS_FALSE; while(sc->queue->size) { jausMessageDestroy(queuePop(sc->queue)); } // Remove Service Connection if(prevSc) { prevSc->nextSc = sc->nextSc; sc->nextSc = NULL; } else { nmi->scm->incommingSc = sc->nextSc; sc->nextSc = NULL; } nmi->scm->incommingScCount--; return JAUS_FALSE; } if(sc->queue->size) { *message = (JausMessage)queuePop(sc->queue); return JAUS_TRUE; } else { return JAUS_FALSE; } } else { prevSc = sc; sc = sc->nextSc; } } cError( "libnodeManager: scManagerServiceConnectionReceive: Requested SC does not exist\n" ); return JAUS_FALSE; }
// Function: vehicleSimShutdown // Access: Public // Description: This function allows the abstracted component functionality contained in this file to be stoped from an external source. // If the component is in the running state, this function will terminate all threads running in this file // This function will also close the Jms connection to the Node Manager and check out the component from the Node Manager int vehicleSimShutdown(void) { double timeOutSec; vehicleSimRun = FALSE; timeOutSec = getTimeSeconds() + VEHICLE_SIM_THREAD_TIMEOUT_SEC; while(vehicleSimThreadRunning) { usleep(100000); if(getTimeSeconds() >= timeOutSec) { pthread_cancel(vehicleSimThreadId); vehicleSimThreadRunning = FALSE; cError("vehicleSim: vehicleSimThread Shutdown Improperly\n"); break; } } propertiesDestroy(vehicleSimProperties); return 0; }
ServiceConnection scManagerGetSendList(NodeManagerInterface nmi, unsigned short commandCode) { SupportedScMessage supportedScMsg; ServiceConnection sc; ServiceConnection newSc = NULL; ServiceConnection firstSc = NULL; double currentTime = getTimeSeconds(); // find the SC object associated with this command code supportedScMsg = scFindSupportedScMsgInList(nmi->scm->supportedScMsgList, commandCode); if(supportedScMsg) { sc = supportedScMsg->scList; } else { return NULL; } while(sc) { // Check for update rate if(sc->isActive && sc->lastSentTime < (currentTime - 1.0/sc->confirmedUpdateRateHz)) { sc->lastSentTime = currentTime; if(newSc == NULL) { newSc = (ServiceConnection)malloc(sizeof(ServiceConnectionStruct)); firstSc = newSc; } else { newSc->nextSc = (ServiceConnection)malloc(sizeof(ServiceConnectionStruct)); newSc = newSc->nextSc; } *newSc = *sc; newSc->nextSc = NULL; sc->sequenceNumber++; } sc = sc->nextSc; } return firstSc; }
void scManagerReceiveMessage(NodeManagerInterface nmi, JausMessage message) { ServiceConnection sc = nmi->scm->incommingSc; char string[32] = {0}; while(sc) { if(sc->commandCode == message->commandCode && sc->address->id == message->source->id ) { if(sc->isActive) { sc->lastSentTime = getTimeSeconds(); if(sc->queueSize && sc->queueSize == sc->queue->size) { jausMessageDestroy(queuePop(sc->queue)); queuePush(sc->queue, (void *)message); } else { queuePush(sc->queue, (void *)message); } // cDebug(3, "Queue Size: %d\n", sc->queue->size); } else { // TODO: Error? received a message for inactive SC jausMessageDestroy(message); } return; } sc = sc->nextSc; } jausAddressToString(message->source, string); cError("libnodeManager: scManangerReceiveMessage: No SC for: %s, from: %s\n", jausMessageCommandCodeString(message), string); jausMessageDestroy(message); }
void InfluenceWander::init(AIFishPlayer *_fish, const char *_name) { Parent::init(_fish, _name); mDirection = mFish->getHeading(); mTimeOut = gRandGen.randF(mFish->mDataBlock->wanderTimeMin, mFish->mDataBlock->wanderTimeMax) + getTimeSeconds(); mTurnSpeed = mFish->mDataBlock->wanderTurnScale; mAlarm.setInterval(0.1f, true); }
void InfluenceWander::reset(Point3F *d) { mTimeOut = gRandGen.randF(mFish->mDataBlock->wanderTimeMin, mFish->mDataBlock->wanderTimeMax) + getTimeSeconds(); mDirection = randomVectorRotation(d? *d : mFish->getHeading()); mMoveSpeed = gRandGen.randF( mFish->mDataBlock->wanderSpeedScaleLo, mFish->mDataBlock->wanderSpeedScaleHi); }
// 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 wdInitState(void) { static double nextRequestTimeSec = 0.0; RequestComponentControlMessage requestControl = NULL; JausMessage txMessage = NULL; char buf[64] = {0}; // Check for critcal service connections or conditions here if(gposSc->isActive && vssSc->isActive && pdWrenchSc->isActive && pdStatusSc->isActive && wdInControl && wdRequestControl) { // Transition to Standby wd->state = JAUS_STANDBY_STATE; //cDebug(4, "wd: Switching to STANDBY State\n"); } else if(getTimeSeconds() > nextRequestTimeSec) { if(!gposSc->isActive) { if(scManagerCreateServiceConnection(wdNmi, gposSc)) { //cDebug(4, "wd: Sent GPOS SC Request\n"); } } if(!vssSc->isActive) { if(scManagerCreateServiceConnection(wdNmi, vssSc)) { //cDebug(4, "wd: Sent VSS SC Request\n"); } } if(!pdWrenchSc->isActive && wdInControl) { if(scManagerCreateServiceConnection(wdNmi, pdWrenchSc)) { //cDebug(4, "wd: Sent PD Wrench SC Request\n"); } } if(!pdStatusSc->isActive && wdInControl) { if(scManagerCreateServiceConnection(wdNmi, pdStatusSc)) { //cDebug(4, "wd: Sent PD Status SC Request\n"); } } nextRequestTimeSec = getTimeSeconds() + REQUEST_TIMEOUT_SEC; } if(!wdInControl && wdRequestControl) { if(!pd->address->node) { pd->address->subsystem = wd->address->subsystem; if(nodeManagerLookupAddress(wdNmi, pd->address)) { jausAddressCopy(wdWrench->destination, pd->address); } } if(pd->address->node) { jausAddressToString(pd->address, buf); //cDebug(4, "wd: Requesting control of PD %s\n", buf); requestControl = requestComponentControlMessageCreate(); jausAddressCopy(requestControl->source, wd->address); jausAddressCopy(requestControl->destination, pd->address); requestControl->authorityCode = wd->authority; txMessage = requestComponentControlMessageToJausMessage(requestControl); nodeManagerSend(wdNmi, txMessage); jausMessageDestroy(txMessage); requestComponentControlMessageDestroy(requestControl); if(scManagerCreateServiceConnection(wdNmi, pdStatusSc)) { //cDebug(4, "wd: Sent PD Status SC Request\n"); } if(scManagerCreateServiceConnection(wdNmi, pdWrenchSc)) { //cDebug(4, "wd: Sent PD Wrench SC Request\n"); } nextRequestTimeSec = getTimeSeconds() + REQUEST_TIMEOUT_SEC; } } }
void wdExcecuteControl(VehicleState vehicleState) { static double speedCommand = 0; static double prevSpeedError = 0; static double dampedSpeedErrorDerivative = 0; static double acceleration = ACCELERATION_MPSPS; //Mpsps static double decceleration = DECCELERATION_MPSPS; //Mpsps static double lastExcecuteTime = -1; static double dt; static double linearEffortInt = 0.0; double linearEffort; double speedError; JausMessage txMessage; if(lastExcecuteTime < 0) { lastExcecuteTime = getTimeSeconds(); dt = 1.0 / WD_THREAD_DESIRED_RATE_HZ; } else { dt = getTimeSeconds() - lastExcecuteTime; lastExcecuteTime = getTimeSeconds(); } if(speedCommand < vehicleState->desiredSpeedMps) { speedCommand += acceleration * dt; if(speedCommand > vehicleState->desiredSpeedMps) { speedCommand = vehicleState->desiredSpeedMps; } } else { speedCommand -= decceleration * dt; if(speedCommand < vehicleState->desiredSpeedMps) { speedCommand = vehicleState->desiredSpeedMps; } } speedError = speedCommand - vehicleState->speedMps; linearEffortInt += speedError * dt; if(linearEffortInt > LINEAR_EFFORT_I_LIM) { linearEffortInt = LINEAR_EFFORT_I_LIM; } if(linearEffortInt < -LINEAR_EFFORT_I_LIM) { linearEffortInt = -LINEAR_EFFORT_I_LIM; } if(dt > 0.001) { dampedSpeedErrorDerivative = 0.9 * dampedSpeedErrorDerivative + 0.1 * (speedError - prevSpeedError) / dt; prevSpeedError = speedError; } linearEffort = LINEAR_EFFORT_K_FF * speedCommand + LINEAR_EFFORT_BIAS_FF; linearEffort += LINEAR_EFFORT_K_P * speedError; linearEffort += LINEAR_EFFORT_K_I * linearEffortInt; linearEffort += LINEAR_EFFORT_K_D * dampedSpeedErrorDerivative; // Pitch feed forward //linearEffort += PITCH_FF_EFFORT_PER_RAD * sin(wdDampedPitchRad); // Pitch feed forward effort // Steering feed forward if(wdReportWrench) { linearEffort += STEERING_FF_EFFORT * (1.0/cos(wdReportWrench->propulsiveRotationalEffortZPercent * WHEEL_ROTATION_RAD_PER_EFFORT) - 1.0); } // Sticktion feed forward if(speedCommand < WD_DEFAULT_MIN_SPEED_MPS) { linearEffort += STICKTION_EFFORT; } if(linearEffort > 0) { wdWrench->propulsiveLinearEffortXPercent = THROTTLE_K * linearEffort; wdWrench->resistiveLinearEffortXPercent = 0; } else { wdWrench->propulsiveLinearEffortXPercent = 0; wdWrench->resistiveLinearEffortXPercent = BRAKE_K * linearEffort; } if(wdWrench->propulsiveLinearEffortXPercent > 100.0) { wdWrench->propulsiveLinearEffortXPercent = 100.0; } if(wdWrench->propulsiveLinearEffortXPercent < 0.0) { wdWrench->propulsiveLinearEffortXPercent = 0.0; } if(wdWrench->resistiveLinearEffortXPercent > 100.0) { wdWrench->resistiveLinearEffortXPercent = 100.0; } if(wdWrench->resistiveLinearEffortXPercent < 0.0) { wdWrench->resistiveLinearEffortXPercent = 0.0; } wdWrench->propulsiveRotationalEffortZPercent = vehicleState->desiredPhiEffort; if(wdWrench->propulsiveRotationalEffortZPercent > 100.0) { wdWrench->propulsiveRotationalEffortZPercent = 100.0; } if(wdWrench->propulsiveRotationalEffortZPercent < -100.0) { wdWrench->propulsiveRotationalEffortZPercent = -100.0; } jausAddressCopy(wdWrench->destination, pd->address); txMessage = setWrenchEffortMessageToJausMessage(wdWrench); nodeManagerSend(wdNmi, txMessage); jausMessageDestroy(txMessage); return; }
void scManagerProcessConfirmEvent(NodeManagerInterface nmi, ConfirmEventMessage message) { ServiceConnection prevSc = NULL; ServiceConnection sc = nmi->scm->incommingSc; CancelEventMessage cancelEvent; JausMessage txMessage; // Loop through all incomming SC's while(sc) { // Test if this incoming SC matches the one replied to if( sc->commandCode == message->messageCode && sc->address->id == message->source->id ) { // Success if( message->responseCode == SUCCESSFUL_RESPONSE ) { sc->confirmedUpdateRateHz = message->confirmedUpdateRate; sc->instanceId = message->eventId; sc->isActive = JAUS_TRUE; sc->sequenceNumber = 65535; sc->lastSentTime = getTimeSeconds(); } else // Failure { // Set SC Inactive sc->isActive = JAUS_FALSE; // Remove Service Connection if(prevSc) { prevSc->nextSc = sc->nextSc; sc->nextSc = NULL; } else { nmi->scm->incommingSc = sc->nextSc; sc->nextSc = NULL; } nmi->scm->incommingScCount--; } return; } prevSc = sc; sc = sc->nextSc; } // The SC was not found, so send a terminate to prevent streaming if( message->responseCode == SUCCESSFUL_RESPONSE ) { cancelEvent = cancelEventMessageCreate(); cancelEvent->source->id = nmi->cmpt->address->id; cancelEvent->destination->id = message->source->id; cancelEvent->messageCode = message->messageCode; cancelEvent->eventId = message->eventId; txMessage = cancelEventMessageToJausMessage(cancelEvent); nodeManagerSend(nmi, txMessage); jausMessageDestroy(txMessage); cancelEventMessageDestroy(cancelEvent); } }
// Function: vehicleSimThread // 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 *vehicleSimThread(void *threadData) { double time, prevTime, dt; double steeringRate; double motorForce; double brakeForce; double dragForce; double turningCurvature; double deltaH; vehicleSimThreadRunning = TRUE; // Initialize the UTM engine utmConversionInit(vehiclePosLla); vehiclePosUtm = pointLlaToPointUtm(vehiclePosLla); time = getTimeSeconds(); while(vehicleSimRun) // Execute state machine code while not in the SHUTDOWN state { prevTime = time; time = getTimeSeconds(); dt = time - prevTime; vehicleSimThreadHz = 1.0/dt; // Compute the update rate of this thread steeringRate = K_STEERING * (vehicleDesiredRotationalEffort - vehicleRotationalEffort); if(steeringRate > MAX_STEERING_RATE) { steeringRate = MAX_STEERING_RATE; } else if(steeringRate < -MAX_STEERING_RATE) { steeringRate = -MAX_STEERING_RATE; } vehicleRotationalEffort += steeringRate * dt; turningCurvature = -vehicleRotationalEffort / 100.0 / MIN_TURNING_RADIUS_M; vehicleLinearEffortX = vehicleDesiredLinearEffortX; motorForce = K_THROTTLE * vehicleLinearEffortX + IDLE_FORCE_N; vehicleResistiveEffortX += BRAKE_TAU * (vehicleDesiredResistiveEffortX - vehicleResistiveEffortX); if(vehicleSpeed > 0.005) { brakeForce = K_DYNAMIC_BRAKE * vehicleResistiveEffortX; } else { brakeForce = K_STATIC_BRAKE * vehicleResistiveEffortX; if(brakeForce > motorForce) { brakeForce = motorForce + 1.0; } } dragForce = DRAG_COEFF_N_PER_MPS_SQ * vehicleSpeed * vehicleSpeed; vehicleSpeed += ( motorForce - brakeForce - dragForce) / VEHICLE_MASS_KG * dt; if(vehicleSpeed < 0) { vehicleSpeed = 0; } deltaH = turningCurvature * vehicleSpeed * dt; vehiclePosUtm->xMeters += cos(vehicleH + deltaH / 2.0) * vehicleSpeed * dt; vehiclePosUtm->yMeters += sin(vehicleH + deltaH / 2.0) * vehicleSpeed * dt; vehicleH += deltaH; vehicleH = fmod(vehicleH, 2*M_PI); if(vehiclePosLla) { pointLlaDestroy(vehiclePosLla); } vehiclePosLla = pointUtmToPointLla(vehiclePosUtm); usleep(25000); } usleep(50000); // Sleep for 50 milliseconds and then exit vehicleSimThreadRunning = FALSE; return NULL; //pthread_exit(NULL); }