sics/site_ansto/hardsup/cameradriver.c

/**
 * \file cameradriver.c
 * \brief This provides the IO layer which feeds inputs from a user or
 * the camera server to the state machine, and then sends any state machine
 * output back to SICS or the camera.
 *
 * \Author Ferdi Franceschini February 2013
 *
 * Copyright: see file Copyright.txt
 */
#include <string.h>
#include <sics.h>
#include <obdes.h>
#include <countdriv.h>
#include <nwatch.h>
#include <asyncqueue.h>
#include "camera.h"

#define ERRLEN 256
#define MSGLEN 256

enum camstates {idle, acquiring, processing, saving};
#define CAMDRIV_ERRTABLE \
  TR(ENONE,     "OK")                           \
  TR(EFAIL,     "command failed")               \
  TR(EBUSYACQ,  "camera busy acquiring image")  \
  TR(EBUSYSAVE, "camera busy saving image")     \
  TE(EBUSYPROC, "camera busy processing image")

#define TR(a,b) a,
#define TE(a,b) a
enum errcodes {CAMDRIV_ERRTABLE};
#undef TR
#undef TE

#define TR(a,b) b,
#define TE(a,b) b
static char *errmsg[] = {CAMDRIV_ERRTABLE};
#undef TR
#undef TE

#define CAMDRIV_PARTABLE \
  TR(CLOCK,    "clock")            \
  TR(BIN,      "bin")              \
  TR(SIZE,     "size")             \
  TR(GAIN,     "gain")             \
  TR(FLIP,     "flip")             \
  TR(XSTART,   "xstart")           \
  TR(YSTART,   "ystart")           \
  TR(XEND,     "xend")             \
  TR(YEND,     "yend")             \
  TR(EXPOSURE, "exposure")         \
  TR(TEMP,     "temperature")      \
  TR(THRESH,   "threshold")        \
  TR(SHOPT,    "shutteropentime")  \
  TE(SHCLT,    "shutterclosetime")
#define TR(a,b) 1+
#define TE(a,b) 1
static int NUMCAMPAR = CAMDRIV_PARTABLE;
#undef TR
#undef TE

#define FLIP_TABLE \
  TR(NORMAL,  "normal")       \
  TR(INVINT,  "invintensity") \
  TR(NORMFH,  "normfh")       \
  TR(NORMFV,  "normfv")       \
  TR(NORMFHV, "normfhv")      \
  TR(INVFH,   "invfh")        \
  TR(INVFV,   "invfv")        \
  TE(INVFHV,  "invfhv")

#define TR(a,b) a,
#define TE(a,b) a
enum campar  {CAMDRIV_PARTABLE, MULTI};
enum flipval {FLIP_TABLE};
#undef TR
#undef TE

#define TR(a,b) b,
#define TE(a,b) b
static char *cacmdstr[] = {CAMDRIV_PARTABLE, "multi", NULL};
static char *flipcmdstr[] = {FLIP_TABLE, NULL};
#undef TR
#undef TE

// Camera get/set commands:  ['status', 'info', 'state', 'camera', 'meta', 'file']
#define ECMDSTART 0
#define EGETSTART 100
#define ESETSTART 200
typedef struct {
  float clockMHz;
  float bin;
  float size;
  float gain;
  enum flipval flip;
  float xstart;
  float ystart;
  float xend;
  float yend;
  float exposure;
  float temp;
  float thresh;
  float shopt;
  float shclt;
  int updatecfg;
} camcfg_t;

typedef struct {
  int debug;
  char *asynq;
  camsm_t state_machine;
  pNWTimer state_timer;
  int status;
  enum errcodes camError;
  camcfg_t camera;
/*  filecfg_t file;*/
  pAsyncUnit asyncUnit;
} CamObj;

/* Camera communications and protocol handlers */
static pAsyncProtocol CAM_Protocol = NULL;
static int cb_state_timer(void *ctx, int mode);
static int cb_getstate(pAsyncTxn txn);

