linse/sicspsi
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
b3ac7dff3f5808d74e6fc3921623b1e02d557875
sicspsi/tcpdornier.c
koennecke eb72d5c486 - Adapted indenation to new agreed upon system 
- Fixed bad status in poldi zug driver
2009-02-13 09:01:03 +00:00

1170 lines
30 KiB
C
Raw Blame History

/*--------------------------------------------------------------------------
Yet another driver for a Astrium == Dornier velocity selector. This one
is for the new control software with the TCP-server installed. Actually
this is only valid for the modified protocoll as implemented at ANSTO.
As the motor for the tilt is included with the Astrium package, this code
also has to implement a motor driver for that motor.
As even the Tcp version of the Astrium Control Software is slow in responding,
a state machine has beem implemented for status request. There are only two
states: status request sent (WAITING) or status message processed (READY).
The idea is that getStatus sends a request and returns VSACCEL. It then
tests for data availability. If no data is available, VSACCEL is returned,
else the data is processed. This is done in order not to make SICS
unresponsive for seconds while driving the velocity selector. This scheme
implies that all other functions must take care of the state the connection
is in and possibly read the connection free before doing their work.
There is another efficiency feauture in place which causes status messages
younger then three times timeout to be reused. The VS does everything very
slowly, thus this is good enough.
The new command set for the VS hides this, but it is a known fact that the
VS has two modes of operation: above a certain threshold he can be normally
driven. Below that threshold (~3000 rpm) it must be started. Starting does
sometimes fail. This code allows the VS half an hour to start before flagging
an error.
copyright: see file COPYRIGHT
Mark Koennecke, December 2005
----------------------------------------------------------------------------*/
#include <sics.h>
#include <string.h>
#include <math.h>
#include <tcl.h>
#include <time.h>
#include <modriv.h>
#include <motor.h>
#include <velo.h>
#include <velodriv.h>
#include <evcontroller.h>
#include <evdriver.i>
#include <velo.i>
#include <rs232controller.h>
#include <stringdict.h>
#include <stptok.h>
#define RPMALIFE 3100
/* defines for the communication state*/
#define READY 0
#define WAITING 1
/* error codes */
#define BADREPLY -17501
#define BADACCEPT -17502
#define SELTOFAST -17503
#define FAILEDSTART -17504
extern char *trim(char *str);
#define ABS(x) (x < 0 ? -(x) : (x))
/*----------------------------- The private data structure ---------------*/
typedef struct {
prs232 controller;
int iLastError;
float fTarget;
int comState;
int timeout;
pStringDict status;
time_t requestTimeout;
time_t statusAge;
time_t driveStart;
int debug;
char user[132];
char pword[132];
} TcpDornier, *pTcpDornier;
/*------------------------------------------------------------------------
Some utility stuff to parse the response from the velocity selector.
Any response contains the whole status message. Thus we can use any
response to update the status.
----------------------------------------------------------------------------*/
static void addAstriumPar(char *token, pStringDict target)
{
char name[80], value[80];
char *pos = NULL;
pos = strstr(token, "=");
if (pos != NULL) {
memset(name, 0, 79);
strncpy(name, token, pos - token);
strcpy(value, pos + 1);
}
if (StringDictExists(target, trim(name))) {
StringDictUpdate(target, trim(name), trim(value));
} else {
StringDictAddPair(target, trim(name), trim(value));
}
}
/*------------------------------------------------------------------------*/
static int parseTcpDornierStatus(char *statusText, pStringDict target)
{
char *pos = NULL;
char token[80];
/*
test if this is a valid message
*/
if (strstr(statusText, "#SOS#") == NULL) {
return 0;
}
/*
extract the command status code
*/
pos = stptok(statusText + 5, token, 79, "#");
if (pos == NULL) {
return 0;
}
if (StringDictExists(target, "commandstatus") == 1) {
StringDictUpdate(target, "commandstatus", trim(token));
} else {
StringDictAddPair(target, "commandstatus", trim(token));
}
/*
extract all the others
*/
while ((pos = stptok(pos, token, 79, "#")) != NULL) {
addAstriumPar(token, target);
}
return 1;
}
/*---------------------------------------------------------------------------*/
static int readAstriumReply(prs232 controller, char *buffer, int buflen,
int timeout)
{
int status;
int bytesRead = 0;
time_t endTime;
char *pos = NULL;
endTime = time(NULL) + timeout;
memset(buffer, 0, buflen * sizeof(char));
while (time(NULL) < endTime) {
if (availableRS232(controller)) {
bytesRead = recv(controller->pSock->sockid, buffer + bytesRead,
buflen - bytesRead, 0);
if (bytesRead < 0) {
return BADREAD;
}
/*
as of december-9-2005 the Astrium protocoll has no proper
terminator. I check here for the last data entry which
happens to be BCUNN. The # after that terminates the
message. This algoritjm may need to be modified when
the protocoll changes.
*/
pos = strstr(buffer, "BCUUN");
if (pos != NULL) {
pos = strstr(pos + 1, "#");
if (pos != NULL) {
return 1;
}
}
} else {
SicsWait(1);
}
}
return TIMEOUT;
}
/*---------------------------------------------------------------------------*/
static int readAndDecodeReply(pTcpDornier pDorn)
{
int status;
char buffer[1024];
status =
readAstriumReply(pDorn->controller, buffer, 1023, pDorn->timeout);
if (pDorn->debug > 0) {
printf("Read status = %d, Read data = %s\n", status, buffer);
}
if (status != 1) {
pDorn->iLastError = status;
return 0;
}
pDorn->comState = READY;
pDorn->statusAge = time(NULL);
status = parseTcpDornierStatus(buffer, pDorn->status);
if (status != 1) {
pDorn->iLastError = BADREPLY;
return 0;
}
return 1;
}
/*-------------------------------------------------------------------------*/
static int readAstriumValue(pTcpDornier pDorn, char *key, float *value)
{
int status;
char sValue[80];
status = readAndDecodeReply(pDorn);
if (status != 1) {
return status;
}
status = StringDictGet(pDorn->status, key, sValue, 79);
if (status != 1) {
pDorn->iLastError = BADREPLY;
return 0;
}
sscanf(sValue, "%f", value);
return 1;
}
/*-------------------------------------------------------------------------*/
static int AstriumConnect(pTcpDornier pDorn)
{
int status;
char buffer[256];
status = initRS232(pDorn->controller);
if (status != 1) {
return 0;
}
status = readRS232UntilWord(pDorn->controller, buffer, 255, "ID");
if (status != 1) {
return 0;
}
snprintf(buffer, 255, "user:%s", pDorn->user);
status = writeRS232(pDorn->controller, buffer, strlen(buffer));
if (status != 1) {
return 0;
}
status = readRS232UntilWord(pDorn->controller, buffer, 255, "password");
if (status != 1) {
return 0;
}
snprintf(buffer, 255, "password:%s", pDorn->pword);
status = writeRS232(pDorn->controller, buffer, strlen(buffer));
if (status != 1) {
return 0;
}
status = readRS232UntilWord(pDorn->controller, buffer, 255, "Hello");
if (status != 1) {
return 0;
}
pDorn->comState = READY;
return 1;
}
/*-------------------------------------------------------------------------*/
static float readSpeed(pStringDict dict)
{
char value[80];
float fPos;
/*
The dornier has two speed ranges: Normally it is ASPEED but under
100 it is SSPEED
*/
StringDictGet(dict, "ASPEED", value, 79);
sscanf(value, "%f", &fPos);
if (fPos < 20.) {
StringDictGet(dict, "SSPEED", value, 79);
sscanf(value, "%f", &fPos);
}
return fPos;
}
/*----------------------------------------------------------------------------*/
static int GetTcpDornierPos(pVelSelDriv self, float *fPos)
{
pTcpDornier pDorn = NULL;
int status;
assert(self);
pDorn = (pTcpDornier) self->pPrivate;
/*
with astrium replying so slowly, we should not be bothered to get
the latest and greatest but rather be efficient in the case of frequent
requests in a short time, such as after driving. statusAge is set in
readAndDecode when succcessful.
*/
if (time(NULL) > pDorn->statusAge + 3 * pDorn->timeout) {
if (pDorn->comState == READY) {
status = writeRS232(pDorn->controller, "#SOS#STATE ", 11);
pDorn->comState = WAITING;
if (status != 1) {
pDorn->iLastError = status;
return 0;
}
}
if (readAndDecodeReply(pDorn) != 1) {
return 0;
}
}
*fPos = readSpeed(pDorn->status);
return 1;
}
/*--------------------------------------------------------------------------*/
static int TcpDornierHalt(pVelSelDriv self)
{
pTcpDornier pDorn = NULL;
int status;
assert(self);
pDorn = (pTcpDornier) self->pPrivate;
pDorn->fTarget = .0;
if (pDorn->comState == WAITING) {
readAndDecodeReply(pDorn);
}
status = writeRS232(pDorn->controller, "#SOS#BRAKE ", 11);
pDorn->comState = WAITING;
if (status != 1) {
pDorn->iLastError = status;
return 0;
}
readAndDecodeReply(pDorn);
return 1;
}
/*----------------------------------------------------------------------------*/
static int TcpDornierRun(pVelSelDriv self, float fVal)
{
int status;
char pCommand[50], pAnswer[50];
pTcpDornier pDorn = NULL;
assert(self);
pDorn = (pTcpDornier) self->pPrivate;
if (pDorn->comState == WAITING) {
readAndDecodeReply(pDorn);
}
pDorn->comState = READY;
/*
a requested value of 0 or very little means to stop
*/
if (fVal < self->fTolerance) {
pDorn->driveStart = time(NULL);
return TcpDornierHalt(self);
}
/* This is the normal logic: new value */
snprintf(pCommand, 49, "#SOS#SPEED %5d", (int) fVal);
status = writeRS232(pDorn->controller, pCommand, strlen(pCommand));
if (status != 1) {
pDorn->iLastError = status;
return 0;
}
pDorn->comState = WAITING;
pDorn->driveStart = time(NULL);
status = readAndDecodeReply(pDorn);
if (status != 1) {
return 0;
}
StringDictGet(pDorn->status, "commandstatus", pAnswer, 49);
if (strstr(pAnswer, "ACCEPT") == NULL) {
pDorn->iLastError = BADACCEPT;
return 0;
}
pDorn->fTarget = fVal;
return 1;
}
/*--------------------------------------------------------------------------*/
static int TcpDornierError(pVelSelDriv self, int *iCode,
char *error, int iErrLen)
{
pTcpDornier pDorn = NULL;
assert(self);
pDorn = (pTcpDornier) self->pPrivate;
*iCode = pDorn->iLastError;
switch (pDorn->iLastError) {
case BADREPLY:
strncpy(error, "Velocity Selector sent invalid reply", iErrLen);
break;
case BADACCEPT:
strncpy(error, "VS refused command or speed out of range", iErrLen);
break;
case SELTOFAST:
strncpy(error, "Cannot drive tilt angle while selector is running",
iErrLen);
break;
case FAILEDSTART:
strncpy(error, "Failed to start velocitty selector", iErrLen);
break;
default:
getRS232Error(pDorn->iLastError, error, iErrLen);
break;
}
return 1;
}
/*---------------------------------------------------------------------------*/
static int TcpDornierFixIt(pVelSelDriv self, int iError)
{
pTcpDornier pDorn = NULL;
int status;
assert(self);
pDorn = (pTcpDornier) self->pPrivate;
switch (iError) {
case BADREPLY:
case TIMEOUT:
pDorn->comState = READY;
return VELOREDO;
break;
case BADACCEPT:
pDorn->comState = READY;
case FAILEDCONNECT:
case SELTOFAST:
case FAILEDSTART:
return VELOFAIL;
break;
default:
/*
these are mostly connection errors
Try to reconnect
*/
closeRS232(pDorn->controller);
status = AstriumConnect(pDorn);
pDorn->comState = READY;
if (status) {
return VELOREDO;
} else {
return VELOFAIL;
}
break;
}
return VELOFAIL;
}
/*--------------------------------------------------------------------------*/
static int TcpDornierStat(pVelSelDriv self, int *iCode, float *fCur)
{
pTcpDornier pDorn = NULL;
float fDelta;
static int count = 0;
int status;
assert(self);
pDorn = (pTcpDornier) self->pPrivate;
*iCode = ROTMOVE;
if (pDorn->comState == READY) {
status = writeRS232(pDorn->controller, "#SOS#STATE ", 11);
if (status != 1) {
pDorn->iLastError = status;
return VSFAIL;
}
pDorn->comState = WAITING;
pDorn->requestTimeout = time(NULL) + pDorn->timeout;
}
status = availableRS232(pDorn->controller);
if (status == 1) {
status = readAndDecodeReply(pDorn);
if (status != 1) {
return VSFAIL;
}
pDorn->comState = READY;
*fCur = readSpeed(pDorn->status);
fDelta = *fCur - pDorn->fTarget;
if (ABS(fDelta) < self->fTolerance) {
count++;
/*
we want at least three readings of the selector within
tolerance before we believe it arrived
*/
if (count > 3) {
return VSOK;
} else {
return VSACCEL;
}
} else {
count = 0;
/*
if the VS is still at low speed after half an hour we must
assume that it failed to start and has already had its three
times worth of start tries. We flag this now...
*/
if (time(NULL) > pDorn->driveStart + 30 * 60 && *fCur < RPMALIFE) {
pDorn->iLastError = FAILEDSTART;
return VSFAIL;
}
}
return VSACCEL;
} else {
if (time(NULL) > pDorn->requestTimeout) {
pDorn->iLastError = TIMEOUT;
return VSFAIL;
} else {
return VSACCEL;
}
}
return VELOOK;
}
/*-------------------------------------------------------------------------*/
static int TcpDornierText(pVelSelDriv self, char *pText, int iTextLen)
{
pTcpDornier pDorn = NULL;
char buffer[1024];
const char *name = NULL;
char value[80], entry[132];
int charUsed = 0, status;
assert(self);
pDorn = (pTcpDornier) self->pPrivate;
memset(pText, 0, iTextLen * sizeof(char));
if (time(NULL) > pDorn->statusAge + 3. * pDorn->timeout) {
if (pDorn->comState == READY) {
status = writeRS232(pDorn->controller, "#SOS#STATE ", 11);
pDorn->comState = WAITING;
if (status != 1) {
pDorn->iLastError = status;
return 0;
}
}
if (readAndDecodeReply(pDorn) != 1) {
pDorn->comState = READY;
return 0;
}
}
strcpy(buffer, "");
while ((name = StringDictGetNext(pDorn->status, value, 79)) != NULL) {
if (strstr(name, "commandstatus") == NULL) {
snprintf(entry, 131, "%s = %s\n", name, value);
if (charUsed + 132 < 1023) {
strcat(buffer, entry);
charUsed += strlen(entry);
}
}
}
strncpy(pText, buffer, iTextLen);
return 1;
}
/*------------------------------------------------------------------------*/
static int TcpDornierLoss(pVelSelDriv self, float *fLoss)
{
pTcpDornier pDorn = NULL;
int status;
time_t endTime;
float speed, soll;
static int count = 0;
char value[80];
assert(self);
pDorn = (pTcpDornier) self->pPrivate;
if (pDorn->comState == WAITING) {
readAndDecodeReply(pDorn);
}
status = writeRS232(pDorn->controller, "#SOS#PLOSS ", 11);
if (status != 1) {
pDorn->iLastError = status;
return 0;
}
pDorn->comState = WAITING;
status = readAndDecodeReply(pDorn);
if (status != 1) {
return 0;
}
count = 0;
/*
loop until at speed again
*/
endTime = time(NULL) + 10 * 60;
while (time(NULL) < endTime) {
status = writeRS232(pDorn->controller, "#SOS#STATE ", 11);
if (status != 1) {
pDorn->iLastError = status;
return 0;
}
status = readAndDecodeReply(pDorn);
if (status != 1) {
return 0;
}
speed = readSpeed(pDorn->status);
StringDictGet(pDorn->status, "RSPEED", value, 79);
sscanf(value, "%f", &soll);
if (ABS(soll - speed) < self->fTolerance) {
count++;
if (count > 3) {
StringDictGet(pDorn->status, "PLOSS", value, 79);
sscanf(value, "%f", fLoss);
return 1;
}
} else {
count = 0;
}
}
pDorn->iLastError = TIMEOUT;
return 0;
}
/*-------------------------------------------------------------------------*/
static void TcpDornierKill(void *pData)
{
pTcpDornier pDorn = NULL;
pDorn = (pTcpDornier) pData;
assert(pDorn);
if (pDorn->controller != NULL) {
KillRS232(pDorn->controller);
}
if (pDorn->status != NULL) {
DeleteStringDict(pDorn->status);
}
free(pDorn);
}
/*------------------------------------------------------------------------*/
static int TcpDornierInit(pVelSelDriv self, SConnection * pCon)
{
pTcpDornier pDorn = NULL;
int status;
assert(self);
pDorn = (pTcpDornier) self->pPrivate;
assert(pDorn);
status = AstriumConnect(pDorn);
if (status != 1) {
SCWrite(pCon,
"ERROR: failed to connect or login to Astrium velocity selector controller",
eError);
return 1;
}
status = writeRS232(pDorn->controller, "#SOS#STATE ", 11);
pDorn->comState = WAITING;
if (status != 1) {
SCWrite(pCon,
"ERROR: failed to write status request to controller", eError);
return 0;
}
status = readAndDecodeReply(pDorn);
if (status != 1) {
SCWrite(pCon,
"ERROR: failed to read and decode status request", eError);
return 0;
}
return 1;
}
/*=========================================================================
Section with the motor driver code
==========================================================================*/
typedef struct __TcpAsMoDriv {
/* general motor driver interface
fields. REQUIRED!
*/
float fUpper; /* upper limit */
float fLower; /* lower limit */
char *name;
int (*GetPosition) (void *self, float *fPos);
int (*RunTo) (void *self, float fNewVal);
int (*GetStatus) (void *self);
void (*GetError) (void *self, int *iCode, char *buffer, int iBufLen);
int (*TryAndFixIt) (void *self, int iError, float fNew);
int (*Halt) (void *self);
int (*GetDriverPar) (void *self, char *name, float *value);
int (*SetDriverPar) (void *self, SConnection * pCon,
char *name, float newValue);
void (*ListDriverPar) (void *self, char *motorName, SConnection * pCon);
void (*KillPrivate) (void *self);
pTcpDornier master;
pVelSelDriv selector;
float tiltTarget;
float fTolerance;
} TcpAsMotorDriver, *pTcpAsMotorDriver;
/*-------------------------------------------------------------------------*/
static int TcpMotGetPosition(void *pData, float *fPos)
{
int status;
pTcpAsMotorDriver self = NULL;
char pAnswer[80];
self = (pTcpAsMotorDriver) pData;
assert(self != NULL);
/*
same as above for slow status reponses
*/
if (time(NULL) > self->master->statusAge + 3 * self->master->timeout) {
if (self->master->comState == READY) {
status = writeRS232(self->master->controller, "#SOS#STATE ", 11);
if (status != 1) {
self->master->comState = WAITING;
self->master->iLastError = status;
return 0;
}
}
status = readAstriumValue(self->master, "TTANG", fPos);
if (status != 1) {
return 0;
}
} else {
StringDictGet(self->master->status, "TTANG", pAnswer, 79);
sscanf(pAnswer, "%f", fPos);
return 1;
}
return 1;
}
/*------------------------------------------------------------------------*/
static int TcpMotRunTo(void *pData, float newValue)
{
int status;
pTcpAsMotorDriver self = NULL;
float speed;
char command[80], pAnswer[80];
self = (pTcpAsMotorDriver) pData;
assert(self != NULL);
if (self->master->comState == WAITING) {
readAndDecodeReply(self->master);
}
/*
just another test to make sure that the selector is stopped before
driving that motor
*/
speed = readSpeed(self->master->status);
if (speed > 10) {
self->master->iLastError = SELTOFAST;
return 0;
}
/*
send a command
*/
snprintf(command, 79, "#SOS#TTANGL %6.3f", newValue);
status = writeRS232(self->master->controller, command, strlen(command));
if (status != 1) {
self->master->iLastError = status;
return 0;
}
/*
check the reply
*/
status = readAndDecodeReply(self->master);
if (status != 1) {
return 0;
}
StringDictGet(self->master->status, "commandstatus", pAnswer, 49);
if (strstr(pAnswer, "ACCEPT") == NULL) {
self->master->iLastError = BADACCEPT;
return 0;
}
self->tiltTarget = newValue;
return 1;
}
/*------------------------------------------------------------------------*/
static int TcpAsMotStat(void *pData)
{
pTcpAsMotorDriver self = NULL;
pTcpDornier pDorn = NULL;
float fDelta, value;
int status;
self = (pTcpAsMotorDriver) pData;
assert(self != NULL);
assert(self);
pDorn = self->master;
if (pDorn->comState == READY) {
status = writeRS232(pDorn->controller, "#SOS#STATE ", 11);
if (status != 1) {
pDorn->iLastError = status;
return HWFault;
}
pDorn->comState = WAITING;
pDorn->requestTimeout = time(NULL) + pDorn->timeout;
}
status = availableRS232(pDorn->controller);
if (status == 1) {
status = readAstriumValue(pDorn, "TTANG", &value);
if (status != 1) {
return HWFault;
}
pDorn->comState = READY;
fDelta = value - self->tiltTarget;
if (ABS(fDelta) < self->fTolerance) {
return HWIdle;
} else {
return HWBusy;
}
} else {
if (time(NULL) > pDorn->requestTimeout) {
pDorn->iLastError = TIMEOUT;
return HWFault;
} else {
return HWBusy;
}
}
return HWIdle;
}
/*------------------------------------------------------------------------*/
static void TcpAsGetError(void *pData, int *iCode, char *error, int errLen)
{
pTcpAsMotorDriver self = NULL;
self = (pTcpAsMotorDriver) pData;
assert(self != NULL);
TcpDornierError(self->selector, iCode, error, errLen);
}
/*-----------------------------------------------------------------------*/
static int TcpAsFixit(void *pData, int code, float fnew)
{
pTcpAsMotorDriver self = NULL;
int status;
self = (pTcpAsMotorDriver) pData;
assert(self != NULL);
if (code == BADACCEPT) {
return MOTREDO;
}
status = TcpDornierFixIt(self->selector, code);
switch (status) {
case VELOFAIL:
return MOTFAIL;
break;
case VELOREDO:
return MOTREDO;
break;
default:
assert(0);
break;
}
return MOTFAIL;
}
/*-----------------------------------------------------------------------*/
static int TcpAsMotHalt(void *pData)
{
/*
There is no command to halt the tilt motor!
*/
return 1;
}
/*================= creation code ========================================*/
pVelSelDriv VSCreateTcpDornierANSTO(char *name, Tcl_Interp * pTcl)
{
pVelSelDriv pNew = NULL;
pTcpDornier pDorn = NULL;
MotorDriver *pAstDriv = NULL;
char *pPtr = NULL;
char host[132];
int iVal, iRet, port;
/* the most likely error is the parameters specified are wrong!
So check this first. We''ll use Tcl's result for error reporting.
name is the name of an Tcl array which should hold the info
necessary
*/
/* allocate a TcpDornier structure */
pDorn = (pTcpDornier) malloc(sizeof(TcpDornier));
if (!pDorn) {
return NULL;
}
memset(pDorn, 0, sizeof(TcpDornier));
/* host name */
pPtr = (char *) Tcl_GetVar2(pTcl, name, "Host", TCL_GLOBAL_ONLY);
if (!pPtr) {
Tcl_AppendResult(pTcl, "ERROR: no hostname found in", name, NULL);
free(pDorn);
return NULL;
}
strncpy(host, pPtr, 131);
/* port number */
pPtr = (char *) Tcl_GetVar2(pTcl, name, "Port", TCL_GLOBAL_ONLY);
if (!pPtr) {
Tcl_AppendResult(pTcl, "ERROR: no port number found in", name, NULL);
free(pDorn);
return NULL;
}
iRet = Tcl_GetInt(pTcl, pPtr, &iVal);
if (iRet != TCL_OK) {
free(pDorn);
return NULL;
}
port = iVal;
pDorn->controller = createRS232(host, port);
pDorn->status = CreateStringDict();
if (pDorn->controller == NULL || pDorn->status == NULL) {
free(pDorn);
return NULL;
}
setRS232SendTerminator(pDorn->controller, "\r\n");
/*
username and password
*/
pPtr = (char *) Tcl_GetVar2(pTcl, name, "User", TCL_GLOBAL_ONLY);
if (!pPtr) {
strncpy(pDorn->user, "NVS", 131);
} else {
strncpy(pDorn->user, pPtr, 131);
}
pPtr = (char *) Tcl_GetVar2(pTcl, name, "Password", TCL_GLOBAL_ONLY);
if (!pPtr) {
strncpy(pDorn->pword, "NVS", 131);
} else {
strncpy(pDorn->pword, pPtr, 131);
}
/* time out. This one gets defaulted when not specified */
pPtr = (char *) Tcl_GetVar2(pTcl, name, "Timeout", TCL_GLOBAL_ONLY);
if (!pPtr) {
pDorn->timeout = 5;
} else {
iRet = Tcl_GetInt(pTcl, pPtr, &iVal);
if (iRet == TCL_OK) {
pDorn->timeout = iVal;
}
}
pPtr = (char *) Tcl_GetVar2(pTcl, name, "Debug", TCL_GLOBAL_ONLY);
if (pPtr != NULL) {
setRS232Debug(pDorn->controller, 10);
pDorn->debug = 1;
}
/* business as usual: allocate memory */
pNew = (pVelSelDriv) malloc(sizeof(VelSelDriv));
if (!pNew) {
return NULL;
}
/* zero the world */
memset(pNew, 0, sizeof(VelSelDriv));
pNew->pPrivate = pDorn;
/* initialise function pointers */
pNew->DeletePrivate = TcpDornierKill;
pNew->Halt = TcpDornierHalt;
pNew->GetError = TcpDornierError;
pNew->TryAndFixIt = TcpDornierFixIt;
pNew->GetRotation = GetTcpDornierPos;
pNew->SetRotation = TcpDornierRun;
pNew->GetStatus = TcpDornierStat;
pNew->GetDriverText = TcpDornierText;
pNew->GetLossCurrent = TcpDornierLoss;
pNew->Init = TcpDornierInit;
/* tolerance This one gets defaulted when not specified */
pPtr = (char *) Tcl_GetVar2(pTcl, name, "Tolerance", TCL_GLOBAL_ONLY);
if (!pPtr) {
pNew->fTolerance = 10.;
} else {
iRet = Tcl_GetInt(pTcl, pPtr, &iVal);
if (iRet != TCL_OK) {
pNew->fTolerance = (float) iVal;
}
}
/* done it */
return pNew;
}
/*----------------------------------------------------------------------*/
static pTcpAsMotorDriver MakeAstriumMotor(pVelSelDriv sel,
pTcpDornier pDorn)
{
pTcpAsMotorDriver pNew = NULL;
pNew = (pTcpAsMotorDriver) malloc(sizeof(TcpAsMotorDriver));
if (pNew == NULL) {
return NULL;
}
memset(pNew, 0, sizeof(TcpAsMotorDriver));
pNew->GetPosition = TcpMotGetPosition;
pNew->RunTo = TcpMotRunTo;
pNew->GetStatus = TcpAsMotStat;
pNew->GetError = TcpAsGetError;
pNew->TryAndFixIt = TcpAsFixit;
pNew->Halt = TcpAsMotHalt;
pNew->selector = sel;
pNew->master = pDorn;
pNew->fTolerance = .1;
return (pTcpAsMotorDriver) pNew;
}
/*----------------------------------------------------------------------*/
extern pEVDriver MakeDummyVel(pVelSel pVel); /* in velo.c */
int VelSelTcpFactory(SConnection * pCon, SicsInterp * pSics, void *pData,
int argc, char *argv[])
{
pVelSelDriv pDriv = NULL;
pVelSel pNew = NULL;
pMotor pTilt = NULL;
char pBueffel[256];
Tcl_Interp *pT = NULL;
int iRet;
pEVDriver pMonDriv = NULL;
pTcpAsMotorDriver pAstDriv = NULL;
char *pPtr = NULL;
double d;
float limit;
assert(pCon);
assert(pSics);
/* minimum 3 arguments! */
if (argc < 3) {
SCWrite(pCon,
"ERROR: Insufficient number of arguments to VelSelFactory",
eError);
return 0;
}
/* first one is name */
strtolower(argv[1]);
/* second is the Tcl-array with the parameters
Create the velocity selector driver
*/
pDriv = VSCreateTcpDornierANSTO(argv[2], pSics->pTcl);
if (pDriv == NULL) {
SCWrite(pCon, "ERROR: failed to create velocity selector driver",
eError);
return 0;
}
pAstDriv = MakeAstriumMotor(pDriv, (pTcpDornier) pDriv->pPrivate);
if (pAstDriv == NULL) {
SCWrite(pCon, "ERROR: failed to create velocity selector motor driver",
eError);
return 0;
}
pTilt =
MotorInit("astriumdriver", "selectortilt", (MotorDriver *) pAstDriv);
if (pTilt == NULL) {
SCWrite(pCon, "ERROR: failed to create velocity selector motor",
eError);
return 0;
}
/*
now initialize additional parameters for the motor
*/
pPtr = (char *) Tcl_GetVar2(pSics->pTcl, argv[2], "TiltTolerance",
TCL_GLOBAL_ONLY);
pAstDriv->fTolerance = .1;
if (pPtr != NULL) {
iRet = Tcl_GetDouble(pSics->pTcl, pPtr, &d);
if (iRet == TCL_OK) {
pAstDriv->fTolerance = (float) d;
}
}
limit = 10.;
pPtr =
(char *) Tcl_GetVar2(pSics->pTcl, argv[2], "TiltUpper",
TCL_GLOBAL_ONLY);
if (pPtr != NULL) {
iRet = Tcl_GetDouble(pSics->pTcl, pPtr, &d);
if (iRet == TCL_OK) {
limit = (float) d;
}
}
pAstDriv->fUpper = limit;
MotorSetPar(pTilt, pCon, "softupperlim", limit);
pPtr =
(char *) Tcl_GetVar2(pSics->pTcl, argv[2], "TiltLower",
TCL_GLOBAL_ONLY);
limit = -10.;
if (pPtr != NULL) {
iRet = Tcl_GetDouble(pSics->pTcl, pPtr, &d);
if (iRet == TCL_OK) {
limit = (float) d;
}
}
pAstDriv->fLower = limit;
MotorSetPar(pTilt, pCon, "softlowerlim", limit);
/* now initialise this and install it as command */
pNew = VSCreate(pTilt, pDriv);
if (!pNew) {
SCWrite(pCon, "ERROR: creating velocity selector, no memory", eError);
return 0;
}
iRet = pDriv->Init(pDriv, pCon);
if (!iRet) {
SCWrite(pCon, "ERROR: failed to initialize velocity selector", eError);
VSDestroy(pNew);
return 0;
}
pNew->pName = strdup(argv[1]);
iRet = AddCommand(pSics, argv[1], VelSelAction, VSDestroy, pNew);
if (!iRet) {
sprintf(pBueffel, "ERROR: duplicate command %s not created", argv[2]);
SCWrite(pCon, pBueffel, eError);
VSDestroy((void *) pNew);
return 0;
}
/* install the evcontroller bit of the velocity selector */
pMonDriv = MakeDummyVel(pNew);
if (!pMonDriv) {
RemoveCommand(pSics, argv[1]);
SCWrite(pCon, "ERROR: failed to create monitor for nvs", eError);
return 0;
}
pBueffel[0] = '\0';
strcpy(pBueffel, argv[1]);
strcat(pBueffel, "watch");
pNew->pMonitor = CreateEVController(pMonDriv, pBueffel, &iRet);
if (!pNew->pMonitor) {
DeleteEVDriver(pMonDriv); /* was missing M.Z. Jul 04 */
SCWrite(pCon, "ERROR: failed to create monitor for nvs", eError);
return 0;
}
iRet = AddCommand(pSics, pBueffel, EVControlWrapper,
NULL, pNew->pMonitor);
if (!iRet) {
sprintf(pBueffel, "ERROR: duplicate command %s not created", pBueffel);
RemoveCommand(pSics, argv[1]);
return 0;
}
EVRegisterController(FindEMON(pSics), pBueffel, pNew->pMonitor, pCon);
return iRet;
}
Reference in New Issue View Git Blame Copy Permalink