Squashed commit of the following: commit 42fb7d3cde591d40060cc740ccbc47f1ae7a5a50 Author: Douglas Clowes <dcl@ansto.gov.au> Date: Tue Aug 26 13:31:11 2014 +1000 Get the MODBUS_AP working commit da785c1434a04c4186d4174eb2dfbaefc850c8e7 Author: Douglas Clowes <dcl@ansto.gov.au> Date: Mon Aug 25 18:01:50 2014 +1000 Bring Modbus protocol closer to Huber, Knauer and Omron commit ef06ed7b6911cb49b35c19fe73e55f7c57cfd049 Author: Douglas Clowes <dcl@ansto.gov.au> Date: Mon Aug 25 18:01:18 2014 +1000 Make Huber, Knauer and Omron protocols more aligned (diffable) commit 3ef1bb06b3f865502ad7dffc4bf5dba4814d9334 Author: Douglas Clowes <dcl@ansto.gov.au> Date: Fri Aug 22 17:47:50 2014 +1000 Get the Huber and Knauer protocols to be more alike commit 2c9932e83f6735e894278648afdcadece654d43b Author: Douglas Clowes <dcl@ansto.gov.au> Date: Fri Aug 22 17:12:31 2014 +1000 Clean up the Knauer dual-mode protocol and refactor commit 333300b19b0e61916e261300ac6ae2b6bab5df09 Author: Douglas Clowes <dcl@ansto.gov.au> Date: Thu Aug 21 15:38:39 2014 +1000 Get the Knauer dual-mode protocol working(-ish) commit b1f9d82f1b9eb8a1ff54694adc3482984b0d3d72 Author: Douglas Clowes <dcl@ansto.gov.au> Date: Thu Aug 21 15:37:44 2014 +1000 Make private functions static (and not duplicated) commit 0b077414eef9e4351956a2b971d7751cced0d3cd Author: Douglas Clowes <dcl@ansto.gov.au> Date: Thu Aug 21 12:46:10 2014 +1000 Knauer moving toward dual protocol commit 13199bea38a1595ce06923e83474b738b10db94d Author: Douglas Clowes <dcl@ansto.gov.au> Date: Thu Aug 21 12:42:48 2014 +1000 Restructure default sendCommand processing in asyncqueue commit 99a8ea3174ca0636503b0ce0cdb6016790315558 Author: Douglas Clowes <dcl@ansto.gov.au> Date: Thu Aug 21 09:48:50 2014 +1000 Add a Modbus Protocol handler derived from sct_tcpmodbus commit 3adf49fb7c8402c8260a0bb20729d551ac88537b Author: Douglas Clowes <dcl@ansto.gov.au> Date: Thu Aug 21 09:43:54 2014 +1000 Leave the free of private data to the asyncqueue mechanism
1438 lines
42 KiB
C
1438 lines
42 KiB
C
/*-------------------------------------------------------------------------
|
|
O R H V P S
|
|
|
|
Support for Oak Ridge High Voltage Power Supply for SICS.
|
|
|
|
The meaning and working of the functions defined is as desribed for a
|
|
general environment controller.
|
|
|
|
Douglas Clowes, December 2007
|
|
|
|
Copyright: site_ansto/doc/Copyright.txt
|
|
|
|
----------------------------------------------------------------------------
|
|
*/
|
|
|
|
#include "orhvps.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>
|
|
|
|
#define CMDLEN 132
|
|
#define MY_ABSOLUTE_MAXIMUM (2600.0)
|
|
#define MY_MINIMUM_RATE (1.0)
|
|
#define MY_MAXIMUM_RATE (40.0)
|
|
/* ERROR CODES */
|
|
#define ORHVPS_ERR_NONE (0)
|
|
#define ORHVPS_ERR_LOCKED (-1)
|
|
#define ORHVPS_ERR_RANGE (-2)
|
|
|
|
/* Device Driver Control Structure */
|
|
struct orhvps_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 iValue; /* integer value from controller (0..63) */
|
|
int iPeriod; /* integer step-rate period in milliseconds */
|
|
};
|
|
|
|
typedef struct orhvps_s ORHVPSDriv, *pORHVPSDriv;
|
|
|
|
/* Functions */
|
|
static int ORHVPSGetValue( pEVDriver self, float* fPos);
|
|
static int ORHVPSSetValue( pEVDriver self, float fPos);
|
|
static int ORHVPSSend(pEVDriver self, char *pCommand, char *pReply, int iLen);
|
|
static int ORHVPSError(pEVDriver self, int *iCode, char *error, int iErrLen);
|
|
static int ORHVPSFix(pEVDriver self, int iError);
|
|
static int ORHVPSInit(pEVDriver self);
|
|
static int ORHVPSClose(pEVDriver self);
|
|
static void ORHVPSKillPrivate(void *pData);
|
|
static void ORHVPSNotify(void* context, int event);
|
|
|
|
static int get_period(float max, float rate) {
|
|
return (int) roundf(1000 * (max / 63.0) / rate);
|
|
}
|
|
|
|
/**
|
|
* \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 ORHV_SendCmd(pORHVPSDriv 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 ORHV_SendReceive(pORHVPSDriv priv,
|
|
char *cmd,
|
|
int cmd_len,
|
|
char* reply,
|
|
int *rep_len) {
|
|
int status;
|
|
|
|
if (!*cmd)
|
|
{
|
|
*reply='\0';
|
|
*rep_len=0;
|
|
return FAILURE;
|
|
}
|
|
|
|
// Ordela 21000 firmware appears to send NAK (15h) code at regular intervals
|
|
// when there is no comms (time out)? To prevent this from interfering with
|
|
// the transaction, perform a dummy transaction first.
|
|
// Reading the jumper settings should be harmless (but in case of true
|
|
// disconnect we are going to get N * timeout - should not normally occur)
|
|
|
|
int max_rep_len=*rep_len; // Value of arguemnt on entry is the max receive buffer len
|
|
AsyncUnitTransact(priv->asyncUnit, "Jz", 2, reply, rep_len);
|
|
*rep_len=max_rep_len;
|
|
// Now do the real transaction. */
|
|
int max_retries=1; // Retries should be built into the async queue object, so set to 1. In practice this works just fine.
|
|
do
|
|
{
|
|
*rep_len=max_rep_len;
|
|
status = AsyncUnitTransact(priv->asyncUnit, cmd, cmd_len, reply, rep_len);
|
|
} while(--max_retries&&status!=1);
|
|
|
|
if (status != 1) {
|
|
return FAILURE;
|
|
}
|
|
|
|
return OKOK;
|
|
}
|
|
|
|
/* State Functions */
|
|
static void ORHVState_Unknown(pStateMachine sm, pEvtEvent event);
|
|
static void ORHVState_Idle(pStateMachine sm, pEvtEvent event);
|
|
static void ORHVState_Raising(pStateMachine sm, pEvtEvent event);
|
|
static void ORHVState_Lowering(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 == ORHVState_Unknown) return "ORHVState_Unknown";
|
|
if (func == ORHVState_Idle) return "ORHVState_Idle";
|
|
if (func == ORHVState_Raising) return "ORHVState_Raising";
|
|
if (func == ORHVState_Lowering) return "ORHVState_Lowering";
|
|
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;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* \brief parse the HV response for several consecutive states [bot, top]
|
|
*
|
|
* It captures the value in the first state and confirms the value in the last
|
|
* state. If any error occurs it returns -1 and resets the substate to bot.
|
|
* When the same value has been read for each substate from [bot, top] it
|
|
* returns 0 to confirm the value. As each value is confirmed it returns +1.
|
|
*
|
|
* \param sm pointer to the state machine
|
|
* \param pCmd pointer to the command and response
|
|
* \param bot first state (capture value)
|
|
* \param top last state confirm value
|
|
*
|
|
* \return -1 Error
|
|
* 0 sequence correct so far
|
|
* +1 sequence complete, can use the value
|
|
*/
|
|
static int getProcess(pStateMachine sm, pAsyncTxn pCmd, int bot, int top) {
|
|
pEVDriver driv = (pEVDriver) sm->context;
|
|
pORHVPSDriv priv = (pORHVPSDriv) driv->pPrivate;
|
|
int iRet = 1; /* Normal return value */
|
|
if (pCmd && pCmd->inp_idx == 3 && pCmd->inp_buf[0] == 'H' && pCmd->inp_buf[2] == 'z') {
|
|
int value = (pCmd->inp_buf[1]);
|
|
if (value >= 0 && value <= 63) {
|
|
if (sm->mySubState == bot) {
|
|
priv->iValue = value;
|
|
++sm->mySubState;
|
|
}
|
|
else if (value != priv->iValue) {
|
|
/* TODO log changed value */
|
|
sm->mySubState = bot;
|
|
iRet = -1; /* Error return value */
|
|
}
|
|
else if (sm->mySubState == top) {
|
|
/* final check passed, do calcs and transition */
|
|
priv->fValue = priv->iValue * (priv->fMax / 63.0);
|
|
++sm->mySubState;
|
|
return 0; /* Completed return value */
|
|
}
|
|
else {
|
|
/* recheck in next state */
|
|
++sm->mySubState;
|
|
}
|
|
}
|
|
else {
|
|
/* TODO log invalid value */
|
|
sm->mySubState = bot;
|
|
iRet = -1; /* Error return value */
|
|
}
|
|
}
|
|
else {
|
|
/* TODO log syntax error */
|
|
sm->mySubState = bot;
|
|
iRet = -1; /* Error return value */
|
|
}
|
|
return iRet;
|
|
}
|
|
|
|
/* State Functions */
|
|
|
|
/*
|
|
* Unknown State
|
|
*
|
|
* Handle initialisation and reset operations
|
|
*/
|
|
static void ORHVState_Unknown(pStateMachine sm, pEvtEvent event) {
|
|
pEVDriver driv = (pEVDriver) sm->context;
|
|
pORHVPSDriv priv = (pORHVPSDriv) driv->pPrivate;
|
|
switch (event->event_type) {
|
|
case eStateEvent:
|
|
if (priv->state_timer)
|
|
NetWatchRemoveTimer(priv->state_timer);
|
|
priv->state_timer = NULL;
|
|
priv->bRunFlag = false;
|
|
ORHV_SendCmd(priv, "vz", 2, fsm_msg_callback);
|
|
sm->mySubState = 1;
|
|
return;
|
|
case eTimerEvent:
|
|
priv->state_timer = NULL;
|
|
return;
|
|
case eMessageEvent:
|
|
do {
|
|
pAsyncTxn pCmd = event->event.msg.cmd;
|
|
pCmd->inp_buf[pCmd->inp_idx] = '\0';
|
|
if (sm->mySubState == 1) {
|
|
/* Version Request */
|
|
char* p = strchr(pCmd->inp_buf, 'z');
|
|
if (p) {
|
|
char line[132];
|
|
*p = '\0';
|
|
sprintf(line, "Version: %s", pCmd->inp_buf);
|
|
SICSLogWrite(line, eLog);
|
|
}
|
|
ORHV_SendCmd(priv, "Hz", 2, fsm_msg_callback);
|
|
sm->mySubState = 2;
|
|
return;
|
|
}
|
|
if (sm->mySubState >= 2 && sm->mySubState <= 5) {
|
|
/* HV Get Request */
|
|
int iRet;
|
|
iRet = getProcess(sm, pCmd, 2, 5);
|
|
if (iRet == 0) {
|
|
priv->fTarget = priv->fValue;
|
|
fsm_change_state(sm, ORHVState_Idle);
|
|
return;
|
|
}
|
|
ORHV_SendCmd(priv, "Hz", 2, fsm_msg_callback);
|
|
return;
|
|
}
|
|
} while (0);
|
|
return;
|
|
case eCommandEvent:
|
|
return;
|
|
case eTimeoutEvent:
|
|
ORHV_SendCmd(priv, "vz", 2, fsm_msg_callback);
|
|
sm->mySubState = 1;
|
|
return;
|
|
}
|
|
return;
|
|
}
|
|
|
|
/*
|
|
* Idle State
|
|
*
|
|
* Just monitoring what's going on
|
|
* and waiting to be told to run somewhere
|
|
*/
|
|
static void ORHVState_Idle(pStateMachine sm, pEvtEvent event){
|
|
pEVDriver driv = (pEVDriver) sm->context;
|
|
pORHVPSDriv priv = (pORHVPSDriv) driv->pPrivate;
|
|
int newVal;
|
|
switch (event->event_type) {
|
|
case eStateEvent:
|
|
if (priv->state_timer)
|
|
NetWatchRemoveTimer(priv->state_timer);
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
200,
|
|
fsm_tmr_callback, sm);
|
|
sm->mySubState = 1;
|
|
return;
|
|
case eTimerEvent:
|
|
priv->state_timer = NULL;
|
|
if (sm->mySubState == 6) {
|
|
if (priv->bRunFlag) {
|
|
priv->bRunFlag = false;
|
|
newVal = roundf(priv->fTarget / (priv->fMax / 63.0));
|
|
if (newVal > priv->iValue) {
|
|
fsm_change_state(sm, ORHVState_Raising);
|
|
return;
|
|
}
|
|
if (newVal < priv->iValue) {
|
|
fsm_change_state(sm, ORHVState_Lowering);
|
|
return;
|
|
}
|
|
}
|
|
/*
|
|
* There was either no commanded to move,
|
|
* on no need to move,
|
|
* so start another sequence
|
|
*/
|
|
sm->mySubState = 1;
|
|
}
|
|
ORHV_SendCmd(priv, "Hz", 2, fsm_msg_callback);
|
|
return;
|
|
case eMessageEvent:
|
|
do {
|
|
pAsyncTxn pCmd = event->event.msg.cmd;
|
|
if (sm->mySubState >= 1 && sm->mySubState <= 5) {
|
|
/* HV Get Request */
|
|
int iRet;
|
|
iRet = getProcess(sm, pCmd, 1, 5);
|
|
if (iRet == 0) { /* final value OK */
|
|
sm->mySubState = 6;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
100, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
}
|
|
if (iRet < 0) { /* error, start again */
|
|
sm->mySubState = 1;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
100, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
}
|
|
/* normal, just ask again for confirmation */
|
|
ORHV_SendCmd(priv, "Hz", 2, fsm_msg_callback);
|
|
return;
|
|
}
|
|
} while (0);
|
|
sm->mySubState = 1;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
100, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
case eCommandEvent:
|
|
return;
|
|
case eTimeoutEvent:
|
|
return;
|
|
}
|
|
return;
|
|
}
|
|
|
|
/* Raising State
|
|
*
|
|
* Increasing controlled value
|
|
*/
|
|
static void ORHVState_Raising(pStateMachine sm, pEvtEvent event){
|
|
pEVDriver driv = (pEVDriver) sm->context;
|
|
pORHVPSDriv priv = (pORHVPSDriv) driv->pPrivate;
|
|
int newVal;
|
|
switch (event->event_type) {
|
|
case eStateEvent:
|
|
if (priv->state_timer)
|
|
NetWatchRemoveTimer(priv->state_timer);
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
200,
|
|
fsm_tmr_callback, sm);
|
|
sm->mySubState = 1;
|
|
return;
|
|
case eTimerEvent:
|
|
priv->state_timer = NULL;
|
|
if (sm->mySubState == 1) {
|
|
ORHV_SendCmd(priv, "Hz", 2, fsm_msg_callback);
|
|
return;
|
|
}
|
|
if (sm->mySubState == 6) {
|
|
char cmd[3];
|
|
cmd[0] = 'h';
|
|
cmd[1] = priv->iValue < 63 ? priv->iValue + 1 : priv->iValue;
|
|
cmd[2] = 'z';
|
|
ORHV_SendCmd(priv, cmd, 3, fsm_msg_callback);
|
|
return;
|
|
}
|
|
sm->mySubState = 1;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
100, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
case eMessageEvent:
|
|
do {
|
|
pAsyncTxn pCmd = event->event.msg.cmd;
|
|
if (sm->mySubState >= 1 && sm->mySubState <= 5) {
|
|
/* HV Get Request */
|
|
int iRet;
|
|
iRet = getProcess(sm, pCmd, 1, 5);
|
|
if (iRet == 0) {
|
|
newVal = roundf(priv->fTarget / (priv->fMax / 63.0));
|
|
if (newVal == priv->iValue) {
|
|
fsm_change_state(sm, ORHVState_Idle);
|
|
return;
|
|
}
|
|
if (newVal < priv->iValue) {
|
|
fsm_change_state(sm, ORHVState_Lowering);
|
|
return;
|
|
}
|
|
sm->mySubState = 6;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
100, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
}
|
|
if (iRet < 0) {
|
|
sm->mySubState = 1;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
100, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
}
|
|
ORHV_SendCmd(priv, "Hz", 2, fsm_msg_callback);
|
|
return;
|
|
}
|
|
if (sm->mySubState == 6) {
|
|
/* HV Set Request */
|
|
if (*pCmd->inp_buf == 0x06) {
|
|
/* TODO: ACK */
|
|
}
|
|
else if (*pCmd->inp_buf == 0x15) {
|
|
/* TODO: NAK */
|
|
}
|
|
else {
|
|
/* TODO: ??? */
|
|
}
|
|
sm->mySubState = 1;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
priv->iPeriod, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
}
|
|
} while (0);
|
|
sm->mySubState = 1;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
100, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
case eCommandEvent:
|
|
return;
|
|
case eTimeoutEvent:
|
|
sm->mySubState = 1;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
100, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
}
|
|
return;
|
|
}
|
|
|
|
/* Lowering State
|
|
*
|
|
* Decreasing controlled value
|
|
*/
|
|
static void ORHVState_Lowering(pStateMachine sm, pEvtEvent event){
|
|
pEVDriver driv = (pEVDriver) sm->context;
|
|
pORHVPSDriv priv = (pORHVPSDriv) driv->pPrivate;
|
|
char cmd[3];
|
|
int newVal;
|
|
switch (event->event_type) {
|
|
case eStateEvent:
|
|
if (priv->state_timer)
|
|
NetWatchRemoveTimer(priv->state_timer);
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
200,
|
|
fsm_tmr_callback, sm);
|
|
sm->mySubState = 1;
|
|
return;
|
|
case eTimerEvent:
|
|
priv->state_timer = NULL;
|
|
if (sm->mySubState == 1) {
|
|
ORHV_SendCmd(priv, "Hz", 2, fsm_msg_callback);
|
|
return;
|
|
}
|
|
if (sm->mySubState == 6) {
|
|
cmd[0] = 'h';
|
|
cmd[1] = priv->iValue > 0 ? priv->iValue - 1 : priv->iValue;
|
|
cmd[2] = 'z';
|
|
ORHV_SendCmd(priv, cmd, 3, fsm_msg_callback);
|
|
return;
|
|
}
|
|
sm->mySubState = 1;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
100, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
case eMessageEvent:
|
|
do {
|
|
pAsyncTxn pCmd = event->event.msg.cmd;
|
|
pCmd->inp_buf[pCmd->inp_idx] = '\0';
|
|
if (sm->mySubState >= 1 && sm->mySubState <= 5) {
|
|
/* HV Get Request */
|
|
int iRet;
|
|
iRet = getProcess(sm, pCmd, 1, 5);
|
|
if (iRet == 0) {
|
|
newVal = roundf(priv->fTarget / (priv->fMax / 63.0));
|
|
if (newVal == priv->iValue) {
|
|
fsm_change_state(sm, ORHVState_Idle);
|
|
return;
|
|
}
|
|
if (newVal > priv->iValue) {
|
|
fsm_change_state(sm, ORHVState_Raising);
|
|
return;
|
|
}
|
|
sm->mySubState = 6;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
100, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
}
|
|
if (iRet < 0) {
|
|
sm->mySubState = 1;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
100, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
}
|
|
ORHV_SendCmd(priv, "Hz", 2, fsm_msg_callback);
|
|
return;
|
|
}
|
|
if (sm->mySubState == 6) { /* HV Set Request */
|
|
if (*pCmd->inp_buf == 0x06) {
|
|
/* TODO: ACK */
|
|
}
|
|
else if (*pCmd->inp_buf == 0x15) {
|
|
/* TODO: NAK */
|
|
}
|
|
else {
|
|
/* TODO: ??? */
|
|
}
|
|
sm->mySubState = 1;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
priv->iPeriod, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
}
|
|
} while (0);
|
|
sm->mySubState = 1;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
100, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
case eCommandEvent:
|
|
return;
|
|
case eTimeoutEvent:
|
|
sm->mySubState = 1;
|
|
NetWatchRegisterTimer(&priv->state_timer,
|
|
100, /* TODO*/
|
|
fsm_tmr_callback, sm);
|
|
return;
|
|
}
|
|
return;
|
|
}
|
|
|
|
|
|
/*
|
|
* Return the current value to SICS
|
|
*/
|
|
static int ORHVPSGetValue( pEVDriver self, float* fPos) {
|
|
pORHVPSDriv priv = NULL;
|
|
int newVal;
|
|
assert(self);
|
|
assert(self->pPrivate);
|
|
priv = (pORHVPSDriv) self->pPrivate;
|
|
newVal = roundf(priv->fTarget / (priv->fMax / 63.0));
|
|
if (newVal == priv->iValue)
|
|
*fPos = priv->fTarget;
|
|
else
|
|
*fPos = priv->fValue;
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
* Set the current value from SICS
|
|
*/
|
|
static int ORHVPSSetValue( pEVDriver self, float fPos) {
|
|
|
|
char reply[CMDLEN];
|
|
int rep_len = CMDLEN;
|
|
|
|
pORHVPSDriv priv = NULL;
|
|
assert(self);
|
|
assert(self->pPrivate);
|
|
priv = (pORHVPSDriv) self->pPrivate;
|
|
if (priv->isLocked && !priv->bInternal) {
|
|
priv->iErrorCode = ORHVPS_ERR_LOCKED;
|
|
return 0;
|
|
}
|
|
if (fPos < 0.0 || fPos > priv->fMax) {
|
|
priv->iErrorCode = ORHVPS_ERR_RANGE;
|
|
return 0;
|
|
}
|
|
|
|
AsyncUnitTransact(priv->asyncUnit, "Jz", 2, reply, &rep_len);
|
|
|
|
priv->fTarget = fPos;
|
|
priv->bRunFlag = true;
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
* Send a command from SICS
|
|
*/
|
|
static int ORHVPSSend(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;
|
|
if (pCommand[i] == '\\') {
|
|
k = 0;
|
|
if (isxdigit(pCommand[i+1]) && isxdigit(pCommand[i+2])) {
|
|
if (pCommand[i+1] >= '0' && pCommand[i+1] <= '9')
|
|
k = (pCommand[i+1] - '0') << 4;
|
|
else if (pCommand[i+1] >= 'a' && pCommand[i+1] <= 'f')
|
|
k = (pCommand[i+1] - 'a' + 10) << 4;
|
|
else if (pCommand[i+1] >= 'A' && pCommand[i+1] <= 'F')
|
|
k = (pCommand[i+1] - 'A' + 10) << 4;
|
|
if (pCommand[i+2] >= '0' && pCommand[i+2] <= '9')
|
|
k += (pCommand[i+2] - '0');
|
|
else if (pCommand[i+2] >= 'a' && pCommand[i+2] <= 'f')
|
|
k += (pCommand[i+2] - 'a' + 10);
|
|
else if (pCommand[i+2] >= 'A' && pCommand[i+2] <= 'F')
|
|
k += (pCommand[i+2] - 'A' + 10);
|
|
i += 2;
|
|
}
|
|
}
|
|
else
|
|
k = pCommand[i];
|
|
if (idx < CMDLEN)
|
|
cmd[idx++] = k;
|
|
}
|
|
if (idx < CMDLEN)
|
|
cmd[idx] = '\0';
|
|
cmd_len = idx;
|
|
rsp_len = CMDLEN;
|
|
int orhvsr_status=ORHV_SendReceive(self->pPrivate, cmd, cmd_len, rsp, &rsp_len);
|
|
idx = 0;
|
|
for (i = 0; i < rsp_len && idx < iLen - 1; ++i) {
|
|
if (rsp[i] < 32 || rsp[i] > 126) {
|
|
idx+=sprintf(&pReply[idx], "%02Xh", rsp[i]);
|
|
}
|
|
else if (idx < iLen)
|
|
pReply[idx++] = rsp[i];
|
|
}
|
|
if (idx < iLen)
|
|
pReply[idx++] = '\0';
|
|
return 1;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
/*
|
|
* SICS error handler
|
|
*/
|
|
static int ORHVPSError(pEVDriver self, int *iCode, char *error, int iErrLen) {
|
|
pORHVPSDriv priv = (pORHVPSDriv) self->pPrivate;
|
|
*iCode = priv->iErrorCode;
|
|
switch (priv->iErrorCode) {
|
|
case ORHVPS_ERR_RANGE:
|
|
strncpy(error,"Value out of range",iErrLen);
|
|
break;
|
|
case ORHVPS_ERR_LOCKED:
|
|
strncpy(error,"Object is locked",iErrLen);
|
|
break;
|
|
default:
|
|
strncpy(error,"TODO Error Messages",iErrLen);
|
|
break;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
* SICS fix handler
|
|
*/
|
|
static int ORHVPSFix(pEVDriver self, int iError) {
|
|
/* TODO */
|
|
return DEVFAULT;
|
|
}
|
|
static int ORHVPSInit(pEVDriver self) {
|
|
/* TODO */
|
|
return 1;
|
|
}
|
|
static int ORHVPSClose(pEVDriver self) {
|
|
/* TODO */
|
|
return -1;
|
|
}
|
|
static void ORHVPSKillPrivate(void *pData) {
|
|
pORHVPSDriv pMe = (pORHVPSDriv) 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;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Protocol receive character - characater by character
|
|
*/
|
|
static int ORHVPS_Rx(pAsyncProtocol p, pAsyncTxn ctx, int rxchar) {
|
|
int iRet = 1;
|
|
pAsyncTxn myCmd = (pAsyncTxn) ctx;
|
|
|
|
switch (myCmd->txn_state) {
|
|
case 0: /* first character */
|
|
if (rxchar == 0x06) { /* ACK */
|
|
/* normal prompt */
|
|
myCmd->txn_state = 99;
|
|
myCmd->txn_status = ATX_COMPLETE;
|
|
}
|
|
else if (rxchar == 0x15) { /* NAK */
|
|
myCmd->txn_state = 99;
|
|
myCmd->txn_status = ATX_COMPLETE;
|
|
}
|
|
else {
|
|
/* normal data */
|
|
myCmd->txn_state = 1;
|
|
}
|
|
/* note fallthrough */
|
|
case 1: /* receiving reply */
|
|
if (myCmd->inp_idx < myCmd->inp_len)
|
|
myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
|
|
if (rxchar == 'z')
|
|
myCmd->txn_state = 99;
|
|
break;
|
|
}
|
|
if (myCmd->txn_state == 99) {
|
|
iRet = 0;
|
|
}
|
|
if (iRet == 0) { /* end of command */
|
|
return AQU_POP_CMD;
|
|
}
|
|
return iRet;
|
|
}
|
|
|
|
/*
|
|
* AsyncUnit Notify Callback
|
|
*/
|
|
static void ORHVPSNotify(void* context, int event)
|
|
{
|
|
/* TODO */
|
|
#if 0
|
|
pEVDriver self = (pEVDriver) context;
|
|
char line[132];
|
|
|
|
switch (event) {
|
|
case AQU_DISCONNECT:
|
|
snprintf(line, 132, "Disconnect on Device '%s'", self->name);
|
|
SICSLogWrite(line, eLog);
|
|
/* TODO: disconnect */
|
|
break;
|
|
case AQU_RECONNECT:
|
|
snprintf(line, 132, "Reconnect on Device '%s'", self->name);
|
|
SICSLogWrite(line, eLog);
|
|
/* TODO: reconnect */
|
|
if (self->has_fsm) {
|
|
/* Reset the state machine */
|
|
if (self->state_timer)
|
|
NetWatchRemoveTimer(self->state_timer);
|
|
self->state_timer = 0;
|
|
change_state(self, DMCState_Unknown);
|
|
/* Schedule a timer event as soon as possible */
|
|
NetWatchRegisterTimer(&self->state_timer,
|
|
0,
|
|
state_tmr_callback, self);
|
|
}
|
|
break;
|
|
}
|
|
#endif
|
|
return;
|
|
}
|
|
|
|
/*
|
|
* AsyncProtocol Event callback
|
|
*/
|
|
static int ORHVPS_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 int ORHVPS_PrepareTxn(pAsyncProtocol p, pAsyncTxn txn, const char* cmd, int cmd_len, int rsp_len) {
|
|
txn->out_buf = (char*) malloc(cmd_len);
|
|
if (txn->out_buf == NULL) {
|
|
SICSLogWrite("ERROR: Out of memory in ORHVPS_PrepareTxn", eError);
|
|
return 0;
|
|
}
|
|
memcpy(txn->out_buf, cmd, cmd_len);
|
|
txn->out_len = cmd_len;
|
|
return 1;
|
|
}
|
|
|
|
static pAsyncProtocol ORHVPS_Protocol = NULL;
|
|
|
|
/*
|
|
* Protocol Initialisation
|
|
*/
|
|
void ORHVPSInitProtocol(SicsInterp *pSics) {
|
|
if (ORHVPS_Protocol == NULL) {
|
|
ORHVPS_Protocol = AsyncProtocolCreate(pSics, "ORHVPS", NULL, NULL);
|
|
ORHVPS_Protocol->sendCommand = NULL;
|
|
ORHVPS_Protocol->handleInput = ORHVPS_Rx;
|
|
ORHVPS_Protocol->handleEvent = ORHVPS_Ev;
|
|
ORHVPS_Protocol->prepareTxn = ORHVPS_PrepareTxn;
|
|
ORHVPS_Protocol->killPrivate = NULL;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Device Factory
|
|
*/
|
|
pEVDriver CreateORHVPSDriver(int argc, char *argv[])
|
|
{
|
|
pEVDriver self = NULL;
|
|
pORHVPSDriv priv = NULL;
|
|
|
|
if (argc < 1)
|
|
return NULL;
|
|
self = CreateEVDriver(argc, argv);
|
|
if (!self)
|
|
return NULL;
|
|
|
|
priv = (pORHVPSDriv)malloc(sizeof(ORHVPSDriv));
|
|
if(!priv) {
|
|
DeleteEVDriver(self);
|
|
return NULL;
|
|
}
|
|
memset(priv,0,sizeof(ORHVPSDriv));
|
|
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, ORHVPSNotify);
|
|
|
|
/* initialise function pointers */
|
|
self->SetValue = ORHVPSSetValue;
|
|
self->GetValue = ORHVPSGetValue;
|
|
self->Send = ORHVPSSend;
|
|
self->GetError = ORHVPSError;
|
|
self->TryFixIt = ORHVPSFix;
|
|
self->Init = ORHVPSInit;
|
|
self->Close = ORHVPSClose;
|
|
|
|
self->pPrivate = priv;
|
|
self->KillPrivate = ORHVPSKillPrivate;
|
|
|
|
priv->fsm.context = self;
|
|
priv->fsm.state_name = state_name;
|
|
priv->fsm.event_name = event_name;
|
|
priv->name = strdup(argv[0]);
|
|
// MJL 9/08 Modified the defaults to cater for the current Ordela 21000N detector on SANS
|
|
// (max HV setting 2600V adjusted via pot). For the spare Ordela detector, the max voltage
|
|
// is pot-adjusted to 2400V as we have been advised that this is the max safe operating voltage.
|
|
// Since this driver uses the fUpper for scaling, in order to prevent any potential damage to the
|
|
// detectors we set the default to the highest of the two. (i.e. if .upper is not initialized
|
|
// in the SICS configuration, it is higher than or equal to the actual detector voltage maximum
|
|
// and this results in the set voltage being equal to or lower than expected - not higher).
|
|
priv->fMax = 2600.0;
|
|
priv->fRate = 10.0;
|
|
priv->fLower = 0.0; // fLower is 'low' safe voltage for driving - nominally 800V - but leave default at 0V for safety
|
|
priv->fUpper = 2350.0;
|
|
priv->iPeriod = get_period(priv->fMax, priv->fRate);
|
|
priv->bRunFlag = false;
|
|
|
|
|
|
return self;
|
|
}
|
|
|
|
/*
|
|
* Register the controller with the driver
|
|
*/
|
|
void ORHVPSRegister(pEVControl self, pEVDriver driv)
|
|
{
|
|
pORHVPSDriv priv = (pORHVPSDriv) 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, ORHVState_Unknown);
|
|
|
|
while (priv->fsm.myState == ORHVState_Unknown) {
|
|
pTaskMan pTasker = GetTasker();
|
|
TaskYield(pTasker);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
/*
|
|
* Action Wrapper routine
|
|
*/
|
|
int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
pEVControl self = (pEVControl)pData;
|
|
pEVDriver driv = self->pDriv;
|
|
pORHVPSDriv priv = (pORHVPSDriv) driv->pPrivate;
|
|
assert(self);
|
|
assert(pCon);
|
|
assert(pSics);
|
|
|
|
if(argc < 2)
|
|
{
|
|
return EVControlWrapper(pCon,pSics,pData,argc,argv);
|
|
}
|
|
// MJL 17/9/08 implement a special command mode to make life easier
|
|
// (otherwise there is a lot of tedious translation between hex/decimal and ASCII codes
|
|
// due to the inability of the code to distinguish between ASCII and returned setting values)
|
|
int is_cmd=(strcasecmp("cmd", argv[1]) == 0);
|
|
if (is_cmd || 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;
|
|
|
|
if (argc<3)
|
|
{
|
|
SCWrite(pCon, "ERROR: Not enough arguments supplied", eError);
|
|
return 0;
|
|
}
|
|
|
|
char *pcmdrspfmt=argv[2];
|
|
if (is_cmd) // "cmd"
|
|
{
|
|
// In command mode, the third argument is normally a single letter command
|
|
// and the remaining arguments are command parameters. For future expansion,
|
|
// we also allow the third argument to be an explicit command/response format.
|
|
// This is in format "<cmd>-<resp>" where the command and response fields
|
|
// are format specifiers containing the required/expected ASCII letters
|
|
// in the command, and the format specifiers '%s' (string),
|
|
// '%d' (integer 0-255), , '%B' (a board name e.g. x0-y15),
|
|
// '%P' (a pot name e.g. x0-y191), or '%A' (ack code as 'ACK'/'NAK').
|
|
// Formats '%c' (a character) and '%x' (char as 2 hex digits) could be
|
|
// added to the code if needed.
|
|
// Note '%P' translates to/from a two-byte board number plus pot number.
|
|
// Code '%A' only useable in response format.
|
|
if (strlen(argv[2])==1)
|
|
switch(*argv[2])
|
|
{
|
|
// All the known commands for the Ordela 21000N, at 9/08.
|
|
// If more become available, add them here, or alternately
|
|
// supply an appropriate command format string explicitly.
|
|
case 'v': pcmdrspfmt="vz-%sz"; break; // Get firmware version
|
|
case 'p': pcmdrspfmt="p%P%dz-%A"; break; // Set a pot value
|
|
case 'P': pcmdrspfmt="P%Pz-P%P%dz"; break; // Get a pot value
|
|
case 'h': pcmdrspfmt="h%dz-%A"; break; // Set HV voltage (also settable directly via the orhvps object)
|
|
case 'H': pcmdrspfmt="Hz-H%dz"; break; // Get HV setting
|
|
case 'd': pcmdrspfmt="dz-%A"; break; // Enter diagnostic mode (disables coincidence detection etc.)
|
|
case 'D': pcmdrspfmt="Dz-D%dz"; break; // Check if in diagnostic mode
|
|
case 'n': pcmdrspfmt="nz-%A"; break; // Exit diagnostic mode
|
|
case 'b': pcmdrspfmt="b%Bz-%A"; break; // Disable a board
|
|
case 'B': pcmdrspfmt="B%Bz-B%B%dz"; break; // Query if board disabled
|
|
case 'l': pcmdrspfmt="l%Bz-%A"; break; // Re-enable a board
|
|
case 'J': pcmdrspfmt="Jz-J%dz"; break; // Check jumper settings
|
|
}
|
|
// Prepare the command string
|
|
char *pcmd=cmd;
|
|
int nfmtspec=0;
|
|
while(*pcmdrspfmt!='-')
|
|
{
|
|
if (*pcmdrspfmt=='\0')
|
|
{
|
|
SCWrite(pCon, "ERROR: Missing '-' separator in format string", eError);
|
|
return 0;
|
|
}
|
|
if (*pcmdrspfmt=='%') // format specifier
|
|
{
|
|
pcmdrspfmt++;
|
|
if (++nfmtspec>argc-3)
|
|
{
|
|
SCWrite(pCon, "ERROR: Not enough arguments supplied for cmd", eError);
|
|
return 0;
|
|
}
|
|
|
|
int v1,v2;
|
|
switch(*pcmdrspfmt++)
|
|
{
|
|
case 's': // probably never used
|
|
pcmd+=sprintf(pcmd,"%s",argv[nfmtspec+2]);
|
|
break;
|
|
case 'd':
|
|
sscanf(argv[nfmtspec+2],"%d",&v1);
|
|
pcmd+=sprintf(pcmd,"%c",(char)v1);
|
|
break;
|
|
case 'B':
|
|
sscanf(argv[nfmtspec+2]+1,"%d",&v1);
|
|
v1+=(v1>=8)*8; // 0-15-> 0-7 and 16-23
|
|
v1+=(*argv[nfmtspec+2]=='y')*8; // y at 8-15 and 24-31
|
|
pcmd+=sprintf(pcmd,"%c",v1);
|
|
break;
|
|
case 'P':
|
|
sscanf(argv[nfmtspec+2]+1,"%d",&v1);
|
|
v2=v1&0xF;
|
|
v1>>=4;
|
|
v1+=(v1>=8)*8;
|
|
v1+=(*argv[nfmtspec+2]=='y')*8;
|
|
pcmd+=sprintf(pcmd,"%c%c",v1,v2);
|
|
break;
|
|
// case 'A': is NOT supported - responses only!
|
|
default: SCWrite(pCon, "ERROR: Unknown % specification in command format", eError); return 0;
|
|
}
|
|
}
|
|
else
|
|
*pcmd++=*pcmdrspfmt++; // Simple ASCII character that is part of command
|
|
}
|
|
pcmdrspfmt++; // skip over the '-' separator into the response format
|
|
cmd_len=pcmd-cmd;
|
|
}
|
|
else // "send"
|
|
{
|
|
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) {
|
|
if (argv[i][j] == '\\') {
|
|
k = 0;
|
|
if (isxdigit(argv[i][j+1]) && isxdigit(argv[i][j+2])) {
|
|
if (argv[i][j+1] >= '0' && argv[i][j+1] <= '9')
|
|
k = (argv[i][j+1] - '0') << 4;
|
|
else if (argv[i][j+1] >= 'a' && argv[i][j+1] <= 'f')
|
|
k = (argv[i][j+1] - 'a' + 10) << 4;
|
|
else if (argv[i][j+1] >= 'A' && argv[i][j+1] <= 'F')
|
|
k = (argv[i][j+1] - 'A' + 10) << 4;
|
|
if (argv[i][j+2] >= '0' && argv[i][j+2] <= '9')
|
|
k += (argv[i][j+2] - '0');
|
|
else if (argv[i][j+2] >= 'a' && argv[i][j+2] <= 'f')
|
|
k += (argv[i][j+2] - 'a' + 10);
|
|
else if (argv[i][j+2] >= 'A' && argv[i][j+2] <= 'F')
|
|
k += (argv[i][j+2] - 'A' + 10);
|
|
j += 2;
|
|
}
|
|
}
|
|
else
|
|
k = argv[i][j];
|
|
if (idx < CMDLEN)
|
|
cmd[idx++] = k;
|
|
}
|
|
if (idx < CMDLEN)
|
|
cmd[idx] = '\0';
|
|
cmd_len = idx;
|
|
}
|
|
}
|
|
|
|
rsp_len = CMDLEN;
|
|
int orhvsr_status=ORHV_SendReceive(priv, cmd, cmd_len, rsp, &rsp_len);
|
|
|
|
int error_in_cmd_rsp_parse=0;
|
|
if (is_cmd) // "cmd" response
|
|
{
|
|
// Start parsing the response according to the format string.
|
|
// If we don't get what we expect, set error_in_cmd_rsp_parse
|
|
// so that the response is output in 'send' format.
|
|
// Only output the parameters in the response format, not ASCII's
|
|
char *pcmd=cmd; // reformat rsp string into cmd string
|
|
char *prsp=rsp;
|
|
int got_fmt_rsp=0;
|
|
while (*pcmdrspfmt&&!error_in_cmd_rsp_parse)
|
|
{
|
|
if (prsp-rsp>=rsp_len)
|
|
error_in_cmd_rsp_parse=1; // response is too short - no bytes left to parse
|
|
else if (*pcmdrspfmt=='%') // Parse byte(s)
|
|
{
|
|
pcmdrspfmt++;
|
|
if (got_fmt_rsp)
|
|
*pcmd++=' '; // separate response fields with spaces (Tcl list can separate these)
|
|
int slen;
|
|
switch(*pcmdrspfmt++)
|
|
{
|
|
case 's':
|
|
pcmd+=(slen=sprintf(pcmd,"%s",prsp));
|
|
*--pcmd='\0'; // string should have been terminated (with a 'z') but that gets checked later
|
|
prsp+=slen-1;
|
|
break;
|
|
case 'd':
|
|
pcmd+=sprintf(pcmd,"%d",*prsp++);
|
|
break;
|
|
case 'B':
|
|
pcmd+=sprintf(pcmd,"%c%d",(prsp[0]&0x8)?'y':'x',(prsp[0]&0x7)+((prsp[0]&0x10)>>1));
|
|
prsp++;
|
|
break;
|
|
case 'P':
|
|
if (rsp_len-(prsp-rsp)<2)
|
|
error_in_cmd_rsp_parse=1;
|
|
else
|
|
{
|
|
pcmd+=sprintf(pcmd,"%c%d",(prsp[0]&0x8)?'y':'x',(prsp[0]&0x7)*0x10+(prsp[0]&0x10)*0x8+prsp[1]);
|
|
prsp+=2;
|
|
}
|
|
break;
|
|
case 'A':
|
|
error_in_cmd_rsp_parse|=(prsp[0]!=0x06&&prsp[0]!=0x15);
|
|
pcmd+=sprintf(pcmd,"%s",(prsp[0]==0x06)?"ACK":"NAK");
|
|
prsp++;
|
|
break;
|
|
default: SCWrite(pCon, "ERROR: Unknown % specification in response format", eError); return 0;
|
|
}
|
|
got_fmt_rsp=1;
|
|
}
|
|
else // expected ASCII characters in response have to match response format
|
|
error_in_cmd_rsp_parse|=(*prsp++!=*pcmdrspfmt++);
|
|
}
|
|
}
|
|
if (!is_cmd||error_in_cmd_rsp_parse) // "send" response, or a "cmd" response that didn't match expected format
|
|
{
|
|
idx = 0;
|
|
if (error_in_cmd_rsp_parse)
|
|
idx=sprintf(cmd,"ERROR: Bad response format: ");
|
|
for (i = 0; i < rsp_len && idx < CMDLEN - 1; ++i) {
|
|
if (rsp[i] < 32 || rsp[i] > 126) {
|
|
idx+=sprintf(&cmd[idx], "%02Xh", rsp[i]);
|
|
}
|
|
else
|
|
cmd[idx++] = rsp[i];
|
|
}
|
|
if (idx < CMDLEN)
|
|
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;
|
|
priv->iPeriod = get_period(priv->fMax, priv->fRate);
|
|
}
|
|
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;
|
|
priv->iPeriod = get_period(priv->fMax, priv->fRate);
|
|
}
|
|
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;
|
|
/* restart state machine */
|
|
fsm_change_state(&priv->fsm, ORHVState_Unknown);
|
|
while (priv->fsm.myState == ORHVState_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);
|
|
snprintf(rsp, CMDLEN, "%s.period = %6d", priv->name, priv->iPeriod);
|
|
SCWrite(pCon, rsp, eValue);
|
|
}
|
|
return iRet;
|
|
}
|
|
if (strcasecmp("period", argv[1]) == 0) {
|
|
char line[CMDLEN];
|
|
if (argc > 2) {
|
|
if (!SCMatchRights(pCon, usUser))
|
|
return 0;
|
|
SCWrite(pCon, "cannot set period, set rate instead", eError);
|
|
return 0;
|
|
}
|
|
snprintf(line, CMDLEN, "%s.period = %6d", priv->name, priv->iPeriod);
|
|
SCWrite(pCon, line, eValue);
|
|
return 1;
|
|
}
|
|
|
|
return EVControlWrapper(pCon,pSics,pData,argc,argv);
|
|
}
|
|
|