sicspsi/docho.c

/*--------------------------------------------------------------------------
                            D o C h o


  A SICS driver for a Dornier Chopper Control System accessed through a 
  RS-232 interface connected to a Macintosh PC running the SerialPortServer
  terminal server program. There are two choppers which ususally run at fixed
  speed ratios against each other. There ia also a phase difference between
  the two choppers. And lots of machine surveillance parameters.

  This driver is used by the generic chopper or device controller as described
  in choco.tex. 


  Mark Koennecke, January 1999

  Modified to support a single chopper only,

  Uwe Filges, Mark Koennecke; November 2001
--------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <time.h>
#include <fortify.h>
#include <sics.h>
#include <stringdict.h>
#include "hardsup/serialsinq.h"
#include "hardsup/el734_errcodes.h"
#include "hardsup/el734fix.h"
#include <codri.h>

/*-----------------------------------------------------------------------
  A private data structure for this Dornier chopper
-------------------------------------------------------------------------*/
typedef struct {
  char *pHost;
  int iPort;
  int iChannel;
  void *pData;
  int iRefreshIntervall;
  pStringDict pPar;
  time_t tRefresh;
  int iStop;
  long lTask;
  int iError;
  int iBusy;
  float fRatio;
  int iSingle;
  char pError[80];
} DoCho, *pDoCho;
/*
  pHost, iPort and iChannel combined are the adress of the chopper
  controller at the  Macintosh terminal server. pData is the serial 
  port connection data structure needed and managed by the SerialIO
  functions.

  As the communication with the Dornier Chopper System is very slow the
  parameter list of this driver will only be updated a predefined time
  intervalls. In between buffered values will be returned for requests.
  The buffered parameters are held in the string dictioanry pPar. 
  iRefreshIntervall is the time between refreshs. tRefresh is the time for
  the next refresh. iBusy is  flag which indicates, that it was tried to 
  modify a variable. This will only be reflected with the next status update.
  In between DoChoCheckPar might conclude, that the chopper is already
  done. iBusy is meant to stop that. It is set when a parameter is changed
  and cleared bu the status message code. DoChoCheckPar checks for it.

  Refreshing will be performed by a special SICS task which will be 
  started when the driver is initialized. In order to stop this task when
  need arises the parameter iStop can be set to true.

  iError is the last error reported on this device. If no error: 0

  fRatio is the target value for the chopper ratio. In contrast to the
  other parameters, its target value cannot be extracted from the chopper
  status message.

  iSingle is a flag which is true if only a single chopper is controlled
  through this driver. This supports the POLDI single choper case.

*/
/*----------------------------------------------------------------------
  ERROR CODES:
*/
#define UNDRIVABLE -8002
#define UNKNOWNPAR -8003
#define PARERROR   -8004
#define BADSYNC    -8005
#define BADSTOP    -8006
#define CHOPERROR  -8007

extern char *trim(char *pTrim); /* trim.c */

/*----------------------------------------------------------------------*/
static void SplitChopperReply(pCodri self, char *prefix, char *pBueffel)
{
  char pToken[30], pValue[20], pEntry[80];
  char *pPtr, *pTok, *pVal;
  int iCount, iRet;
  pDoCho pPriv = NULL;

  pPriv = (pDoCho) self->pPrivate;

  /* decompose pBueffel and store into string dictionary */
  pPtr = strtok(pBueffel, ";");
  while (pPtr != NULL) {
    iCount = sscanf(pPtr, "%s %s", pToken, pValue);
    if (iCount == 2) {
      pTok = trim(pToken);
      pVal = trim(pValue);
      pEntry[0] = '\0';
      sprintf(pEntry, "%s.%s", prefix, pTok);
      iRet = StringDictUpdate(pPriv->pPar, pEntry, pVal);
      if (!iRet) {
        StringDictAddPair(pPriv->pPar, pEntry, pVal);
        strcat(self->pParList, pEntry);
        strcat(self->pParList, ",");
      }
    } else {
      /* this fixes a bug with oversized messages in dphas oder averl*/
      if (strstr(pPtr, "dphas") != NULL ) {
        sprintf(pEntry, "%s.dphas", prefix);
        iRet = StringDictUpdate(pPriv->pPar, pEntry, pPtr + 5);
        if (!iRet) {
          StringDictAddPair(pPriv->pPar, pEntry, pPtr + 5);
          strcat(self->pParList, pEntry);
          strcat(self->pParList, ",");
        }
      }
      if (strstr(pPtr, "averl") != NULL ) {
        sprintf(pEntry, "%s.averl", prefix);
        iRet = StringDictUpdate(pPriv->pPar, pEntry, pPtr + 5);
        if (!iRet) {
          StringDictAddPair(pPriv->pPar, pEntry, pPtr + 5);
          strcat(self->pParList, pEntry);
          strcat(self->pParList, ",");
        }
      }
    }
    pPtr = strtok(NULL, ";");
  }
}

