/*------------------------------------------------------------------------- N H Q 2 x x Support for FAST ComTec NHQ2xx High Voltage Power Supply for SICS. The meaning and working of the functions defined is as desribed for a general environment controller. Douglas Clowes, May 2008 Copyright: site_ansto/doc/Copyright.txt ---------------------------------------------------------------------------- */ #include "nhq200.h" #include "sics.h" #include "asyncqueue.h" #include "nwatch.h" #include "fsm.h" #include "anstoutil.h" #include #include #include #include #include #include #include #define CMDLEN 132 #define MY_ABSOLUTE_MAXIMUM (6000.0) #define MY_MINIMUM_RATE (1.0) #define MY_MAXIMUM_RATE (255.0) #define NHQ_FAST_POLL (500.0) #define NHQ_SLOW_POLL (2000.0) /* ERROR CODES */ #define NHQ200_ERR_NONE (0) #define NHQ200_ERR_LOCKED (-1) #define NHQ200_ERR_RANGE (-2) /* Device Driver Control Structure */ struct nhq200_s { pEVControl controller; int iErrorCode; /* error code */ float fTarget; /* requested target voltage in volts */ float fValue; /* current voltage in volts */ float fMax; /* maximum voltage in volts */ float fRate; /* voltage slew rate in volts per second */ float fUpper; /* Normal Operating Voltage */ float fLower; /* Normal Moving Voltage */ bool isLocked; /* changes no longer permitted */ bool bRunFlag; /* set by the run command */ bool bInternal; /* Flags an internal run request */ char* name; pAsyncUnit asyncUnit; StateMachine fsm; pNWTimer state_timer; /**< state timer */ int iControl; /* integer controller (1..2) */ int iErrorCount; /* error count */ char serial_number[20]; char software_version[20]; char voltage_max[20]; char current_max[20]; char status_s[20]; char status_t[20]; int module_status; int vmax_percent; int imax_percent; int ramp_input; }; typedef struct nhq200_s NHQ200Driv, *pNHQ200Driv; /* Functions */ static int NHQ200GetValue( pEVDriver self, float* fPos); static int NHQ200SetValue( pEVDriver self, float fPos); static int NHQ200Send(pEVDriver self, char *pCommand, char *pReply, int iLen); static int NHQ200Error(pEVDriver self, int *iCode, char *error, int iErrLen); static int NHQ200Fix(pEVDriver self, int iError); static int NHQ200Init(pEVDriver self); static int NHQ200Close(pEVDriver self); static void NHQ200KillPrivate(void *pData); static void NHQ200Notify(void* context, int event); static void NHQ_SetTimer(pNHQ200Driv priv, int msecs) { NetWatchRegisterTimer(&priv->state_timer, msecs, fsm_tmr_callback, &priv->fsm); } /** * \brief Sends a command and set up for a response event * * \param self motor data * \param cmd command to send * \param reply space to return response * \return */ static int NHQ_SendCmd(pNHQ200Driv priv, char* command, int cmd_len, AsyncTxnHandler callback) { pStateMachine sm = &priv->fsm; return AsyncUnitSendTxn(priv->asyncUnit, command, cmd_len, callback, sm, CMDLEN); } /** * \brief Sends a command and waits for a response * * \param priv motor data * \param cmd command to send * \param reply space to return response * \return */ static int NHQ_SendReceive(pNHQ200Driv priv, char *cmd, int cmd_len, char* reply, int *rep_len) { int status; status = AsyncUnitTransact(priv->asyncUnit, cmd, cmd_len, reply, rep_len); if (status != 1) { return FAILURE; } return OKOK; } /* State Functions */ static void NHQState_Unknown(pStateMachine sm, pEvtEvent event); static void NHQState_Idle(pStateMachine sm, pEvtEvent event); static void NHQState_Raising(pStateMachine sm, pEvtEvent event); static void NHQState_Lowering(pStateMachine sm, pEvtEvent event); static void NHQState_Error(pStateMachine sm, pEvtEvent event); static void str_n_cat(char* s1, int len, const char* s2) { int i = strlen(s1); const char* p = s2; while (i < len - 3 && *p) { if (*p == '\r') { s1[i++] = '\\'; s1[i++] = 'r'; ++p; } else if (*p == '\n') { s1[i++] = '\\'; s1[i++] = 'n'; ++p; } else s1[i++] = *p++; } s1[i] = '\0'; } static const char* state_name(StateFunc func) { if (func == NULL) return ""; if (func == NHQState_Unknown) return "NHQState_Unknown"; if (func == NHQState_Idle) return "NHQState_Idle"; if (func == NHQState_Raising) return "NHQState_Raising"; if (func == NHQState_Lowering) return "NHQState_Lowering"; if (func == NHQState_Error) return "NHQState_Error"; return ""; } static const char* event_name(pEvtEvent event, char* text, int length) { char line[1024]; if (event == NULL) return ""; switch (event->event_type) { case eStateEvent: snprintf(text, length, "eStateEvent"); return text; case eTimerEvent: snprintf(text, length, "eTimerEvent (%d mSec)", event->event.tmr.timerValue); return text; case eMessageEvent: snprintf(text, length, "eMessageEvent:"); fsm_textify(event->event.msg.cmd->out_buf, event->event.msg.cmd->out_len, line, sizeof(line)); str_n_cat(text, length, line); str_n_cat(text, length, "|"); fsm_textify(event->event.msg.cmd->inp_buf, event->event.msg.cmd->inp_idx, line, sizeof(line)); str_n_cat(text, length, line); return text; case eCommandEvent: /* TODO Command Events */ snprintf(text, length, "eCommandEvent:unknown"); return text; case eTimeoutEvent: snprintf(text, length, "eTimeoutEvent"); return text; default: snprintf(text, length, ""); return text; } } static void read_status(pNHQ200Driv priv, char* line, int line_len) { static struct { char t[6], f[6]; } txt[] = { { ", CHA", ", CHB" }, { ", MAN", ", SER" }, { ", POS", ", NEG" }, { ", OFF", ", ON" }, { ", ENA", "" }, { ", INH", "" }, { ", ERR", "" }, { ", QUA", "" } }; char cmd[CMDLEN]; int cmd_len; char rsp[CMDLEN]; int rsp_len; int i, j1, j2; snprintf(line, CMDLEN, "%s.status = ", priv->name); /* read status word */ cmd_len = snprintf(cmd, CMDLEN, "S%d\r\n", priv->iControl); rsp_len = CMDLEN; NHQ_SendReceive(priv, cmd, cmd_len, rsp, &rsp_len); strncat(line, rsp, line_len); /* read module status */ cmd_len = snprintf(cmd, CMDLEN, "T%d\r\n", priv->iControl); rsp_len = CMDLEN; NHQ_SendReceive(priv, cmd, cmd_len, rsp, &rsp_len); strncat(line, ", ", line_len); strncat(line, rsp, line_len); j1 = strtol(rsp, NULL, 10); for (i = 7; i > 0; --i) if (j1 & (1 << i)) strncat(line, txt[i].t, line_len); else strncat(line, txt[i].f, line_len); if (priv->iControl == 1) { rsp_len = CMDLEN; NHQ_SendReceive(priv, "T2\r\n", 4, rsp, &rsp_len); j2 = strtol(rsp, NULL, 10); } else { j2 = j1; rsp_len = CMDLEN; NHQ_SendReceive(priv, "T1\r\n", 4, rsp, &rsp_len); j1 = strtol(rsp, NULL, 10); } if (j1 & 1) strncat(line, ", VLT", line_len); else strncat(line, ", AMP", line_len); if (j2 & 1) strncat(line, ", CHA", line_len); else strncat(line, ", CHB", line_len); } static bool parse_hash(pNHQ200Driv priv, const char* resp, int resp_len) { int iSrc; int iDst; iSrc = 0; iDst = 0; if (strstr(resp, "V;") == NULL) return false; if (strstr(resp, "mA") == NULL && strstr(resp, "uA") == NULL) return false; while (iSrc < resp_len && resp[iSrc]) { if (resp[iSrc] == ';') { ++iSrc; break; } priv->serial_number[iDst++] = resp[iSrc++]; } priv->serial_number[iDst] = '\0'; iDst = 0; while (iSrc < resp_len && resp[iSrc]) { if (resp[iSrc] == ';') { ++iSrc; break; } priv->software_version[iDst++] = resp[iSrc++]; } priv->software_version[iDst] = '\0'; iDst = 0; while (iSrc < resp_len && resp[iSrc]) { if (resp[iSrc] == ';') { ++iSrc; break; } priv->voltage_max[iDst++] = resp[iSrc++]; } priv->voltage_max[iDst] = '\0'; iDst = 0; while (iSrc < resp_len && resp[iSrc]) { if (resp[iSrc] == ';') { ++iSrc; break; } priv->current_max[iDst++] = resp[iSrc++]; } priv->current_max[iDst] = '\0'; /* TODO: convert voltage and current */ if (strstr(priv->voltage_max, "V") == NULL) return false; if (strstr(priv->current_max, "mA") == NULL && strstr(priv->current_max, "uA") == NULL) return false; return true; } static void parse_Sx(pNHQ200Driv priv, const char* resp, int resp_len) { memcpy(priv->status_s, resp, resp_len); if (resp_len > 0 && priv->status_s[resp_len - 1] == ' ') --resp_len; priv->status_s[resp_len] = '\0'; } static void parse_Tx(pNHQ200Driv priv, const char* resp, int resp_len) { memcpy(priv->status_t, resp, resp_len); priv->status_t[resp_len] = '\0'; priv->module_status = strtol(priv->status_t, NULL, 10); } static void parse_Mx(pNHQ200Driv priv, const char* resp, int resp_len) { priv->vmax_percent = strtol(resp, NULL, 10); } static void parse_Nx(pNHQ200Driv priv, const char* resp, int resp_len) { priv->imax_percent = strtol(resp, NULL, 10); } static void parse_Vx(pNHQ200Driv priv, const char* resp, int resp_len) { priv->ramp_input = strtol(resp, NULL, 10); priv->fRate = priv->ramp_input; } static void parse_Ux(pNHQ200Driv priv, const char* resp, int resp_len) { if (*resp == '+' || *resp == '-') ++resp; priv->fValue = strtol(resp, NULL, 10); } static void parse_Dx(pNHQ200Driv priv, const char* resp, int resp_len) { return; } static void parse_Gx(pNHQ200Driv priv, const char* resp, int resp_len) { return; /* L2H or H2L */ } #define STATE_HASH 1 #define STATE_SX 2 #define STATE_TX 3 #define STATE_MX 4 #define STATE_NX 5 #define STATE_VX 6 #define STATE_END 9 /* State Functions */ /* * Unknown State * * Handle initialisation and reset operations */ static void NHQState_Unknown(pStateMachine sm, pEvtEvent event) { pEVDriver driv = (pEVDriver) sm->context; pNHQ200Driv priv = (pNHQ200Driv) driv->pPrivate; char cmd[CMDLEN]; int cmd_len; switch (event->event_type) { case eStateEvent: if (priv->state_timer) NetWatchRemoveTimer(priv->state_timer); priv->state_timer = NULL; NHQ_SendCmd(priv, "#", 1, fsm_msg_callback); sm->mySubState = STATE_HASH; return; case eTimerEvent: priv->state_timer = NULL; NHQ_SendCmd(priv, "#", 1, fsm_msg_callback); sm->mySubState = STATE_HASH; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; pCmd->inp_buf[pCmd->inp_idx] = '\0'; const char* rqst = pCmd->out_buf; const char* resp = pCmd->inp_buf; int resp_len = pCmd->inp_idx; char line[CMDLEN]; switch (sm->mySubState) { case STATE_HASH: /* # */ if (*rqst != '#') break; if (!parse_hash(priv, resp, resp_len)) break; cmd_len = snprintf(cmd, sizeof(cmd), "S%d", priv->iControl); NHQ_SendCmd(priv, cmd, cmd_len, fsm_msg_callback); sm->mySubState = STATE_SX; return; case STATE_SX: /* Sx */ if (*rqst != 'S') break; parse_Sx(priv, resp, resp_len); cmd_len = snprintf(cmd, sizeof(cmd), "T%d", priv->iControl); NHQ_SendCmd(priv, cmd, cmd_len, fsm_msg_callback); sm->mySubState = STATE_TX; return; case STATE_TX: /* Tx */ if (*rqst != 'T') break; parse_Tx(priv, resp, resp_len); cmd_len = snprintf(cmd, sizeof(cmd), "M%d", priv->iControl); NHQ_SendCmd(priv, cmd, cmd_len, fsm_msg_callback); sm->mySubState = STATE_MX; return; case STATE_MX: /* Mx */ if (*rqst != 'M') break; parse_Mx(priv, resp, resp_len); cmd_len = snprintf(cmd, sizeof(cmd), "N%d", priv->iControl); NHQ_SendCmd(priv, cmd, cmd_len, fsm_msg_callback); sm->mySubState = STATE_NX; return; case STATE_NX: /* Nx */ if (*rqst != 'N') break; parse_Nx(priv, resp, resp_len); cmd_len = snprintf(cmd, sizeof(cmd), "V%d", priv->iControl); NHQ_SendCmd(priv, cmd, cmd_len, fsm_msg_callback); sm->mySubState = STATE_VX; return; case STATE_VX: /* Vx */ if (*rqst != 'V') break; parse_Vx(priv, resp, resp_len); if (strstr(priv->voltage_max, "V") && priv->vmax_percent) { priv->fMax = strtod(priv->voltage_max, NULL) * (priv->vmax_percent * 0.01); if (priv->fMax > 0.0 && priv->fMax < 10000.0) { SConnection *pDumCon; pDumCon = SCCreateDummyConnection(pServ->pSics); EVCSetPar(priv->controller, "lowerlimit", 0.0, pDumCon); EVCSetPar(priv->controller, "upperlimit", priv->fMax, pDumCon); SCDeleteConnection(pDumCon); } } fsm_change_state(sm, NHQState_Idle); return; } snprintf(line, CMDLEN, "Command '%s' response '%s' unexpected in substate %d", rqst, resp, sm->mySubState); SICSLogWrite(line, eError); } while (0); /* Unrecognised response - let it settle down before retrying */ if (priv->state_timer) NetWatchRemoveTimer(priv->state_timer); /* Limit how many times we go around before we call it quits */ if (++priv->iErrorCount > 10) { fsm_change_state(sm, NHQState_Error); return; } NetWatchRegisterTimer(&priv->state_timer, NHQ_SLOW_POLL, fsm_tmr_callback, sm); return; case eCommandEvent: return; case eTimeoutEvent: /* Command timeout - let it settle down before retrying */ if (priv->state_timer) NetWatchRemoveTimer(priv->state_timer); /* Limit how many times we go around before we call it quits */ if (++priv->iErrorCount > 10) { fsm_change_state(sm, NHQState_Error); return; } NetWatchRegisterTimer(&priv->state_timer, NHQ_SLOW_POLL, fsm_tmr_callback, sm); return; } return; } /* * Idle State * * Just monitoring what's going on * and waiting to be told to run somewhere */ static void NHQState_Idle(pStateMachine sm, pEvtEvent event){ pEVDriver driv = (pEVDriver) sm->context; pNHQ200Driv priv = (pNHQ200Driv) driv->pPrivate; char cmd[CMDLEN]; int cmd_len; int newVal; switch (event->event_type) { case eStateEvent: if (priv->state_timer) NetWatchRemoveTimer(priv->state_timer); NetWatchRegisterTimer(&priv->state_timer, NHQ_FAST_POLL, fsm_tmr_callback, sm); /* reset the error counter in state unknown */ priv->iErrorCount = 0; return; case eTimerEvent: priv->state_timer = NULL; if (priv->bRunFlag) { if (priv->fTarget > priv->fValue) fsm_change_state(sm, NHQState_Raising); else fsm_change_state(sm, NHQState_Lowering); priv->bRunFlag = false; return; } cmd_len = snprintf(cmd, sizeof(cmd), "U%d", priv->iControl); NHQ_SendCmd(priv, cmd, cmd_len, fsm_msg_callback); sm->mySubState = 1; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; pCmd->inp_buf[pCmd->inp_idx] = '\0'; const char* rqst = pCmd->out_buf; const char* resp = pCmd->inp_buf; int resp_len = pCmd->inp_idx; char line[CMDLEN]; switch (sm->mySubState) { case 1: if (*rqst != 'U') break; parse_Ux(priv, resp, resp_len); NetWatchRegisterTimer(&priv->state_timer, NHQ_FAST_POLL, fsm_tmr_callback, sm); return; } snprintf(line, CMDLEN, "Command '%s' response '%s' unexpected in substate %d", rqst, resp, sm->mySubState); SICSLogWrite(line, eError); } while (0); /* Unrecognised response - let it settle down before retrying */ if (priv->state_timer) NetWatchRemoveTimer(priv->state_timer); NetWatchRegisterTimer(&priv->state_timer, NHQ_SLOW_POLL, fsm_tmr_callback, sm); return; case eCommandEvent: return; case eTimeoutEvent: /* Command timeout - let it settle down before retrying */ if (priv->state_timer) NetWatchRemoveTimer(priv->state_timer); NetWatchRegisterTimer(&priv->state_timer, NHQ_SLOW_POLL, fsm_tmr_callback, sm); return; } return; } /* Raising State * * Increasing controlled value */ static void NHQState_Raising(pStateMachine sm, pEvtEvent event) { pEVDriver driv = (pEVDriver) sm->context; pNHQ200Driv priv = (pNHQ200Driv) driv->pPrivate; char cmd[CMDLEN]; int cmd_len; int newVal; switch (event->event_type) { case eStateEvent: case eTimerEvent: if (priv->state_timer) NetWatchRemoveTimer(priv->state_timer); priv->state_timer = NULL; cmd_len = snprintf(cmd, sizeof(cmd), "D%d=%0.0f", priv->iControl, priv->fTarget); NHQ_SendCmd(priv, cmd, cmd_len, fsm_msg_callback); sm->mySubState = 1; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; pCmd->inp_buf[pCmd->inp_idx] = '\0'; const char* rqst = pCmd->out_buf; const char* resp = pCmd->inp_buf; int resp_len = pCmd->inp_idx; char line[CMDLEN]; switch (sm->mySubState) { case 1: if (*rqst != 'D') break; cmd_len = snprintf(cmd, sizeof(cmd), "V%d=%0.0f", priv->iControl, priv->fRate); NHQ_SendCmd(priv, cmd, cmd_len, fsm_msg_callback); sm->mySubState = 2; return; case 2: if (*rqst != 'V') break; cmd_len = snprintf(cmd, sizeof(cmd), "G%d", priv->iControl); NHQ_SendCmd(priv, cmd, cmd_len, fsm_msg_callback); sm->mySubState = 3; return; case 3: if (*rqst != 'G') break; parse_Gx(priv, resp, resp_len); fsm_change_state(sm, NHQState_Idle); return; } snprintf(line, CMDLEN, "Command '%s' response '%s' unexpected in substate %d", rqst, resp, sm->mySubState); SICSLogWrite(line, eError); } while (0); /* Unrecognised response - let it settle down before retrying */ if (priv->state_timer) NetWatchRemoveTimer(priv->state_timer); if (++priv->iErrorCount > 10) { fsm_change_state(sm, NHQState_Error); return; } NetWatchRegisterTimer(&priv->state_timer, NHQ_SLOW_POLL, fsm_tmr_callback, sm); return; case eCommandEvent: return; case eTimeoutEvent: /* Command timeout - let it settle down before retrying */ if (priv->state_timer) NetWatchRemoveTimer(priv->state_timer); if (++priv->iErrorCount > 10) { fsm_change_state(sm, NHQState_Error); return; } NetWatchRegisterTimer(&priv->state_timer, NHQ_SLOW_POLL, fsm_tmr_callback, sm); return; } return; } /* Lowering State * * Decreasing controlled value */ static void NHQState_Lowering(pStateMachine sm, pEvtEvent event) { pEVDriver driv = (pEVDriver) sm->context; pNHQ200Driv priv = (pNHQ200Driv) driv->pPrivate; char cmd[CMDLEN]; int cmd_len; int newVal; switch (event->event_type) { case eStateEvent: case eTimerEvent: if (priv->state_timer) NetWatchRemoveTimer(priv->state_timer); priv->state_timer = NULL; cmd_len = snprintf(cmd, sizeof(cmd), "D%d=%0.0f", priv->iControl, priv->fTarget); NHQ_SendCmd(priv, cmd, cmd_len, fsm_msg_callback); sm->mySubState = 1; return; case eMessageEvent: do { pAsyncTxn pCmd = event->event.msg.cmd; pCmd->inp_buf[pCmd->inp_idx] = '\0'; const char* rqst = pCmd->out_buf; const char* resp = pCmd->inp_buf; int resp_len = pCmd->inp_idx; char line[CMDLEN]; switch (sm->mySubState) { case 1: if (*rqst != 'D') break; cmd_len = snprintf(cmd, sizeof(cmd), "V%d=%0.0f", priv->iControl, priv->fRate); NHQ_SendCmd(priv, cmd, cmd_len, fsm_msg_callback); sm->mySubState = 2; return; case 2: if (*rqst != 'V') break; cmd_len = snprintf(cmd, sizeof(cmd), "G%d", priv->iControl); NHQ_SendCmd(priv, cmd, cmd_len, fsm_msg_callback); sm->mySubState = 3; return; case 3: if (*rqst != 'G') break; parse_Gx(priv, resp, resp_len); fsm_change_state(sm, NHQState_Idle); return; } snprintf(line, CMDLEN, "Command '%s' response '%s' unexpected in substate %d", rqst, resp, sm->mySubState); SICSLogWrite(line, eError); } while (0); /* Unrecognised response - let it settle down before retrying */ if (priv->state_timer) NetWatchRemoveTimer(priv->state_timer); if (++priv->iErrorCount > 10) { fsm_change_state(sm, NHQState_Error); return; } NetWatchRegisterTimer(&priv->state_timer, NHQ_SLOW_POLL, fsm_tmr_callback, sm); return; case eCommandEvent: return; case eTimeoutEvent: /* Command timeout - let it settle down before retrying */ if (priv->state_timer) NetWatchRemoveTimer(priv->state_timer); if (++priv->iErrorCount > 10) { fsm_change_state(sm, NHQState_Error); return; } NetWatchRegisterTimer(&priv->state_timer, NHQ_SLOW_POLL, fsm_tmr_callback, sm); return; } return; } /* Error State * * Something bad has happened, we need a reset */ static void NHQState_Error(pStateMachine sm, pEvtEvent event) { /* TODO */ } /* * Return the current value to SICS */ static int NHQ200GetValue( pEVDriver self, float* fPos) { pNHQ200Driv priv = NULL; assert(self); assert(self->pPrivate); priv = (pNHQ200Driv) self->pPrivate; *fPos = priv->fValue; return 1; } /* * Set the current value from SICS */ static int NHQ200SetValue( pEVDriver self, float fPos) { pNHQ200Driv priv = NULL; assert(self); assert(self->pPrivate); priv = (pNHQ200Driv) self->pPrivate; if (priv->isLocked && !priv->bInternal) { priv->iErrorCode = NHQ200_ERR_LOCKED; return 0; } if (fPos < 0.0 || fPos > priv->fMax) { priv->iErrorCode = NHQ200_ERR_RANGE; return 0; } priv->fTarget = fPos; priv->bRunFlag = true; return 1; } /* * Send a command from SICS */ static int NHQ200Send(pEVDriver self, char *pCommand, char *pReply, int iLen) { char cmd[CMDLEN]; int cmd_len; char rsp[CMDLEN]; int rsp_len; int idx = 0; int i; cmd[0] = '\0'; if (pCommand && *pCommand && pReply && iLen > 0) { for (i = 0; pCommand[i]; ++i) { int k; k = pCommand[i]; if (idx < CMDLEN) cmd[idx++] = k; } if (idx < CMDLEN) cmd[idx] = '\0'; cmd_len = idx; rsp_len = CMDLEN; NHQ_SendReceive(self->pPrivate, cmd, cmd_len, rsp, &rsp_len); idx = 0; for (i = 0; i < rsp_len; ++i) { if ((rsp[i] < 32 || rsp[i] > 126) && idx < iLen - 3) { snprintf(&pReply[idx], 3, "\\%02x", rsp[i]); idx += 3; } else if (idx < iLen) pReply[idx++] = rsp[i]; } pReply[idx] = '\0'; return 1; } return -1; } /* * SICS error handler */ static int NHQ200Error(pEVDriver self, int *iCode, char *error, int iErrLen) { pNHQ200Driv priv = (pNHQ200Driv) self->pPrivate; *iCode = priv->iErrorCode; switch (priv->iErrorCode) { case NHQ200_ERR_RANGE: strncpy(error,"Value out of range",iErrLen); break; case NHQ200_ERR_LOCKED: strncpy(error,"Object is locked",iErrLen); break; default: strncpy(error,"TODO Error Messages",iErrLen); break; } return 1; } /* * SICS fix handler */ static int NHQ200Fix(pEVDriver self, int iError) { /* TODO */ return DEVFAULT; } static int NHQ200Init(pEVDriver self) { /* TODO */ return 1; } static int NHQ200Close(pEVDriver self) { /* TODO */ return -1; } static void NHQ200KillPrivate(void *pData) { pNHQ200Driv pMe = (pNHQ200Driv) pData; if (pMe) { if (pMe->asyncUnit) { AsyncUnitDestroy(pMe->asyncUnit); pMe->asyncUnit = NULL; } if (pMe ->name) { free(pMe ->name); pMe ->name = NULL; } /* Not required as performed in caller * free(pMe); */ return; } } /* * Auxilliary transmit function * Called internaly to transmit a character */ static int NHQ200_Tx1(pAsyncProtocol p, pAsyncTxn myCmd) { int iRet = 1; assert(myCmd); iRet = AsyncUnitWrite(myCmd->unit, &myCmd->out_buf[myCmd->out_idx], 1); return iRet; } /* * Protocol transmit function * Called by AsyncQueue to transmit a line */ static int NHQ200_Tx(pAsyncProtocol p, pAsyncTxn myCmd) { int iRet = 1; if (myCmd) { myCmd->txn_status = ATX_ACTIVE; /* * Set/reset command states for send/resend of command */ myCmd->txn_state = 0; myCmd->out_idx = 0; myCmd->inp_idx = 0; iRet = NHQ200_Tx1(p, myCmd); return iRet; } return 1; } /* * Protocol receive character - characater by character */ static int NHQ200_Rx(pAsyncProtocol p, pAsyncTxn myCmd, int rxchar) { int iRet = 1; switch (myCmd->txn_state) { case 0: /* send with echo */ if (rxchar != myCmd->out_buf[myCmd->out_idx]) { /* TODO: bad echo */ } else if (rxchar == 0x0A && myCmd->out_idx > 0 && myCmd->out_buf[myCmd->out_idx - 1] == 0x0D) { myCmd->inp_idx = 0; myCmd->txn_state = 1; /* TODO: end of line */ } else if (myCmd->out_idx < myCmd->out_len) { myCmd->out_idx++; iRet = NHQ200_Tx1(p, myCmd); } else { /* TODO: out of data */ } break; case 1: /* receiving reply */ if (myCmd->inp_idx < myCmd->inp_len) myCmd->inp_buf[myCmd->inp_idx++] = rxchar; if (rxchar == 0x0D) myCmd->txn_state = 2; break; case 2: /* received CR and looking for LF */ if (myCmd->inp_idx < myCmd->inp_len) myCmd->inp_buf[myCmd->inp_idx++] = rxchar; if (rxchar == 0x0A) { /* end of line */ myCmd->txn_state = 3; myCmd->inp_idx -= 2; myCmd->inp_buf[myCmd->inp_idx] = '\0'; myCmd->txn_status = ATX_COMPLETE; iRet = 0; } else myCmd->txn_state = 1; break; } if (iRet == 0) { /* end of command */ return AQU_POP_CMD; } return iRet; } /* * AsyncUnit Notify Callback */ static void NHQ200Notify(void* context, int event) { pNHQ200Driv priv = (pNHQ200Driv) context; switch (event) { case AQU_DISCONNECT: return; case AQU_RECONNECT: do { mkChannel* sock = AsyncUnitGetSocket(priv->asyncUnit); int flag = 1; setsockopt(sock->sockid, /* socket affected */ IPPROTO_TCP, /* set option at TCP level */ TCP_NODELAY, /* name of option */ (char *) &flag, /* the cast is historical cruft */ sizeof(int)); /* length of option value */ return; } while (0); } return; } /* * AsyncProtocol Event callback */ static int NHQ200_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) { if (event == AQU_TIMEOUT) { /* handle command timeout */ pTxn->txn_status = ATX_TIMEOUT; return AQU_POP_CMD; } return AQU_POP_CMD; } static pAsyncProtocol NHQ200_Protocol = NULL; /* * Protocol Initialisation */ void NHQ200InitProtocol(SicsInterp *pSics) { if (NHQ200_Protocol == NULL) { NHQ200_Protocol = AsyncProtocolCreate(pSics, "NHQ200", NULL, NULL); NHQ200_Protocol->sendCommand = NHQ200_Tx; NHQ200_Protocol->handleInput = NHQ200_Rx; NHQ200_Protocol->handleEvent = NHQ200_Ev; // NHQ200_Protocol->prepareTxn = NULL; // NHQ200_Protocol->killPrivate = NULL; } } /* * Device Factory */ pEVDriver CreateNHQ200Driver(int argc, char *argv[]) { pEVDriver self = NULL; pNHQ200Driv priv = NULL; if (argc < 1) return NULL; self = CreateEVDriver(argc, argv); if (!self) return NULL; priv = (pNHQ200Driv)malloc(sizeof(NHQ200Driv)); if(!priv) { DeleteEVDriver(self); return NULL; } memset(priv,0,sizeof(NHQ200Driv)); priv->fValue = 0.0; if (!AsyncUnitCreate(argv[0], &priv->asyncUnit)) { char line[132]; snprintf(line, 132, "Error: did not find AsyncQueue %s for Device %s", argv[1], argv[0]); SICSLogWrite(line, eError); DeleteEVDriver(self); free(priv); return NULL; } AsyncUnitSetNotify(priv->asyncUnit, self, NHQ200Notify); /* initialise function pointers */ self->SetValue = NHQ200SetValue; self->GetValue = NHQ200GetValue; self->Send = NHQ200Send; self->GetError = NHQ200Error; self->TryFixIt = NHQ200Fix; self->Init = NHQ200Init; self->Close = NHQ200Close; self->pPrivate = priv; self->KillPrivate = NHQ200KillPrivate; priv->fsm.context = self; priv->fsm.state_name = state_name; priv->fsm.event_name = event_name; priv->name = strdup(argv[0]); priv->fMax = 2400.0; priv->fRate = 10.0; priv->fLower = 0.0; priv->fUpper = 0.0; priv->bRunFlag = false; priv->iControl = 1; if (argc > 1) priv->iControl = strtol(argv[1], NULL, 10); if (priv->iControl < 1 || priv->iControl > 2) priv->iControl = 1; return self; } /* * Register the controller with the driver */ void NHQ200Register(pEVControl self, pEVDriver driv) { pNHQ200Driv priv = (pNHQ200Driv) driv->pPrivate; priv->controller = self; if (self->pName) { if (priv->name) free(priv->name); priv->name = strdup(self->pName); priv->fsm.name = priv->name; } fsm_change_state(&priv->fsm, NHQState_Unknown); while (priv->fsm.myState == NHQState_Unknown) { pTaskMan pTasker = GetTasker(); TaskYield(pTasker); } } /* * Action Wrapper routine */ int NHQ200Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]) { pEVControl self = (pEVControl)pData; pEVDriver driv = self->pDriv; pNHQ200Driv priv = (pNHQ200Driv) driv->pPrivate; assert(self); assert(pCon); assert(pSics); if(argc < 2) { return EVControlWrapper(pCon,pSics,pData,argc,argv); } if (strcasecmp("send", argv[1]) == 0) { char cmd[CMDLEN]; int cmd_len; char rsp[CMDLEN]; int rsp_len; int idx = 0; int i; /* Managers only */ if (!SCMatchRights(pCon, usMugger)) return 0; cmd[idx] = '\0'; for (i = 2; i < argc; ++i) { int j, k; if (i > 2 && idx < CMDLEN) cmd[idx++] = ' '; for (j = 0; argv[i][j]; ++j) { k = argv[i][j]; if (idx < CMDLEN) cmd[idx++] = k; } if (idx < CMDLEN) cmd[idx] = '\0'; cmd_len = idx; } rsp_len = CMDLEN; NHQ_SendReceive(priv, cmd, cmd_len, rsp, &rsp_len); idx = 0; for (i = 0; i < rsp_len; ++i) { if (rsp[i] < 32 || rsp[i] > 126) { snprintf(&cmd[idx], 3, "\\%02x", rsp[i]); idx += 3; } else cmd[idx++] = rsp[i]; } cmd[idx] = '\0'; SCWrite(pCon, cmd, eValue); return 1; } if (strcasecmp("up", argv[1]) == 0 || strcasecmp("down", argv[1]) == 0) { int iRet; if(!SCMatchRights(pCon,usUser)) return 0; if (strcasecmp("up", argv[1]) == 0) priv->fTarget = priv->fUpper; else priv->fTarget = priv->fLower; if (priv->fTarget > priv->fMax) priv->fTarget = priv->fMax; if (priv->fTarget < 0) priv->fTarget = 0.0; priv->bInternal = true; iRet = EVCDrive(priv->controller, pCon, priv->fTarget); priv->bInternal = false; if(iRet) SCSendOK(pCon); return iRet; } if (strcasecmp("off", argv[1]) == 0) { int iRet; if(!SCMatchRights(pCon,usUser)) return 0; priv->fTarget = 0.0; priv->bInternal = true; iRet = EVCDrive(priv->controller, pCon, priv->fTarget); priv->bInternal = false; if(iRet) SCSendOK(pCon); return iRet; } if (strcasecmp("upper", argv[1]) == 0) { char rsp[CMDLEN]; if (argc > 2) { if (!SCMatchRights(pCon, usUser)) return 0; if (priv->isLocked && !SCMatchRights(pCon, usMugger)) { SCWrite(pCon, "object is locked", eError); return 0; } else { float value = atof(argv[2]); if (value >= 0.0 && value <= priv->fMax) { priv->fUpper = value; } else { SCWrite(pCon, "upper value must be between 0.0 and ", eError); return 0; } } } snprintf(rsp, CMDLEN, "%s.upper = %8.2f", priv->name, priv->fUpper); SCWrite(pCon, rsp, eValue); return 1; } if (strcasecmp("lower", argv[1]) == 0) { char rsp[CMDLEN]; if (argc > 2) { if (!SCMatchRights(pCon, usUser)) return 0; if (priv->isLocked && !SCMatchRights(pCon, usMugger)) { SCWrite(pCon, "object is locked", eError); return 0; } else { float value = atof(argv[2]); if (value >= 0.0 && value <= priv->fMax) { priv->fLower = value; } else { SCWrite(pCon, "lower value must be between 0.0 and ", eError); return 0; } } } snprintf(rsp, CMDLEN, "%s.lower = %8.2f", priv->name, priv->fLower); SCWrite(pCon, rsp, eValue); return 1; } if (strcasecmp("max", argv[1]) == 0) { char rsp[CMDLEN]; if (argc > 2) { if (!SCMatchRights(pCon, usUser)) return 0; if (priv->isLocked) { SCWrite(pCon, "object is locked", eError); return 0; } else { float value = atof(argv[2]); if (value >= 0.0 && value <= MY_ABSOLUTE_MAXIMUM) { priv->fMax = value; } else { char line[CMDLEN]; snprintf(line, CMDLEN, "max must be between 0.0 and %.0f", MY_ABSOLUTE_MAXIMUM); SCWrite(pCon, line, eError); return 0; } } } snprintf(rsp, CMDLEN, "%s.max = %8.2f", priv->name, priv->fMax); SCWrite(pCon, rsp, eValue); return 1; } if (strcasecmp("debug", argv[1]) == 0 && SCMatchRights(pCon, usMugger)) { char rsp[CMDLEN]; if (argc > 2) { int debug = atoi(argv[2]); if (debug != 0) priv->fsm.debug = true; else priv->fsm.debug = false; } snprintf(rsp, CMDLEN, "%s.debug = %d", priv->name, priv->fsm.debug ? 1 : 0); SCWrite(pCon, rsp, eValue); return 1; } if (strcasecmp("rate", argv[1]) == 0) { char rsp[CMDLEN]; if (argc > 2) { if (!SCMatchRights(pCon, usUser)) return 0; if (priv->isLocked && !SCMatchRights(pCon, usMugger)) { SCWrite(pCon, "object is locked", eError); return 0; } else { float value = atof(argv[2]); if (value >= MY_MINIMUM_RATE && value <= MY_MAXIMUM_RATE) { priv->fRate = value; char cmd[CMDLEN]; int cmd_len; char rsp[CMDLEN]; int rsp_len; cmd_len = snprintf(cmd, CMDLEN, "V%d=%0.0f\r\n", priv->iControl, priv->fRate); rsp_len = CMDLEN; NHQ_SendReceive(priv, cmd, cmd_len, rsp, &rsp_len); } else { char line[CMDLEN]; snprintf(line, CMDLEN, "rate must be between %.0f and %.0f", MY_MINIMUM_RATE, MY_MAXIMUM_RATE); SCWrite(pCon, line, eError); return 0; } } } snprintf(rsp, CMDLEN, "%s.rate = %8.2f", priv->name, priv->fRate); SCWrite(pCon, rsp, eValue); return 1; } if (strcasecmp("reset", argv[1]) == 0) { char line[CMDLEN]; if (!SCMatchRights(pCon, usUser)) return 0; read_status(priv, line, sizeof(line)); SCWrite(pCon, line, eValue); /* restart state machine */ fsm_change_state(&priv->fsm, NHQState_Unknown); while (priv->fsm.myState == NHQState_Unknown) { pTaskMan pTasker = GetTasker(); TaskYield(pTasker); } /* give feedback */ snprintf(line, CMDLEN, "%s.reset = 1", priv->name); SCWrite(pCon, line, eValue); return 1; } if (strcasecmp("lock", argv[1]) == 0) { char rsp[CMDLEN]; if (!SCMatchRights(pCon, usUser)) return 0; priv->isLocked = 1; snprintf(rsp, CMDLEN, "%s.lock = %d", priv->name, priv->isLocked); SCWrite(pCon, rsp, eValue); return 1; } if (strcasecmp("unlock", argv[1]) == 0 && SCMatchRights(pCon, usMugger) && priv->fsm.debug) { char rsp[CMDLEN]; priv->isLocked = 0; snprintf(rsp, CMDLEN, "%s.lock = %d", priv->name, priv->isLocked); SCWrite(pCon, rsp, eValue); return 1; } if (strcasecmp("list", argv[1]) == 0) { int iRet; iRet = EVControlWrapper(pCon,pSics,pData,argc,argv); if (iRet) { char rsp[CMDLEN]; snprintf(rsp, CMDLEN, "%s.lower = %8.2f", priv->name, priv->fLower); SCWrite(pCon, rsp, eValue); snprintf(rsp, CMDLEN, "%s.upper = %8.2f", priv->name, priv->fUpper); SCWrite(pCon, rsp, eValue); snprintf(rsp, CMDLEN, "%s.max = %8.2f", priv->name, priv->fMax); SCWrite(pCon, rsp, eValue); snprintf(rsp, CMDLEN, "%s.rate = %8.2f", priv->name, priv->fRate); SCWrite(pCon, rsp, eValue); } if(strcasecmp("state", argv[1]) == 0) { char line[132]; snprintf(line, 132, "%s.state = %s(%d) (timer=%s)", priv->name, state_name(priv->fsm.myState), priv->fsm.mySubState, priv->state_timer ? "active" : "inactive"); SCWrite(pCon, line, eValue); return 1; } return iRet; } if (strcasecmp("status", argv[1]) == 0) { char line[CMDLEN]; read_status(priv, line, sizeof(line)); SCWrite(pCon, line, eValue); return 1; } return EVControlWrapper(pCon,pSics,pData,argc,argv); }