CigiIGCtrlV2.cpp

Go to the documentation of this file.

#define _EXPORT_CCL_

#include "CigiIGCtrlV2.h"
#include "CigiExceptions.h"
#include "CigiSwapping.h"
#include <memory.h>


#ifdef CIGI_LITTLE_ENDIAN
   #define CIGI_SCOPY2 CigiSwap2
   #define CIGI_SCOPY4 CigiSwap4
   #define CIGI_SCOPY8 CigiSwap8
#else
   #define CIGI_SCOPY2 CigiCopy2
   #define CIGI_SCOPY4 CigiCopy4
   #define CIGI_SCOPY8 CigiCopy8
#endif

// ====================================================================
// Construction/Destruction
// ====================================================================

// ================================================
// CigiIGCtrlV2
// vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
CigiIGCtrlV2::CigiIGCtrlV2()
{

   PacketID = CIGI_IG_CTRL_PACKET_ID_V2;
   PacketSize = CIGI_IG_CTRL_PACKET_SIZE_V2;
   Version = 2;
   MinorVersion = 0;

   BSwapEn = false;
   DatabaseID = 0;
   FrameCntr = 0;
   TimeStampV1 = 0;
   TimeStampV2 = 0.0f;
   TimeStampV3 = 0;
   ByteSwap = 0x8000;
   IGMode = Standby;
   TrackDeviceEn = Disable;
   BoresightTrackDevice = false;
   TimestampValid = false;
   SmoothingEn = false;
   LastRcvdIGFrame = 0;

}


// ================================================
// ~CigiIGCtrlV2
// vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
CigiIGCtrlV2::~CigiIGCtrlV2()
{

}



// ====================================================================
// Pack and Unpack
// ====================================================================

// ================================================
// Pack
// vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
int CigiIGCtrlV2::Pack(CigiBasePacket * Base, Cigi_uint8 * Buff, void *Spec) const
{
   PackPointer CDta;

   CigiBaseIGCtrl *Data = ( CigiBaseIGCtrl *) Base;


   CDta.c = Buff;

   *CDta.c++ = PacketID;
   *CDta.c++ = PacketSize;
   *CDta.c++ = Version;

   *CDta.b++ = Data->DatabaseID;

   Cigi_uint8 HDta = (Data->BoresightTrackDevice) ? 0x10 : 0;
   HDta |= (Cigi_uint8)((Data->TrackDeviceEn << 5) & 0x20);
   HDta |= (Cigi_uint8)((Data->IGMode << 6) & 0xc0);

   *CDta.c++ = HDta;
   *CDta.c++ = 0;
   *CDta.c++ = 0;
   *CDta.c++ = 0;

   CIGI_SCOPY4(CDta.l++, &Data->FrameCntr);
   CIGI_SCOPY4(CDta.f++, &Data->TimeStampV2);

   return(PacketSize);

}


// ================================================
// Unpack
// vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
int CigiIGCtrlV2::Unpack(Cigi_uint8 * Buff, bool Swap, void *Spec)
{

   PackPointer CDta;

   CDta.c = Buff;

   CDta.c += 3;

   DatabaseID = *CDta.b++;
   Cigi_uint8 HDta = *CDta.c;

   IGMode = (IGModeGrp)((HDta >> 6) & 0x03);
   TrackDeviceEn = (TrackDeviceEnGrp)((HDta >> 5) & 0x01);
   BoresightTrackDevice = ((HDta & 0x10) != 0);

   CDta.l++;

   CIGI_SCOPY4(&FrameCntr,CDta.l++);
   CIGI_SCOPY4(&TimeStampV2,CDta.f++);

   TimeStampV1 = (Cigi_uint32)TimeStampV2;
   TimeStampV3 = TimeStampV1 / 10;

   LastRcvdIGFrame = FrameCntr;

   Swap = false;

   return(CIGI_SUCCESS);

}


// ====================================================================
// Accessors
// ====================================================================



// ================================================
// SetTimeStamp
// vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
int CigiIGCtrlV2::SetTimeStamp(const float TimeStamp, bool bndchk)
{

   TimeStampV2 = TimeStamp;
   TimeStampV1 = (Cigi_uint32)TimeStamp;
   TimeStampV3 = TimeStampV1 / 10;

   TimestampValid = true;

   return(CIGI_SUCCESS);

}


// ================================================
// SetTrackDeviceEn
// vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
int CigiIGCtrlV2::SetTrackDeviceEn(const TrackDeviceEnGrp TrackDeviceEnIn, bool bndchk)
{

#ifndef CIGI_NO_BND_CHK
   if(bndchk && ((TrackDeviceEnIn < 0)||(TrackDeviceEnIn > 1)))
   {
#ifndef CIGI_NO_EXCEPT
      throw CigiValueOutOfRangeException("TrackDeviceEn",TrackDeviceEnIn,0,1);
#endif
      return(CIGI_ERROR_VALUE_OUT_OF_RANGE);
   }
#endif

   TrackDeviceEn = TrackDeviceEnIn;

   return(CIGI_SUCCESS);

}

---