void gposReadyState(void)
{
JausMessage txMessage;
ServiceConnection scList;
ServiceConnection sc;
char buf[64] = {0};
char buf2[64] = {0};
PointLla vehiclePosLla;
vehiclePosLla = vehicleSimGetPositionLla();
if(vehiclePosLla)
{
gposMessage->latitudeDegrees = vehiclePosLla->latitudeRadians * DEG_PER_RAD;
gposMessage->longitudeDegrees = vehiclePosLla->longitudeRadians * DEG_PER_RAD;
}
gposMessage->yawRadians = -(vehicleSimGetH() - PI/2.0);
gposMessage->elevationMeters = 0;
gposMessage->positionRmsMeters = 1.0;
gposMessage->rollRadians = 0.0;
gposMessage->pitchRadians = 0.0;
gposMessage->attitudeRmsRadians = 0.05;
gposMessage->timeStamp = 0;
// Send message
if(scManagerQueryActiveMessage(gposNmi, JAUS_REPORT_GLOBAL_POSE))
{
gposScActive = JAUS_TRUE;
scList = scManagerGetSendList(gposNmi, JAUS_REPORT_GLOBAL_POSE);
sc = scList;
while(sc)
{
gposMessage->destination->id = sc->address->id;
gposMessage->presenceVector = sc->presenceVector;
gposMessage->sequenceNumber = sc->sequenceNumber;
gposMessage->scFlag = JAUS_SERVICE_CONNECTION_MESSAGE;
txMessage = reportGlobalPoseMessageToJausMessage(gposMessage);
nodeManagerSend(gposNmi, txMessage);
jausMessageDestroy(txMessage);
jausAddressToString(gposMessage->source, buf);
jausAddressToString(gposMessage->destination, buf2);
cDebug(9, "Sent GPOS SC from %s to %s\n", buf, buf2);
sc = sc->nextSc;
}
scManagerDestroySendList(scList);
}
else
{
gposScActive = JAUS_FALSE;
}
}
void wdAllState(void)
{
JausMessage message = NULL;
ServiceConnection scList;
ServiceConnection sc;
ReportComponentStatusMessage reportStatus;
ReleaseComponentControlMessage releaseControl = NULL;
// Retrieve the Gpos SC Message
if(gposSc && gposSc->isActive)
{
if(scManagerReceiveServiceConnection(wdNmi, gposSc, &message))
{
wdProcessMessage(message);
}
}
if(vssSc && vssSc->isActive)
{
if(scManagerReceiveServiceConnection(wdNmi, vssSc, &message))
{
wdProcessMessage(message);
}
}
if(pdWrenchSc && pdWrenchSc->isActive)
{
if(scManagerReceiveServiceConnection(wdNmi, pdWrenchSc, &message))
{
wdProcessMessage(message);
}
}
if(pdStatusSc && pdStatusSc->isActive)
{
if(scManagerReceiveServiceConnection(wdNmi, pdStatusSc, &message))
{
wdProcessMessage(message);
}
}
// Respond to a ReportComponentStatus Service Connection
scList = scManagerGetSendList(wdNmi, JAUS_REPORT_COMPONENT_STATUS);
if(scList)
{
reportStatus = reportComponentStatusMessageCreate();
jausAddressCopy(reportStatus->source, wd->address);
reportStatus->properties.scFlag = JAUS_SERVICE_CONNECTION_MESSAGE;
reportStatus->primaryStatusCode = wd->state;
sc = scList;
while(sc)
{
jausAddressCopy(reportStatus->destination, sc->address);
reportStatus->sequenceNumber = sc->sequenceNumber;
message = reportComponentStatusMessageToJausMessage(reportStatus);
nodeManagerSend(wdNmi, message);
jausMessageDestroy(message);
sc = sc->nextSc;
}
reportComponentStatusMessageDestroy(reportStatus);
}
scManagerDestroySendList(scList);
// Check Control
if(!wdRequestControl && wdInControl)
{
// Release Control
//cDebug(4, "wd: Releasing control of PD\n");
releaseControl = releaseComponentControlMessageCreate();
jausAddressCopy(releaseControl->source, wd->address);
jausAddressCopy(releaseControl->destination, pd->address);
message = releaseComponentControlMessageToJausMessage(releaseControl);
nodeManagerSend(wdNmi, message);
jausMessageDestroy(message);
releaseComponentControlMessageDestroy(releaseControl);
wdInControl = JAUS_FALSE;
pd->state = JAUS_UNKNOWN_STATE;
if(pdStatusSc->isActive)
{
scManagerTerminateServiceConnection(wdNmi, pdStatusSc);
}
if(pdWrenchSc->isActive)
{
scManagerTerminateServiceConnection(wdNmi, pdWrenchSc);
}
wd->state = JAUS_INITIALIZE_STATE;
}
}
ServiceConnection ojCmptGetScSendList(OjCmpt ojCmpt, unsigned short commandCode)
{
return scManagerGetSendList(ojCmpt->nmi, commandCode);
}
