Exemple #1
0
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);
}
Exemple #2
0
JausBoolean jausServiceAddCoreServices(JausArray jausServices)
{
	JausService service;
	
	// create a new service for core message support
	service = jausServiceCreate(CORE_MESSAGE_SUPPORT);
	
	// add each supported core input message 
	jausServiceAddInputCommand(service, JAUS_SET_COMPONENT_AUTHORITY, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_SHUTDOWN, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_STANDBY, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_RESUME, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_RESET, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_SET_EMERGENCY, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_CLEAR_EMERGENCY, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_CREATE_SERVICE_CONNECTION, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_CONFIRM_SERVICE_CONNECTION, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_ACTIVATE_SERVICE_CONNECTION, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_SUSPEND_SERVICE_CONNECTION, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_TERMINATE_SERVICE_CONNECTION, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_REQUEST_COMPONENT_CONTROL, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_RELEASE_COMPONENT_CONTROL, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_CONFIRM_COMPONENT_CONTROL, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_REJECT_COMPONENT_CONTROL, NO_PRESENCE_VECTOR);
//	jausServiceAddInputCommand(service, JAUS_SET_TIME, 0xFF);
	jausServiceAddInputCommand(service, JAUS_QUERY_COMPONENT_AUTHORITY, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_QUERY_COMPONENT_STATUS, NO_PRESENCE_VECTOR);
//	jausServiceAddInputCommand(service, JAUS_QUERY_TIME, 0xFF);
	jausServiceAddInputCommand(service, JAUS_REPORT_COMPONENT_AUTHORITY, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_REPORT_COMPONENT_STATUS, NO_PRESENCE_VECTOR);
//	jausServiceAddInputCommand(service, JAUS_REPORT_TIME, 0xFF);
	jausServiceAddInputCommand(service, JAUS_QUERY_IDENTIFICATION, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_REPORT_CONFIGURATION, NO_PRESENCE_VECTOR);
	jausServiceAddInputCommand(service, JAUS_REPORT_SERVICES, NO_PRESENCE_VECTOR);
	
	// add each supported core output message
	jausServiceAddOutputCommand(service, JAUS_CREATE_SERVICE_CONNECTION, NO_PRESENCE_VECTOR);
	jausServiceAddOutputCommand(service, JAUS_CONFIRM_SERVICE_CONNECTION, NO_PRESENCE_VECTOR);
	jausServiceAddOutputCommand(service, JAUS_TERMINATE_SERVICE_CONNECTION, NO_PRESENCE_VECTOR);
	jausServiceAddOutputCommand(service, JAUS_CONFIRM_COMPONENT_CONTROL, NO_PRESENCE_VECTOR);
	jausServiceAddOutputCommand(service, JAUS_REJECT_COMPONENT_CONTROL, NO_PRESENCE_VECTOR);
	jausServiceAddOutputCommand(service, JAUS_REPORT_COMPONENT_AUTHORITY, NO_PRESENCE_VECTOR);
	jausServiceAddOutputCommand(service, JAUS_REPORT_COMPONENT_STATUS, NO_PRESENCE_VECTOR);
	jausServiceAddOutputCommand(service, JAUS_REPORT_IDENTIFICATION, NO_PRESENCE_VECTOR);
	jausServiceAddOutputCommand(service, JAUS_QUERY_CONFIGURATION, NO_PRESENCE_VECTOR);
	jausServiceAddOutputCommand(service, JAUS_QUERY_SERVICES, NO_PRESENCE_VECTOR);
	
	// add the service to the component's services jausArray
	if(jausServiceAddService(jausServices, service))
	{
		return JAUS_TRUE;
	}
	else
	{
		return JAUS_FALSE;
	}
}
Exemple #3
0
Fichier : ojCmpt.c Projet : pfg/qgc
JausBoolean ojCmptAddService(OjCmpt ojCmpt, JausUnsignedShort serviceType)
{
	JausService service = jausServiceCreate(serviceType); 
	if(service)
	{
		return jausServiceAddService(ojCmpt->jaus->services, service);
	}
	return JAUS_FALSE;
}
Exemple #4
0
JausArray jausServicesClone(JausArray sourceServices)
{
	JausArray destinationServices;
	JausService sourceService, destinationService;
	JausCommand sourceCommand;
	int i = 0;
	
	destinationServices = jausArrayCreate();
	if(!destinationServices) return NULL;
	
	for(i = 0; i < sourceServices->elementCount; i++)
	{
		sourceService = (JausService) sourceServices->elementData[i];
		destinationService = jausServiceCreate(sourceService->type);
		
		jausServiceAddService(destinationServices, destinationService);
		
		// Duplicate Input Commands from sourceService to destinationService
		sourceCommand = sourceService->inputCommandList;			
		while(sourceCommand)
		{
			if(!jausServiceAddInputCommand(destinationService, sourceCommand->commandCode, sourceCommand->presenceVector))
			{
				jausServicesDestroy(destinationServices);
				return NULL;
			}
			sourceCommand = sourceCommand->next;
		}
		
		// Duplicate Output Commands from sourceService to destinationService
		sourceCommand = sourceService->outputCommandList;			
		while(sourceCommand)
		{
			if(!jausServiceAddOutputCommand(destinationService, sourceCommand->commandCode, sourceCommand->presenceVector))
			{
				jausServicesDestroy(destinationServices);
				return NULL;
			}			
			sourceCommand = sourceCommand->next;
		}
	}
	
	return destinationServices;
}
Exemple #5
0
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();

}