int jausStateToString(JausState state, char *buf) { return sprintf(buf, "%s", jausStateGetString(state)); }
// 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(); }