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 scManagerReceiveMessage(NodeManagerInterface nmi, JausMessage message)
{
ServiceConnection sc;
char string[32] = {0};
pthread_mutex_lock(&nmi->scm->mutex);
sc = nmi->scm->incomingSc;
while(sc)
{
if(sc->commandCode == message->commandCode && jausAddressEqual(sc->address, message->source) )
{
if(sc->isActive)
{
sc->lastSentTime = ojGetTimeSec();
if(sc->queueSize && sc->queueSize == sc->queue->size)
{
jausMessageDestroy(queuePop(sc->queue));
queuePush(sc->queue, (void *)message);
}
else
{
queuePush(sc->queue, (void *)message);
}
}
else
{
// TODO: Error? received a message for inactive SC
jausMessageDestroy(message);
}
pthread_mutex_unlock(&nmi->scm->mutex);
return;
}
sc = sc->nextSc;
}
jausAddressToString(message->source, string);
jausMessageDestroy(message);
pthread_mutex_unlock(&nmi->scm->mutex);
}
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 lmHandlerReceiveLargeMessage(NodeManagerInterface nmi, JausMessage message)
{
LargeMessageList msgList;
JausMessage tempMessage;
JausMessage outMessage;
int i;
unsigned long sequenceNumber;
unsigned long index;
JausUnsignedInteger newDataSize = 0;
JausUnsignedInteger bufferIndex = 0;
char address[128] = {0};
switch(message->dataFlag)
{
case JAUS_FIRST_DATA_PACKET:
// Check for valid SeqNumber(0), else Error
if(message->sequenceNumber)
{
cError("LargeMessageHandler: Received First Data Packet with invalid Sequence Number(%d)\n", message->sequenceNumber);
jausMessageDestroy(message);
return;
}
// Check if LargeMessageList exists (same CC & Source)
msgList = lmHandlerGetMessageList(nmi->lmh, message);
if(msgList)
{
// Destroy the list and all messages
vectorRemove(nmi->lmh->messageLists, msgList, (void *)lmHandlerMessageListEqual);
lmListDestroy(msgList);
}
// create LargeMessageList
msgList = lmListCreate();
vectorAdd(nmi->lmh->messageLists, msgList);
// add message to LargeMessageList at first position
msgList->commandCode = message->commandCode;
msgList->source->id = message->source->id;
vectorAdd(msgList->messages, message);
break;
case JAUS_NORMAL_DATA_PACKET:
// Check if LargeMessageList exists, error if not
msgList = lmHandlerGetMessageList(nmi->lmh, message);
if(msgList)
{
// Check if item exists in LargeMessageList with seqNumber
if(vectorContains(msgList->messages, message, (void *)lmHandlerLargeMessageCheck) != -1)
{
cError("LargeMessageHandler: Received duplicate NORMAL_DATA_PACKET\n");
jausMessageDestroy(message);
}
else
{
// insert to Vector
vectorAdd(msgList->messages, message);
}
}
else
{
// Destroy Message
cError("LargeMessageHandler: Received NORMAL_DATA_PACKET (0x%4X) for unknown Large Message Set (never received JAUS_FIRST_DATA_PACKET)\n", message->commandCode);
jausMessageDestroy(message);
}
break;
case JAUS_RETRANSMITTED_DATA_PACKET:
// Check if LargeMessageList exists, error if not
msgList = lmHandlerGetMessageList(nmi->lmh, message);
if(msgList)
{
// Check if item exists in LargeMessageList with seqNumber
if(vectorContains(msgList->messages, message, (void *)lmHandlerLargeMessageCheck) != -1)
{
tempMessage = (JausMessage) vectorRemove(msgList->messages, message, (void *)lmHandlerLargeMessageCheck);
jausMessageDestroy(tempMessage);
}
// insert to Vector
vectorAdd(msgList->messages, message);
}
else
{
cError("LargeMessageHandler: Received RETRANSMITTED_DATA_PACKET for unknown Large Message Set (never received JAUS_FIRST_DATA_PACKET)\n");
jausMessageDestroy(message);
}
break;
case JAUS_LAST_DATA_PACKET:
// Check if LargeMessageList exists, error if not
msgList = lmHandlerGetMessageList(nmi->lmh, message);
if(msgList)
{
// insert message to end of list
vectorAdd(msgList->messages, message);
// Create JausMessage object
outMessage = jausMessageCreate();
// Calculate new message size
newDataSize = 0;
for(i = 0; i < msgList->messages->elementCount; i++)
{
tempMessage = (JausMessage)msgList->messages->elementData[i];
newDataSize += tempMessage->dataSize;
}
// Setup Header and Data Buffer
outMessage->properties = tempMessage->properties;
outMessage->commandCode = tempMessage->commandCode;
outMessage->destination->id = tempMessage->destination->id;
outMessage->source->id = tempMessage->source->id;
outMessage->dataControl = tempMessage->dataControl;
outMessage->sequenceNumber = tempMessage->sequenceNumber;
outMessage->data = (unsigned char *) malloc(newDataSize);
// Populate new message
sequenceNumber = 0;
bufferIndex = 0;
while(sequenceNumber <= message->sequenceNumber)
{
index = 0;
do
{
tempMessage = (JausMessage)msgList->messages->elementData[index];
index++;
if(index > msgList->messages->elementCount)
{
// Invalid set of messages
//TODO: Here is when you would request a retransmittal if ACK/NAK is set and ask the sending component to resend some packets
// Received LAST_DATA_PACKET, but do not have proper sequence of messages
// Destroy the list and all messages
vectorRemove(nmi->lmh->messageLists, msgList, (void *)lmHandlerMessageListEqual);
lmListDestroy(msgList);
cError("LargeMessageHandler: Received LAST_DATA_PACKET, but do not have proper sequence of messages\n");
jausMessageDestroy(outMessage);
return;
}
}
while(tempMessage->sequenceNumber != sequenceNumber);
// Move data from tempMessage to outMessage
memcpy(outMessage->data + bufferIndex, tempMessage->data, tempMessage->dataSize);
bufferIndex += tempMessage->dataSize;
sequenceNumber++; // TOM: switched from i++
}
// Set DataSize
// Set proper header flags (dataFlag JAUS_SINGLE_DATA_PACKET)
outMessage->dataSize = newDataSize;
outMessage->dataFlag = JAUS_SINGLE_DATA_PACKET;
if(outMessage->scFlag)
{
scManagerReceiveMessage(nmi, outMessage);
}
else
{
queuePush(nmi->receiveQueue, (void *)outMessage);
}
// Destroy LargeMessageList
vectorRemove(nmi->lmh->messageLists, msgList, (void *)lmHandlerMessageListEqual);
lmListDestroy(msgList);
}
else
{
cError("LargeMessageHandler: Received LAST_DATA_PACKET for unknown Large Message Set (never received JAUS_FIRST_DATA_PACKET)\n");
jausMessageDestroy(message);
}
break;
default:
jausAddressToString(message->source, address);
cError("lmHandler: Received (%s) with improper dataFlag (%d) from %s\n", jausMessageCommandCodeString(message), message->dataFlag, address);
jausMessageDestroy(message);
break;
}
}
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;
}
}
}
// Function: wdProcessMessage
// Access: Private
// Description: This function is responsible for handling incoming JAUS messages from the Node Manager.
// Incoming messages are processed according to message type.
void wdProcessMessage(JausMessage message)
{
JausMessage txMessage;
ConfirmComponentControlMessage confirmComponentControl;
RejectComponentControlMessage rejectComponentControl;
ReportComponentStatusMessage reportComponentStatus;
QueryGlobalWaypointMessage queryGlobalWaypointMessage;
ReportGlobalWaypointMessage reportGlobalWaypointMessage;
QueryWaypointCountMessage queryWaypointCountMessage;
ReportWaypointCountMessage reportWaypointCountMessage;
int i;
char buf[64] = {0};
// This block of code is intended to reject commands from non-controlling components
if(wd->controller.active && !jausAddressEqual(message->source, wd->controller.address) && jausMessageIsRejectableCommand(message))
{
//cError("wd: Received command message %s from non-controlling component.\n", jausMessageCommandCodeString(message));
jausMessageDestroy(message); // Ignore this message
return;
}
switch(message->commandCode) // Switch the processing algorithm according to the JAUS message type
{
case JAUS_REPORT_COMPONENT_STATUS:
reportComponentStatus = reportComponentStatusMessageFromJausMessage(message);
if(reportComponentStatus)
{
if(jausAddressEqual(reportComponentStatus->source, pd->address))
{
pd->state = reportComponentStatus->primaryStatusCode;
}
reportComponentStatusMessageDestroy(reportComponentStatus);
}
else
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_CONFIRM_COMPONENT_CONTROL:
confirmComponentControl = confirmComponentControlMessageFromJausMessage(message);
if(confirmComponentControl)
{
if(jausAddressEqual(confirmComponentControl->source, pd->address))
{
//cDebug(4,"wd: Confirmed control of PD\n");
wdInControl = JAUS_TRUE;
}
confirmComponentControlMessageDestroy(confirmComponentControl);
}
else
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_REJECT_COMPONENT_CONTROL:
rejectComponentControl = rejectComponentControlMessageFromJausMessage(message);
if(rejectComponentControl)
{
if(jausAddressEqual(rejectComponentControl->source, pd->address))
{
//cDebug(4,"wd: Lost control of PD\n");
wdInControl = JAUS_FALSE;
}
rejectComponentControlMessageDestroy(rejectComponentControl);
}
else
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_SET_TRAVEL_SPEED:
if(wdSpeed)
{
setTravelSpeedMessageDestroy(wdSpeed);
}
wdSpeed = setTravelSpeedMessageFromJausMessage(message);
if(!wdSpeed)
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_REPORT_GLOBAL_POSE:
if(wdReportGpos)
{
reportGlobalPoseMessageDestroy(wdReportGpos);
}
wdReportGpos = reportGlobalPoseMessageFromJausMessage(message);
if(wdReportGpos)
{
// Nothing to do
jausAddressToString(message->source, buf);
//cDebug(9, "Recv GPOS msg from %s\n", buf);
}
else
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
if(currentWaypointIndex < wdWaypoints->elementCount)
{
// update waypoint index
SetGlobalWaypointMessage tempGlobalWaypoint;
tempGlobalWaypoint = (SetGlobalWaypointMessage) wdWaypoints->elementData[currentWaypointIndex];
wdWaypointDistance = greatCircleDistance(wdReportGpos->latitudeDegrees * RAD_PER_DEG, wdReportGpos->longitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->latitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->longitudeDegrees * RAD_PER_DEG);
if(wdWaypointDistance < WAYPOINT_POP_DISTANCE_M)
{
//cError("wd: popping waypoint: %d\n",currentWaypointIndex);
currentWaypointIndex++;
}
}
// else
// {
// if(wdSpeed) wdSpeed->speedMps = 0;
// }
jausMessageDestroy(message);
break;
case JAUS_REPORT_VELOCITY_STATE:
if(wdReportVss)
{
reportVelocityStateMessageDestroy(wdReportVss);
}
wdReportVss = reportVelocityStateMessageFromJausMessage(message);
if(!wdReportVss)
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_REPORT_WRENCH_EFFORT:
if(wdReportWrench)
{
reportWrenchEffortMessageDestroy(wdReportWrench);
}
wdReportWrench = reportWrenchEffortMessageFromJausMessage(message);
if(!wdReportWrench)
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_SET_GLOBAL_WAYPOINT:
wdGlobalWaypoint = setGlobalWaypointMessageFromJausMessage(message);
if(!wdGlobalWaypoint)
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
jausArrayAdd(wdWaypoints, wdGlobalWaypoint);
break;
case JAUS_QUERY_GLOBAL_WAYPOINT:
queryGlobalWaypointMessage = queryGlobalWaypointMessageFromJausMessage(message);
if(!queryGlobalWaypointMessage)
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
// loop thru waypoints to find the one that matches the request
// if there's a match, prepare/send the report, else whine
for(i = 0; i < wdWaypoints->elementCount; i++)
{
SetGlobalWaypointMessage tempGlobalWaypoint;
tempGlobalWaypoint = (SetGlobalWaypointMessage) wdWaypoints->elementData[i];
if(tempGlobalWaypoint->waypointNumber == queryGlobalWaypointMessage->waypointNumber)
{
reportGlobalWaypointMessage = reportGlobalWaypointMessageCreate();
jausAddressCopy(reportGlobalWaypointMessage->destination, queryGlobalWaypointMessage->source);
jausAddressCopy(reportGlobalWaypointMessage->source, wd->address);
reportGlobalWaypointMessage->presenceVector = NO_PRESENCE_VECTOR;
reportGlobalWaypointMessage->waypointNumber = tempGlobalWaypoint->waypointNumber;
reportGlobalWaypointMessage->latitudeDegrees = tempGlobalWaypoint->latitudeDegrees;
reportGlobalWaypointMessage->longitudeDegrees = tempGlobalWaypoint->longitudeDegrees;
txMessage = reportGlobalWaypointMessageToJausMessage(reportGlobalWaypointMessage);
reportGlobalWaypointMessageDestroy(reportGlobalWaypointMessage);
nodeManagerSend(wdNmi, txMessage);
jausMessageDestroy(txMessage);
}
}
queryGlobalWaypointMessageDestroy(queryGlobalWaypointMessage);
jausMessageDestroy(message);
break;
case JAUS_QUERY_WAYPOINT_COUNT:
queryWaypointCountMessage = queryWaypointCountMessageFromJausMessage(message);
if(!queryWaypointCountMessage)
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
reportWaypointCountMessage = reportWaypointCountMessageCreate();
jausAddressCopy(reportWaypointCountMessage->destination, queryWaypointCountMessage->source);
jausAddressCopy(reportWaypointCountMessage->source, wd->address);
reportWaypointCountMessage->waypointCount = wdWaypoints->elementCount;
txMessage = reportWaypointCountMessageToJausMessage(reportWaypointCountMessage);
reportWaypointCountMessageDestroy(reportWaypointCountMessage);
nodeManagerSend(wdNmi, txMessage);
queryWaypointCountMessageDestroy(queryWaypointCountMessage);
jausMessageDestroy(message);
break;
default:
defaultJausMessageProcessor(message, wdNmi, wd);
break;
}
}
// Refresh screen in curses mode
void updateScreen(int keyboardLock, int keyPress)
{
int row = 0;
int col = 0;
char string[256] = {0};
PointLla vehiclePosLla;
static int lastChoice = '1';
JausAddress address;
if(!keyboardLock && keyPress != -1 && keyPress != 27 && keyPress != 12) //Magic Numbers: 27 = ESC, 12 = Ctrl+l
{
switch(keyPress)
{
case ' ':
wdToggleRequestControl(wd);
break;
case 'S':
wdSetSpeed(wd);
break;
case 'W':
wdCreateWaypoint(wd);
break;
default:
lastChoice = keyPress;
}
}
clear();
mvprintw(row,35,"Keyboard Lock: %s", keyboardLock?"ON, Press ctrl+L to unlock":"OFF, Press ctrl+L to lock");
mvprintw(row++,0,"+---------------------------+");
mvprintw(row++,0,"| Component Menu |");
mvprintw(row++,0,"| |");
mvprintw(row++,0,"| 1. Vehicle Sim |");
mvprintw(row++,0,"| 2. Primitive Driver |");
mvprintw(row++,0,"| 3. GPOS / VSS |");
mvprintw(row++,0,"| 4. Waypoint Driver |");
mvprintw(row++,0,"| |");
mvprintw(row++,0,"| ESC to Exit |");
mvprintw(row++,0,"+---------------------------+");
row = 2;
col = 40;
switch(lastChoice)
{
case '1':
mvprintw(row++,col,"Vehicle Simulator");
mvprintw(row++,col,"VS Update Rate: %7.2f", vehicleSimGetUpdateRate());
mvprintw(row++,col,"VS Run/Pause: %s", vehicleSimGetRunPause() == VEHICLE_SIM_PAUSE ? "Pause" : "Run");
row++;
mvprintw(row++,col,"VS Vehicle X:\t%9.2f", vehicleSimGetX());
mvprintw(row++,col,"VS Vehicle Y:\t%9.2f", vehicleSimGetY());
mvprintw(row++,col,"VS Vehicle H:\t%9.2f", vehicleSimGetH());
mvprintw(row++,col,"VS Vehicle Speed: %7.2f", vehicleSimGetSpeed());
row++;
mvprintw(row++,col,"VS Throttle:\t%9.2f", vehicleSimGetLinearEffortX());
mvprintw(row++,col,"VS Brake:\t%9.2f", vehicleSimGetResistiveEffortX());
mvprintw(row++,col,"VS Steering:\t%9.2f", vehicleSimGetRotationalEffort());
row++;
vehiclePosLla = vehicleSimGetPositionLla();
mvprintw(row++,col,"VS Vehicle Latitude: %+10.7f", vehiclePosLla? vehiclePosLla->latitudeRadians*DEG_PER_RAD : 0.0);
mvprintw(row++,col,"VS Vehicle Longitude: %+10.7f", vehiclePosLla? vehiclePosLla->longitudeRadians*DEG_PER_RAD : 0.0);
break;
case '2':
mvprintw(row++,col,"Primitive Driver");
mvprintw(row++,col,"PD Update Rate: %5.2f", ojCmptGetRateHz(pd));
address = ojCmptGetAddress(pd);
jausAddressToString(address, string);
jausAddressDestroy(address);
mvprintw(row++,col,"PD Address:\t%s", string);
mvprintw(row++,col,"PD State:\t%s", jausStateGetString(ojCmptGetState(pd)));
row++;
if(ojCmptHasController(pd))
{
address = ojCmptGetControllerAddress(pd);
jausAddressToString(address, string);
jausAddressDestroy(address);
mvprintw(row++,col,"PD Controller: %s", string);
}
else
{
mvprintw(row++,col,"PD Controller: None");
}
mvprintw(row++,col,"PD Controller SC: %s", pdGetControllerScStatus(pd)?"Active":"Inactive");
mvprintw(row++,col,"PD Controller State: %s", jausStateGetString(pdGetControllerState(pd)));
row++;
mvprintw(row++,col,"PD Prop Effort X: %0.0lf", pdGetWrenchEffort(pd)? pdGetWrenchEffort(pd)->propulsiveLinearEffortXPercent:-1.0);
mvprintw(row++,col,"PD Rstv Effort X: %0.0lf", pdGetWrenchEffort(pd)? pdGetWrenchEffort(pd)->resistiveLinearEffortXPercent:-1.0);
mvprintw(row++,col,"PD Rtat Effort Z: %0.0lf", pdGetWrenchEffort(pd)? pdGetWrenchEffort(pd)->propulsiveRotationalEffortZPercent:-1.0);
break;
case '3':
mvprintw(row++,col,"Global Pose Sensor");
mvprintw(row++,col,"GPOS Update Rate: %7.2f", ojCmptGetRateHz(gpos));
address = ojCmptGetAddress(gpos);
jausAddressToString(address, string );
jausAddressDestroy(address);
mvprintw(row++,col,"GPOS Address:\t %s", string);
mvprintw(row++,col,"GPOS State:\t %s", jausStateGetString(ojCmptGetState(gpos)));
mvprintw(row++,col,"GPOS SC State:\t %s", gposGetScActive(gpos)? "Active" : "Inactive");
row++;
mvprintw(row++,col,"Velocity State Sensor");
mvprintw(row++,col,"VSS Update Rate: %7.2f", ojCmptGetRateHz(vss));
address = ojCmptGetAddress(vss);
jausAddressToString(address, string );
jausAddressDestroy(address);
mvprintw(row++,col,"VSS Address:\t %s", string);
mvprintw(row++,col,"VSS State:\t %s", jausStateGetString(ojCmptGetState(vss)));
mvprintw(row++,col,"VSS SC State:\t %s", vssGetScActive(vss)? "Active" : "Inactive");
break;
case '4':
mvprintw(row++,col,"Waypoint Driver");
mvprintw(row++,col,"WD Update Rate: %7.2f", ojCmptGetRateHz(wd));
address = ojCmptGetAddress(wd);
jausAddressToString(address, string );
jausAddressDestroy(address);
mvprintw(row++,col,"WD Address:\t %s", string);
mvprintw(row++,col,"WD State:\t %s", jausStateGetString(ojCmptGetState(wd)));
address = ojCmptGetControllerAddress(wd);
if(address)
{
jausAddressToString(address, string);
mvprintw(row++,col,"WD Controller:\t %s", string);
jausAddressDestroy(address);
}
else
{
mvprintw(row++,col,"WD Controller:\t None");
}
row = 11;
col = 2;
mvprintw(row++,col,"GPOS SC:\t %s", wdGetGposScStatus(wd)? "Active" : "Inactive");
mvprintw(row++,col,"VSS SC:\t %s", wdGetVssScStatus(wd)? "Active" : "Inactive");
mvprintw(row++,col,"PD Wrench SC:\t %s", wdGetPdWrenchScStatus(wd)? "Active" : "Inactive");
mvprintw(row++,col,"PD State SC:\t %s", wdGetPdStatusScStatus(wd)? "Active" : "Inactive");
row++;
mvprintw(row++,col,"WD Request Control:\t%s", wdGetRequestControl(wd)? "True" : "False");
mvprintw(row++,col,"(Space to Toggle)");
mvprintw(row++,col,"WD Control:\t\t%s", wdGetInControlStatus(wd)? "True" : "False");
mvprintw(row++,col,"PD State:\t\t%s", jausStateGetString(wdGetPdState(wd)));
row = 11;
col = 40;
if(wdGetGlobalWaypoint(wd))
{
mvprintw(row++,col,"Global Waypoint: (%9.7lf,%9.7lf)", wdGetGlobalWaypoint(wd)->latitudeDegrees, wdGetGlobalWaypoint(wd)->longitudeDegrees);
}
else
{
mvprintw(row++,col,"Global Waypoint: None");
}
if(wdGetTravelSpeed(wd))
{
mvprintw(row++,col,"Travel Speed: %7.2f", wdGetTravelSpeed(wd)->speedMps);
}
else
{
mvprintw(row++,col,"Travel Speed: None");
}
mvprintw(row++,col,"dSpeedMps: %7.2f", wdGetDesiredVehicleState(wd)? wdGetDesiredVehicleState(wd)->desiredSpeedMps : 0.0);
mvprintw(row++,col,"dPhi: %7.2f", wdGetDesiredVehicleState(wd)? wdGetDesiredVehicleState(wd)->desiredPhiEffort : 0.0);
break;
default:
mvprintw(row++,col,"NONE.");
break;
}
move(24,0);
refresh();
}