static void CAM_Notify(void* context, int event) {
  CamObj *self = (CamObj *) context;

  switch (event) {
    case AQU_DISCONNECT:
      SICSLogWrite("CAM:(AQU_DISCONNECT)", eLogError);
      break;
    case AQU_RECONNECT:
      SICSLogWrite("CAM:(AQU_RECONNECT)", eLogError);
      if (self->state_timer) {
        NetWatchRemoveTimer(self->state_timer);
        self->state_timer=0;
      }
      break;
  }
  return;
}

static int CAM_Tx(pAsyncProtocol p, pAsyncTxn txn) {

  if (txn == NULL) {
    return 0;
  }
  txn->txn_status = ATX_ACTIVE;
  if (AsyncUnitWrite(txn->unit, txn->out_buf, txn->out_len) < 0) {
    return 0;
  }
  return 1;
}

int defaultHandleInput(pAsyncProtocol p, pAsyncTxn txn, int ch);
static int CAM_Rx(pAsyncProtocol p, pAsyncTxn txn, int ch) {
  int ret = 1;

  if (ch == '\r')
    ret = 1;
  else if (ch == '\n')
    ret = AQU_POP_CMD; 
  else if (txn->inp_idx < txn->inp_len)
    txn->inp_buf[txn->inp_idx++] = ch;
  else
    ret = AQU_POP_CMD;

  return ret;
}
static int CAM_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) {
  if (event == AQU_TIMEOUT) {
    pTxn->txn_status = ATX_TIMEOUT;
    return AQU_POP_CMD;
  }
  return AQU_POP_CMD;
}

void CameraInitProtocol(SicsInterp *pSics) {
  if (CAM_Protocol == NULL) {
    CAM_Protocol = AsyncProtocolCreate(pSics, "CAMERA", NULL, NULL);
    CAM_Protocol->sendCommand = CAM_Tx;
    CAM_Protocol->handleInput = CAM_Rx;
    CAM_Protocol->prepareTxn  = NULL;
    CAM_Protocol->killPrivate = NULL;
    CAM_Protocol->handleEvent = CAM_Ev;
    #if 0
    CAM_Protocol->sendTerminator = strdup("\r\n");
    CAM_Protocol->replyTerminator[0] = strdup("\r\n");
    #endif
  }
}

/* CounterDriver interface functions */
static int CamGetStatus(CounterDriver *cntrData, float *fControl) {
  CamObj *camdrv= (CamObj *)cntrData->pData;

  return camdrv->status;
}

/* \brief run_sm, call the state machine with the given input.
 * \param self, driver context including current state 
 * \param ev_sym, input event
 */
static void run_sm(CamObj *self, enum event_codes ev_sym) {
  char sscur[SSLEN+1], ssnext[SSLEN+1], esout[ESLEN+1], message[MSGLEN+1];

  if (self->debug)
    strncpy(sscur, strstate(self->state_machine.Sc), SSLEN);

  camdriv_input(self, &self->state_machine, ev_sym);

  if (self->debug) {
    strncpy(ssnext, strstate(self->state_machine.Sc), SSLEN);
    strncpy(esout, strevent(self->state_machine.Eo), ESLEN);
    snprintf(message, MSGLEN, "DEBUG:(run_sm) Scurr:%s Ei:%s",
        sscur,event_names[ev_sym]);
    SICSLogWrite(message, eLog);
    snprintf(message, MSGLEN, "DEBUG:(run_sm) Snext:%s Eo:%s", ssnext,esout);
    SICSLogWrite(message, eLog);
  }
}

/* \brief sendcfg, Send the camera configuration to the camera server
 */
