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;
}
}
JausBoolean ojCmptIsOutgoingScActive(OjCmpt ojCmpt, unsigned short commandCode)
{
return scManagerQueryActiveMessage(ojCmpt->nmi, commandCode);
}