/*-------------------------------------------------------------------------
  Well, DoChoStatus sends a status request to the Dornier chopper control
  system. There is a gotcha, you need three reads to get the full information.
  Then the answer is parsed and decomposed into parameter content for the
  string dictionary. The single status components are separated by ;.  
-------------------------------------------------------------------------*/

static int DoChoStatus(pCodri self)
{
  int iRet, iCount, iCode;
  char pBueffel[1024], pToken[30], pValue[20];
  char *pPtr, *pTok, *pVal;
  pDoCho pPriv = NULL;

  assert(self);
  pPriv = (pDoCho) self->pPrivate;
  assert(pPriv);
  pPriv->iBusy = 0;
  pPriv->iError = 0;


  /* first send, command, returns the echo */
  iRet = SerialWriteRead(&(pPriv->pData), "asyst 1", pBueffel, 1023);
  if (iRet < 0) {
    pPriv->iError = iRet;
    return 0;
  }

  /* next send: reads first chopper line */
  iRet = SerialWriteRead(&(pPriv->pData), "", pBueffel, 1023);
  if (iRet < 0) {
    pPriv->iError = iRet;
    return 0;
  }
  SplitChopperReply(self, "chopper1", pBueffel);

  if (!pPriv->iSingle) {
    /* second send: get next second chopper line */
    iRet = SerialWriteRead(&(pPriv->pData), "", pBueffel, 1023);
    if (iRet < 0) {
      pPriv->iError = iRet;
      return 0;
    }
    SplitChopperReply(self, "chopper2", pBueffel);
  }


  return 1;
}

/*-------------------------------------------------------------------------*/
static int DoChoTask(void *pData)
{
  pCodri self = NULL;
  pDoCho pPriv = NULL;
  int iCode, iRet;
  char pDummy[60];

  self = (pCodri) pData;
  assert(self);
  pPriv = (pDoCho) self->pPrivate;
  assert(pPriv);

  /* check for stop */
  if (pPriv->iStop)
    return 0;


  /* check if it is time to run a status request */
  if (time(NULL) > pPriv->tRefresh) {
    /* try, fix error */
    if (pPriv->iError != 0) {
      self->GetError(self, &iCode, pDummy, 59);
      iRet = self->TryFixIt(self, iCode);
      if (iRet == CHFAIL) {
        pPriv->tRefresh = time(NULL) + pPriv->iRefreshIntervall;
        return 1;
      }
    }
    /* do it */
    DoChoStatus(self);
    pPriv->tRefresh = time(NULL) + pPriv->iRefreshIntervall;
  }
  return 1;
}

/*------------------------------------------------------------------------*/
static void DoChoKill(void *pData)
{
  pCodri self = NULL;
  pDoCho pPriv = NULL;

  self = (pCodri) pData;
  if (!self)
    return;
  pPriv = (pDoCho) self->pPrivate;
  if (!pPriv)
    return;


  if (pPriv->pData) {
    SerialClose(&(pPriv->pData));
    pPriv->pData = NULL;
  }

  if (pPriv->pHost)
    free(pPriv->pHost);
  if (pPriv->pPar)
    DeleteStringDict(pPriv->pPar);

  free(pPriv);
}

/*-------------------------------------------------------------------------*/
static int DoChoInit(pCodri self)
{
  pDoCho pPriv = NULL;
  int iRet;

  assert(self);
  pPriv = (pDoCho) self->pPrivate;
  assert(pPriv);
  pPriv->iError = 0;

  /* first open the connection to the serial port server and channel */
  iRet = SerialOpen(&(pPriv->pData), pPriv->pHost, pPriv->iPort,
                    pPriv->iChannel);
  if (iRet <= 0) {
    pPriv->iError = iRet;
    return 0;
  }
  /* configure the connection */
  SerialConfig(&(pPriv->pData), 10000);
  SerialATerm(&(pPriv->pData), "1\r\n");
  SerialSendTerm(&(pPriv->pData), "\r");

  pPriv->iStop = 0;
  pPriv->tRefresh = 0;          /* force a status request when first run */

  /* start the update task */
  if (pPriv->lTask == 0) {
    pPriv->lTask = TaskRegister(pServ->pTasker,
                                DoChoTask, NULL, NULL, self, 1);
  }
  return 1;
}

