379 lines
11 KiB
C
379 lines
11 KiB
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
|
|
char *errmsg[] = {CAMDRIV_ERRTABLE};
|
|
#undef TR
|
|
#undef TE
|
|
|
|
// Camera get/set commands: ['status', 'info', 'state', 'camera', 'meta', 'file']
|
|
#define ECMDSTART 0
|
|
#define EGETSTART 100
|
|
#define ESETSTART 200
|
|
enum CommandCodes {cmdTake = 1, cmdClear, cmdGet, cmdSet, cmdEnd};
|
|
enum GetCodes {gCamera = 1, gFile, gMeta, gInfo, gStatus, gState, gEnd};
|
|
enum SetCodes {sCamera = 1, sFile, sMeta, sInfo, sEnd};
|
|
char *CMD[13] = {"temp", "shspd", "ffmt", "binres", "imsz", "xstart", "ystart", "floc", "fname", "fcntr", "acq", "status", "shtime"};
|
|
|
|
struct __cameraObj {
|
|
int debug;
|
|
camdriv_t state_machine;
|
|
pNWTimer state_timer;
|
|
int status;
|
|
enum errcodes camError;
|
|
pAsyncUnit asyncUnit;
|
|
};
|
|
typedef struct __cameraObj 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 int replyOK(enum CommandCodes cc, char *reply) {
|
|
return 1;
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|
|
static int CamStart(CounterDriver *cntrData) {
|
|
CamObj *self = NULL;
|
|
enum event_codes cd_sym;
|
|
|
|
self = cntrData->pData;
|
|
|
|
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) {
|
|
return 1;
|
|
}
|
|
static int CamGet(CounterDriver *cntrData, char *name, int iCter, float *fVal) {
|
|
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;
|
|
}
|
|
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;
|
|
}
|
|
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;
|
|
|
|
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
|