1386 lines
38 KiB
C
1386 lines
38 KiB
C
/*-------------------------------------------------------------------------
|
|
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 <stdlib.h>
|
|
#include <stdbool.h>
|
|
#include <assert.h>
|
|
#include <string.h>
|
|
#include <ctype.h>
|
|
#include <math.h>
|
|
#include <netinet/tcp.h>
|
|
|
|
#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 "<null_state>";
|
|
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 "<unknown_state>";
|
|
}
|
|
|
|
static const char* event_name(pEvtEvent event, char* text, int length)
|
|
{
|
|
char line[1024];
|
|
if (event == NULL)
|
|
return "<null_event>";
|
|
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, "<unknown_event>");
|
|
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 <maximum>", 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 <maximum>", 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);
|
|
}
|
|
|