/*------------------------------------------------------------------------*/
static int DoChoClose(pCodri self)
{
  pDoCho pPriv = NULL;
  int iRet;
  long lVal;

  assert(self);
  pPriv = (pDoCho) self->pPrivate;
  assert(pPriv);

  if (pPriv->pData) {
    SerialClose(&(pPriv->pData));
    pPriv->pData = NULL;
  }
  return 1;
}

/*------------------------------------------------------------------------*/
static int DoChoDelete(pCodri self)
{
  pDoCho pPriv = NULL;

  assert(self);
  pPriv = (pDoCho) self->pPrivate;
  assert(pPriv);

  if (pPriv->pData) {
    SerialClose(&(pPriv->pData));
    pPriv->pData = NULL;
  }

  if (pPriv->pHost)
    free(pPriv->pHost);
  if (pPriv->pPar)
    DeleteStringDict(pPriv->pPar);

  free(pPriv);

  return 1;
}

/*--------------------------------------------------------------------------*/
static int DoChoSetPar2(pCodri self, char *parname, char *pValue)
{
  pDoCho pPriv = NULL;
  char pCommand[80], pReply[132];
  char pState[20];
  int iRet;

  assert(self);
  pPriv = (pDoCho) self->pPrivate;
  assert(pPriv);

  /* deal with our four parameters */
  if (strcmp(parname, "chopper1.nspee") == 0) {
    sprintf(pCommand, "nspee 1 %s", pValue);
  } else if (strcmp(parname, "chopper2.nspee") == 0) {
    iRet = StringDictGet(pPriv->pPar, "chopper2.state", pState, 19);
    if (iRet && strstr(pState, "async") != NULL) {
      sprintf(pCommand, "nspee 2 %s", pValue);
    } else {
      pPriv->iError = BADSYNC;
      return 0;
    }
  } else if (strcmp(parname, "chopper2.nphas") == 0) {
    sprintf(pCommand, "nphas 2 %s", pValue);
  } else if (strcmp(parname, "chopper1.nphas") == 0) {
    sprintf(pCommand, "nphas 1 %s", pValue);
  } else if (strcmp(parname, "chopper2.ratio") == 0) {
    sprintf(pCommand, "ratio 2 %s", pValue);
  } else {
    pPriv->iError = UNDRIVABLE;
    return 0;
  }

  iRet = SerialWriteRead(&(pPriv->pData), pCommand, pReply, 131);
  if (iRet != 1) {
    pPriv->iError = iRet;
    return 0;
  }
  if (strstr(pReply, "error") != NULL) {
    pPriv->iError = CHOPERROR;
    strncpy(pPriv->pError, pReply, 79);
    return 0;
  } else {
    pPriv->iError = 0;
  }
  pPriv->iBusy = 1;
  return 1;
}

/*-------------------------------------------------------------------------*/
static int DoChoHalt(pCodri self)
{
  pDoCho pPriv = NULL;

  assert(self);
  pPriv = (pDoCho) self->pPrivate;
  assert(pPriv);

  /* 
     there is no documented way to stop the Dornier chopper 
     system. This at least makes SICS happy.
   */
  pPriv->iError = BADSTOP;
  pPriv->iBusy = 0;
  return 1;
}

/*---------------------------------------------------------------------------*/
static int DoChoSetPar(pCodri self, char *parname, float fValue)
{
  char pValue[50];
  pDoCho pPriv = NULL;

  assert(self);
  pPriv = (pDoCho) self->pPrivate;
  assert(pPriv);

  if (strstr(parname, "nspee") != NULL) {
    sprintf(pValue, "%d", (int) fValue);
  } else if (strstr(parname, "ratio") != NULL) {
    sprintf(pValue, "%d", (int) fValue);
    pPriv->fRatio = (int) fValue;
  } else if (strcmp(parname, "updateintervall") == 0) {
    sprintf(pValue, "%d", (int) fValue);
    StringDictUpdate(pPriv->pPar, "updateintervall", pValue);
    pPriv->iRefreshIntervall = (int) fValue;
    return 1;
  } else {
    sprintf(pValue, "%f", fValue);
  }
  return DoChoSetPar2(self, parname, pValue);
}

