// Initialize the reference dimensions at the point of the earth we want // to compute the distances relative to. // This will be called automatically by FastCreateCircleDistance, // but it should be called again if you need to dramatically alter // the reference point on the earth. void Distance::initFastDimensions(double longRef, double latRef ) { // Use the great circle distance to compute the distance of // one degree in each direction from the reference point. // These distances will be used in the fast linear interpolation. longitude_1_degree_distance = greatCircleDistance(longRef, latRef, longRef+1.0, latRef); latitude_1_degree_distance = greatCircleDistance(longRef, latRef, longRef, latRef+1.0); longReference = longRef; latReference = latRef; }
void wdReadyState(void) { double waypointHeading = 0; double headingDelta = 0; if(!gposSc->isActive || !vssSc->isActive || !pdWrenchSc->isActive || !pdStatusSc->isActive || !wdInControl) { wd->state = JAUS_EMERGENCY_STATE; // cError( "wd: Switching to EMERGENCY: GposSc: %s, VssSc: %s, PdWrenchSc: %s, PdStatusSc: %s, Rd: %s\n", // gposSc->isActive? "Active" : "Inactive", // vssSc->isActive? "Active" : "Inactive", // pdWrenchSc->isActive? "Active" : "Inactive", // pdStatusSc->isActive? "Active" : "Inactive", // wdInControl? "In Control" : "Not In Control" // ); return; } if(pd->state != JAUS_READY_STATE) { wd->state = JAUS_STANDBY_STATE; return; } vehicleState->speedMps = wdReportVss? (float) wdReportVss->velocityXMps : (float) 0.0; if(currentWaypointIndex < wdWaypoints->elementCount) { SetGlobalWaypointMessage tempGlobalWaypoint; tempGlobalWaypoint = (SetGlobalWaypointMessage) wdWaypoints->elementData[currentWaypointIndex]; waypointHeading = greatCircleCourse(wdReportGpos->latitudeDegrees * RAD_PER_DEG, wdReportGpos->longitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->latitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->longitudeDegrees * RAD_PER_DEG); wdWaypointDistance = greatCircleDistance(wdReportGpos->latitudeDegrees * RAD_PER_DEG, wdReportGpos->longitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->latitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->longitudeDegrees * RAD_PER_DEG); headingDelta = wdAngleSubtract(waypointHeading, wdReportGpos->yawRadians); //cLog("HeadingDelta: %7.2f\n", headingDelta); vehicleState->desiredPhiEffort = (float) (WHEEL_ROTATION_EFFORT_PER_RAD * headingDelta); vehicleState->desiredSpeedMps = wdSpeed? (float) wdSpeed->speedMps : (float) 0.0; //cLog("vehicleState->desiredPhiEffort: %7.2f\n", vehicleState->desiredPhiEffort); } else { //hang out vehicleState->desiredSpeedMps = 0; vehicleState->desiredPhiEffort = 0; } wdExcecuteControl(vehicleState); }
// 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; } }