int sendcfg(CamObj *self) {
  int status, replen=MSGLEN;
  char reply[MSGLEN+1], logmsg[MSGLEN+1];
  char cfgCmd[MSGLEN+1];
  float clock = self->camera.clockMHz;

  sprintf(cfgCmd,
      "set camera,clock=%.*fmhz,bin=%dx,size=%d,gain=%dxhs,flip=%s,xstart=%d,ystart=%d,xend=%d,yend=%d,exposure=%f,temperature=%f,threshold=%d,shutteropentime=%d,shutterclosetime=%d",
      clock>=1 ? 0 : 1, clock, (int)self->camera.bin, (int)self->camera.size,
      (int)self->camera.gain, flipcmdstr[self->camera.flip],
      (int)self->camera.xstart, (int)self->camera.ystart,
      (int)self->camera.xend, (int)self->camera.yend, self->camera.exposure,
      self->camera.temp, (int)self->camera.thresh, (int)self->camera.shopt,
      (int)self->camera.shclt
      );

  status = AsyncUnitTransact(self->asyncUnit, cfgCmd, strlen(cfgCmd), reply, &replen);
  if (status <= 0)
    return 0;
  else
    if (strncmp("OK", reply, 2) == 0)
      return 1;
    else {
      snprintf(logmsg, MSGLEN, "CAM:(sendcfg) reply=%s", reply);
      SICSLogWrite(logmsg, eLogError);
      return 0;
    }
}

/* Called by the scan command and via the count and countnb subcommands of a
 * counter object. Will update the configuration if necessary.
 */
static int CamStart(CounterDriver *cntrData) {
  CamObj *self = NULL;
  enum event_codes cd_sym;
  char logmsg[MSGLEN+1];

  self = cntrData->pData;

  /* Send the updated configuration to the camera server if it has been changed
   * on SICS since the last shot was taken. */
  if (self->camera.updatecfg) {
    if (sendcfg(self) == 0) {
      snprintf(logmsg, MSGLEN, "CAM:(CamStart) Failed to upload configuration");
      SICSLogWrite(logmsg, eLogError);
      return 0;
    }
    self->camera.updatecfg = 0;
  }

  if (self->state_machine.multi) {
    cd_sym = ECD_MLTI_ON;
  } else {
    cd_sym = ECD_TK_SHOT;
  }
  run_sm(self, cd_sym);
  return 1;
}

static int CamPause(CounterDriver *cntrData) {
  return 1;
}
static int CamContinue(CounterDriver *cntrData) {
  return 1;
}
static int CamHalt(CounterDriver *cntrData) {
  CamObj *self = cntrData->pData;

  if (self->state_machine.multi) {
    run_sm(self, ECD_MLTI_OFF);
  }
  return 1;
}
/* TODO what should the counter data be set to? Total intensity? */
static int CamReadValues(CounterDriver *cntrData) {
  int status, iReplyLen=MSGLEN;
  char *cmd="TODO ", pReply[MSGLEN];
  CamObj *self = NULL;
  return 1;

  self = cntrData->pData;
  status = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), pReply, &iReplyLen);
  return 1;
}

static int CamGetError(CounterDriver *cntrData, int *iCode, char *error, int iErrLen) {
  CamObj *camdrv=NULL;
  camdrv = (CamObj *) cntrData->pData;
  *iCode = camdrv->camError;

  camdrv->camError = ENONE;
  switch (*iCode) {
    case EBUSYACQ:
      snprintf(error, (size_t) iErrLen,
          "CAM: Can't complete operation, %s", errmsg[EBUSYACQ]);
    break;
    case EBUSYSAVE:
      snprintf(error, (size_t) iErrLen,
          "CAM: Can't complete operation, %s", errmsg[EBUSYSAVE]);
    break;
    case EBUSYPROC:
      snprintf(error, (size_t) iErrLen,
          "CAM: Can't complete operation, %s", errmsg[EBUSYPROC]);
    break;
    case ENONE:
      snprintf(error, (size_t) iErrLen,
          "CAM: Can't complete operation, %s", errmsg[ENONE]);
    break;
    case EFAIL:
      snprintf(error, (size_t) iErrLen, "CAM: %s", errmsg[EFAIL]);
    break;
  }
  return 1;
}

static int CamTryAndFixIt(CounterDriver *cntrData, int iCode) {
  return COTERM;
}

