1179 lines
34 KiB
C
1179 lines
34 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:
|
|
|
|
Labor fuer Neutronenstreuung
|
|
Paul Scherrer Institut
|
|
CH-5423 Villigen-PSI
|
|
|
|
|
|
The authors hereby grant permission to use, copy, modify, distribute,
|
|
and license this software and its documentation for any purpose, provided
|
|
that existing copyright notices are retained in all copies and that this
|
|
notice is included verbatim in any distributions. No written agreement,
|
|
license, or royalty fee is required for any of the authorized uses.
|
|
Modifications to this software may be copyrighted by their authors
|
|
and need not follow the licensing terms described here, provided that
|
|
the new terms are clearly indicated on the first page of each file where
|
|
they apply.
|
|
|
|
IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY
|
|
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
|
ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY
|
|
DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE
|
|
POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES,
|
|
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,
|
|
FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT. THIS SOFTWARE
|
|
IS PROVIDED ON AN "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE
|
|
NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
|
|
MODIFICATIONS.
|
|
----------------------------------------------------------------------------
|
|
*/
|
|
|
|
#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 ORHVPS_ERR_NONE (0)
|
|
#define ORHVPS_ERR_LOCKED (-1)
|
|
#define ORHVPS_ERR_RANGE (-2)
|
|
|
|
typedef struct orhvps_s {
|
|
pEVControl controller;
|
|
int iValue; /* integer value from controller (0..63) */
|
|
int iPeriod; /* integer step-rate period in milliseconds */
|
|
int iError; /* error code */
|
|
float fValue; /* current voltage in volts */
|
|
float fMax; /* maximum voltage in volts at iValue=63 */
|
|
float fRate; /* voltage slew rate in volts per second */
|
|
float fTarget; /* requested target voltage */
|
|
float fUpper; /* Normal Operating Voltage */
|
|
float fLower; /* Normal Moving Voltage */
|
|
bool isLocked; /* changes no longer permitted */
|
|
bool bRunFlag; /* set by the run command */
|
|
char* name;
|
|
pAsyncUnit asyncUnit;
|
|
StateMachine fsm;
|
|
pNWTimer state_timer; /**< state timer */
|
|
} ORHVPSDriv, *pORHVPSDriv;
|
|
|
|
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 ORHV_SendCmd(pORHVPSDriv self,
|
|
char* command,
|
|
int cmd_len,
|
|
AsyncTxnHandler callback)
|
|
{
|
|
pStateMachine sm = &self->fsm;
|
|
return AsyncUnitSendTxn(self->asyncUnit,
|
|
command, cmd_len,
|
|
callback, sm, CMDLEN);
|
|
}
|
|
|
|
/**
|
|
* \brief Sends a command and waits for a response
|
|
*
|
|
* \param self motor data
|
|
* \param cmd command to send
|
|
* \param reply space to return response
|
|
* \return
|
|
*/
|
|
static int ORHV_SendReceive(pORHVPSDriv self,
|
|
char *cmd,
|
|
int cmd_len,
|
|
char* reply,
|
|
int *rep_len) {
|
|
int status;
|
|
|
|
status = AsyncUnitTransact(self->asyncUnit, cmd, cmd_len, reply, rep_len);
|
|
|
|
if (status != 1) {
|
|
return FAILURE;
|
|
}
|
|
|
|
return OKOK;
|
|
}
|
|
|
|
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';
|
|
}
|
|
|
|
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>";
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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, eStatus);
|
|
}
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
static int ORHVPSGetValue( pEVDriver self, float* fPos) {
|
|
pORHVPSDriv me = NULL;
|
|
int newVal;
|
|
assert(self);
|
|
assert(self->pPrivate);
|
|
me = (pORHVPSDriv) self->pPrivate;
|
|
newVal = roundf(me->fTarget / (me->fMax / 63.0));
|
|
if (newVal == me->iValue)
|
|
*fPos = me->fTarget;
|
|
else
|
|
*fPos = me->fValue;
|
|
return 1;
|
|
}
|
|
static int ORHVPSSetValue( pEVDriver self, float fPos) {
|
|
pORHVPSDriv me = NULL;
|
|
assert(self);
|
|
assert(self->pPrivate);
|
|
me = (pORHVPSDriv) self->pPrivate;
|
|
if (fPos < 0.0 || fPos > me->fMax) {
|
|
me->iError = ORHVPS_ERR_RANGE;
|
|
return 0;
|
|
}
|
|
me->fTarget = fPos;
|
|
me->bRunFlag = true;
|
|
return 1;
|
|
}
|
|
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;
|
|
ORHV_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;
|
|
}
|
|
static int ORHVPSError(pEVDriver self, int *iCode, char *error, int iErrLen) {
|
|
pORHVPSDriv priv = (pORHVPSDriv) self->pPrivate;
|
|
*iCode = priv->iError;
|
|
switch (priv->iError) {
|
|
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;
|
|
}
|
|
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;
|
|
}
|
|
}
|
|
|
|
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, eStatus);
|
|
/* TODO: disconnect */
|
|
break;
|
|
case AQU_RECONNECT:
|
|
snprintf(line, 132, "Reconnect on Device '%s'", self->name);
|
|
SICSLogWrite(line, eStatus);
|
|
/* 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;
|
|
}
|
|
|
|
static pAsyncProtocol ORHVPS_Protocol = NULL;
|
|
|
|
static int ORHVPS_Tx(pAsyncProtocol p, pAsyncTxn ctx)
|
|
{
|
|
int iRet = 1;
|
|
pAsyncTxn myCmd = (pAsyncTxn) ctx;
|
|
|
|
if (myCmd) {
|
|
myCmd->txn_status = ATX_ACTIVE;
|
|
iRet = AsyncUnitWrite(myCmd->unit, myCmd->out_buf, myCmd->out_len);
|
|
/* TODO handle errors */
|
|
if (iRet < 0) { /* TODO: EOF */
|
|
/*
|
|
iRet = AsyncUnitReconnect(myCmd->unit);
|
|
if (iRet == 0)
|
|
*/
|
|
return 0;
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
void ORHVPSInitProtocol(SicsInterp *pSics) {
|
|
if (ORHVPS_Protocol == NULL) {
|
|
ORHVPS_Protocol = AsyncProtocolCreate(pSics, "ORHVPS", NULL, NULL);
|
|
ORHVPS_Protocol->sendCommand = ORHVPS_Tx;
|
|
ORHVPS_Protocol->handleInput = ORHVPS_Rx;
|
|
ORHVPS_Protocol->handleEvent = ORHVPS_Ev;
|
|
ORHVPS_Protocol->prepareTxn = ORHVPS_PrepareTxn;
|
|
ORHVPS_Protocol->killPrivate = NULL;
|
|
}
|
|
}
|
|
|
|
static int get_period(float max, float rate) {
|
|
return (int) roundf(1000 * (max / 63.0) / rate);
|
|
//return (int) roundf((rate * 60 * 1000) / 63.0);
|
|
}
|
|
|
|
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]);
|
|
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]);
|
|
priv->fMax = 2400.0;
|
|
priv->fRate = 10.0;
|
|
priv->fLower = 0.0;
|
|
priv->fUpper = 1800.0;
|
|
priv->iPeriod = get_period(priv->fMax, priv->fRate);
|
|
priv->bRunFlag = false;
|
|
|
|
fsm_change_state(&priv->fsm, ORHVState_Unknown);
|
|
|
|
return self;
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
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);
|
|
}
|
|
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) {
|
|
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);
|
|
i += 2;
|
|
}
|
|
}
|
|
else
|
|
k = argv[i][j];
|
|
if (idx < CMDLEN)
|
|
cmd[idx++] = k;
|
|
}
|
|
if (idx < CMDLEN)
|
|
cmd[idx] = '\0';
|
|
cmd_len = idx;
|
|
}
|
|
rsp_len = CMDLEN;
|
|
ORHV_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;
|
|
iRet = EVCDrive(priv->controller, pCon, priv->fTarget);
|
|
if(iRet)
|
|
SCSendOK(pCon);
|
|
return iRet;
|
|
}
|
|
if (strcasecmp("off", argv[1]) == 0) {
|
|
int iRet;
|
|
if(!SCMatchRights(pCon,usUser))
|
|
return 0;
|
|
priv->fTarget = 0.0;
|
|
iRet = EVCDrive(priv->controller, pCon, priv->fTarget);
|
|
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 >= priv->fLower && value <= priv->fMax) {
|
|
priv->fUpper = value;
|
|
}
|
|
else {
|
|
SCWrite(pCon, "upper value must be between <lower> and <max>", 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->fUpper) {
|
|
priv->fLower = value;
|
|
}
|
|
else {
|
|
SCWrite(pCon, "lower value must be between 0.0 and <upper>", 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 <= 2600.0) {
|
|
priv->fMax = value;
|
|
priv->iPeriod = get_period(priv->fMax, priv->fRate);
|
|
}
|
|
else {
|
|
SCWrite(pCon, "max must be between 0.0 and 2600.0", 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 >= 1.0 && value <= 40.0) {
|
|
priv->fRate = value;
|
|
priv->iPeriod = get_period(priv->fMax, priv->fRate);
|
|
}
|
|
else {
|
|
SCWrite(pCon, "rate must be between 1.0 and 40.0", eError);
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
snprintf(rsp, CMDLEN, "%s.rate = %8.2f", priv->name, priv->fRate);
|
|
SCWrite(pCon, rsp, eValue);
|
|
return 1;
|
|
}
|
|
if (strcasecmp("period", argv[1]) == 0) {
|
|
char rsp[CMDLEN];
|
|
if (argc > 2) {
|
|
if (!SCMatchRights(pCon, usUser))
|
|
return 0;
|
|
SCWrite(pCon, "cannot set period, set max and rate instead", eError);
|
|
return 0;
|
|
}
|
|
snprintf(rsp, CMDLEN, "%s.period = %6d", priv->name, priv->iPeriod);
|
|
SCWrite(pCon, rsp, eValue);
|
|
return 1;
|
|
}
|
|
if (strcasecmp("reset", argv[1]) == 0) {
|
|
char rsp[CMDLEN];
|
|
if (!SCMatchRights(pCon, usUser))
|
|
return 0;
|
|
fsm_change_state(&priv->fsm, ORHVState_Unknown);
|
|
snprintf(rsp, CMDLEN, "%s.reset = 1", priv->name);
|
|
SCWrite(pCon, rsp, 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;
|
|
}
|
|
return EVControlWrapper(pCon,pSics,pData,argc,argv);
|
|
}
|
|
|