diff --git a/site_ansto/nhq200.c b/site_ansto/nhq200.c index e8f8b63b..3b479280 100644 --- a/site_ansto/nhq200.c +++ b/site_ansto/nhq200.c @@ -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 #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); } diff --git a/site_ansto/orhvps.c b/site_ansto/orhvps.c index a86b1f55..01a333a9 100644 --- a/site_ansto/orhvps.c +++ b/site_ansto/orhvps.c @@ -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 #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 ""; if (func == ORHVState_Unknown) return "ORHVState_Unknown"; @@ -157,7 +152,7 @@ const char* state_name(StateFunc func) return ""; } -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 and ", eError); + SCWrite(pCon, "upper value must be between 0.0 and ", 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 ", eError); + SCWrite(pCon, "lower value must be between 0.0 and ", 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); }