/*----------------------------------------------------------------------*/
static int DoChoGetPar(pCodri self, char *parname,
                       char *pBuffer, int iBufLen)
{
  pDoCho pPriv = NULL;
  int iRet;

  assert(self);
  pPriv = (pDoCho) self->pPrivate;
  assert(pPriv);

  if (pPriv->iError != 0) {
    self->GetError(self, &iRet, pBuffer, iBufLen);
    return 0;
  }

  iRet = StringDictGet(pPriv->pPar, parname, pBuffer, iBufLen);
  if (!iRet) {
    pPriv->iError = UNKNOWNPAR;
    return 0;
  }
  return 1;
}

/*-----------------------------------------------------------------------*/
static int DoChoCheckPar(pCodri self, char *parname)
{
  pDoCho pPriv = NULL;
  char pVal1[20], pVal2[20];
  float fTarget, fIst, fDelta;
  int iRet;

  assert(self);
  pPriv = (pDoCho) self->pPrivate;
  assert(pPriv);

  /* check the busy flag first */
  if (pPriv->iBusy)
    return HWBusy;

  /* was there an error in the status show? */
  if (pPriv->iError != 0) {
    return HWFault;
  }

  /* updateintervall is always HWIdle */
  if (strcmp(parname, "updateintervall") == 0) {
    return HWIdle;
  }

  /* OK, got a new status let us check the parameter */
  /* chopper 1 speed */
  if (strcmp(parname, "chopper1.nspee") == 0) {
    iRet = StringDictGet(pPriv->pPar, "chopper1.nspee", pVal1, 19);
    iRet += StringDictGet(pPriv->pPar, "chopper1.aspee", pVal2, 19);
    if (iRet != 2) {
      pPriv->iError = PARERROR;
      return HWFault;
    }
    sscanf(pVal1, "%f", &fTarget);
    sscanf(pVal2, "%f", &fIst);
    fDelta = fTarget - fIst;
    if (fDelta < 0.0)
      fDelta = -fDelta;
    if (fDelta > 50) {
      return HWBusy;
    } else {
      return HWIdle;
    }
  }
  /* chopper 2 speed */
  if (strcmp(parname, "chopper2.nspee") == 0) {
    iRet = StringDictGet(pPriv->pPar, "chopper2.nspee", pVal1, 19);
    iRet += StringDictGet(pPriv->pPar, "chopper2.aspee", pVal2, 19);
    if (iRet != 2) {
      pPriv->iError = PARERROR;
      return HWFault;
    }
    sscanf(pVal1, "%f", &fTarget);
    sscanf(pVal2, "%f", &fIst);
    fDelta = fTarget - fIst;
    if (fDelta < 0.0)
      fDelta = -fDelta;
    if (fDelta > 5.) {
      return HWBusy;
    } else {
      return HWIdle;
    }
  }

  /* phase */
  if (strcmp(parname, "chopper2.nphas") == 0) {
    iRet = StringDictGet(pPriv->pPar, "chopper2.dphas", pVal1, 19);
    sscanf(pVal1, "%f", &fDelta);
    if (fDelta < 0.)
      fDelta = -fDelta;
    if (fDelta > 0.3) {
      return HWBusy;
    } else {
      return HWIdle;
    }
  }
  if (strcmp(parname, "chopper1.nphas") == 0) {
    iRet = StringDictGet(pPriv->pPar, "chopper1.dphas", pVal1, 19);
    sscanf(pVal1, "%f", &fDelta);
    if (fDelta < 0.)
      fDelta = -fDelta;
    if (fDelta > 0.3) {
      return HWBusy;
    } else {
      return HWIdle;
    }
  }

  /* ratio */
  if (strcmp(parname, "chopper2.ratio") == 0) {
    iRet = StringDictGet(pPriv->pPar, "chopper2.ratio", pVal1, 19);
    sscanf(pVal1, "%f", &fIst);
    fDelta = fIst - pPriv->fRatio;
    if (fDelta < 0.)
      fDelta = -fDelta;
    if (fDelta > 0.3) {
      return HWBusy;
    } else {
      return HWIdle;
    }
  }
  pPriv->iError = UNKNOWNPAR;
  return HWFault;
}

