linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Align nhq200.c and orhvps.c to be more alike (and more diffable)

Browse Source 
r2629 | dcl | 2008-06-12 17:23:40 +1000 (Thu, 12 Jun 2008) | 2 lines
This commit is contained in:
Douglas Clowes
2008-06-12 17:23:40 +10:00
parent 1795e07504
commit e59b073066
2 changed files with 394 additions and 201 deletions
Download Patch File Download Diff File Expand all files Collapse all files

258
site_ansto/nhq200.c
View File

@@ -8,7 +8,7 @@
Douglas Clowes, May 2008
copyright: site_ansto/doc/Copyright.txt
Copyright: site_ansto/doc/Copyright.txt
----------------------------------------------------------------------------
*/
@@ -28,30 +28,36 @@
#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)
#define NHQ_FAST_POLL (500)
#define NHQ_SLOW_POLL (2000)
/* ERROR CODES */
#define NHQ200_ERR_NONE (0)
#define NHQ200_ERR_LOCKED (-1)
#define NHQ200_ERR_RANGE (-2)
typedef struct nhq200_s {
/* Device Driver Control Structure */
struct nhq200_s {
pEVControl controller;
int iControl; /* integer controller (1..2) */
int iErrorCode; /* error code */
int iErrorCount; /* error count */
float fTarget; /* target voltage in volts */
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 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];
@@ -62,9 +68,11 @@ typedef struct nhq200_s {
int vmax_percent;
int imax_percent;
int ramp_input;
bool run_flag;
} NHQ200Driv, *pNHQ200Driv;
};
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);
@@ -75,6 +83,18 @@ 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,
@@ -110,10 +130,7 @@ static int NHQ_SendReceive(pNHQ200Driv priv,
return OKOK;
}
static void NHQ_SetTimer(pNHQ200Driv priv, int msecs) {
NetWatchRegisterTimer(&priv->state_timer, msecs, fsm_tmr_callback, &priv->fsm);
}
/* 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);
@@ -353,6 +370,13 @@ static void parse_Gx(pNHQ200Driv priv, const char* resp, int resp_len)
#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;
@@ -473,6 +497,12 @@ static void NHQState_Unknown(pStateMachine sm, pEvtEvent event) {
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;
@@ -492,12 +522,12 @@ static void NHQState_Idle(pStateMachine sm, pEvtEvent event){
return;
case eTimerEvent:
priv->state_timer = NULL;
if (priv->run_flag) {
if (priv->bRunFlag) {
if (priv->fTarget > priv->fValue)
fsm_change_state(sm, NHQState_Raising);
else
fsm_change_state(sm, NHQState_Lowering);
priv->run_flag = false;
priv->bRunFlag = false;
return;
}
cmd_len = snprintf(cmd, sizeof(cmd), "U%d", priv->iControl);
@@ -545,6 +575,11 @@ static void NHQState_Idle(pStateMachine sm, pEvtEvent event){
}
return;
}
/* Raising State
*
* Increasing controlled value
*/
static void NHQState_Raising(pStateMachine sm, pEvtEvent event) {
pEVDriver driv = (pEVDriver) sm->context;
pNHQ200Driv priv = (pNHQ200Driv) driv->pPrivate;
@@ -626,6 +661,10 @@ static void NHQState_Raising(pStateMachine sm, pEvtEvent event) {
return;
}
/* Lowering State
*
* Decreasing controlled value
*/
static void NHQState_Lowering(pStateMachine sm, pEvtEvent event) {
pEVDriver driv = (pEVDriver) sm->context;
pNHQ200Driv priv = (pNHQ200Driv) driv->pPrivate;
@@ -707,10 +746,17 @@ static void NHQState_Lowering(pStateMachine sm, pEvtEvent event) {
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);
@@ -719,12 +765,16 @@ static int NHQ200GetValue( pEVDriver self, float* fPos) {
*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) {
if (priv->isLocked && !priv->bInternal) {
priv->iErrorCode = NHQ200_ERR_LOCKED;
return 0;
}
@@ -733,9 +783,13 @@ static int NHQ200SetValue( pEVDriver self, float fPos) {
return 0;
}
priv->fTarget = fPos;
priv->run_flag = true;
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;
@@ -770,6 +824,10 @@ static int NHQ200Send(pEVDriver self, char *pCommand, char *pReply, int iLen) {
}
return -1;
}
/*
* SICS error handler
*/
static int NHQ200Error(pEVDriver self, int *iCode, char *error, int iErrLen) {
pNHQ200Driv priv = (pNHQ200Driv) self->pPrivate;
*iCode = priv->iErrorCode;
@@ -787,6 +845,9 @@ static int NHQ200Error(pEVDriver self, int *iCode, char *error, int iErrLen) {
return 1;
}
/*
* SICS fix handler
*/
static int NHQ200Fix(pEVDriver self, int iError) {
/* TODO */
return DEVFAULT;
@@ -817,28 +878,42 @@ static void NHQ200KillPrivate(void *pData) {
}
}
static int NHQ200_Tx1(pAsyncProtocol p, void* ctx)
{
/*
* Auxilliary transmit function
* Called internaly to transmit a character
*/
static int NHQ200_Tx1(pAsyncProtocol p, pAsyncTxn myCmd) {
int iRet = 1;
pAsyncTxn myCmd = (pAsyncTxn) ctx;
assert(myCmd);
iRet = AsyncUnitWrite(myCmd->unit, &myCmd->out_buf[myCmd->out_idx], 1);
return iRet;
}
static int NHQ200_Tx(pAsyncProtocol p, pAsyncTxn myCmd)
{
/*
* Set/reset command states for send/resend of command
*/
myCmd->txn_state = 0;
myCmd->out_idx = 0;
myCmd->inp_idx = 0;
myCmd->txn_status = ATX_ACTIVE;
return NHQ200_Tx1(p, myCmd);
/*
* 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;
@@ -890,6 +965,9 @@ static int NHQ200_Rx(pAsyncProtocol p, pAsyncTxn myCmd, int rxchar)
return iRet;
}
/*
* AsyncUnit Notify Callback
*/
static void NHQ200Notify(void* context, int event)
{
pNHQ200Driv priv = (pNHQ200Driv) context;
@@ -912,6 +990,9 @@ static void NHQ200Notify(void* context, int event)
return;
}
/*
* AsyncProtocol Event callback
*/
static int NHQ200_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) {
if (event == AQU_TIMEOUT) {
/* handle command timeout */
@@ -921,30 +1002,26 @@ static int NHQ200_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) {
return AQU_POP_CMD;
}
static int NHQ200_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 NHQ200_PrepareTxn", eError);
return 0;
}
memcpy(txn->out_buf, cmd, cmd_len);
txn->out_len = cmd_len;
return 1;
}
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_PrepareTxn; */
NHQ200_Protocol->killPrivate = NULL;
// NHQ200_Protocol->prepareTxn = NULL;
// NHQ200_Protocol->killPrivate = NULL;
}
}
/*
* Device Factory
*/
pEVDriver CreateNHQ200Driver(int argc, char *argv[])
{
pEVDriver self = NULL;
@@ -966,7 +1043,7 @@ pEVDriver CreateNHQ200Driver(int argc, char *argv[])
if (!AsyncUnitCreate(argv[0], &priv->asyncUnit)) {
char line[132];
snprintf(line, 132, "Error: did not find AsyncQueue %s for Device", argv[0]);
snprintf(line, 132, "Error: did not find AsyncQueue %s for Device %s", argv[1], argv[0]);
SICSLogWrite(line, eError);
DeleteEVDriver(self);
free(priv);
@@ -994,7 +1071,7 @@ pEVDriver CreateNHQ200Driver(int argc, char *argv[])
priv->fRate = 10.0;
priv->fLower = 0.0;
priv->fUpper = 0.0;
priv->iControl = 1;
priv->bRunFlag = false;
priv->iControl = 1;
if (argc > 1)
priv->iControl = strtol(argv[1], NULL, 10);
@@ -1004,6 +1081,9 @@ pEVDriver CreateNHQ200Driver(int argc, char *argv[])
return self;
}
/*
* Register the controller with the driver
*/
void NHQ200Register(pEVControl self, pEVDriver driv)
{
pNHQ200Driv priv = (pNHQ200Driv) driv->pPrivate;
@@ -1025,6 +1105,9 @@ void NHQ200Register(pEVControl self, pEVDriver driv)
}
/*
* Action Wrapper routine
*/
int NHQ200Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
int argc, char *argv[])
{
@@ -1090,7 +1173,9 @@ int NHQ200Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
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;
@@ -1100,7 +1185,9 @@ int NHQ200Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
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;
@@ -1108,7 +1195,9 @@ int NHQ200Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
if (strcasecmp("upper", argv[1]) == 0) {
char rsp[CMDLEN];
if (argc > 2) {
if (priv->isLocked) {
if (!SCMatchRights(pCon, usUser))
return 0;
if (priv->isLocked && !SCMatchRights(pCon, usMugger)) {
SCWrite(pCon, "object is locked", eError);
return 0;
}
@@ -1130,7 +1219,9 @@ int NHQ200Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
if (strcasecmp("lower", argv[1]) == 0) {
char rsp[CMDLEN];
if (argc > 2) {
if (priv->isLocked) {
if (!SCMatchRights(pCon, usUser))
return 0;
if (priv->isLocked && !SCMatchRights(pCon, usMugger)) {
SCWrite(pCon, "object is locked", eError);
return 0;
}
@@ -1152,17 +1243,21 @@ int NHQ200Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
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 <= 6000.0) {
if (value >= 0.0 && value <= MY_ABSOLUTE_MAXIMUM) {
priv->fMax = value;
}
else {
SCWrite(pCon, "max must be between 0.0 and 6000.0", eError);
char line[CMDLEN];
snprintf(line, CMDLEN, "max must be between 0.0 and %.0f", MY_ABSOLUTE_MAXIMUM);
SCWrite(pCon, line, eError);
return 0;
}
}
@@ -1171,7 +1266,7 @@ int NHQ200Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
SCWrite(pCon, rsp, eValue);
return 1;
}
if (strcasecmp("debug", argv[1]) == 0) {
if (strcasecmp("debug", argv[1]) == 0 && SCMatchRights(pCon, usMugger)) {
char rsp[CMDLEN];
if (argc > 2) {
int debug = atoi(argv[2]);
@@ -1187,13 +1282,15 @@ int NHQ200Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
if (strcasecmp("rate", argv[1]) == 0) {
char rsp[CMDLEN];
if (argc > 2) {
if (priv->isLocked) {
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 >= 2.0 && value <= 255.0) {
if (value >= MY_MINIMUM_RATE && value <= MY_MAXIMUM_RATE) {
priv->fRate = value;
char cmd[CMDLEN];
int cmd_len;
@@ -1204,7 +1301,9 @@ int NHQ200Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
NHQ_SendReceive(priv, cmd, cmd_len, rsp, &rsp_len);
}
else {
SCWrite(pCon, "rate must be between 2 and 255", eError);
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;
}
}
@@ -1213,15 +1312,10 @@ int NHQ200Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
SCWrite(pCon, rsp, eValue);
return 1;
}
if (strcasecmp("status", argv[1]) == 0) {
char line[CMDLEN];
read_status(priv, line, sizeof(line));
SCWrite(pCon, line, 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 */
@@ -1235,20 +1329,21 @@ int NHQ200Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
SCWrite(pCon, line, eValue);
return 1;
}
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;
}
if (strcasecmp("lock", argv[1]) == 0) {
char rsp[CMDLEN];
if (!SCMatchRights(pCon, usUser))
return 0;
priv->isLocked = 1;
snprintf(rsp, CMDLEN, "%s.lock = 1", priv->name);
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;
}
@@ -1266,8 +1361,25 @@ int NHQ200Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
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);
}

337
site_ansto/orhvps.c
View File

@@ -8,35 +8,8 @@
Douglas Clowes, December 2007
Copyright:
Copyright: site_ansto/doc/Copyright.txt
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.
----------------------------------------------------------------------------
*/
@@ -54,29 +27,38 @@
#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)
typedef struct orhvps_s {
/* Device Driver Control Structure */
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 */
int iErrorCode; /* error code */
float fTarget; /* requested target voltage in volts */
float fValue; /* current voltage in volts */
float fMax; /* maximum voltage in volts at iValue=63 */
float fMax; /* maximum voltage in volts */
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 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 */
} ORHVPSDriv, *pORHVPSDriv;
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);
@@ -87,13 +69,25 @@ static int ORHVPSClose(pEVDriver self);
static void ORHVPSKillPrivate(void *pData);
static void ORHVPSNotify(void* context, int event);
static int ORHV_SendCmd(pORHVPSDriv self,
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 = &self->fsm;
return AsyncUnitSendTxn(self->asyncUnit,
pStateMachine sm = &priv->fsm;
return AsyncUnitSendTxn(priv->asyncUnit,
command, cmd_len,
callback, sm, CMDLEN);
}
@@ -101,19 +95,19 @@ static int ORHV_SendCmd(pORHVPSDriv self,
/**
* \brief Sends a command and waits for a response
*
* \param self motor data
* \param priv motor data
* \param cmd command to send
* \param reply space to return response
* \return
*/
static int ORHV_SendReceive(pORHVPSDriv self,
static int ORHV_SendReceive(pORHVPSDriv priv,
char *cmd,
int cmd_len,
char* reply,
int *rep_len) {
int status;
status = AsyncUnitTransact(self->asyncUnit, cmd, cmd_len, reply, rep_len);
status = AsyncUnitTransact(priv->asyncUnit, cmd, cmd_len, reply, rep_len);
if (status != 1) {
return FAILURE;
@@ -122,6 +116,7 @@ static int ORHV_SendReceive(pORHVPSDriv self,
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);
@@ -147,7 +142,7 @@ static void str_n_cat(char* s1, int len, const char* s2) {
s1[i] = '\0';
}
const char* state_name(StateFunc func)
static const char* state_name(StateFunc func)
{
if (func == NULL) return "<null_state>";
if (func == ORHVState_Unknown) return "ORHVState_Unknown";
@@ -157,7 +152,7 @@ const char* state_name(StateFunc func)
return "<unknown_state>";
}
const char* event_name(pEvtEvent event, char* text, int length)
static const char* event_name(pEvtEvent event, char* text, int length)
{
char line[1024];
if (event == NULL)
@@ -252,6 +247,13 @@ static int getProcess(pStateMachine sm, pAsyncTxn pCmd, int bot, int top) {
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;
@@ -308,6 +310,12 @@ static void ORHVState_Unknown(pStateMachine sm, pEvtEvent event) {
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;
@@ -384,6 +392,10 @@ static void ORHVState_Idle(pStateMachine sm, pEvtEvent event){
return;
}
/* Raising State
*
* Increasing controlled value
*/
static void ORHVState_Raising(pStateMachine sm, pEvtEvent event){
pEVDriver driv = (pEVDriver) sm->context;
pORHVPSDriv priv = (pORHVPSDriv) driv->pPrivate;
@@ -484,6 +496,10 @@ static void ORHVState_Raising(pStateMachine sm, pEvtEvent event){
return;
}
/* Lowering State
*
* Decreasing controlled value
*/
static void ORHVState_Lowering(pStateMachine sm, pEvtEvent event){
pEVDriver driv = (pEVDriver) sm->context;
pORHVPSDriv priv = (pORHVPSDriv) driv->pPrivate;
@@ -585,32 +601,47 @@ static void ORHVState_Lowering(pStateMachine sm, pEvtEvent event){
}
/*
* Return the current value to SICS
*/
static int ORHVPSGetValue( pEVDriver self, float* fPos) {
pORHVPSDriv me = NULL;
pORHVPSDriv priv = 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;
priv = (pORHVPSDriv) self->pPrivate;
newVal = roundf(priv->fTarget / (priv->fMax / 63.0));
if (newVal == priv->iValue)
*fPos = priv->fTarget;
else
*fPos = me->fValue;
*fPos = priv->fValue;
return 1;
}
/*
* Set the current value from SICS
*/
static int ORHVPSSetValue( pEVDriver self, float fPos) {
pORHVPSDriv me = NULL;
pORHVPSDriv priv = NULL;
assert(self);
assert(self->pPrivate);
me = (pORHVPSDriv) self->pPrivate;
if (fPos < 0.0 || fPos > me->fMax) {
me->iError = ORHVPS_ERR_RANGE;
priv = (pORHVPSDriv) self->pPrivate;
if (priv->isLocked && !priv->bInternal) {
priv->iErrorCode = ORHVPS_ERR_LOCKED;
return 0;
}
me->fTarget = fPos;
me->bRunFlag = true;
if (fPos < 0.0 || fPos > priv->fMax) {
priv->iErrorCode = ORHVPS_ERR_RANGE;
return 0;
}
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;
@@ -664,10 +695,14 @@ static int ORHVPSSend(pEVDriver self, char *pCommand, char *pReply, int iLen) {
}
return -1;
}
/*
* SICS error handler
*/
static int ORHVPSError(pEVDriver self, int *iCode, char *error, int iErrLen) {
pORHVPSDriv priv = (pORHVPSDriv) self->pPrivate;
*iCode = priv->iError;
switch (priv->iError) {
*iCode = priv->iErrorCode;
switch (priv->iErrorCode) {
case ORHVPS_ERR_RANGE:
strncpy(error,"Value out of range",iErrLen);
break;
@@ -680,6 +715,10 @@ static int ORHVPSError(pEVDriver self, int *iCode, char *error, int iErrLen) {
}
return 1;
}
/*
* SICS fix handler
*/
static int ORHVPSFix(pEVDriver self, int iError) {
/* TODO */
return DEVFAULT;
@@ -710,46 +749,12 @@ static void ORHVPSKillPrivate(void *pData) {
}
}
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)
{
/*
* Protocol transmit function
* Called by AsyncQueue to transmit a line
*/
static int ORHVPS_Tx(pAsyncProtocol p, pAsyncTxn myCmd) {
int iRet = 1;
pAsyncTxn myCmd = (pAsyncTxn) ctx;
if (myCmd) {
myCmd->txn_status = ATX_ACTIVE;
@@ -766,6 +771,9 @@ static int ORHVPS_Tx(pAsyncProtocol p, pAsyncTxn ctx)
return 1;
}
/*
* Protocol receive character - characater by character
*/
static int ORHVPS_Rx(pAsyncProtocol p, pAsyncTxn ctx, int rxchar) {
int iRet = 1;
pAsyncTxn myCmd = (pAsyncTxn) ctx;
@@ -802,6 +810,46 @@ static int ORHVPS_Rx(pAsyncProtocol p, pAsyncTxn ctx, int rxchar) {
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, 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;
}
/*
* AsyncProtocol Event callback
*/
static int ORHVPS_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) {
if (event == AQU_TIMEOUT) {
/* handle command timeout */
@@ -822,6 +870,11 @@ static int ORHVPS_PrepareTxn(pAsyncProtocol p, pAsyncTxn txn, const char* cmd, i
return 1;
}
static pAsyncProtocol ORHVPS_Protocol = NULL;
/*
* Protocol Initialisation
*/
void ORHVPSInitProtocol(SicsInterp *pSics) {
if (ORHVPS_Protocol == NULL) {
ORHVPS_Protocol = AsyncProtocolCreate(pSics, "ORHVPS", NULL, NULL);
@@ -833,11 +886,9 @@ void ORHVPSInitProtocol(SicsInterp *pSics) {
}
}
static int get_period(float max, float rate) {
return (int) roundf(1000 * (max / 63.0) / rate);
//return (int) roundf((rate * 60 * 1000) / 63.0);
}
/*
* Device Factory
*/
pEVDriver CreateORHVPSDriver(int argc, char *argv[])
{
pEVDriver self = NULL;
@@ -860,6 +911,7 @@ pEVDriver CreateORHVPSDriver(int argc, char *argv[])
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;
@@ -889,11 +941,13 @@ pEVDriver CreateORHVPSDriver(int argc, char *argv[])
priv->iPeriod = get_period(priv->fMax, priv->fRate);
priv->bRunFlag = false;
fsm_change_state(&priv->fsm, ORHVState_Unknown);
return self;
}
/*
* Register the controller with the driver
*/
void ORHVPSRegister(pEVControl self, pEVDriver driv)
{
pORHVPSDriv priv = (pORHVPSDriv) driv->pPrivate;
@@ -904,8 +958,20 @@ void ORHVPSRegister(pEVControl self, pEVDriver driv)
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[])
{
@@ -990,7 +1056,9 @@ int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
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;
@@ -1000,7 +1068,9 @@ int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
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;
@@ -1016,11 +1086,11 @@ int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
}
else {
float value = atof(argv[2]);
if (value >= priv->fLower && value <= priv->fMax) {
if (value >= 0.0 && value <= priv->fMax) {
priv->fUpper = value;
}
else {
SCWrite(pCon, "upper value must be between <lower> and <max>", eError);
SCWrite(pCon, "upper value must be between 0.0 and <maximum>", eError);
return 0;
}
}
@@ -1040,11 +1110,11 @@ int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
}
else {
float value = atof(argv[2]);
if (value >= 0.0 && value <= priv->fUpper) {
if (value >= 0.0 && value <= priv->fMax) {
priv->fLower = value;
}
else {
SCWrite(pCon, "lower value must be between 0.0 and <upper>", eError);
SCWrite(pCon, "lower value must be between 0.0 and <maximum>", eError);
return 0;
}
}
@@ -1064,12 +1134,14 @@ int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
}
else {
float value = atof(argv[2]);
if (value >= 0.0 && value <= 2600.0) {
if (value >= 0.0 && value <= MY_ABSOLUTE_MAXIMUM) {
priv->fMax = value;
priv->iPeriod = get_period(priv->fMax, priv->fRate);
}
else {
SCWrite(pCon, "max must be between 0.0 and 2600.0", eError);
char line[CMDLEN];
snprintf(line, CMDLEN, "max must be between 0.0 and %.0f", MY_ABSOLUTE_MAXIMUM);
SCWrite(pCon, line, eError);
return 0;
}
}
@@ -1102,12 +1174,14 @@ int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
}
else {
float value = atof(argv[2]);
if (value >= 1.0 && value <= 40.0) {
if (value >= MY_MINIMUM_RATE && value <= MY_MAXIMUM_RATE) {
priv->fRate = value;
priv->iPeriod = get_period(priv->fMax, priv->fRate);
}
else {
SCWrite(pCon, "rate must be between 1.0 and 40.0", eError);
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;
}
}
@@ -1116,31 +1190,25 @@ int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
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];
char line[CMDLEN];
if (!SCMatchRights(pCon, usUser))
return 0;
/* restart state machine */
fsm_change_state(&priv->fsm, ORHVState_Unknown);
snprintf(rsp, CMDLEN, "%s.reset = 1", priv->name);
SCWrite(pCon, rsp, eValue);
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;
if (!SCMatchRights(pCon, usUser))
return 0;
priv->isLocked = 1;
snprintf(rsp, CMDLEN, "%s.lock = %d", priv->name, priv->isLocked);
SCWrite(pCon, rsp, eValue);
@@ -1173,6 +1241,19 @@ int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
}
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);
}