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

lots of changes

Browse Source 
r2287 | dcl | 2008-01-21 09:55:49 +1100 (Mon, 21 Jan 2008) | 2 lines
This commit is contained in:
Douglas Clowes
2008-01-21 09:55:49 +11:00
parent 6016d8d46b
commit 7fffc9f97a
1 changed files with 244 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

372
site_ansto/orhvps.c
View File

@@ -47,21 +47,29 @@
#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 */
char* name;
pAsyncUnit asyncUnit;
StateMachine fsm;
@@ -89,85 +97,6 @@ static int ORHV_SendCmd(pORHVPSDriv self,
callback, sm, CMDLEN);
}
#if 0
/**
* \brief SendCallback is the callback for the general command.
*/
static int ORHV_SendCallback(pAsyncTxn pCmd)
{
char* cmnd = pCmd->out_buf;
char* resp = pCmd->inp_buf;
pORHVPSDriv self = (pORHVPSDriv) pCmd->cntx;
#if 0
if (pCmd->txn_status == ATX_TIMEOUT) {
if (self->debug) {
SICSLogWrite(pCmd->out_buf, eStatus);
SICSLogWrite("<TIMEOUT>", eStatus);
}
strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
self->errorCode = MOTCMDTMO;
}
else {
switch (resp[0]) {
case ':':
case ' ':
if (self->debug) {
SICSLogWrite(cmnd, eStatus);
SICSLogWrite(resp, eStatus);
}
break;
case '?':
strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
strncpy(self->dmc2280Error, &resp[1], CMDLEN);
SICSLogWrite(cmnd, eError);
SICSLogWrite(resp, eError);
self->errorCode = BADCMD;
break;
default:
SICSLogWrite(cmnd, eError);
SICSLogWrite(resp, eError);
self->errorCode = BADUNKNOWN;
break;
}
}
#endif
return 0;
}
static int ORHV_QueueCmd(pORHVPSDriv self,
char *cmd,
int cmd_len,
AsyncTxnHandler cb) {
if (cb == NULL)
cb = ORHV_SendCallback;
return ORHV_SendCmd(self, cmd, cmd_len, cb);
}
/** \brief Sends a DMC2280 command to the motor controller.
*
* If the command fails it displays the DMC2280 error message to the client
* and writes it to the log file, also sets errorCode field in motor's
* data structure.
*
* \param self (rw) provides access to the motor's data structure
* \param *command (r) DMC2280 command
* \return
* - SUCCESS
* - FAILURE
* \see SUCCESS FAILURE
*/
/* First character returned by controller is
'?' for an invalid command or
':' for a valid command with no response
' ' for a valid command with a response (':' follows)
*/
static int ORHV_Send(pORHVPSDriv self, char *command, int cmd_len) {
return ORHV_QueueCmd(self, command, cmd_len, ORHV_SendCallback);
}
#endif
/**
* \brief Sends a command and waits for a response
*
@@ -229,6 +158,7 @@ const char* state_name(StateFunc func)
const char* event_name(pEvtEvent event, char* text, int length)
{
char line[1024];
if (event == NULL)
return "<null_event>";
switch (event->event_type) {
@@ -240,9 +170,15 @@ const char* event_name(pEvtEvent event, char* text, int length)
return text;
case eMessageEvent:
snprintf(text, length, "eMessageEvent:");
str_n_cat(text, length, event->event.msg.cmd->out_buf);
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, "|");
str_n_cat(text, length, event->event.msg.cmd->inp_buf);
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 */
@@ -257,6 +193,47 @@ const char* event_name(pEvtEvent event, char* text, int length)
}
}
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;
@@ -275,7 +252,8 @@ static void ORHVState_Unknown(pStateMachine sm, pEvtEvent event) {
do {
pAsyncTxn pCmd = event->event.msg.cmd;
pCmd->inp_buf[pCmd->inp_idx] = '\0';
if (sm->mySubState == 1) { /* Version Request */
if (sm->mySubState == 1) {
/* Version Request */
char* p = strchr(pCmd->inp_buf, 'z');
if (p) {
char line[132];
@@ -287,14 +265,16 @@ static void ORHVState_Unknown(pStateMachine sm, pEvtEvent event) {
sm->mySubState = 2;
return;
}
if (sm->mySubState == 2) { /* HV Get Request */
char* p = strchr(pCmd->inp_buf, 'z');
if (p) {
priv->iValue = (pCmd->inp_buf[1] & 0x3F);
priv->fValue = priv->iValue * (priv->fMax / 63.0);
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;
}
fsm_change_state(sm, ORHVState_Idle);
ORHV_SendCmd(priv, "Hz", 2, fsm_msg_callback);
return;
}
} while (0);
@@ -366,7 +346,7 @@ static void ORHVState_Raising(pStateMachine sm, pEvtEvent event){
ORHV_SendCmd(priv, "Hz", 2, fsm_msg_callback);
return;
}
if (sm->mySubState == 2) {
if (sm->mySubState == 6) {
char cmd[3];
cmd[0] = 'h';
cmd[1] = priv->iValue < 63 ? priv->iValue + 1 : priv->iValue;
@@ -382,11 +362,11 @@ static void ORHVState_Raising(pStateMachine sm, pEvtEvent event){
case eMessageEvent:
do {
pAsyncTxn pCmd = event->event.msg.cmd;
pCmd->inp_buf[pCmd->inp_idx] = '\0';
if (sm->mySubState == 1) { /* HV Get Request */
if (pCmd->inp_buf[0] == 'H' && pCmd->inp_buf[2] == 'z') {
priv->iValue = (pCmd->inp_buf[1] & 0x3F);
priv->fValue = priv->iValue * (priv->fMax / 63.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);
@@ -396,14 +376,24 @@ static void ORHVState_Raising(pStateMachine sm, pEvtEvent event){
fsm_change_state(sm, ORHVState_Lowering);
return;
}
sm->mySubState = 2;
sm->mySubState = 6;
NetWatchRegisterTimer(&priv->state_timer,
100, /* TODO*/
fsm_tmr_callback, sm);
return;
}
NetWatchRegisterTimer(&priv->state_timer,
100, /* TODO*/
fsm_tmr_callback, sm);
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 == 2) { /* HV Set Request */
if (sm->mySubState == 6) {
/* HV Set Request */
if (*pCmd->inp_buf == 0x06) {
/* TODO: ACK */
}
@@ -457,7 +447,7 @@ static void ORHVState_Lowering(pStateMachine sm, pEvtEvent event){
ORHV_SendCmd(priv, "Hz", 2, fsm_msg_callback);
return;
}
if (sm->mySubState == 2) {
if (sm->mySubState == 6) {
cmd[0] = 'h';
cmd[1] = priv->iValue > 0 ? priv->iValue - 1 : priv->iValue;
cmd[2] = 'z';
@@ -473,10 +463,11 @@ static void ORHVState_Lowering(pStateMachine sm, pEvtEvent event){
do {
pAsyncTxn pCmd = event->event.msg.cmd;
pCmd->inp_buf[pCmd->inp_idx] = '\0';
if (sm->mySubState == 1) { /* HV Get Request */
if (pCmd->inp_buf[0] == 'H' && pCmd->inp_buf[2] == 'z') {
priv->iValue = (pCmd->inp_buf[1] & 0x3F);
priv->fValue = priv->iValue * (priv->fMax / 63.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);
@@ -486,14 +477,23 @@ static void ORHVState_Lowering(pStateMachine sm, pEvtEvent event){
fsm_change_state(sm, ORHVState_Raising);
return;
}
sm->mySubState = 2;
sm->mySubState = 6;
NetWatchRegisterTimer(&priv->state_timer,
100, /* TODO*/
fsm_tmr_callback, sm);
return;
}
NetWatchRegisterTimer(&priv->state_timer,
100, /* TODO*/
fsm_tmr_callback, sm);
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 == 2) { /* HV Set Request */
if (sm->mySubState == 6) { /* HV Set Request */
if (*pCmd->inp_buf == 0x06) {
/* TODO: ACK */
}
@@ -546,8 +546,14 @@ static int ORHVPSSetValue( pEVDriver self, float fPos) {
assert(self);
assert(self->pPrivate);
me = (pORHVPSDriv) self->pPrivate;
if (fPos < 0.0 || fPos > me->fMax)
if (me->isLocked) {
me->iError = ORHVPS_ERR_LOCKED;
return 0;
}
if (fPos < 0.0 || fPos > me->fMax) {
me->iError = ORHVPS_ERR_RANGE;
return 0;
}
me->fTarget = fPos;
return 1;
}
@@ -605,9 +611,19 @@ static int ORHVPSSend(pEVDriver self, char *pCommand, char *pReply, int iLen) {
return -1;
}
static int ORHVPSError(pEVDriver self, int *iCode, char *error, int iErrLen) {
/* TODO */
*iCode = -1;
strncpy(error,"TODO Error Messages",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) {
@@ -642,6 +658,7 @@ static void ORHVPSKillPrivate(void *pData) {
static void ORHVPSNotify(void* context, int event)
{
/* TODO */
#if 0
pEVDriver self = (pEVDriver) context;
char line[132];
@@ -762,9 +779,9 @@ void ORHVPSInitProtocol(SicsInterp *pSics) {
}
}
int get_period(float max, float rate) {
//return (int) roundf(1000 * (priv->fMax / 63.0) / priv->fRate);
return (int) roundf((rate * 60 * 1000) / 63.0);
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[])
@@ -813,6 +830,8 @@ pEVDriver CreateORHVPSDriver(int argc, char *argv[])
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);
fsm_change_state(&priv->fsm, ORHVState_Unknown);
@@ -824,6 +843,11 @@ 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);
}
}
int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
@@ -847,6 +871,9 @@ int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
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;
@@ -895,17 +922,75 @@ int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
SCWrite(pCon, cmd, eValue);
return 1;
}
if (strcasecmp("up", argv[1]) == 0) {
priv->fTarget = priv->fUpper;
return 1;
}
if (strcasecmp("down", argv[1]) == 0) {
priv->fTarget = priv->fLower;
return 1;
}
if (strcasecmp("upper", argv[1]) == 0) {
char rsp[CMDLEN];
if (argc > 2) {
if (priv->isLocked) {
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 (priv->isLocked) {
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) {
float max = atof(argv[2]);
if (max >= 0.0 && max <= 2600.0) {
priv->fMax = max;
priv->iPeriod = get_period(priv->fMax, priv->fRate);
if (priv->isLocked) {
SCWrite(pCon, "object is locked", eError);
return 0;
}
else {
SCWrite(pCon, "max must be between 0.0 and 2600.0", eError);
return 0;
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);
@@ -928,14 +1013,20 @@ int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
if (strcasecmp("rate", argv[1]) == 0) {
char rsp[CMDLEN];
if (argc > 2) {
float rate = atof(argv[2]);
if (rate >= 1.0 && rate <= 10.0) {
priv->fRate = rate;
priv->iPeriod = get_period(priv->fMax, priv->fRate);
if (priv->isLocked) {
SCWrite(pCon, "object is locked", eError);
return 0;
}
else {
SCWrite(pCon, "rate must be between 1.0 and 10.0", eError);
return 0;
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);
@@ -959,6 +1050,31 @@ int ORHVPSWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
SCWrite(pCon, rsp, eValue);
return 1;
}
if (strcasecmp("lock", argv[1]) == 0) {
char rsp[CMDLEN];
priv->isLocked = 1;
snprintf(rsp, CMDLEN, "%s.lock = 1", priv->name);
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);
}