sics/simchop.c

/*--------------------------------------------------------------------------
                       S I M C H O P

  This is a general controller driver for a simulated chopper controller.
  It supports just a few parameters and returns random numbers. For more
  information, see choco.w, choco.*.

  Mark Koennecke, January 1998
---------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <time.h>
#include <math.h>
#include "fortify.h"
#include "sics.h"
#include "codri.h"

#define UNKNOWNPAR    -7001
#define RANDOMERROR   -7002
#define RANDOMWARNING -7003
#define STOPPED       -7004

#define FAILRATE 0.00005
#define WARNRATE 0.00001
/*-----------------------------------------------------------------------*/
typedef struct {
  time_t tTarget;
  float fRot;
  float fPhase;
  float fRatio;
  int iError;
  int iStop;
} ChopPriv, *pChopPriv;
/*------------------------------------------------------------------------*/
static float SimRandom(void)
{
  float fVal;

  fVal = ((float) rand() / (float) RAND_MAX) * 100.0;
  return fVal;
}

/*--------------- Driver functions ---------------------------------------*/
static int SDInit(pCodri self)
{
  return 1;
}

/*---------------------------------------------------------------------  */
static int SDClose(pCodri self)
{
  return 1;
}

/*-----------------------------------------------------------------------*/
static int SDDelete(pCodri self)
{
  if (self->pParList)
    free(self->pParList);
  if (self->pPrivate)
    free(self->pPrivate);
  return 1;
}

/*----------------------------------------------------------------------*/
static int SDHalt(pCodri self)
{
  pChopPriv pPriv = NULL;

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

  pPriv->iStop = 1;
  return 1;
}

/*-----------------------------------------------------------------------*/
static int SDSetPar(pCodri self, char *parname, float fValue)
{
  pChopPriv pPriv = NULL;

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

  /* clear stop flag */
  pPriv->iStop = 0;

  if (strcmp(parname, "speed") == 0) {
    pPriv->fRot = fValue;
    pPriv->tTarget = time(NULL) + 20;
    return 1;
  } else if (strcmp(parname, "phase") == 0) {
    pPriv->fPhase = fValue;
    pPriv->tTarget = time(NULL) + 20;
    return 1;
  } else if (strcmp(parname, "ratio") == 0) {
    pPriv->fRatio = fValue;
    pPriv->tTarget = time(NULL) + 20;
    return 1;
  } else {
    pPriv->iError = UNKNOWNPAR;
    return 0;
  }
}

/*------------------------------------------------------------------------*/
static int SDSetPar2(pCodri self, char *parname, char *pValue)
{
  pChopPriv pPriv = NULL;

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

  pPriv->iError = UNKNOWNPAR;
  return 0;
}

/*-------------------------------------------------------------------------*/
static int SDGetPar(pCodri self, char *parname, char *pBuffer, int iBufLen)
{
  pChopPriv pPriv = NULL;
  float fFail;
  char pBueffel[50];

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

  /* failure calculation first, we do so randomly in order to
     check error processing code
   */
  fFail = SimRandom();
  if (fFail < WARNRATE) {
    pPriv->iError = RANDOMWARNING;
    return 0;
  }
  if (fFail < FAILRATE) {
    pPriv->iError = RANDOMERROR;
    return 0;
  }

  /* are we busy driving something? */
  if (time(NULL) < pPriv->tTarget) {
    if (strcmp(parname, "speed") == 0) {
      snprintf(pBueffel,sizeof(pBueffel)-1, "%f", pPriv->fRot - 1.);
    } else if (strcmp(parname, "phase") == 0) {
      snprintf(pBueffel,sizeof(pBueffel)-1, "%f", pPriv->fPhase - 1.);
    } else if (strcmp(parname, "ratio") == 0) {
      snprintf(pBueffel,sizeof(pBueffel)-1, "%f", pPriv->fRatio - 1.);
    } else {
      pPriv->iError = UNKNOWNPAR;
      return 0;
    }
    strlcpy(pBuffer, pBueffel, iBufLen);
    return 1;
  }

  /* all normal */
  if (strcmp(parname, "speed") == 0) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "%f", pPriv->fRot);
  } else if (strcmp(parname, "phase") == 0) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "%f", pPriv->fPhase);
  } else if (strcmp(parname, "ratio") == 0) {
    snprintf(pBueffel,sizeof(pBueffel)-1, "%f", pPriv->fRatio);
  } else {
    pPriv->iError = UNKNOWNPAR;
    return 0;
  }
  strlcpy(pBuffer, pBueffel, iBufLen);
  return 1;
}

/*------------------------------------------------------------------------*/
static int SDCheckPar(pCodri self, char *parname)
{
  pChopPriv pPriv = NULL;
  float fFail;

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

  /*
     throw a HWFalut when stopped */
  if (pPriv->iStop) {
    pPriv->iError = STOPPED;
    return HWFault;
  }

  /* failure calculation first, we do so randomly in order to
     check error processing code
   */
  fFail = SimRandom();
  if (fFail < WARNRATE) {
    pPriv->iError = RANDOMWARNING;
    return HWFault;
  }
  if (fFail < FAILRATE) {
    pPriv->iError = RANDOMERROR;
    return HWFault;
  }

  /* are we busy driving something? */
  if (time(NULL) < pPriv->tTarget) {
    return HWBusy;
  }
  return HWIdle;
}

/*-----------------------------------------------------------------------*/
static int SDGetError(pCodri self, int *iCode, char *pError, int iErrLen)
{
  pChopPriv pPriv = NULL;

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

  *iCode = pPriv->iError;

  switch (pPriv->iError) {
  case UNKNOWNPAR:
    strlcpy(pError, "Parameter Unknown", iErrLen);
    break;
  case RANDOMWARNING:
    strlcpy(pError, "Random non deadly error", iErrLen);
    break;
  case RANDOMERROR:
    strlcpy(pError, "Random DEADLY Error", iErrLen);
    break;
  case STOPPED:
    strlcpy(pError, "User initiated STOP", iErrLen);
    break;
  default:
    strlcpy(pError, "Unknown error code", iErrLen);
    break;
  }
  return 1;
}

/*-------------------------------------------------------------------------*/
static int SDTryFixIt(pCodri self, int iCode)
{
  pChopPriv pPriv = NULL;

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

  switch (iCode) {
  case UNKNOWNPAR:
  case RANDOMERROR:
  case STOPPED:
    return CHFAIL;
  case RANDOMWARNING:
    return CHREDO;
  default:
    break;
  }
  return CHFAIL;
}

/*------------------------------------------------------------------------*/
pCodri MakeSimChopper(void)
{
  pCodri pNew = NULL;
  pChopPriv pPriv = NULL;

  /* allocate space */
  pNew = (pCodri) malloc(sizeof(Codri));
  pPriv = (pChopPriv) malloc(sizeof(ChopPriv));
  memset(pPriv, 0, sizeof(ChopPriv));
  pPriv->tTarget = time(NULL);

  if ((pPriv == NULL) || (pNew == NULL)) {
    return NULL;
  }

  /* initialize this thing */
  pNew->Init = SDInit;
  pNew->Close = SDClose;
  pNew->Delete = SDDelete;
  pNew->SetPar = SDSetPar;
  pNew->SetPar2 = SDSetPar2;
  pNew->GetPar = SDGetPar;
  pNew->CheckPar = SDCheckPar;
  pNew->GetError = SDGetError;
  pNew->TryFixIt = SDTryFixIt;
  pNew->Halt = SDHalt;

  pNew->pParList = strdup("speed,phase,ratio");
  pNew->pPrivate = pPriv;

  return pNew;
}