Logo Search packages:      
Sourcecode: acm version File versions  Download package

decoy.c

#include <sys/time.h>
#include <unistd.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/*
 *  GNU C library includes getopt_long()
 */

#ifdef __GNU_LIBRARY__
#include <getopt.h>
#endif

#include <Vlibmath.h>
#include <dis/dis.h>

void MatrixToEuler(VMatrix * mt, double *heading, double *pitch, double *roll);
void BuildEulerMatrix(double roll, double pitch, double heading, VMatrix * m);
void GenerateWorldToLocalMatrix(WorldCoordinates * w, VMatrix * m);
dis_timestamp TimeDoubleToDIS(double time, int reference);
void transpose(VMatrix * m, VMatrix * r);

static struct itimerval it, ot;

#define FEETtoMETERS(x) ((x) * 0.3048)

void
timer_alarm (int val)
{
}

#define OPT_SITE      1
#define OPT_APPL      2
#define OPT_ENTITY    3
#define OPT_LATITUDE  4
#define OPT_LONGITUDE 5
#define OPT_ALTITUDE  6
#define OPT_ETYPE     7

#ifdef __GNU_LIBRARY__

/*
 *  long command line options
 */

static struct option long_options[] =
{
      { "site-id",     1, 0, 's' },
      { "appl-id",     1, 0, 'a' },
      { "entity-id",   1, 0, 'e' },
      { "latitude",    1, 0, 'l' },
      { "longitude",   1, 0, 'L' },
      { "altitude",    1, 0, 'A' },
      { "entity-type", 1, 0, 't' },
      { "exercise-id", 1, 0, 'E' },
      { "force-id",    1, 0, 'f' },
};

#endif

