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 gposReadyState(OjCmpt gpos)
{
JausMessage txMessage;
ServiceConnection scList;
ServiceConnection sc;
PointLla vehiclePosLla;
ReportGlobalPoseMessage message;
message = (ReportGlobalPoseMessage)ojCmptGetUserData(gpos);
vehiclePosLla = vehicleSimGetPositionLla();
if(vehiclePosLla)
{
message->latitudeDegrees = vehiclePosLla->latitudeRadians * DEG_PER_RAD;
message->longitudeDegrees = vehiclePosLla->longitudeRadians * DEG_PER_RAD;
}
message->yawRadians = -(vehicleSimGetH() - M_PI/2.0);
message->elevationMeters = 0;
message->positionRmsMeters = 1.0;
message->rollRadians = 0.0;
message->pitchRadians = 0.0;
message->attitudeRmsRadians = 0.05;
jausTimeSetCurrentTime(message->time);
// Send message
if(ojCmptIsOutgoingScActive(gpos, JAUS_REPORT_GLOBAL_POSE))
{
scList = ojCmptGetScSendList(gpos, JAUS_REPORT_GLOBAL_POSE);
sc = scList;
while(sc)
{
jausAddressCopy(message->destination, sc->address);
message->presenceVector = sc->presenceVector;
message->sequenceNumber = sc->sequenceNumber;
message->properties.scFlag = JAUS_SERVICE_CONNECTION_MESSAGE;
txMessage = reportGlobalPoseMessageToJausMessage(message);
ojCmptSendMessage(gpos, txMessage);
jausMessageDestroy(txMessage);
sc = sc->nextSc;
}
ojCmptDestroySendList(scList);
}
}
// 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();
}
