linse/sicspsi
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
a5f315b56af38da92578a76edd2c5f8b56f507fb
sicspsi/velodornier.c
koennecke a5f315b56a - 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
2005-12-22 22:16:13 +00:00

684 lines
20 KiB
C
Raw Blame History

/*--------------------------------------------------------------------------
V E L O D O R N I E R
A driver for a Dornier velocity selector, connected to our world
via a terminal server and TCP/IP. Please note, that the protocoll
implemented by the velocity selector PC has been changed in the following
ways from the standard as supplied by Dornier:
- no messages we have not asked for.
- The whole response will be concatenated in a string, each item
separated by a /. At the end is a single <CR><LF>. This is because
our terminal server reads only up to the first terminator.
Author: Mark Koennecke, Juli 1997
Thoroughly revised: October 1997 Mark Koennecke
Copyright:
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.
----------------------------------------------------------------------------*/
#include "sics.h"
#include <string.h>
#include <math.h>
#include <tcl.h>
#include <time.h>
#include <fortify.h>
typedef struct __VelSelDriv *pVelSelDriv;
#include <velodriv.h>
#include "velodorn.h"
#include "hardsup/serialsinq.h"
#include "hardsup/el734_def.h"
#include "hardsup/el734fix.h"
#include <serialwait.h>
/* VELO* MUST be the same as in velo.i!*/
#define VELOREDO -1
#define VELOFAIL 0
#define VELOOK 1
#define VSNOCON 0
#define VSOK 1
#define VSACCEL -7
#define VSFAIL -2
typedef enum {vStart, eRegel, eHalted} eVeloMode;
#define RPMALIFE 3090
/*----------------------------- The private data structure ---------------*/
typedef struct{
char *pComputer;
int iPort;
int iChannel;
int iTimeOut;
int iLastError;
void *pData;
eVeloMode eVelo;
time_t t_End;
float fTarget;
int iBusy;
float fLastRPM;
} Dornier, *pDornier;
/*----------------------------------------------------------------------------*/
static int GetDornierPos(pVelSelDriv self, float *fPos)
{
pDornier pDorn = NULL;
DornierStatus DStatus;
int iRet;
assert(self);
pDorn = (pDornier)self->pPrivate;
if(pDorn->iBusy)
{
*fPos = pDorn->fLastRPM;
return 1;
}
pDorn->iBusy = 1;
iRet = GetDornierStatus(&pDorn->pData, &DStatus);
pDorn->iBusy = 0;
if(iRet < 0)
{
*fPos = -9999.;
return 0;
}
*fPos = DStatus.cur_rpm;
pDorn->fLastRPM = DStatus.cur_rpm;
return 1;
}
/*--------------------------------------------------------------------------*/
static int DornierHalt(pVelSelDriv self)
{
pDornier pDorn = NULL;
int iRet;
char pCom[] = {"HAL"};
char pAnswer[80];
assert(self);
pDorn = (pDornier)self->pPrivate;
iRet = SerialWriteRead(&pDorn->pData, pCom,pAnswer,79);
/* iRet = SerialSicsExecute(&pDorn->pData, pCom,pAnswer,79);
*/
pDorn->eVelo = eHalted;
if(iRet != 1)
{
pDorn->iLastError = iRet;
return 0;
}
return 1;
}
/*----------------------------------------------------------------------------*/
static int DornierRun(pVelSelDriv self, float fVal)
{
int iRet;
char pCommand[50], pAnswer[50];
pDornier pDorn = NULL;
assert(self);
pDorn = (pDornier)self->pPrivate;
/* less then zero, means halt in this case */
if(fVal < RPMALIFE)
{
DornierHalt(self);
return 1;
}
/* if busy, wait until free */
while(pDorn->iBusy)
{
SicsWait(1);
}
switch(pDorn->eVelo)
{
case eHalted:
strcpy(pCommand,"SST");
pDorn->fTarget = fVal;
pDorn->eVelo = vStart;
pDorn->t_End = time(NULL) + 1800; /* start time + 30 min */
break;
case eRegel:
sprintf(pCommand,"SDR %5u",(int)fVal);
pDorn->fTarget = fVal;
break;
}
pDorn->iBusy = 1;
iRet = SerialWriteRead(&pDorn->pData,pCommand,pAnswer,49);
/* iRet = SerialSicsExecute(&pDorn->pData,pCommand,pAnswer,49);
*/
pDorn->iBusy = 0;
if(iRet != 1)
{
pDorn->iLastError = iRet;
return 0;
}
return 1;
}
/*--------------------------------------------------------------------------*/
static int DornierError(pVelSelDriv self, int *iCode, char *error, int iErrLen)
{
pDornier pDorn = NULL;
assert(self);
pDorn = (pDornier)self->pPrivate;
*iCode = pDorn->iLastError;
switch(pDorn->iLastError)
{
case NOCOMMAND:
strncpy(error,"No command was specified, internal error",iErrLen);
break;
case ECHOMISSING:
strncpy(error,"No echo received, may be busy",iErrLen);
break;
case BADANALYSIS:
strncpy(error,"Error analysing status messge",iErrLen);
break;
case STARTTIMEOUT:
strncpy(error,"Velocity Selector failed to start",iErrLen);
break;
default:
SerialError(pDorn->iLastError,error,iErrLen);
break;
}
return 1;
}
/*---------------------------------------------------------------------------*/
static int DornierFixIt(pVelSelDriv self, int iError)
{
pDornier pDorn = NULL;
int iRet;
assert(self);
pDorn = (pDornier)self->pPrivate;
switch(iError)
{
/* network errors */
case NOCONNECTION:
case EL734__BAD_FLUSH:
case EL734__BAD_RECV:
case EL734__BAD_RECV_NET:
case EL734__BAD_RECV_UNKN:
case EL734__BAD_RECVLEN:
case EL734__BAD_RECV1:
case EL734__BAD_RECV1_PIPE:
case EL734__BAD_RNG:
case EL734__BAD_SEND:
case EL734__BAD_SEND_PIPE:
case EL734__BAD_SEND_NET:
case EL734__BAD_SEND_UNKN:
case EL734__BAD_SENDLEN:
SerialClose(&pDorn->pData);
iRet = SerialForceOpen(&pDorn->pData,pDorn->pComputer,
pDorn->iPort, pDorn->iChannel);
if(iRet != 1)
{
return VELOREDO;
}
else
{
return VELOFAIL;
}
break;
/* handable protocoll errors */
case ECHOMISSING:
case EL734__BAD_TMO:
case -1:
case TIMEOUT:
return VELOREDO;
break;
case STARTTIMEOUT:
return VELOFAIL;
break;
case INVALIDSTATUS:
return VELOREDO;
break;
default:
return VELOFAIL;
break;
}
return VELOFAIL;
}
/*--------------------------------------------------------------------------*/
static int DornierStat(pVelSelDriv self, int *iCode, float *fCur)
{
pDornier pDorn = NULL;
int iRet, iErrStat;
DornierStatus sStatus;
char pCommand[80];
char pAnswer[80];
float fDelta;
static int iCount = 0;
assert(self);
pDorn = (pDornier)self->pPrivate;
/* if busy, return VSACCEL */
if(pDorn->iBusy)
{
*fCur = pDorn->fLastRPM;
return VSACCEL;
}
*iCode = ROTMOVE;
/* get the status */
pDorn->iBusy = 1;
iRet = GetDornierStatus(&pDorn->pData,&sStatus);
pDorn->iBusy = 0;
if( iRet < 0)
{
iErrStat = DornierFixIt(self,iRet);
*iCode = 9384; /* ignore this one */
if(iErrStat == VELOREDO)
{
/* the velcocity selector will not respond when busy.
Therefore such cases are interpreted as busy!
*/
return VSACCEL;
}
else
{
/* this is what we got if there is network trouble */
return VELOFAIL;
}
}
/* some serious logic because of multi - modes */
switch(pDorn->eVelo)
{
case vStart:
*iCode = ROTSTART;
*fCur = 0.;
if(sStatus.cur_rpm >= RPMALIFE)
{
sprintf(pCommand,"SDR %5u",(int)pDorn->fTarget);
pDorn->iBusy = 1;
iRet = SerialWriteRead(&pDorn->pData,pCommand,pAnswer,49);
/* iRet = SerialSicsExecute(&pDorn->pData,pCommand,pAnswer,49);
*/
pDorn->iBusy = 0;
pDorn->eVelo = eRegel;
iCount = 0;
if(iRet != 1)
{
pDorn->iLastError = iRet;
return VELOFAIL;
}
return VSACCEL;
}
else
{
if(time(NULL) > pDorn->t_End)
{
pDorn->iLastError = STARTTIMEOUT;
return VELOFAIL;
}
return VSACCEL;
}
break;
case eRegel:
*iCode = ROTMOVE;
*fCur = (float)sStatus.cur_rpm;
pDorn->fLastRPM = *fCur;
fDelta = sStatus.cur_rpm - sStatus.nom_rpm;
if(fDelta < 0)
{
fDelta = - fDelta;
}
if(fDelta > self->fTolerance)
{
iCount = 0;
return VSACCEL;
}
else
{
iCount++;
if(iCount > 4)
{
return VSOK;
}
else
{
return VSACCEL;
}
}
case eHalted:
if(sStatus.iHz > 5)
{
*iCode = ROTMOVE;
*fCur = (float)sStatus.cur_rpm;
pDorn->fLastRPM = *fCur;
return VSACCEL;
}
else
{
return VSOK;
}
break;
}
return VELOOK;
}
/*-------------------------------------------------------------------------*/
static int DornierText(pVelSelDriv self, char *pText, int iTextLen)
{
pDornier pDorn = NULL;
int iRet, iErrStat;
DornierStatus sStatus;
char pBueffel[1024];
char pHelp[80];
assert(self);
pDorn = (pDornier)self->pPrivate;
/* get the status */
iRet = GetDornierStatus(&pDorn->pData,&sStatus);
if(iRet < 0)
{
pDorn->iLastError = iRet;
return 0;
}
/* format it to a string */
sprintf(pHelp,"RPM: %d , should %d\n",sStatus.cur_rpm,sStatus.nom_rpm);
strcpy(pBueffel,pHelp);
sprintf(pHelp,"State: %s\n",sStatus.rm);
strcat(pBueffel,pHelp);
sprintf(pHelp,"Current: %d\n",sStatus.pwr);
strcat(pBueffel,pHelp);
sprintf(pHelp,"Rotor T: %d, Housing T: %d\n",sStatus.rot_temp,
sStatus.cont_temp);
strcat(pBueffel,pHelp);
sprintf(pHelp,"Cooling: In-T: %d, Out-T: %d, Flow: %f\n",
sStatus.inl_temp,sStatus.outl_temp,sStatus.cool_wat);
strcat(pBueffel,pHelp);
sprintf(pHelp,"Vaccum: %f, Accel: %f",sStatus.vacuum, sStatus.accel);
strcat(pBueffel,pHelp);
strncpy(pText,pBueffel, iTextLen);
return 1;
}
/*------------------------------------------------------------------------*/
static int SimSetRot(pVelSelDriv self, float fNew)
{
return 1;
}
/*------------------------------------------------------------------------*/
static int DornierLoss(pVelSelDriv self, float *fLoss)
{
pDornier pDorn = NULL;
int iRet, iErrStat, iDelta;
DornierStatus DStatus;
char pCommand[] = {"BRE"};
char pAnswer[80];
static int iCount;
static int iError;
assert(self);
pDorn = (pDornier)self->pPrivate;
/* wait until not busy, to do it */
while(pDorn->iBusy)
{
SicsWait(1);
}
/* send a command */
pDorn->iBusy = 1;
iRet = SerialWriteRead(&pDorn->pData,pCommand,pAnswer,79);
pDorn->iBusy = 0;
if(iRet != 1)
{
pDorn->iLastError = iRet;
return 0;
}
/* wait 10 seconds before doing anything */
SicsWait(10);
/* loop until back to speed again */
for( ; ; )
{
iRet = GetDornierStatus(&pDorn->pData, &DStatus);
if(iRet)
{
iError = 0;
iDelta = DStatus.cur_rpm - DStatus.nom_rpm;
if(iDelta < 0)
{
iDelta = -iDelta;
}
if(iDelta < 15)
{
iCount++;
if(iCount > 4)
{
break;
}
}
else
{
iCount = 0;
}
}
else
{
iError++;
if(iError > 10)
{
break;
}
}
}
*fLoss = DStatus.pwr;
return 1;
}
/*-------------------------------------------------------------------------*/
static void DornierKill(void *pData)
{
pDornier pDorn = NULL;
pDorn = (pDornier)pData;
assert(pDorn);
SerialSend(&pDorn->pData,"TTY");
SerialClose(&pDorn->pData);
if(pDorn->pComputer)
{
free(pDorn->pComputer);
}
free(pDorn);
}
/*------------------------------------------------------------------------*/
static int DornierInit(pVelSelDriv self, SConnection *pCon)
{
pDornier pDorn = NULL;
int iRet, iError;
char pError[80], pBueffel[256];
assert(self);
pDorn = (pDornier)self->pPrivate;
assert(pDorn);
/* if there is no connection, open it */
if(!pDorn->pData)
{
iRet = SerialForceOpen(&pDorn->pData, pDorn->pComputer,
pDorn->iPort, pDorn->iChannel);
if(iRet != 1)
{
SerialError(iRet,pError,79);
sprintf(pBueffel,"ERROR: %s when initialising Velocity selector",
pError);
SCWrite(pCon,pBueffel,eError);
return 0;
}
SerialConfig(&pDorn->pData,pDorn->iTimeOut);
SerialATerm(&pDorn->pData,"1\r");
SerialSendTerm(&pDorn->pData,"\n");
}
/* tell him that we want control */
iRet = DornierSend(&pDorn->pData,"REM",pError,79);
if(iRet != 1)
{
sprintf(pBueffel,"ERROR: %s while switching velocity selector to remote",
pError);
SCWrite(pCon,pBueffel,eError);
return 1;
}
pDorn->eVelo = eHalted;
pDorn->iBusy = 0;
return 1;
}
/*-------------------------------------------------------------------------*/
pVelSelDriv VSCreateDornierSINQ(char *name, Tcl_Interp *pTcl)
{
pVelSelDriv pNew = NULL;
pDornier pDorn = NULL;
const char *pPtr = NULL;
int iVal, iRet;
/* 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 Dornier structure */
pDorn = (pDornier)malloc(sizeof(Dornier));
if(!pDorn)
{
return NULL;
}
memset(pDorn,0,sizeof(Dornier));
/* host name */
pPtr = Tcl_GetVar2(pTcl,name,"Host",TCL_GLOBAL_ONLY);
if(!pPtr)
{
Tcl_AppendResult(pTcl,"ERROR: no hostname found in",name,NULL);
free(pDorn);
return NULL;
}
pDorn->pComputer = strdup(pPtr);
/* port number */
pPtr = 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->pComputer);
free(pDorn);
return NULL;
}
pDorn->iPort = iVal;
/* channel number */
pPtr = Tcl_GetVar2(pTcl,name,"Channel",TCL_GLOBAL_ONLY);
if(!pPtr)
{
Tcl_AppendResult(pTcl,"ERROR: no channel number found in",name,NULL);
free(pDorn);
return NULL;
}
iRet = Tcl_GetInt(pTcl,pPtr,&iVal);
if(iRet != TCL_OK)
{
free(pDorn->pComputer);
free(pDorn);
return NULL;
}
pDorn->iChannel = iVal;
/* time out. This one gets defaulted when not specified */
pPtr = Tcl_GetVar2(pTcl,name,"Timeout",TCL_GLOBAL_ONLY);
if(!pPtr)
{
pDorn->iTimeOut = 1000;
}
else
{
iRet = Tcl_GetInt(pTcl,pPtr,&iVal);
if(iRet != TCL_OK)
{
pDorn->iTimeOut = 1000;
}
pDorn->iTimeOut = iVal;
}
/* 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 = DornierKill;
pNew->Halt = DornierHalt;
pNew->GetError = DornierError;
pNew->TryAndFixIt = DornierFixIt;
pNew->GetRotation = GetDornierPos;
pNew->SetRotation = DornierRun;
pNew->GetStatus = DornierStat;
pNew->GetDriverText = DornierText;
pNew->GetLossCurrent = DornierLoss;
pNew->Init = DornierInit;
/* done it */
return pNew;
}
Reference in New Issue View Git Blame Copy Permalink