int
main(int argc, char **argv)
{

      struct sigaction sa, old_sa;
      struct timeval curtime;
      int status;
      pid_t pid;
      int c;
      int exercise_id = 1;
      double unix_time_sec;
      int option_index = 0;


      VMatrix XYZtoNED, NEDtoXYZ, trihedral, ABCtoXYZ;
      double orientation[3];
      VPoint velocity = { 0, 0, 0 };

      dis_pdu estate, em;
      dis_entity_state_pdu *esPDU;
      DISxApplicationInfo * app;
      WorldCoordinates pos;

      memset (&estate, 0, sizeof(dis_pdu));
      memset (&em,     0, sizeof(dis_pdu));

      /*
       * Fill out PDUs
       */

      esPDU = (dis_entity_state_pdu *) &estate;
      esPDU->id.sim_id.site_id = 1;
      esPDU->id.sim_id.application_id = 1;
    esPDU->id.entity_id = 1;
      esPDU->force_id = 1;

      esPDU->hdr.protocol_version;
      esPDU->hdr.exercise_id = 1;
      esPDU->hdr.pdu_type = PDUTypeEntityState;
      esPDU->hdr.protocol_family;
      esPDU->hdr.time_stamp;
      esPDU->hdr.length;
      esPDU->hdr.padding;

      /*
       *  SA-13 SAM launcher
       */

      esPDU->type.kind = 1;
      esPDU->type.domain = 1;
      esPDU->type.country = 222;
      esPDU->type.category = 4;
      esPDU->type.subcategory = 21;
      esPDU->type.specific = 1;

      /*
       * process command line arguments
       */

      while (1) {

#ifdef __GNU_LIBRARY__
            c = getopt_long ( argc, argv, "f:a:s:bc:d:012",
                                      long_options, &option_index );
#else
            c = getopt ( argc, argv, "f:a:s:bc:d:" );
#endif

            if (c == -1) {
                  break;
            }

            switch (c) {
            case 'f':
                  estate.entity_state.force_id = atoi(optarg);
                  break;
            case 'a':
                  estate.entity_state.id.sim_id.application_id = atoi(optarg);
                  break;
            case 's':
                  estate.entity_state.id.sim_id.site_id = atoi(optarg);
                  break;
            case 'e':
                  estate.entity_state.id.entity_id = atoi(optarg);
                  break;
            case 'E':
                  exercise_id = atoi(optarg);
                  break;
            case 'l':
                  pos.latitude = DEGtoRAD(atof(optarg));
                  break;
            case 'L':
                  pos.longitude = DEGtoRAD(atof(optarg));
                  break;
            case 'A':
                  pos.z = FEETtoMETERS(atof(optarg));
                  break;
            case 't':
                  break;
            }
      }

      memset ( &sa, 0, sizeof(sa) );
      sa.sa_handler = timer_alarm;
#ifdef linux
      sa.sa_flags = SA_NOMASK;
#endif

      it.it_interval.tv_sec = 3;
      it.it_interval.tv_usec = 0;
      it.it_value.tv_sec = 3;
      it.it_value.tv_usec = 0;

      if (sigaction( SIGALRM, &sa, &old_sa )) {
            perror( "sigaction failed" );
            exit ( 1 );
      }

    if (setitimer( ITIMER_REAL, &it, &ot )) {
            perror ( "setitimer failed" );
            exit ( 1 );
      }

      app = DISxInitializeApplication( 1,
                                      estate.entity_state.id.sim_id.site_id,
                                      estate.entity_state.id.sim_id.application_id );

      /*
       * loop until interrupted ...
       */

      while (1) {

            gettimeofday( &curtime, NULL );

            unix_time_sec = (double) curtime.tv_sec + 
                  (double) curtime.tv_usec / 1000000.0;

            esPDU->hdr.time_stamp = TimeDoubleToDIS( unix_time_sec, 1 );

            /* esPDU->id already set */

            esPDU->force_id = DISForceOpposing;

            esPDU->art_parm_count = 0;

            DISWorldCoordinatesToGeocentric( &pos, &esPDU->pos );
            BuildEulerMatrix (0.0, 0.0, 0.0, &trihedral);
            GenerateWorldToLocalMatrix ( &pos, &XYZtoNED );

            /*
             *  Derive ECI [Geocentric] heading, pitch, roll
             */
            transpose(&XYZtoNED, &NEDtoXYZ);
            /* the trihedral is an "ABCtoNED" transformation */
            VMatrixMultByRank(&trihedral, &NEDtoXYZ, &ABCtoXYZ, 3);
            MatrixToEuler(&ABCtoXYZ, 
                                &orientation[0], 
                                &orientation[1], 
                                &orientation[2]);

            esPDU->vel.x = velocity.x;
            esPDU->vel.y = velocity.y;
            esPDU->vel.z = velocity.z;

            esPDU->orientation.psi = orientation[0];
            esPDU->orientation.theta = orientation[1];
            esPDU->orientation.phi = orientation[2];

            esPDU->appearance = ( DISAppearancePaintUniform |
                                            DISAppearanceFirepowerNormal |
                                            DISAppearanceHatchClosed |
                                            DISAppearanceLandLauncherRaised );

            esPDU->dr_parm.algorithm = DISDRMethodRVW;

            esPDU->dr_parm.linear_acc.x = 0.0f;
            esPDU->dr_parm.linear_acc.y = 0.0f;
            esPDU->dr_parm.linear_acc.z = 0.0f;

            esPDU->dr_parm.angular_vel.x = 0.0f;
            esPDU->dr_parm.angular_vel.y = 0.0f;
            esPDU->dr_parm.angular_vel.z = 0.0f;

            esPDU->marking.charset = DISCharSetASCII;
            memset(esPDU->marking.marking, 0, sizeof(esPDU->marking.marking));
            strcpy(esPDU->marking.marking, "HI THERE");
            esPDU->capabilities = 0;
            esPDU->art_parm = NULL;

            if (DISxWritePDU(app, (dis_pdu *) esPDU) != 0) {
                  printf ( "error writing PDU\n" );
            }
            else {
                  printf ( "." );
                  fflush ( stdout );
            }

            pause();
      }
}

/*
 *  Generate a transform matrix to get from geocentric to local NED coordinates
 */

