void gposStartupState(void) { JausService service; // Populate Core Service if(!jausServiceAddCoreServices(gpos->services)) { cError("gpos: Addition of Core Services FAILED! Switching to FAILURE_STATE\n"); gpos->state = JAUS_FAILURE_STATE; } // USER: Add the rest of your component specific service(s) here service = jausServiceCreate(gpos->address->component); if(!service) { cError("gpos:%d: Creation of JausService FAILED! Switching to FAILURE_STATE\n", __LINE__); gpos->state = JAUS_FAILURE_STATE; } jausServiceAddService(gpos->services, service); jausServiceAddInputCommand(service, JAUS_QUERY_GLOBAL_POSE, 0xFF); jausServiceAddOutputCommand(service, JAUS_REPORT_GLOBAL_POSE, 0xFF); gposMessage = reportGlobalPoseMessageCreate(); gposMessage->source->id = gpos->address->id; //add support for ReportGlovalPose Service Connections scManagerAddSupportedMessage(gposNmi, JAUS_REPORT_GLOBAL_POSE); }
void wdStartupState(void) { JausService service; // Populate Core Service if(!jausServiceAddCoreServices(wd->services)) { //cError("wd: Addition of Core Services FAILED! Switching to FAILURE_STATE\n"); wd->state = JAUS_FAILURE_STATE; } // USER: Add the rest of your component specific service(s) here // create a new service for waypoint driver message support service = jausServiceCreate(JAUS_GLOBAL_WAYPOINT_DRIVER); // add each supported waypoint driver input message jausServiceAddInputCommand(service, JAUS_SET_TRAVEL_SPEED, NO_PRESENCE_VECTOR); jausServiceAddInputCommand(service, JAUS_SET_GLOBAL_WAYPOINT, NO_PRESENCE_VECTOR);//actually, there is a PV and it's equal to zero jausServiceAddInputCommand(service, JAUS_QUERY_WAYPOINT_COUNT, NO_PRESENCE_VECTOR); jausServiceAddInputCommand(service, JAUS_QUERY_GLOBAL_WAYPOINT, NO_PRESENCE_VECTOR); // add each supported waypoint driver output message jausServiceAddOutputCommand(service, JAUS_REPORT_WAYPOINT_COUNT, NO_PRESENCE_VECTOR); jausServiceAddOutputCommand(service, JAUS_REPORT_GLOBAL_WAYPOINT, NO_PRESENCE_VECTOR); // add the service to the component's services vector if(!jausServiceAddService(wd->services, service)) { //cError("wd: Could not add services"); } wd->authority = 127; pd = jausComponentCreate(); pd->address->component = JAUS_PRIMITIVE_DRIVER; // Setup GPOS Service Connection handle gposSc = serviceConnectionCreate(); gposSc->requestedUpdateRateHz = GPOS_SC_UPDATE_RATE_HZ; gposSc->address->component = JAUS_GLOBAL_POSE_SENSOR; gposSc->presenceVector = GPOS_SC_PRESENCE_VECTOR; gposSc->commandCode = JAUS_REPORT_GLOBAL_POSE; gposSc->isActive = JAUS_FALSE; gposSc->queueSize = GPOS_SC_QUEUE_SIZE; gposSc->timeoutSec = GPOS_SC_TIMEOUT_SECONDS; // Setup VSS Service Connection handle vssSc = serviceConnectionCreate(); vssSc->requestedUpdateRateHz = VSS_SC_UPDATE_RATE_HZ; vssSc->address->component = JAUS_VELOCITY_STATE_SENSOR; vssSc->presenceVector = VSS_SC_PRESENCE_VECTOR; vssSc->commandCode = JAUS_REPORT_VELOCITY_STATE; vssSc->isActive = JAUS_FALSE; vssSc->queueSize = VSS_SC_QUEUE_SIZE; vssSc->timeoutSec = VSS_SC_TIMEOUT_SECONDS; // Setup PD Service Connection handle pdWrenchSc = serviceConnectionCreate(); pdWrenchSc->requestedUpdateRateHz = PD_WRENCH_SC_UPDATE_RATE_HZ; pdWrenchSc->address->component = JAUS_PRIMITIVE_DRIVER; pdWrenchSc->presenceVector = PD_WRENCH_SC_PRESENCE_VECTOR; pdWrenchSc->commandCode = JAUS_REPORT_WRENCH_EFFORT; pdWrenchSc->isActive = JAUS_FALSE; pdWrenchSc->queueSize = PD_WRENCH_SC_QUEUE_SIZE; pdWrenchSc->timeoutSec = PD_WRENCH_SC_TIMEOUT_SECONDS; pdStatusSc = serviceConnectionCreate(); pdStatusSc->requestedUpdateRateHz = PD_STATUS_SC_UPDATE_RATE_HZ; pdStatusSc->address->component = JAUS_PRIMITIVE_DRIVER; pdStatusSc->presenceVector = PD_STATUS_SC_PRESENCE_VECTOR; pdStatusSc->commandCode = JAUS_REPORT_COMPONENT_STATUS; pdStatusSc->isActive = JAUS_FALSE; pdStatusSc->queueSize = PD_STATUS_SC_QUEUE_SIZE; pdStatusSc->timeoutSec = PD_STATUS_SC_TIMEOUT_SECONDS; // Setup message to be sent to the PD wdWrench = setWrenchEffortMessageCreate(); jausAddressCopy(wdWrench->source, wd->address); jausShortPresenceVectorSetBit(&wdWrench->presenceVector, JAUS_WRENCH_PV_PROPULSIVE_LINEAR_X_BIT); jausShortPresenceVectorSetBit(&wdWrench->presenceVector, JAUS_WRENCH_PV_PROPULSIVE_ROTATIONAL_Z_BIT); jausShortPresenceVectorSetBit(&wdWrench->presenceVector, JAUS_WRENCH_PV_RESISTIVE_LINEAR_X_BIT); scManagerAddSupportedMessage(wdNmi, JAUS_REPORT_COMPONENT_STATUS); wdWaypoints = jausArrayCreate(); vehicleState = vehicleStateCreate(); }
void ojCmptAddSupportedSc(OjCmpt ojCmpt, unsigned short commandCode) // Add service connection support for this message { scManagerAddSupportedMessage(ojCmpt->nmi, commandCode); }