text: /*****************************************************************************
   *  Copyright (c) 2009, OpenJAUS.com
   *  All rights reserved.
   *
   *  This file is part of OpenJAUS.  OpenJAUS is distributed under the BSD
   *  license.  See the LICENSE file for details.
   *
   *  Redistribution and use in source and binary forms, with or without
   *  modification, are permitted provided that the following conditions
   *  are met:
   *
   *     * Redistributions of source code must retain the above copyright
   *       notice, this list of conditions and the following disclaimer.
   *     * Redistributions in binary form must reproduce the above
   *       copyright notice, this list of conditions and the following
   *       disclaimer in the documentation and/or other materials provided
   *       with the distribution.
   *     * Neither the name of the University of Florida nor the names of its
   *       contributors may be used to endorse or promote products derived from
   *       this software without specific prior written permission.
   *
   *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
   *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
   *   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
   *   OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
   *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
   *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
   *   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
   *   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
   *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
   *   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
   ****************************************************************************/
  // File:		vss.c
  // Version:		3.3.0b
  // Written by:	Tom Galluzzo (galluzzt@ufl.edu) and Danny Kent (kentd@ufl.edu)
  // Date:		09/08/09
  // Description:	This file contains the skeleton C code for implementing a JAUS component in a Linux environment
  //				This code is designed to work with the node manager and JAUS library software written by CIMAR
  
  #include <jaus.h>			// JAUS message set (USER: JAUS libraries must be installed first)
  #include <openJaus.h>	// Node managment functions for sending and receiving JAUS messages (USER: Node Manager must be installed)
  #include <stdlib.h>
  #include "vss.h"	// USER: Implement and rename this header file. Include prototypes for all public functions contained in this file.
  #include "vehicleSim.h"
  
  // Private function prototypes
  void vssReadyState(OjCmpt vss);
  void vssQueryVelocityStateCallback(OjCmpt vss, JausMessage query);
  
  
  OjCmpt vssCreate(void)
  {
  	OjCmpt cmpt;
  	ReportVelocityStateMessage message;
  	JausAddress vssAddr;
  
  	cmpt = ojCmptCreate("vss", JAUS_VELOCITY_STATE_SENSOR, VSS_THREAD_DESIRED_RATE_HZ);
  
  	ojCmptAddService(cmpt, JAUS_VELOCITY_STATE_SENSOR);
  	ojCmptAddServiceInputMessage(cmpt, JAUS_VELOCITY_STATE_SENSOR, JAUS_QUERY_VELOCITY_STATE, 0xFF);
  	ojCmptAddServiceOutputMessage(cmpt, JAUS_VELOCITY_STATE_SENSOR, JAUS_REPORT_VELOCITY_STATE, 0xFF);
  
  	ojCmptSetStateCallback(cmpt, JAUS_READY_STATE, vssReadyState);
  	ojCmptSetMessageCallback(cmpt, JAUS_QUERY_VELOCITY_STATE, vssQueryVelocityStateCallback);
  	ojCmptAddSupportedSc(cmpt, JAUS_REPORT_VELOCITY_STATE);
  
  	message = reportVelocityStateMessageCreate();
  	vssAddr = ojCmptGetAddress(cmpt);
  	jausAddressCopy(message->source, vssAddr);
  	jausAddressDestroy(vssAddr);
  
  	ojCmptSetUserData(cmpt, (void *)message);
  
  	ojCmptSetState(cmpt, JAUS_READY_STATE);
  
  	if(ojCmptRun(cmpt))
  	{
  		ojCmptDestroy(cmpt);
  		return NULL;
  	}
  
  	return cmpt;
  }
  
  void vssDestroy(OjCmpt vss)
  {
  	ReportVelocityStateMessage message;
  
  	message = (ReportVelocityStateMessage)ojCmptGetUserData(vss);
  
  	ojCmptRemoveSupportedSc(vss, JAUS_REPORT_VELOCITY_STATE);
  	ojCmptDestroy(vss);
  
  	reportVelocityStateMessageDestroy(message);
  }
  
  // The series of functions below allow public access to essential component information
  // Access:		Public (All)
  double vssGetVelocityX(OjCmpt vss)
  {
  	ReportVelocityStateMessage message;
  
  	message = (ReportVelocityStateMessage)ojCmptGetUserData(vss);
  
  	return message->velocityXMps;
  }
  
  int vssGetScActive(OjCmpt vss)
  {
  	return ojCmptIsOutgoingScActive(vss, JAUS_REPORT_VELOCITY_STATE);
  }
  
  void vssQueryVelocityStateCallback(OjCmpt vss, JausMessage query)
  {
  	ReportVelocityStateMessage message;
  	JausMessage txMessage;
  	QueryVelocityStateMessage queryVelocityState;
  
  	message = (ReportVelocityStateMessage)ojCmptGetUserData(vss);
  
  	queryVelocityState = queryVelocityStateMessageFromJausMessage(query);
  	if(queryVelocityState)
  	{
  		jausAddressCopy(message->destination, queryVelocityState->source);
  		message->presenceVector = queryVelocityState->presenceVector;
  		message->sequenceNumber = 0;
  		message->properties.scFlag = 0;
  
  		txMessage = reportVelocityStateMessageToJausMessage(message);
  		ojCmptSendMessage(vss, txMessage);
  		jausMessageDestroy(txMessage);
  
  		queryVelocityStateMessageDestroy(queryVelocityState);
  	}
  	else
  	{
  		////cError("vss: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
  	}
  }
  
  
  void vssReadyState(OjCmpt vss)
  {
  	JausMessage txMessage;
  	ServiceConnection scList;
  	ServiceConnection sc;
  
  	ReportVelocityStateMessage message;
  
  	message = (ReportVelocityStateMessage)ojCmptGetUserData(vss);
  
  	message->velocityXMps = vehicleSimGetSpeed();
  	message->velocityYMps = 0;
  	message->velocityZMps = 0;
  	message->velocityRmsMps = 0.01;
  	message->rollRateRps = 0.0;
  	message->pitchRateRps = 0.0;
  	message->yawRateRps = 0.0;
  	message->rateRmsRps = 0;
  
  	// send message
  	if(ojCmptIsOutgoingScActive(vss, JAUS_REPORT_VELOCITY_STATE))
  	{
  		scList = ojCmptGetScSendList(vss, JAUS_REPORT_VELOCITY_STATE);
  		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 = reportVelocityStateMessageToJausMessage(message);
  			ojCmptSendMessage(vss, txMessage);
  			jausMessageDestroy(txMessage);
  
  			sc = sc->nextSc;
  		}
  
  		ojCmptDestroySendList(scList);
  	}
  }
  