void
GenerateWorldToLocalMatrix(WorldCoordinates * w, VMatrix * m)
{
      dis_world_coordinates gc;
      VPoint    p;

      VIdentMatrix(m);
      VRotate(m, ZRotation, -w->longitude);
      VRotate(m, YRotation, -w->latitude);
      VRotate(m, YRotation, -DEGtoRAD(90.0));
      DISWorldCoordinatesToGeocentric(w, &gc);
      VTransform((VPoint *) & gc, m, &p);
      m->m[0][3] = -p.x;
      m->m[1][3] = -p.y;
      m->m[2][3] = -p.z;
}

void
BuildEulerMatrix(double roll, double pitch, double heading, VMatrix * m)
{

      register double sinPhi, cosPhi, sinTheta, cosTheta, sinPsi, cosPsi;

      sinPhi = sin(roll);
      cosPhi = cos(roll);
      sinTheta = sin(pitch);
      cosTheta = cos(pitch);
      sinPsi = sin(heading);
      cosPsi = cos(heading);

      m->m[0][0] = cosTheta * cosPsi;
      m->m[0][1] = sinPhi * sinTheta * cosPsi - cosPhi * sinPsi;
      m->m[0][2] = cosPhi * sinTheta * cosPsi + sinPhi * sinPsi;
      m->m[1][0] = cosTheta * sinPsi;
      m->m[1][1] = sinPhi * sinTheta * sinPsi + cosPhi * cosPsi;
      m->m[1][2] = cosPhi * sinTheta * sinPsi - sinPhi * cosPsi;
      m->m[2][0] = -sinTheta;
      m->m[2][1] = sinPhi * cosTheta;
      m->m[2][2] = cosPhi * cosTheta;
      m->m[0][3] = m->m[1][3] = m->m[2][3] = 0.0;
      m->m[3][0] = m->m[3][1] = m->m[3][2] = 0.0;
      m->m[3][3] = 1.0;

}

/*
 *  Convert a transformation matrix into the equivalent
 *  heading, pitch and roll angles.
 */

#define EPSILON 1.0e-6

void
MatrixToEuler(VMatrix * mt, double *heading, double *pitch, double *roll)
{
      double    sin_theta;

      sin_theta = -mt->m[2][0];

      if (fabs(sin_theta) > 1.0 - EPSILON) {
            /* we have the nose pointing very close to straight up or straight down,
               set roll to zero and compute the resulting heading */

            *heading = atan2(-mt->m[0][1], mt->m[1][1]);
            if (*heading < 0.0)
                  *heading += 2.0 * M_PI;

            if (sin_theta > 0.0)
                  *pitch = M_PI / 2.0;
            else
                  *pitch = -M_PI / 2.0;

            *roll = 0.0;
      }
      else {
            *heading = atan2(mt->m[1][0], mt->m[0][0]);
            if (*heading < 0.0)
                  *heading += 2.0 * M_PI;

            *pitch = asin(sin_theta);

            *roll = atan2(mt->m[2][1], mt->m[2][2]);
      }
}

/*
 *  T i m e D o u b l e T o D I S
 *
 *  Convert a double in UNIX format (seconds since 1970) to a DIS timestamp.
 *  If reference is 0, the time will be marked relative.
 *  If reference is 1, the time will be marked absolute, i.e. true UTC time.
 */

dis_timestamp
TimeDoubleToDIS(double time, int reference)
{
      unsigned long tmp;
      dis_timestamp res;

      tmp = (unsigned long) (fmod(time, 3600.0));
      if (tmp > 2147483647L)        /* 2^31 - 1 */
            res.time = 2147483647L;
      else
            res.time = tmp;
      res.type = reference;

      return res;
}

void
transpose(VMatrix * m, VMatrix * r)
{

      int       i, j;

      for (i = 0; i < 4; ++i)
            for (j = 0; j < 4; ++j)
                  r->m[i][j] = m->m[j][i];
}








Generated by  Doxygen 1.6.0   Back to index