dmu.c

Go to the documentation of this file.

/************************************************************************************************/
/*! \file dmu.c
*  \brief Interact with DMU.
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.4 $
*  \date    $Date: 2007/08/02 23:53:42 $
************************************************************************************************/
/*!
*
************************************************************************************************/

#include "dmu.h"

/* Global variables for MT scope */

/* DMU Data Conversion Factors
 * The first group needs to be set in dmuInit or during the setup/config
 * stage of a finished application, but accessed in the DMU polling
 * function, which is actually a signal handler (using the current
 * realtime timer setup).
 */
int dmufd;              /* fd to the open DMU serial port */
char *dmudata;          /* DMU raw data buffer */
char *cmd;              /* DMU cmd string buffer */
float bias[7];          /* DMU channel biases */
const float stepSize = ADCV_REF/4095.0; /* ADC voltage stepsize */

/* Shared memory objects
 * The next group is for sharing data between the two main threads
 * of the finished app.  One thread will read data from the DMU at
 * a rate as close as possible (using timers) to the commanded rate,
 * while the rest of the app will run in the other thread.
 */
adat_t *sh_attdat;      /* Shared attitude data */
int sh_nextw, sh_nextr; /* Next shared data write and read indexes */
long sh_serial;         /* Shared data serial number */
int rrate;              /* Commanded read rate */

int dmuInit(long maxBufferSize, int rate, int flags) {
  pthread_t daq_thr;

  /* Flags are not currently used.
   * This was originally coded around 5/02.  The prototype was
   *   substantially updated with new args (it used to have just rate)
   *   around 7/8/02,but the actual code changes were never made.
   *   maxBufferSize implemented 3/4/03.
   */

  if (maxBufferSize == 0) maxBufferSize = 1024;
  sh_attdat = (adat_t *)malloc(sizeof(adat_t) * maxBufferSize);
  sh_serial = 0;
  sh_nextw = sh_nextr = 0;
  rrate = rate;

  if (pthread_create(&daq_thr, NULL, _daq, NULL)) {
    perror("pthread_create()");
    /* commented out because couldnt find "errlog"
     * errlog("dmuInit(): pthread_create() failed to create DMU read thread.\n");
     */
    return(-1);
  }

  /* This is the parent process. */
  return(0);
}


/* This is the child process, where all the fun stuff happens... */

void* _daq(void* nada) {
  struct timeval rp_tv;
  struct itimerval rptimer;
  int i;
  
  /* First, let's initialize the DMU... */

  init_serial(DMU_PORT, B19200, &dmufd);        /* Initialize the port */
  cmd = (char *)malloc(sizeof(char)*6);         /* Setup the command string */
  sprintf(cmd, "!0RA%c", 6);
  dmudata = (char *)malloc(sizeof(char)*20);    /* Setup the data buffer */

  /* Next, we setup the period timer */
  rp_tv.tv_sec = 0;             /* 0 seconds */
  rp_tv.tv_usec = 1000000/rrate;        /* 10^6 / rate microseconds->
                                 * 10^6 usec/sec * 1/rate sec/read ->
                                 * sec's cancel -> 10^6/rate usec/read */
  if (rp_tv.tv_usec < 12500) {
    rp_tv.tv_usec = 1;
  } else {
    rp_tv.tv_usec -= 12500;
  }
  rptimer.it_interval =
    rptimer.it_value = rp_tv;   /* Set rptimer to rp_tv and reset to rp_tv */

  for (i = 0; i <= 6; i++) bias[i] = BIAS_DFLT;  /* Init bias settings */
  /* Then enter the tuned values (this should be done in dynamic or
   * perhaps even automatic configuration at some point. */
  bias[XQRS] = 2.467;
  bias[YQRS] = 2.520;
  bias[ZQRS] = 2.515;
  bias[XACC] = 2.423;
  bias[YACC] = 2.540;
  bias[ZACC] = 3.690;
  bias[TSEN] = TS_OFFSET;

  /* Start the timer */
  setitimer(ITIMER_REAL, &rptimer, NULL);
  signal(SIGALRM, polldmu);     /* Set the sighandler */
  while(1)                      /* And loop forever */
    sleep(5);
  return NULL;
}

/* This is our SIGALRM handler.  It WILL be called every 10^6/rate usec,
 * +- 10 usec max.  If the write()'s and/or read()'s fail repeatedly
 * (they're non-blocking and will just loop) enough that the timer
 * expires again before this function finishes, we have a loop counter
 * that will add the timer reset value to our deltat calculation so that
 * when we do get data, it will have the right age.
 *
 * The danger here is that if we the timer expires during a read() or
 * write() loop, we'll have sent half a command, or our read data will
 * be permanently misaligned.  If this gets to be a problem, we may need
 * to go to blocking read()'s.  We simply cannot afford to catch a signal
 * midway through a read() loop, because since the data is all numeric
 * and includes no sentinel values, any misalignment will be noncorrectable.
 *
 * A possible workaround without doing blocking read()'s (which would
 * mean we couldn't count the signal) would be to simply change the signal
 * disposition for the duration of the function to either SIGIGN or
 * perhaps even a different handler that would only increment the loop count.
 */
