- Added Sycamore protocol and command context to SICS
- Added sinfo to SICS - Added driver for TCP/IP Astrium velocity selector - Added driver for TCP/IP Astrium chopper controller
This commit is contained in:
koennecke
929
tcpdocho.c
Normal file
929
tcpdocho.c
Normal file
@@ -0,0 +1,929 @@
|
||||
/*--------------------------------------------------------------
|
||||
This is a driver for the newer Astrium == Dornier chopper
|
||||
systems which use a TCP/IP server for communication.
|
||||
This driver has to take care of some ugliness:
|
||||
- As of december 2005, the communication is in unicode!
|
||||
To go from ASCII to unicode and back one has to
|
||||
add a 0x00 before each character or to remove it.
|
||||
- The controller is slow in responding and the controller
|
||||
must be watched in the environment monitor. This is taken
|
||||
care of by a special SICS task which updates the status
|
||||
regularly and returning cached values anytime else.
|
||||
|
||||
|
||||
Inititial Implementation: Mark Koennecke, December 2005
|
||||
------------------------------------------------------------*/
|
||||
#include <stdlib.h>
|
||||
#include <assert.h>
|
||||
#include <time.h>
|
||||
#include <fortify.h>
|
||||
#include <sics.h>
|
||||
#include <stringdict.h>
|
||||
#include <rs232controller.h>
|
||||
#include <codri.h>
|
||||
#include <stptok.h>
|
||||
#include <commandlog.h>
|
||||
/*========================================================================
|
||||
Our private data structure
|
||||
========================================================================*/
|
||||
#define ASYNMODE 0
|
||||
#define SYNCMODE 1
|
||||
|
||||
typedef struct{
|
||||
prs232 controller;
|
||||
int iRefreshIntervall;
|
||||
time_t nextRefresh;
|
||||
long lTask;
|
||||
pStringDict parameters;
|
||||
int lastError;
|
||||
int numChoppers;
|
||||
int mode;
|
||||
int timeout;
|
||||
char user[132];
|
||||
char pword[132];
|
||||
int stop;
|
||||
int busy;
|
||||
char *config;
|
||||
}TcpDoCho, *pTcpDoCho;
|
||||
/*-----------------------------------------------------------------------
|
||||
Error codes:
|
||||
-----------------------------------------------------------------------*/
|
||||
#define WRONGMODE -8301
|
||||
#define BADCONVERSION -8302
|
||||
#define FAILEDCOMMAND -8303
|
||||
#define BADWRITE -8304
|
||||
#define BADRESPONSE -8306
|
||||
#define UNDRIVABLE -8307
|
||||
#define BADPAR -8308
|
||||
#define ESTOP -8309
|
||||
|
||||
extern char *trim(char *str);
|
||||
#define ABS(x) (x < 0 ? -(x) : (x))
|
||||
|
||||
#define SPEEDTOL 2
|
||||
#define PHASETOL .5
|
||||
/*=============== support functions ===================================*/
|
||||
static int asciiToUnicode(char *input, char **output){
|
||||
int len, i;
|
||||
char *result = NULL;
|
||||
|
||||
len = strlen(input);
|
||||
result = (char *)malloc(2*len*sizeof(char));
|
||||
if(result == NULL){
|
||||
return 0;
|
||||
}
|
||||
memset(result,0,2*len*sizeof(char));
|
||||
for(i = 0; i < len; i++){
|
||||
result[i*2] = input[i];
|
||||
}
|
||||
*output = result;
|
||||
return 2*len;
|
||||
}
|
||||
/*-------------------------------------------------------------------*/
|
||||
static int unicodeToAscii(char *input, int len, char **output){
|
||||
int i, alen;
|
||||
char *result = NULL;
|
||||
|
||||
alen = len/2;
|
||||
|
||||
result = (char *)malloc((alen+1)*sizeof(char));
|
||||
if(result == NULL){
|
||||
return 0;
|
||||
}
|
||||
memset(result,0,(alen+1)*sizeof(char));
|
||||
for(i = 0; i < alen; i++){
|
||||
result[i] = input[i*2];
|
||||
}
|
||||
*output = result;
|
||||
return alen;
|
||||
}
|
||||
/*----------------------------------------------------------------*/
|
||||
static int tcpDoChoSend(pTcpDoCho self, char *command, int choNum,
|
||||
char *value){
|
||||
char buffer[1024];
|
||||
char *converted;
|
||||
int sendlen, status;
|
||||
|
||||
/*
|
||||
format and send command
|
||||
*/
|
||||
self->lastError = 0;
|
||||
if(choNum < 0){
|
||||
snprintf(buffer,1023,"#SOS#%s \r\n",command);
|
||||
} else if(value != NULL){
|
||||
snprintf(buffer,1023,"#SOS#%s%1.1d: %s\r\n",command, choNum,
|
||||
value);
|
||||
} else {
|
||||
snprintf(buffer,1023,"#SOS#%s%1.1d:\r\n",command, choNum);
|
||||
}
|
||||
sendlen = asciiToUnicode(buffer,&converted);
|
||||
if(sendlen == 0){
|
||||
self->lastError = BADCONVERSION;
|
||||
return 0;
|
||||
}
|
||||
status = send(self->controller->pSock->sockid,converted,sendlen,0);
|
||||
free(converted);
|
||||
if(status < 0){
|
||||
self->lastError = BADWRITE;
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*----------------------------------------------------------------*/
|
||||
static int statusComplete(char *statusMessage){
|
||||
char *pPtr = NULL;
|
||||
|
||||
/*
|
||||
currently the status message has no terminator. I try to find
|
||||
the last component of the message which happens to be TIME and
|
||||
the last # after that
|
||||
*/
|
||||
pPtr = strstr(statusMessage,"TIME");
|
||||
if(pPtr != NULL){
|
||||
pPtr = strstr(pPtr+6,"#");
|
||||
if(pPtr != NULL){
|
||||
return 1;
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
/*---------------------------------------------------------------*/
|
||||
static int readChopperNum(char *entry){
|
||||
int num;
|
||||
char *pPtr = NULL;
|
||||
|
||||
pPtr = strstr(entry,"CH");
|
||||
if(pPtr == NULL){
|
||||
return -1;
|
||||
}
|
||||
sscanf(pPtr+3,"%d",&num);
|
||||
return num;
|
||||
}
|
||||
/*---------------------------------------------------------------*/
|
||||
static void addEntry(pTcpDoCho self, int choNum, char *entry){
|
||||
char name[80], value[80], *pPtr = NULL;
|
||||
char num[5];
|
||||
|
||||
memset(name,0,80);
|
||||
memset(value,0,80);
|
||||
snprintf(num,5,"_%1.1d",choNum);
|
||||
pPtr = strstr(entry,"=");
|
||||
if(pPtr != NULL){
|
||||
strncpy(name,entry,pPtr -entry);
|
||||
strncat(trim(name),num,80-strlen(trim(name)));
|
||||
strcpy(value,pPtr+1);
|
||||
if(StringDictExists(self->parameters,name)){
|
||||
StringDictUpdate(self->parameters,name,trim(value));
|
||||
} else {
|
||||
StringDictAddPair(self->parameters,name,trim(value));
|
||||
}
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------*/
|
||||
static int parseStatus(pTcpDoCho self, char *statusMessage){
|
||||
int choNum;
|
||||
char entry[80], *pPtr;
|
||||
|
||||
pPtr = statusMessage;
|
||||
|
||||
/* skip over SOS */
|
||||
pPtr = stptok(pPtr,entry,79,"#");
|
||||
pPtr = stptok(pPtr,entry,79,"#");
|
||||
pPtr = stptok(pPtr,entry,79,"#");
|
||||
|
||||
choNum = readChopperNum(entry);
|
||||
if(choNum < 0){
|
||||
self->lastError = BADRESPONSE;
|
||||
return 0;
|
||||
}
|
||||
while((pPtr = stptok(pPtr,entry,79,"#")) != NULL){
|
||||
addEntry(self,choNum,entry);
|
||||
memset(entry,0,80);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*-----------------------------------------------------------------*/
|
||||
static int tcpDoChoReceive(pTcpDoCho self){
|
||||
char buffer[1024];
|
||||
int len, bytesRead, bytesConverted, bufferStart, status;
|
||||
char *converted = NULL;
|
||||
time_t endTime;
|
||||
|
||||
endTime = time(NULL) + self->timeout;
|
||||
bufferStart = 0;
|
||||
bytesRead = 0;
|
||||
memset(buffer,0,1024*sizeof(char));
|
||||
while(time(NULL) < endTime){
|
||||
if(availableRS232(self->controller)){
|
||||
bytesRead += recv(self->controller->pSock->sockid,buffer+bufferStart,
|
||||
1024 - bytesRead,0);
|
||||
if(bytesRead < 0){
|
||||
self->lastError = BADREAD;
|
||||
return 0;
|
||||
}
|
||||
bytesConverted = unicodeToAscii(buffer,bytesRead,&converted);
|
||||
if(bytesConverted == 0){
|
||||
self->lastError = BADCONVERSION;
|
||||
return 0;
|
||||
}
|
||||
if(strstr(converted,"State") != NULL){
|
||||
if(statusComplete(converted) == 0) {
|
||||
continue;
|
||||
} else {
|
||||
status = parseStatus(self,converted);
|
||||
free(converted);
|
||||
return status;
|
||||
}
|
||||
}
|
||||
if(strstr(converted,"ACCEPT") != NULL){
|
||||
free(converted);
|
||||
return 1;
|
||||
} else if(strstr(converted,"NCCEPT") != NULL){
|
||||
free(converted);
|
||||
self->lastError = FAILEDCOMMAND;
|
||||
return 0;
|
||||
} else {
|
||||
self->lastError = BADRESPONSE;
|
||||
return 0;
|
||||
}
|
||||
} else {
|
||||
SicsWait(1);
|
||||
}
|
||||
}
|
||||
self->lastError = TIMEOUT;
|
||||
return 0;
|
||||
}
|
||||
/*-----------------------------------------------------------------*/
|
||||
static int tcpDoChoCommand(pTcpDoCho self, char *command, int choNum,
|
||||
char *value){
|
||||
int status;
|
||||
|
||||
status = tcpDoChoSend(self,command,choNum,value);
|
||||
if(status == 0){
|
||||
return 0;
|
||||
}
|
||||
return tcpDoChoReceive(self);
|
||||
}
|
||||
/*---------------------------------------------------------------*/
|
||||
static int TcpDoChoConnect(pTcpDoCho self){
|
||||
int status, sendLen, readLen;
|
||||
char *converted = NULL;
|
||||
char buffer[256];
|
||||
|
||||
status = initRS232(self->controller);
|
||||
if(status != 1){
|
||||
self->lastError = status;
|
||||
return 0;
|
||||
}
|
||||
setRS232Timeout(self->controller,5);
|
||||
|
||||
readLen = 255;
|
||||
readRS232(self->controller,(void *)buffer,&readLen);
|
||||
unicodeToAscii(buffer,readLen,&converted);
|
||||
free(converted);
|
||||
|
||||
/*
|
||||
user name
|
||||
*/
|
||||
snprintf(buffer,255,"user:%s\r\n",self->user);
|
||||
sendLen = asciiToUnicode(buffer,&converted);
|
||||
if(sendLen == 0){
|
||||
self->lastError = BADCONVERSION;
|
||||
return 0;
|
||||
}
|
||||
status = send(self->controller->pSock->sockid,converted,sendLen,0);
|
||||
free(converted);
|
||||
if(status < 0){
|
||||
self->lastError = BADSEND;
|
||||
return 0;
|
||||
}
|
||||
readLen = 255;
|
||||
readRS232(self->controller,(void *)buffer,&readLen);
|
||||
unicodeToAscii(buffer,readLen,&converted);
|
||||
free(converted);
|
||||
|
||||
/*
|
||||
password
|
||||
*/
|
||||
snprintf(buffer,255,"password:%s\r\n",self->pword);
|
||||
sendLen = asciiToUnicode(buffer,&converted);
|
||||
if(sendLen == 0){
|
||||
self->lastError = BADCONVERSION;
|
||||
return 0;
|
||||
}
|
||||
status = send(self->controller->pSock->sockid,converted,sendLen,0);
|
||||
free(converted);
|
||||
if(status < 0){
|
||||
self->lastError = BADSEND;
|
||||
return 0;
|
||||
}
|
||||
readLen = 255;
|
||||
readRS232(self->controller,(void *)buffer,&readLen);
|
||||
unicodeToAscii(buffer,readLen,&converted);
|
||||
free(converted);
|
||||
|
||||
/*
|
||||
TODO: responses should be checked to test for a valid login.
|
||||
I do not know at this time how the controller reacts upon a
|
||||
bad login.
|
||||
*/
|
||||
|
||||
return 1;
|
||||
}
|
||||
/*==================== actual driver implementation code ==================*/
|
||||
static int TcpChopperTask(void *pData){
|
||||
pCodri self = NULL;
|
||||
pTcpDoCho pPriv = NULL;
|
||||
int status, code, i;
|
||||
char buffer[80];
|
||||
char error[512];
|
||||
|
||||
self = (pCodri)pData;
|
||||
assert(self);
|
||||
pPriv = (pTcpDoCho)self->pPrivate;
|
||||
assert(pPriv);
|
||||
|
||||
if(pPriv->stop == 1){
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(time(NULL) > pPriv->nextRefresh){
|
||||
if(pPriv->lastError != 0){
|
||||
self->GetError(self,&code,buffer,79);
|
||||
snprintf(error,511,"WARNING: chopper tries to fix: %s",buffer);
|
||||
WriteToCommandLog("Chopper-task:>>", error);
|
||||
status = self->TryFixIt(self,code);
|
||||
if(status == CHFAIL){
|
||||
pPriv->nextRefresh = time(NULL) + pPriv->iRefreshIntervall;
|
||||
return 1;
|
||||
}
|
||||
} else {
|
||||
pPriv->busy = 1;
|
||||
for(i = 0; i < pPriv->numChoppers; i++){
|
||||
status = tcpDoChoCommand(pPriv,"STATE ", i+1,NULL);
|
||||
if(status != 1){
|
||||
/*
|
||||
force error correction
|
||||
*/
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
pPriv->nextRefresh = time(NULL) + pPriv->iRefreshIntervall;
|
||||
pPriv->busy = 0;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*------------------------------------------------------------------------------*/
|
||||
static int TcpDoChoKill(pCodri self){
|
||||
pTcpDoCho pPriv = NULL;
|
||||
|
||||
pPriv = (pTcpDoCho)self->pPrivate;
|
||||
if(!pPriv)
|
||||
return 1;
|
||||
|
||||
if(pPriv->controller != NULL){
|
||||
KillRS232(pPriv->controller);
|
||||
}
|
||||
if(pPriv->parameters != NULL){
|
||||
DeleteStringDict(pPriv->parameters);
|
||||
}
|
||||
if(pPriv->config != NULL){
|
||||
free(pPriv->config);
|
||||
}
|
||||
free(pPriv);
|
||||
return 1;
|
||||
}
|
||||
/*-------------------------------------------------------------------*/
|
||||
static int TcpDoChoConfigure(pCodri pDriv){
|
||||
pTcpDoCho self = NULL;
|
||||
int status;
|
||||
char *pPtr = NULL;
|
||||
char command[80];
|
||||
|
||||
assert(pDriv != NULL);
|
||||
self = (pTcpDoCho)pDriv->pPrivate;
|
||||
assert(self != NULL);
|
||||
|
||||
if(self->config != NULL){
|
||||
pPtr = self->config;
|
||||
while( (pPtr = stptok(pPtr,command,79,"\n")) != NULL){
|
||||
status = tcpDoChoCommand(self,command,-1,NULL);
|
||||
if(status != 1){
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*---------------------------------------------------------------------*/
|
||||
static int TcpDoChoInit(pCodri pDriv){
|
||||
pTcpDoCho self = NULL;
|
||||
int status;
|
||||
|
||||
assert(pDriv != NULL);
|
||||
self = (pTcpDoCho)pDriv->pPrivate;
|
||||
assert(self != NULL);
|
||||
|
||||
self->lastError = 0;
|
||||
self->stop = 0;
|
||||
self->nextRefresh = 0;
|
||||
|
||||
status = TcpDoChoConnect(self);
|
||||
if(status != 1){
|
||||
return 0;
|
||||
}
|
||||
|
||||
status = TcpDoChoConfigure(pDriv);
|
||||
if(status != 1){
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* start the update task */
|
||||
if(self->lTask == 0){
|
||||
self->lTask = TaskRegister(pServ->pTasker,
|
||||
TcpChopperTask,
|
||||
NULL,
|
||||
NULL,
|
||||
pDriv,
|
||||
1);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*-------------------------------------------------------------------*/
|
||||
static void waitForBusy(pTcpDoCho self){
|
||||
time_t endTime;
|
||||
|
||||
endTime = time(NULL) + 10 *60; /* max 10 min */
|
||||
while(time(NULL) < endTime){
|
||||
if(self->busy == 1){
|
||||
SicsWait(3);
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
}
|
||||
WriteToCommandLog("Chopper-task>> ","WARNING: timeout on busy flag, flag forced");
|
||||
self->busy = 0;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int TcpDoChoClose(pCodri pDriv){
|
||||
pTcpDoCho self = NULL;
|
||||
int status;
|
||||
|
||||
assert(pDriv != NULL);
|
||||
self = (pTcpDoCho)pDriv->pPrivate;
|
||||
assert(self != NULL);
|
||||
|
||||
self->stop = 1;
|
||||
if(self->controller != NULL){
|
||||
KillRS232(self->controller);
|
||||
self->controller = NULL;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int dissectName(char *name, char par[80], int *num){
|
||||
char *pPtr = NULL;
|
||||
|
||||
pPtr = strrchr(name,(int)'_');
|
||||
if(pPtr == NULL){
|
||||
return 0;
|
||||
}
|
||||
memset(par,0,80*sizeof(char));
|
||||
strncpy(par,name,pPtr - name);
|
||||
if(sscanf(pPtr+1,"%d",num) != 1){
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int TcpDoChoSetPar2(pCodri pDriv, char *parname, char *value){
|
||||
|
||||
pTcpDoCho self = NULL;
|
||||
int status, choNum;
|
||||
char par[80], buffer[80], state[80];
|
||||
|
||||
assert(pDriv != NULL);
|
||||
self = (pTcpDoCho)pDriv->pPrivate;
|
||||
assert(self != NULL);
|
||||
|
||||
|
||||
/*
|
||||
force status requests right after setting something in order
|
||||
to make the stored status represent the new target values
|
||||
*/
|
||||
if(dissectName(parname,par,&choNum)){
|
||||
/*
|
||||
check for emergency stop
|
||||
*/
|
||||
snprintf(buffer,79,"State_%1.1d",choNum);
|
||||
memset(state,0,80*sizeof(char));
|
||||
StringDictGet(self->parameters,buffer,state,79);
|
||||
if(strstr(state,"E-Stop") != NULL){
|
||||
self->lastError = ESTOP;
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(strcmp(par,"speed") == 0){
|
||||
waitForBusy(self);
|
||||
status = tcpDoChoCommand(self,"SPEED ",choNum,trim(value));
|
||||
tcpDoChoCommand(self,"STATE ",choNum,NULL);
|
||||
if(status != 1){
|
||||
return 0;
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
} else if(strcmp(par,"phase") == 0){
|
||||
waitForBusy(self);
|
||||
status = tcpDoChoCommand(self,"PHASE ",choNum,trim(value));
|
||||
tcpDoChoCommand(self,"STATE ",choNum,NULL);
|
||||
if(status != 1){
|
||||
return 0;
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
self->lastError = UNDRIVABLE;
|
||||
return 0;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static int TcpDoChoHalt(pCodri pDriv){
|
||||
|
||||
pTcpDoCho self = NULL;
|
||||
int status;
|
||||
|
||||
assert(pDriv != NULL);
|
||||
self = (pTcpDoCho)pDriv->pPrivate;
|
||||
assert(self != NULL);
|
||||
|
||||
waitForBusy(self);
|
||||
tcpDoChoCommand(self,"ESTOP :",-1,NULL);
|
||||
return 1;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static int TcpDoChoSetPar(pCodri pDriv, char *parname, float fValue){
|
||||
|
||||
pTcpDoCho self = NULL;
|
||||
int status, choNum;
|
||||
char value[80];
|
||||
char par[80];
|
||||
|
||||
|
||||
assert(pDriv != NULL);
|
||||
self = (pTcpDoCho)pDriv->pPrivate;
|
||||
assert(self != NULL);
|
||||
|
||||
if(dissectName(parname,par,&choNum)){
|
||||
if(strcmp(par,"speed") == 0){
|
||||
snprintf(value,79,"%5.1f", fValue);
|
||||
return TcpDoChoSetPar2(pDriv,parname,value);
|
||||
} else if(strcmp(par,"phase") == 0){
|
||||
snprintf(value,79,"%6.2f",fValue);
|
||||
return TcpDoChoSetPar2(pDriv,parname,value);
|
||||
}
|
||||
}
|
||||
if(strcmp(parname,"updateintervall") == 0){
|
||||
sprintf(value,"%d",(int)fValue);
|
||||
StringDictUpdate(self->parameters,"updateintervall",value);
|
||||
self->iRefreshIntervall = (int)fValue;
|
||||
return 1;
|
||||
} else {
|
||||
snprintf(value,79,"%f",fValue);
|
||||
return TcpDoChoSetPar2(pDriv,parname,value);
|
||||
}
|
||||
}
|
||||
/*---------------------------------------------------------------------*/
|
||||
static int TcpDoChoGetPar(pCodri pDriv, char *parname,
|
||||
char *pBuffer, int iBuflen){
|
||||
pTcpDoCho self = NULL;
|
||||
int status = 0, choNum;
|
||||
char par[80], buffer[80];
|
||||
float val;
|
||||
|
||||
assert(pDriv != NULL);
|
||||
self = (pTcpDoCho)pDriv->pPrivate;
|
||||
assert(self != NULL);
|
||||
|
||||
memset(par,0,80);
|
||||
dissectName(parname,par,&choNum);
|
||||
if(strcmp(par,"speed") == 0){
|
||||
snprintf(buffer,80,"ASPEED_%1.1d", choNum);
|
||||
status = StringDictGet(self->parameters,buffer,pBuffer,iBuflen);
|
||||
} else if(strcmp(par,"phase") == 0){
|
||||
snprintf(buffer,80,"APHASE_%1.1d", choNum);
|
||||
status = StringDictGet(self->parameters,buffer,pBuffer,iBuflen);
|
||||
} else {
|
||||
status = StringDictGet(self->parameters,parname,pBuffer,iBuflen);
|
||||
}
|
||||
return status;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int TcpDoChoCheckPar(pCodri pDriv, char *parname){
|
||||
pTcpDoCho self = NULL;
|
||||
int status = 0, choNum;
|
||||
float val, soll, delta;
|
||||
char value[80], csoll[80], par[80], buffer[80], state[80];
|
||||
|
||||
assert(pDriv != NULL);
|
||||
self = (pTcpDoCho)pDriv->pPrivate;
|
||||
assert(self != NULL);
|
||||
|
||||
/*
|
||||
check for flags first
|
||||
*/
|
||||
if(self->busy){
|
||||
return HWBusy;
|
||||
}
|
||||
if(self->lastError != 0) {
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
/*
|
||||
updateintervall is always Idle
|
||||
*/
|
||||
if(strcmp(parname,"updateintervall") == 0){
|
||||
return HWIdle;
|
||||
}
|
||||
|
||||
/*
|
||||
check for emergency stop
|
||||
*/
|
||||
snprintf(buffer,79,"State_%1.1d",choNum);
|
||||
memset(state,0,80*sizeof(char));
|
||||
StringDictGet(self->parameters,buffer,state,79);
|
||||
if(strstr(state,"E-Stop") != NULL){
|
||||
self->lastError = HWFault;
|
||||
return 0;
|
||||
}
|
||||
|
||||
memset(par,0,80);
|
||||
dissectName(parname,par,&choNum);
|
||||
if(strcmp(par,"speed") == 0){
|
||||
snprintf(buffer,79,"RSPEED_%1.1d", choNum);
|
||||
StringDictGet(self->parameters,buffer,csoll,79);
|
||||
sscanf(csoll,"%f",&soll);
|
||||
snprintf(buffer,79,"ASPEED_%1.1d", choNum);
|
||||
StringDictGet(self->parameters,buffer,value,79);
|
||||
sscanf(value,"%f",&val);
|
||||
delta = ABS(soll - val);
|
||||
if(delta > SPEEDTOL){
|
||||
return HWBusy;
|
||||
} else {
|
||||
return HWIdle;
|
||||
}
|
||||
} else if(strcmp(par,"phase") == 0){
|
||||
snprintf(buffer,79,"RPHASE_%1.1d", choNum);
|
||||
StringDictGet(self->parameters,buffer,csoll,79);
|
||||
sscanf(value,"%f",&soll);
|
||||
snprintf(buffer,79,"APHASE_%1.1d", choNum);
|
||||
StringDictGet(self->parameters,buffer,value,79);
|
||||
sscanf(value,"%f",&val);
|
||||
delta = ABS(soll - val);
|
||||
if(delta > PHASETOL){
|
||||
return HWBusy;
|
||||
} else {
|
||||
return HWIdle;
|
||||
}
|
||||
}
|
||||
self->lastError = BADPAR;
|
||||
return HWFault;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int TcpDoChoError(pCodri pDriv, int *iCode, char *pError, int iLen){
|
||||
pTcpDoCho self = NULL;
|
||||
int status = 0;
|
||||
float val, soll, delta;
|
||||
char value[80];
|
||||
|
||||
assert(pDriv != NULL);
|
||||
self = (pTcpDoCho)pDriv->pPrivate;
|
||||
assert(self != NULL);
|
||||
|
||||
*iCode = self->lastError;
|
||||
switch(self->lastError){
|
||||
case WRONGMODE:
|
||||
strncpy(pError,"Chopper in wrong mode",iLen);
|
||||
break;
|
||||
case BADCONVERSION:
|
||||
strncpy(pError,"Bad ASCII to unicode conversion",iLen);
|
||||
break;
|
||||
case FAILEDCOMMAND:
|
||||
strncpy(pError,"Command not accepted",iLen);
|
||||
break;
|
||||
case BADWRITE:
|
||||
strncpy(pError,"Failed to write to chopper controller",iLen);
|
||||
break;
|
||||
case BADRESPONSE:
|
||||
strncpy(pError,"Chopper controller send invalid command",iLen);
|
||||
break;
|
||||
case UNDRIVABLE:
|
||||
strncpy(pError,"Parameter cannot be changed",iLen);
|
||||
break;
|
||||
case BADPAR:
|
||||
strncpy(pError,"No such parameter",iLen);
|
||||
break;
|
||||
case ESTOP:
|
||||
strncpy(pError,"Emergency stop is engaged",iLen);
|
||||
break;
|
||||
default:
|
||||
getRS232Error(self->lastError,pError,iLen);
|
||||
break;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*---------------------------------------------------------------------*/
|
||||
static int TcpDoChoFix(pCodri pDriv, int iCode){
|
||||
pTcpDoCho self = NULL;
|
||||
int status = 0;
|
||||
float val, soll, delta;
|
||||
char value[80];
|
||||
|
||||
assert(pDriv != NULL);
|
||||
self = (pTcpDoCho)pDriv->pPrivate;
|
||||
assert(self != NULL);
|
||||
|
||||
self->lastError = 0;
|
||||
switch(iCode){
|
||||
case BADCONVERSION:
|
||||
case BADRESPONSE:
|
||||
return CHREDO;
|
||||
break;
|
||||
case WRONGMODE:
|
||||
case FAILEDCOMMAND:
|
||||
case UNDRIVABLE:
|
||||
case BADPAR:
|
||||
case ESTOP:
|
||||
return CHFAIL;
|
||||
break;
|
||||
default:
|
||||
closeRS232(self->controller);
|
||||
status = TcpDoChoConnect(self);
|
||||
if(status == 1){
|
||||
return CHREDO;
|
||||
} else {
|
||||
return CHFAIL;
|
||||
}
|
||||
break;
|
||||
}
|
||||
return CHFAIL;
|
||||
}
|
||||
/*-------------------------------------------------------------------*/
|
||||
pCodri MakeTcpDoChoDriver(char *tclArray, SConnection *pCon){
|
||||
pCodri pNew = NULL;
|
||||
pTcpDoCho self = NULL;
|
||||
const char *pPtr = NULL;
|
||||
char buffer[132];
|
||||
int port, i, count;
|
||||
Tcl_DString pars;
|
||||
char *parnames[] = {"State",
|
||||
"ASPEED",
|
||||
"RSPEED",
|
||||
"APHASE",
|
||||
"RPHASE",
|
||||
"AVETO",
|
||||
"DIR",
|
||||
"MONIT",
|
||||
"FLOWR",
|
||||
"WTEMP",
|
||||
"MTEMP",
|
||||
"MVIBR",
|
||||
"MVACU",
|
||||
"speed",
|
||||
"phase",
|
||||
NULL,
|
||||
};
|
||||
|
||||
/*
|
||||
allocate memory
|
||||
*/
|
||||
pNew = (pCodri)malloc(sizeof(Codri));
|
||||
self = (pTcpDoCho)malloc(sizeof(TcpDoCho));
|
||||
if(!pNew || !self){
|
||||
return NULL;
|
||||
}
|
||||
memset(pNew,0,sizeof(Codri));
|
||||
memset(self,0,sizeof(TcpDoCho));
|
||||
|
||||
/* port and host name */
|
||||
pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"port",TCL_GLOBAL_ONLY);
|
||||
if(!pPtr){
|
||||
SCWrite(pCon,"ERROR: port not found in configuration array for TCP Dornier Chopper",
|
||||
eError);
|
||||
free(pNew);
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
sscanf(pPtr,"%d",&port);
|
||||
pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"host",TCL_GLOBAL_ONLY);
|
||||
if(!pPtr){
|
||||
SCWrite(pCon,"ERROR: host not found in configuration array for TCP Dornier Chopper",
|
||||
eError);
|
||||
free(pNew);
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
memset(buffer,0,132);
|
||||
strncpy(buffer,pPtr, 131);
|
||||
self->controller = createRS232(buffer,port);
|
||||
|
||||
/* number of choppers */
|
||||
pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"nchopper",TCL_GLOBAL_ONLY);
|
||||
if(!pPtr){
|
||||
SCWrite(pCon,"ERROR: nchopper not found in configuration array for TCP Dornier Chopper",
|
||||
eError);
|
||||
free(pNew);
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
sscanf(pPtr,"%d",&port);
|
||||
if(port < 0 || port > 8){
|
||||
SCWrite(pCon,"ERROR: number of choppers not in range 1 - 8",eError);
|
||||
free(pNew);
|
||||
free(self);
|
||||
}
|
||||
self->numChoppers = port;
|
||||
|
||||
/* timeout */
|
||||
pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"timeout",TCL_GLOBAL_ONLY);
|
||||
if(!pPtr){
|
||||
SCWrite(pCon,"ERROR: timeout not found in configuration array for TCP Dornier Chopper",
|
||||
eError);
|
||||
free(pNew);
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
sscanf(pPtr,"%d",&port);
|
||||
self->timeout = port;
|
||||
|
||||
/* username and password */
|
||||
pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"user",TCL_GLOBAL_ONLY);
|
||||
if(!pPtr){
|
||||
SCWrite(pCon,"ERROR: user not found in configuration array for TCP Dornier Chopper",
|
||||
eError);
|
||||
free(pNew);
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
strncpy(self->user,pPtr, 131);
|
||||
pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"password",TCL_GLOBAL_ONLY);
|
||||
if(!pPtr){
|
||||
SCWrite(pCon,"ERROR: password not found in configuration array for TCP Dornier Chopper",
|
||||
eError);
|
||||
free(pNew);
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
strncpy(self->pword,pPtr, 131);
|
||||
|
||||
/*
|
||||
chopper configuration
|
||||
*/
|
||||
pPtr = Tcl_GetVar2(pServ->pSics->pTcl,tclArray,"config",TCL_GLOBAL_ONLY);
|
||||
if(pPtr != NULL){
|
||||
self->config = strdup(pPtr);
|
||||
}
|
||||
|
||||
|
||||
/* initialize some more */
|
||||
self->parameters = CreateStringDict();
|
||||
if(self->parameters == NULL || self->controller == NULL){
|
||||
SCWrite(pCon,"ERROR: out of memory in MakeTcpDoCho",eError);
|
||||
free(pNew);
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
self->iRefreshIntervall = 60;
|
||||
pNew->Init = TcpDoChoInit;
|
||||
pNew->Close = TcpDoChoClose;
|
||||
pNew->Delete = TcpDoChoKill;
|
||||
pNew->SetPar = TcpDoChoSetPar;
|
||||
pNew->SetPar2 = TcpDoChoSetPar2;
|
||||
pNew->GetPar = TcpDoChoGetPar;
|
||||
pNew->CheckPar = TcpDoChoCheckPar;
|
||||
pNew->GetError = TcpDoChoError;
|
||||
pNew->TryFixIt = TcpDoChoFix;
|
||||
pNew->Halt = TcpDoChoHalt;
|
||||
StringDictAddPair(self->parameters,"updateintervall","60");
|
||||
pNew->pPrivate = self;
|
||||
|
||||
/*
|
||||
create parameter list
|
||||
*/
|
||||
Tcl_DStringInit(&pars);
|
||||
count = 0;
|
||||
Tcl_DStringAppend(&pars,"updateintervall",15);
|
||||
while(parnames[count] != NULL){
|
||||
for(i = 0; i < self->numChoppers; i++){
|
||||
snprintf(buffer,131,",%s_%1.1d", parnames[count], i + 1);
|
||||
Tcl_DStringAppend(&pars,buffer,strlen(buffer));
|
||||
}
|
||||
count++;
|
||||
}
|
||||
pNew->pParList = strdup(Tcl_DStringValue(&pars));
|
||||
Tcl_DStringFree(&pars);
|
||||
return pNew;
|
||||
}
|
||||
Reference in New Issue
Block a user