/*-------------------------------------------------------------------------*/
static int DoChoError(pCodri self, int *iCode, char *pError, int iLen)
{
  pDoCho pPriv = NULL;

  assert(self);
  pPriv = (pDoCho) self->pPrivate;
  assert(pPriv);

  *iCode = pPriv->iError;
  switch (pPriv->iError) {
  case UNDRIVABLE:
    strncpy(pError, "Parameter is not drivable", iLen);
    break;
  case UNKNOWNPAR:
    strncpy(pError, "Parameter is unknown", iLen);
    break;
  case PARERROR:
    strncpy(pError, "Internal parameter error", iLen);
    break;
  case BADSYNC:
    strncpy(pError, "Cannot drive slave chopper", iLen);
    break;
  case CHOPERROR:
    strncpy(pError, pPriv->pError, iLen);
    break;
  case BADSTOP:
    strncpy(pError,
            "User called STOP. WARNING: chopper is still untamed!", iLen);
    break;
  default:
    SerialError(pPriv->iError, pError, iLen);
    break;
  }
  pPriv->iError = 0;
  return 1;
}

/*------------------------------------------------------------------------*/
static int DoChoFix(pCodri self, int iCode)
{
  pDoCho pPriv = NULL;
  int iRet;

  assert(self);
  pPriv = (pDoCho) self->pPrivate;
  assert(pPriv);

  switch (iCode) {
    /* network errors */
  case EL734__BAD_FLUSH:
  case EL734__BAD_RECV:
  case EL734__BAD_RECV_NET:
  case EL734__BAD_RECV_UNKN:
  case EL734__BAD_RECVLEN:
  case EL734__BAD_RECV1:
  case EL734__BAD_RECV1_PIPE:
  case EL734__BAD_RNG:
  case EL734__BAD_SEND:
  case EL734__BAD_SEND_PIPE:
  case EL734__BAD_SEND_NET:
  case EL734__BAD_SEND_UNKN:
  case EL734__BAD_SENDLEN:
  case NOCONNECTION:
    SerialForceClose(&(pPriv->pData));
    pPriv->pData = NULL;
    iRet = SerialOpen(&(pPriv->pData), pPriv->pHost,
                      pPriv->iPort, pPriv->iChannel);
    if (iRet == 1) {
      return CHREDO;
    } else {
      return CHFAIL;
    }
    break;
  case EL734__FORCED_CLOSED:
    iRet = DoChoInit(self);
    if (iRet) {
      return CHREDO;
    } else {
      return CHFAIL;
    }
    break;
  default:
    return CHFAIL;
    break;
  }
  return CHFAIL;
}

/*-------------------------------------------------------------------------*/
pCodri MakeDoChoDriver(char *pHost, int iPort, int iChannel, int iSingle)
{
  pCodri pNew = NULL;
  pDoCho pPriv = NULL;
  char *pText;

  /* allocate memory */
  pText = (char *) malloc(4096 * sizeof(char));
  pNew = (pCodri) malloc(sizeof(Codri));
  pPriv = (pDoCho) malloc(sizeof(DoCho));
  if (!pText || !pNew || !pPriv) {
    return NULL;
  }
  memset(pText, 0, 4096);
  memset(pNew, 0, sizeof(Codri));
  memset(pPriv, 0, sizeof(DoCho));

  /* initialize private data structure */
  pPriv->pHost = strdup(pHost);
  pPriv->iPort = iPort;
  pPriv->iChannel = iChannel;
  pPriv->pData = NULL;
  pPriv->iRefreshIntervall = 60;
  pPriv->pPar = CreateStringDict();
  pPriv->tRefresh = time(NULL);
  pPriv->iSingle = iSingle;
  if (!pPriv->pPar) {
    free(pText);
    free(pNew);
    free(pPriv);
    return NULL;
  }

  /* install codri */
  pNew->Init = DoChoInit;
  pNew->Close = DoChoClose;
  pNew->Delete = DoChoDelete;
  pNew->SetPar = DoChoSetPar;
  pNew->SetPar2 = DoChoSetPar2;
  pNew->GetPar = DoChoGetPar;
  pNew->CheckPar = DoChoCheckPar;
  pNew->GetError = DoChoError;
  pNew->TryFixIt = DoChoFix;
  pNew->Halt = DoChoHalt;
  pNew->pParList = pText;
  strcpy(pNew->pParList, "updateintervall,");
  StringDictAddPair(pPriv->pPar, "updateintervall", "60");
  pNew->pPrivate = pPriv;

  return pNew;
}