void polldmu(int sig) {
  static int tbw,               /* Total bytes written */
        tbr,                    /* Total bytes read */
        lc=0,                   /* Loop counter */
        curchan,                /* Current channel */
        i, hob, lob;
  static long deltat,           /* Total deltat from last read */
        curts,                  /* Current timestamp */
        lastts;                 /* Last timestamp */
  static float adVolts[7],
        temp,
        xRate, yRate, zRate,
        xAcc, yAcc, zAcc;
  static struct timeval tod;

  lc++;         /* Increment the loop counter.  This way, if the timer
                 * expires before we finish handling the data, we'll know,
                 * and at least we can calculate a correct deltat when we
                 * do get that far.
                 */
  for (tbw = 0; tbw < 5; )              /* Loop the write until the entire */
    tbw += write(dmufd, cmd, 5);        /* command is written */

  for (tbr = 0; tbr < 14; )               /* Loop the read until we get a */
    tbr += read(dmufd, dmudata+tbr, 14-tbr); /* full set of 14 data bytes */

  gettimeofday(&tod, NULL);             /* Get the timer value */
  curts = tod.tv_usec;                  /* Fetch the usec element.  This is
                                         * the raw timestamp for this data. */
  deltat = curts - lastts;              /* Calc deltat.  lc should be >= 1 */
  if (deltat < 0) deltat += 1000000;

  /* Calc individual channel values */
  /* This is where we were screwing up the upper-half of the data range because
   * we didn't account for the fact that the implicit cast from char to int
   * interprets the bits using 8-bit 2's complement notation, meaning the range
   * is -127 to 127, and not 0 to 255.  Thus, 0-127 are mapped as-is, while
   * 128-255 get mapped to -127-(-1).  A cleaner solution than the one implemented
   * may be to do a byte-wise copy from &dmudata[i] to &some_int to sidestep the
   * issues of typecasting, like perhaps:
   * memcpy(&some_int, &(dmudata[i]), 2) OR
   * memcpy((void *)(1+&some_int), &(dmudata[i]), 1);
   * memcpy(&some_int, &(dmudata[i+1]), 1);
   * But what's here works, so it's staying for now.
   */
  for (i = 0, curchan = 6; i < 14; i+=2, curchan--) {
    if (dmudata[i] < 0)
        hob = dmudata[i] + 255;
    else
        hob = dmudata[i];
    if (dmudata[i+1] < 0)
        lob = dmudata[i+1] + 255;
    else
        lob = dmudata[i+1];
    adVolts[curchan] = (256 * hob + lob) * stepSize;
    dmudata[i] = dmudata[i+1] = 0;                  /* Then clear buffer */
  }

  /* Calculate data values... */
  xRate = (-1)*(adVolts[XQRS] - bias[XQRS]) / RATE_SF;
  yRate = (-1)*(adVolts[YQRS] - bias[YQRS]) / RATE_SF;
  zRate = (-1)*(adVolts[ZQRS] - bias[ZQRS]) / RATE_SF;
  xAcc = ((adVolts[XACC] - bias[XACC]) / ACC_SF * G);
  yAcc = ((adVolts[YACC] - bias[YACC]) / ACC_SF * G);
  zAcc = ((adVolts[ZACC] - bias[ZACC]) / ACC_SF * G);
  temp = adVolts[TSEN] / TEMP_SF - bias[TSEN];

  /* Put the data in the shared mem space... */
  sh_attdat[sh_nextw].xrate = xRate;
  sh_attdat[sh_nextw].yrate = yRate;
  sh_attdat[sh_nextw].zrate = zRate;
  sh_attdat[sh_nextw].xaccel = xAcc;
  sh_attdat[sh_nextw].yaccel = yAcc;
  sh_attdat[sh_nextw].zaccel = zAcc;
  sh_attdat[sh_nextw].temp = temp;
  sh_attdat[sh_nextw].deltat = deltat;
  sh_attdat[sh_nextw].serial = sh_serial;
  if (sh_nextw==1022) sh_nextw=0;
    else sh_nextw++;
  sh_serial++;
  
  /* Whew!  We made it.  Let's reset the loop counter and update the
   * timestamps and return.
   */
  lc = 0;
  lastts = curts;
  return;
}


int dmuReadData(adat_t **accData, int maxsets) {
  static int i,
        ind;

  if (sh_nextr < 1023)                  /* We haven't reached the end, so ... */
    ind = sh_nextr;                     /* the index is the nextread ptr */
  else
    ind = sh_nextr - 1023;              /* Else, it rolls back around */

  /*  Now, we run the loop until we get to the nextwrite ptr (the ptr
   *   that points to the sharedmem address where the next data will go.
   */
  for (i = 0; i < maxsets && ind != sh_nextw; i++) {
    if ((sh_nextr + i) < 1023)
      ind = sh_nextr + i;
    else
      ind = sh_nextr + i - 1023;
/*
    printf("%d -> %d\t", ind, i);
    fflush(stdout);
*/
    (*accData)[i].xrate = sh_attdat[ind].xrate;
    (*accData)[i].yrate = sh_attdat[ind].yrate;
    (*accData)[i].zrate = sh_attdat[ind].zrate;
    (*accData)[i].xaccel = sh_attdat[ind].xaccel;
    (*accData)[i].yaccel = sh_attdat[ind].yaccel;
    (*accData)[i].zaccel = sh_attdat[ind].zaccel;
    (*accData)[i].temp = sh_attdat[ind].temp;
    (*accData)[i].deltat = sh_attdat[ind].deltat;
  }
  sh_nextr = ind++;
  if (i) return(i-1);
    else return(0);
}

/* Do not change the comments below - they will be added automatically by CVS*/
/*****************************************************************************
*       $Log: dmu.c,v $
*       Revision 1.4  2007/08/02 23:53:42  jayhawk_hokie
*       Fixed Warnings.
*
*       Revision 1.3  2007/08/02 23:40:44  jayhawk_hokie
*       Fixed Warnings.
*
*
*
******************************************************************************/

---