static int CamSet(CounterDriver *cntrData, char *name, int iCter, float fVal) {
  CamObj *camdriv= (CamObj *)cntrData->pData;

  int found=0;
  char *cmd;
  enum campar id;
  enum flipval flip;

  for (id=0; (cmd = cacmdstr[id]) != NULL; id++) {
    if (strcmp(cmd,name) == 0) {
      found=1;
      break;
    }
  }

  if (!found) {
    return 0;
  }
  switch (id) {
    case MULTI:
      if (fVal != 0 && fVal != 1) {
        return 0;
      } else {
        camdriv->state_machine.multi = fVal;
      }
      break;
    case CLOCK:
      if (fVal > 0) {
        camdriv->camera.clockMHz = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    case BIN:
      if (fVal > 0) {
        camdriv->camera.bin = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    case SIZE:
      if (fVal > 0) {
        camdriv->camera.size = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    case GAIN:
      if (fVal > 0) {
        camdriv->camera.gain = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    case FLIP:
      flip = fVal;
      switch (flip) {
        case NORMAL:
        case INVINT:
        case NORMFH:
        case NORMFV:
        case NORMFHV:
        case INVFH:
        case INVFV:
        case INVFHV:
          camdriv->camera.flip = flip;
          camdriv->camera.updatecfg = 1;
          break;
        default:
          return 0;
          break;
      }
      break;
    case XSTART:
      if (fVal > 0) {
        camdriv->camera.xstart = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    case YSTART:
      if (fVal > 0) {
        camdriv->camera.ystart = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    case XEND:
      if (fVal > 0) {
        camdriv->camera.xend = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    case YEND:
      if (fVal > 0) {
        camdriv->camera.yend = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    case EXPOSURE:
      if (fVal > 0) {
        camdriv->camera.exposure = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    case TEMP:
        camdriv->camera.temp = fVal;
        camdriv->camera.updatecfg = 1;
      break;
    case THRESH:
      if (fVal > 0) {
        camdriv->camera.thresh = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    case SHOPT:
      if (fVal > 0) {
        camdriv->camera.shopt = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    case SHCLT:
      if (fVal > 0) {
        camdriv->camera.shclt = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    default:
      return 0;
  }
  return 1;
}

static int CamGet(CounterDriver *cntrData, char *name, int iCter, float *fVal) {
  CamObj *camdriv= (CamObj *)cntrData->pData;

  int found=0;
  char *cmd;
  enum campar id;

  for (id=0; (cmd = cacmdstr[id]) != NULL; id++) {
    if (strcmp(cmd,name) == 0) {
      found=1;
      break;
    }
  }

  if (!found) {
    return 0;
  }
  switch (id) {
    case MULTI:
      *fVal = (float) camdriv->state_machine.multi;
      break;
    case CLOCK:
      *fVal = camdriv->camera.clockMHz;
      break;
    case BIN:
      *fVal = camdriv->camera.bin;
      break;
    case SIZE:
      *fVal = camdriv->camera.size;
      break;
    case GAIN:
      *fVal = camdriv->camera.gain;
      break;
    case FLIP:
      *fVal = camdriv->camera.flip;
      break;
    case XSTART:
      *fVal = camdriv->camera.xstart;
      break;
    case YSTART:
      *fVal = camdriv->camera.ystart;
      break;
    case XEND:
      *fVal = camdriv->camera.xend;
      break;
    case YEND:
      *fVal = camdriv->camera.yend;
      break;
    case EXPOSURE:
      *fVal = camdriv->camera.exposure;
      break;
    case TEMP:
      *fVal = camdriv->camera.temp;
      break;
    case THRESH:
      *fVal = camdriv->camera.thresh;
      break;
    case SHOPT:
      *fVal = camdriv->camera.shopt;
      break;
    case SHCLT:
      *fVal = camdriv->camera.shclt;
      break;
    default:
      return 0;
  }
  return 1;
}

static int CamSend(CounterDriver *cntrData, char *pText, char *pReply, int iReplyLen) {
  int status;
  CamObj *self = NULL;

  self = cntrData->pData;

  status = AsyncUnitTransact(self->asyncUnit, pText, strlen(pText), pReply, &iReplyLen);
  if (status <= 0)
    return 0;
  else
    return 1;
}

static int cb_shotcmd(pAsyncTxn txn) {
  CamObj *self = (CamObj *) txn->cntx;
  char *resp = txn->inp_buf, message[MSGLEN+1];
  enum event_codes cd_sym;

  if (strncmp(resp, "OK", 2) != 0) {
    self->camError = EFAIL;
    return 0;
  }
  return 1;
}

int camdriv_out(void *me, event_t Eo) {
  int len;
  char cmd[MSGLEN]="", logmsg[MSGLEN+1]="";
  CamObj *self = (CamObj *)me;

  if (Eo.ca) {
    /* send command to camera */
    switch (Eo.ca) {
      case ECA_TK_SHOT:
        len = strlen(event_signatures[ECA_TK_SHOT]);
        strncpy(cmd, event_signatures[ECA_TK_SHOT], len);
        break;
      case ECA_MLTI_ON:
        len = strlen(event_signatures[ECA_MLTI_ON]);
        strncpy(cmd, event_signatures[ECA_MLTI_ON], len);
        break;
      case ECA_MLTI_OFF:
        len = strlen(event_signatures[ECA_MLTI_OFF]);
        strncpy(cmd, event_signatures[ECA_MLTI_OFF], len);
        break;
      default:
        snprintf(logmsg, MSGLEN, "CAM:(camdriv_out) Unhandled event %s", event_names[Eo.ca]);
        SICSLogWrite(logmsg, eLogError);
        return 0;
    }
    AsyncUnitSendTxn(self->asyncUnit, cmd, len, cb_shotcmd, self, MSGLEN);
    if (self->debug) {
      snprintf(logmsg, MSGLEN, "DEBUG:(camdriv_out:Eo.ca): ev=%s, output=%s\n",
          event_names[Eo.ca], event_signatures[Eo.ca]);
      SICSLogWrite(logmsg, eLog);
    }
  }
  if (Eo.cm) {
    snprintf(logmsg, MSGLEN, "TODO:(camdriv_out:Eo.cm): ev=%s, output=%s\n",
        event_names[Eo.cm], event_signatures[Eo.cm]);
    SICSLogWrite(logmsg, eLogError);
  }
  if (Eo.cd) {
    snprintf(logmsg, MSGLEN, "TODO:(camdriv_out:Eo.cm): ev=%s, output=%s\n",
        event_names[Eo.cd], event_signatures[Eo.cd]);
    SICSLogWrite(logmsg, eLogError);
  }
  if (Eo.dr) {
    /* send msg to SICS */
    switch (Eo.dr) {
      case EDR_IDLE:
        self->status = HWIdle;
        break;
      case EDR_BUSY:
        self->status = HWBusy;
        break;
      case EDR_FAULT:
        self->status = HWFault;
        break;
      default:
        snprintf(logmsg, MSGLEN, "CAM:(camdriv_out) Unhandled event %s", event_names[Eo.dr]);
        SICSLogWrite(logmsg, eLogError);
        return 0;
    }
    if (self->debug) {
      snprintf(logmsg, MSGLEN, "DEBUG:(camdriv_out): ev=%s, output=%s\n",
          event_names[Eo.dr], event_signatures[Eo.dr]);
      SICSLogWrite(logmsg, eLog);
    }
  } 
  return 1;
}
static int cb_state_timer(void *ctx, int mode) {
  CamObj *self = (CamObj *) ctx;
  char errmsg[32]="";
  char cmd[MSGLEN];
  int len, status;

  len = strlen(event_signatures[ECA_GET_STATE]);
  strncpy(cmd, event_signatures[ECA_GET_STATE], len);
  status = AsyncUnitSendTxn(self->asyncUnit, cmd, len, cb_getstate, self, MSGLEN);

  if (status==1) {
    return 1;
  } else {
    snprintf(errmsg, 31, "CAM:(cb_getstate) AsyncUnitSendTxn failed");
    SICSLogWrite(errmsg, eLogError);
    return 0;
  }
}

/* Input from camera state feedback */
static int cb_getstate(pAsyncTxn txn) {
  CamObj *self = (CamObj *) txn->cntx;
  char *resp = txn->inp_buf, message[MSGLEN+1];
  int len = txn->inp_idx, ret=1, time_rem, time_tot;
  enum event_codes ca_sym, cm_sym;


  if (txn->txn_status == ATX_TIMEOUT) {
    ret = 0;
  } else if ( cam_parse_status(resp, &ca_sym, &time_rem, &time_tot) == -1) {
    snprintf(message, MSGLEN,
        "CAM:(cb_getstate) cam_parse_status failed to parse '%s'",resp);
    SICSLogWrite(message, eLogError);
    ret = 0;
  } else {
    cm_sym = camera_model(ca_sym);
    run_sm(self, cm_sym);
  }
  if (self->state_timer) {
    NetWatchRemoveTimer(self->state_timer);
    self->state_timer=0;
  }
  NetWatchRegisterTimer(&self->state_timer, 500, cb_state_timer, self);

  return ret;
}

pCounterDriver CreateCam(SConnection *pCon, char *name, char *asynq) {
  char msg[ERRLEN], cmd[MSGLEN], reply[MSGLEN];
  int len, reply_len;
  state_t start_state = {.cl=SCL_RDY, .cm=SCM_IDLE, .dr=SDR_IDLE};

  pCounterDriver pCntDriv = NULL;
  CamObj *pNewCam = NULL;

  pNewCam = (CamObj *) malloc(sizeof(CamObj));
  memset(pNewCam, 0, sizeof(CamObj));
  STset(&pNewCam->state_machine.Sc, start_state);
  EVclr(&pNewCam->state_machine.Eo);
  pNewCam->state_machine.output_fn = camdriv_out;
  pNewCam->state_machine.multi = 0;
  pNewCam->state_timer = 0;
  pNewCam->status = HWIdle;
  pNewCam->camError = ENONE;
  pNewCam->debug = 1;
  pNewCam->camera.updatecfg = 1;
  pNewCam->asynq = strdup(asynq);

  if (!AsyncUnitCreate(asynq, &pNewCam->asyncUnit)) {
    snprintf(msg, ERRLEN, "CAM:AsyncQueue %s has not been defined", asynq);
    SCWrite(pCon, msg, eError);
    return NULL;
  }
  AsyncUnitSetTimeout(pNewCam->asyncUnit, 1000);
  AsyncUnitSetNotify(pNewCam->asyncUnit, pNewCam, CAM_Notify);

  pCntDriv = CreateCounterDriver(name, "anstocamera");
  if (pCntDriv == NULL)
    return NULL;


  pCntDriv->GetStatus = CamGetStatus;
  pCntDriv->Start = CamStart;
  pCntDriv->Pause = CamPause;
  pCntDriv->Continue = CamContinue;
  pCntDriv->Halt = CamHalt;
  pCntDriv->ReadValues = CamReadValues;
  pCntDriv->GetError = CamGetError;
  pCntDriv->TryAndFixIt = CamTryAndFixIt;
  pCntDriv->Set = CamSet;
  pCntDriv->Get = CamGet;
  pCntDriv->Send = CamSend;

  pCntDriv->iNoOfMonitors = 1;
  pCntDriv->pData = pNewCam;

  len = strlen(event_signatures[ECA_GET_STATE]);
  strncpy(cmd, event_signatures[ECA_GET_STATE], len);
  AsyncUnitSendTxn(pNewCam->asyncUnit, cmd, len, cb_getstate, pNewCam, MSGLEN);
  return pCntDriv;
}