a5f315b56a
- Added sinfo to SICS - Added driver for TCP/IP Astrium velocity selector - Added driver for TCP/IP Astrium chopper controller
930 lines
24 KiB
C
930 lines
24 KiB
C
/*--------------------------------------------------------------
|
|
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;
|
|
}
|