sics/site_ansto/hardsup/cameradriver.c

/* MGS_ :Message from camera Get Status command
 */
#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(MULTI,  "multi")            \
  TR(CLOCK,  "clock")            \
  TR(BIN,    "bin")              \
  TR(SIZE,   "size")             \
  TR(GAIN,   "gain")             \
  TR(FLIP,   "flip")             \
  TR(XPOS,   "xpos")             \
  TR(YPOS,   "ypos")             \
  TR(TEMP,   "temperature")      \
  TR(THRESH, "threshold")        \
  TR(SHOPT,  "shutteropentime")  \
  TE(SHCLT,  "shutterclosetime")

#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};
enum flipval {FLIP_TABLE};
#undef TR
#undef TE

#define TR(a,b) b,
#define TE(a,b) b
static char *cacmdstr[] = {CAMDRIV_PARTABLE, 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 xpos;
  float ypos;
  float temp;
  float thresh;
  float shopt;
  float shclt;
  int updatecfg;
} camcfg_t;

typedef struct {
  int debug;
  camdriv_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 CAM_Tx(pAsyncProtocol p, pAsyncTxn txn) {
  int ret = 1;

  if (txn) {
    txn->txn_status = ATX_ACTIVE;
    if (AsyncUnitWrite(txn->unit, txn->out_buf, txn->out_len) < 0) {
      if (AsyncUnitReconnect(txn->unit) < 0)
        ret = 0;
    }
    ret = 1;
  } else {
    ret = 0;
  }
  return ret;
}
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;
}
/* TODO
static int CAM_Ev(pAsyncProtocol p, pAsyncTxn txn, int event) {
}
*/

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;
    #if 0
    CAM_Protocol->handleEvent = CAM_Ev;
    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;
}

static void crank_state_machine(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:(%s:%d) Scurr:%s Ein:%s|Snext:%s Eout:%s",__FILE__,__LINE__,
        sscur,event_names[ev_sym],ssnext,esout);
    SICSLogWrite(message, eLog);
  }
}

int sendcfg(CamObj *self) {
  int status, replen=MSGLEN;
  char reply[MSGLEN+1];
  char *cfgCmd = "set camera,clock=1MHz";


  status = AsyncUnitTransact(self->asyncUnit, cfgCmd, strlen(cfgCmd), reply, &replen);
  return 1;
}

static int CamStart(CounterDriver *cntrData) {
  CamObj *self = NULL;
  enum event_codes cd_sym;

  self = cntrData->pData;

  if (self->camera.updatecfg) {
    if (sendcfg(self) == 0) {
      /* ERROR failed to update configuration */
      return 0;
    }
  }

  if (self->state_machine.multi) {
    cd_sym = ECD_TK_MLTI;
  } else {
    cd_sym = ECD_TK_SHOT;
  }
  crank_state_machine(self, cd_sym);
  return 1;
}
static int CamPause(CounterDriver *cntrData) {
  return 1;
}
static int CamContinue(CounterDriver *cntrData) {
  return 1;
}
static int CamHalt(CounterDriver *cntrData) {
  return 1;
}
static int CamReadValues(CounterDriver *cntrData) {
  int status, iReplyLen=MSGLEN;
  char *cmd="get imsz", pReply[MSGLEN];
  CamObj *self = NULL;
  return 1;

  self = cntrData->pData;
 // fTime = [get shtime]
  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, "CAMERR: Can't complete operation, %s", errmsg[EBUSYACQ]);
    break;
    case EBUSYSAVE:
      snprintf(error, (size_t) iErrLen, "CAMERR: Can't complete operation, %s", errmsg[EBUSYSAVE]);
    break;
    case EBUSYPROC:
      snprintf(error, (size_t) iErrLen, "CAMERR: Can't complete operation, %s", errmsg[EBUSYPROC]);
    break;
    case ENONE:
      snprintf(error, (size_t) iErrLen, "CAMERR: Can't complete operation, %s", errmsg[ENONE]);
    break;
    case EFAIL:
      snprintf(error, (size_t) iErrLen, "CAMERR: %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,cacmdstr[id]) == 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 NORMFHV:
        case INVFH:
        case INVFV:
        case INVFHV:
          camdriv->camera.flip = flip;
          break;
        default:
          return 0;
          break;
      }
      break;
    case XPOS:
      if (fVal > 0) {
        camdriv->camera.xpos = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    case YPOS:
      if (fVal > 0) {
        camdriv->camera.ypos = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      break;
    case TEMP:
      if (fVal > 0) {
        camdriv->camera.temp = fVal;
        camdriv->camera.updatecfg = 1;
      } else {
        return 0;
      }
      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,cacmdstr[id]) == 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 XPOS:
      *fVal = camdriv->camera.xpos;
      break;
    case YPOS:
      *fVal = camdriv->camera.ypos;
      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);
  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];
  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;
      default:
        break;
    }
    AsyncUnitSendTxn(self->asyncUnit, cmd, len, cb_shotcmd, self, MSGLEN);
    if (self->debug) {
      snprintf(logmsg, MSGLEN, "DEBUG: camdriv_out: ev=%s, output=%s\n", event_names[Eo.ca], event_signatures[Eo.ca]);
      SICSLogWrite(logmsg, eLog);
    }
  }
  if (Eo.cm) {
    snprintf(logmsg, MSGLEN, "ERROR: NOT IMPLEMENTED, camdriv_out: ev=%s, output=%s\n", event_names[Eo.cm], event_signatures[Eo.cm]);
    SICSLogWrite(logmsg, eLogError);
  }
  if (Eo.cd) {
    snprintf(logmsg, MSGLEN, "ERROR: NOT IMPLEMENTED, camdriv_out: 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:
        break;
    }
    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_getstate(pAsyncTxn txn);
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, "ERROR:(%s) AsyncUnitTransact failed",__FILE__);
    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 ( cam_parse_status(resp, &ca_sym, &time_rem, &time_tot) == -1) {
    snprintf(message, MSGLEN, "ERROR:(%s:%d) cam_parse_status failed on '%s'",__FILE__,__LINE__,resp);
    SICSLogWrite(message, eLogError);
    ret = 0;
  } else {
    cm_sym = camera_model(ca_sym);
    crank_state_machine(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 = 0;

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

  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;
}
#if 0
int CameraAction(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]) {
CamObj *pCam = (CamObj *) pData;
}
int CamMakeCmd(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]) {
  CamObj *pNew = NULL;

  pNew = CreateCam(argv[1], argv[2]);
  if (pNew == NULL)
    return 0;
  AddCommand(pSics, argv[1], CameraAction, NULL, pNew);
  return 1;
}
#endif