- New drivers for EL737 and EL734 high performance
- Changes to makefiles
This commit is contained in:
cvs
610
dornier2.c
Normal file
610
dornier2.c
Normal file
@@ -0,0 +1,610 @@
|
||||
/*------------------------------------------------------------------------
|
||||
|
||||
Another driver for a Dornier velocity selector. This is for a newer
|
||||
version of the velocity selector driver as delivered with SANS-2. It
|
||||
also uses a direct connection to the terminal server without David Maden's
|
||||
SerPortServer program in between.
|
||||
|
||||
I believe this is for Dornier software version: NGS037 of 2002.
|
||||
|
||||
The protocoll is inconsistent: status messages come back with a <cr>,
|
||||
command responses tend to come back with a \ and no <cr>!
|
||||
|
||||
There is a scheme here: while waiting for status reponses during driving,
|
||||
the last status read is used for any requests.
|
||||
|
||||
copyright: see file COPYRIGHT
|
||||
|
||||
Mark Koennecke, July 2003
|
||||
---------------------------------------------------------------------------*/
|
||||
#include <sics.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <tcl.h>
|
||||
#include <time.h>
|
||||
#include <fortify.h>
|
||||
#include <rs232controller.h>
|
||||
typedef struct __VelSelDriv *pVelSelDriv;
|
||||
|
||||
#include <velodriv.h>
|
||||
#include "velodorn.h"
|
||||
|
||||
/* VELO* MUST be the same as in velo.i!*/
|
||||
#define VELOREDO 2
|
||||
#define VELOFAIL 0
|
||||
#define VELOOK 1
|
||||
#define VSNOCON 0
|
||||
#define VSOK 1
|
||||
#define VSACCEL -7
|
||||
#define VSFAIL -2
|
||||
|
||||
/*--------- special Dornier conditions*/
|
||||
#define STARTED -88
|
||||
#define HALTREQ -77
|
||||
|
||||
/*---------- DORNIER status modes */
|
||||
#define STATSEND 1
|
||||
#define STATREAD 2
|
||||
/*----------------------------- The private data structure ---------------*/
|
||||
typedef struct{
|
||||
prs232 controller;
|
||||
int iTimeOut;
|
||||
int iLastError;
|
||||
time_t t_End;
|
||||
time_t t_timeout;
|
||||
float fTarget;
|
||||
float fLastRPM;
|
||||
int statusMode;
|
||||
DornierStatus lastStatus;
|
||||
int minRPM; /* the minimum control speed of the thing*/
|
||||
int haltCount;
|
||||
} Dornier, *pDornier;
|
||||
/*------------------------------------------------------------------*/
|
||||
static int requestDornierStatus(pDornier pDorn){
|
||||
int status;
|
||||
|
||||
status = writeRS232(pDorn->controller,"???\n",4);
|
||||
if(status < 0){
|
||||
pDorn->iLastError = status;
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*------------------------------------------------------------------*/
|
||||
static int readAndInterpretStatus(pDornier pDorn, DornierStatus *DStatus){
|
||||
int status, datalen;
|
||||
char reply[512];
|
||||
|
||||
datalen = 512;
|
||||
status = readRS232TillTerm(pDorn->controller,reply,&datalen);
|
||||
if(status < 0){
|
||||
pDorn->iLastError = status;
|
||||
return 0;
|
||||
}
|
||||
|
||||
DecodeNewDornierStatus(reply,DStatus);
|
||||
return 1;
|
||||
}
|
||||
/*--------------------------------------------------------------------*/
|
||||
static int GetDornierPos(pVelSelDriv self, float *fPos)
|
||||
{
|
||||
pDornier pDorn = NULL;
|
||||
DornierStatus DStatus;
|
||||
int status;
|
||||
|
||||
assert(self);
|
||||
pDorn = (pDornier)self->pPrivate;
|
||||
|
||||
if(pDorn->statusMode == STATSEND){
|
||||
if(!requestDornierStatus(pDorn)){
|
||||
*fPos = -9999.;
|
||||
return 0;
|
||||
}
|
||||
if(!readAndInterpretStatus(pDorn,&DStatus)){
|
||||
*fPos = -9999.;
|
||||
return 0;
|
||||
}
|
||||
pDorn->lastStatus = DStatus;
|
||||
}
|
||||
*fPos = pDorn->lastStatus.cur_rpm;
|
||||
pDorn->fLastRPM = pDorn->lastStatus.cur_rpm;
|
||||
return 1;
|
||||
}
|
||||
/*--------------------------------------------------------------------------*/
|
||||
static int DornierHalt(pVelSelDriv self)
|
||||
{
|
||||
pDornier pDorn = NULL;
|
||||
int iRet;
|
||||
char pCom[50];
|
||||
char pAnswer[80];
|
||||
|
||||
assert(self);
|
||||
pDorn = (pDornier)self->pPrivate;
|
||||
|
||||
snprintf(pCom,49,"SDR %d\n",pDorn->minRPM);
|
||||
iRet = transactRS232(pDorn->controller,pCom,strlen(pCom),
|
||||
pAnswer,79);
|
||||
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;
|
||||
int startFlag = 0;
|
||||
|
||||
assert(self);
|
||||
pDorn = (pDornier)self->pPrivate;
|
||||
|
||||
/*
|
||||
less then minRPM, means halt in this case.
|
||||
Accept this only after three times, see code in GetError as well.
|
||||
*/
|
||||
if(fVal < pDorn->minRPM - self->fTolerance)
|
||||
{
|
||||
if(pDorn->haltCount < 3){
|
||||
pDorn->iLastError = HALTREQ;
|
||||
return 0;
|
||||
}
|
||||
strcpy(pCommand,"HAL\n");
|
||||
} else {
|
||||
if(pDorn->lastStatus.cur_rpm < pDorn->minRPM){
|
||||
strcpy(pCommand,"SST\n");
|
||||
startFlag = 1;
|
||||
pDorn->fTarget = pDorn->minRPM;
|
||||
} else {
|
||||
snprintf(pCommand,49,"SDR %5u\n",(int)nintf(fVal));
|
||||
pDorn->fTarget = fVal;
|
||||
}
|
||||
}
|
||||
|
||||
setRS232ReplyTerminator(pDorn->controller,"\\");
|
||||
iRet = transactRS232(pDorn->controller,pCommand,strlen(pCommand),
|
||||
pAnswer,49);
|
||||
setRS232ReplyTerminator(pDorn->controller,"\r\n");
|
||||
if(iRet != 1)
|
||||
{
|
||||
pDorn->iLastError = iRet;
|
||||
return 0;
|
||||
}
|
||||
pDorn->statusMode = STATSEND;
|
||||
if(startFlag){
|
||||
pDorn->iLastError = STARTED;
|
||||
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 HALTREQ:
|
||||
strncpy(error,"Repeat command if you really want to HALT selector",
|
||||
iErrLen);
|
||||
pDorn->haltCount++;
|
||||
break;
|
||||
case STARTED:
|
||||
strncpy(error,
|
||||
"Started selector, standby and check manually when ready",
|
||||
iErrLen);
|
||||
break;
|
||||
default:
|
||||
getRS232Error(pDorn->iLastError,error,iErrLen);
|
||||
break;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*-------------------------------------------------------------------*/
|
||||
static int DornierFixIt(pVelSelDriv self, int iCode){
|
||||
pDornier pDorn = NULL;
|
||||
int status;
|
||||
|
||||
assert(self);
|
||||
pDorn = (pDornier)self->pPrivate;
|
||||
|
||||
switch(iCode){
|
||||
case NOTCONNECTED:
|
||||
status = initRS232(pDorn->controller);
|
||||
if(status){
|
||||
return VELOREDO;
|
||||
} else {
|
||||
return VELOFAIL;
|
||||
}
|
||||
break;
|
||||
case TIMEOUT:
|
||||
case INCOMPLETE:
|
||||
return VELOREDO;
|
||||
break;
|
||||
default:
|
||||
return VELOFAIL;
|
||||
}
|
||||
}
|
||||
/*---------------------------------------------------------------------*/
|
||||
static int statusSendHandler(pDornier pDorn){
|
||||
int status;
|
||||
|
||||
if(!requestDornierStatus(pDorn)){
|
||||
return VSFAIL;
|
||||
}
|
||||
pDorn->t_timeout = time(NULL) + pDorn->iTimeOut/1000;
|
||||
pDorn->statusMode = STATREAD;
|
||||
return VSACCEL;
|
||||
}
|
||||
/*------------------------------------------------------------------*/
|
||||
static int evaluateStatus(pVelSelDriv self, int *iCode){
|
||||
int status;
|
||||
DornierStatus sStatus;
|
||||
char pCommand[80];
|
||||
char pAnswer[80];
|
||||
float fDelta;
|
||||
static int iCount = 0;
|
||||
pDornier pDorn = NULL;
|
||||
|
||||
pDorn = (pDornier)self->pPrivate;
|
||||
|
||||
pDorn->statusMode = STATSEND;
|
||||
status = readAndInterpretStatus(pDorn,&sStatus);
|
||||
if(!status){
|
||||
return VELOFAIL;
|
||||
}
|
||||
|
||||
*iCode = ROTMOVE;
|
||||
/*
|
||||
This code considers the velocity selector arrived if it reads
|
||||
four times a difference between requested spped and actual speed
|
||||
below difference
|
||||
*/
|
||||
pDorn->fLastRPM = sStatus.cur_rpm;
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
/*---------------------------------------------------------------------*/
|
||||
static int statusReceiveHandler(pVelSelDriv self, int *iCode){
|
||||
int status;
|
||||
pDornier pDorn = NULL;
|
||||
|
||||
pDorn = (pDornier)self->pPrivate;
|
||||
|
||||
status = availableRS232(pDorn->controller);
|
||||
if(!status){
|
||||
if(time(NULL) > pDorn->t_timeout){
|
||||
pDorn->iLastError = TIMEOUT;
|
||||
pDorn->statusMode = STATSEND;
|
||||
return VELOFAIL;
|
||||
} else {
|
||||
return VSACCEL;
|
||||
}
|
||||
}
|
||||
|
||||
return evaluateStatus(self, iCode);
|
||||
}
|
||||
/*--------------------------------------------------------------------------
|
||||
The Dornier takes a long time to answer a status message. In order to keep
|
||||
SICS responsive the following state machine is implemented:
|
||||
- a status request is sent.
|
||||
- next data availability will be checked, if available: process!
|
||||
---------------------------------------------------------------------------*/
|
||||
static int DornierStatNew(pVelSelDriv self, int *iCode, float *fCur){
|
||||
pDornier pDorn = NULL;
|
||||
int status;
|
||||
|
||||
assert(self);
|
||||
pDorn = (pDornier)self->pPrivate;
|
||||
|
||||
if(pDorn->statusMode == STATSEND){
|
||||
return statusSendHandler(pDorn);
|
||||
} else {
|
||||
status = statusReceiveHandler(self,iCode);
|
||||
*fCur = pDorn->fLastRPM;
|
||||
return status;
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
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;
|
||||
|
||||
/*
|
||||
use cached status while waiting for reply during drive
|
||||
*/
|
||||
if(pDorn->statusMode == STATSEND){
|
||||
if(!requestDornierStatus(pDorn)){
|
||||
return 0;
|
||||
}
|
||||
if(!readAndInterpretStatus(pDorn,&sStatus)){
|
||||
return 0;
|
||||
}
|
||||
pDorn->lastStatus = sStatus;
|
||||
} else {
|
||||
sStatus = pDorn->lastStatus;
|
||||
}
|
||||
|
||||
/* 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 DornierLoss(pVelSelDriv self, float *fLoss)
|
||||
{
|
||||
pDornier pDorn = NULL;
|
||||
int iRet, iErrStat, iDelta;
|
||||
DornierStatus DStatus;
|
||||
char pCommand[] = {"BRE\n"};
|
||||
char pAnswer[80];
|
||||
static int iCount;
|
||||
static int iError;
|
||||
int i;
|
||||
|
||||
assert(self);
|
||||
pDorn = (pDornier)self->pPrivate;
|
||||
|
||||
/* send a command */
|
||||
iRet = transactRS232(pDorn->controller,pCommand,strlen(pCommand),
|
||||
pAnswer,79);
|
||||
if(iRet != 1)
|
||||
{
|
||||
pDorn->iLastError = iRet;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* wait 10 seconds before doing anything */
|
||||
SicsWait(10);
|
||||
|
||||
/* loop until back to speed again */
|
||||
for(i = 0; i < 100; i++ )
|
||||
{
|
||||
if(!requestDornierStatus(pDorn)){
|
||||
return 0;
|
||||
}
|
||||
if(!readAndInterpretStatus(pDorn,&DStatus)){
|
||||
return 0;
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
*fLoss = DStatus.pwr;
|
||||
return 1;
|
||||
}
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static void DornierKill(void *pData)
|
||||
{
|
||||
pDornier pDorn = NULL;
|
||||
char pAns[10];
|
||||
|
||||
pDorn = (pDornier)pData;
|
||||
assert(pDorn);
|
||||
|
||||
transactRS232(pDorn->controller,"TTY\n",4,pAns,10);
|
||||
KillRS232(pDorn->controller);
|
||||
free(pDorn);
|
||||
}
|
||||
/*------------------------------------------------------------------------*/
|
||||
static int DornierInit(pVelSelDriv self, SConnection *pCon)
|
||||
{
|
||||
pDornier pDorn = NULL;
|
||||
int iRet, iError;
|
||||
float fRot;
|
||||
char pError[80], pBueffel[256];
|
||||
|
||||
assert(self);
|
||||
pDorn = (pDornier)self->pPrivate;
|
||||
assert(pDorn);
|
||||
|
||||
iRet = initRS232(pDorn->controller);
|
||||
if(iRet < 0){
|
||||
return 1;
|
||||
}
|
||||
setRS232SendTerminator(pDorn->controller,"\n");
|
||||
setRS232Timeout(pDorn->controller,pDorn->iTimeOut);
|
||||
setRS232Debug(pDorn->controller,1);
|
||||
|
||||
/*
|
||||
tell him that we want control.
|
||||
Funny enough no <cr> or <nl> is sent in the reply to this.
|
||||
*/
|
||||
setRS232ReplyTerminator(pDorn->controller,"\\");
|
||||
iRet = transactRS232(pDorn->controller,"REM\n",4,pError,79);
|
||||
setRS232ReplyTerminator(pDorn->controller,"\r\n");
|
||||
if(iRet != 1)
|
||||
{
|
||||
sprintf(pBueffel,
|
||||
"ERROR: %s while switching velocity selector to remote",
|
||||
pError);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
}
|
||||
/*
|
||||
check which status the velo is in
|
||||
*/
|
||||
pDorn->statusMode = STATSEND;
|
||||
GetDornierPos(self,&fRot);
|
||||
if(fRot < 0){
|
||||
GetDornierPos(self,&fRot);
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
/*-------------------------------------------------------------------------*/
|
||||
pVelSelDriv VSCreateDornier2003(char *name, Tcl_Interp *pTcl)
|
||||
{
|
||||
pVelSelDriv pNew = NULL;
|
||||
pDornier pDorn = NULL;
|
||||
char *pPtr = NULL;
|
||||
int iVal, iRet, iPort;
|
||||
char pHost[132];
|
||||
|
||||
|
||||
/* 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;
|
||||
}
|
||||
strncpy(pHost,pPtr,131);
|
||||
|
||||
/* 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,&iPort);
|
||||
if(iRet != TCL_OK)
|
||||
{
|
||||
free(pDorn);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
/* minimum control speed */
|
||||
pPtr = Tcl_GetVar2(pTcl,name,"MinControl",TCL_GLOBAL_ONLY);
|
||||
if(!pPtr)
|
||||
{
|
||||
pDorn->minRPM = 3100;
|
||||
}
|
||||
else
|
||||
{
|
||||
iRet = Tcl_GetInt(pTcl,pPtr,&iVal);
|
||||
if(iRet != TCL_OK)
|
||||
{
|
||||
pDorn->minRPM = 3100;
|
||||
}
|
||||
pDorn->minRPM = 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;
|
||||
pDorn->controller = createRS232(pHost,iPort);
|
||||
if(!pDorn->controller){
|
||||
DornierKill(pNew->pPrivate);
|
||||
free(pNew);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/* initialise function pointers */
|
||||
pNew->DeletePrivate = DornierKill;
|
||||
pNew->Halt = DornierHalt;
|
||||
pNew->GetError = DornierError;
|
||||
pNew->TryAndFixIt = DornierFixIt;
|
||||
pNew->GetRotation = GetDornierPos;
|
||||
pNew->SetRotation = DornierRun;
|
||||
pNew->GetStatus = DornierStatNew;
|
||||
pNew->GetDriverText = DornierText;
|
||||
pNew->GetLossCurrent = DornierLoss;
|
||||
pNew->Init = DornierInit;
|
||||
|
||||
/* done it */
|
||||
return pNew;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user