diff --git a/make_gen b/make_gen index 890d914..a8cb126 100644 --- a/make_gen +++ b/make_gen @@ -10,7 +10,7 @@ tjxp $*.tc $*.c OBJ=psi.o buffer.o ruli.o sps.o pimotor.o \ - pipiezo.o sanswave.o faverage.o \ + pipiezo.o sanswave.o faverage.o fetchwrite.o\ amorstat.o tasinit.o ptasdrive.o tasutil.o tasscan.o swmotor.o \ polterwrite.o ecb.o frame.o el734driv.o el734dc.o ecbdriv.o \ ecbcounter.o el737driv.o sinqhmdriv.o tdchm.o velodorn.o \ @@ -19,7 +19,7 @@ OBJ=psi.o buffer.o ruli.o sps.o pimotor.o \ el755driv.o serial.o scontroller.o t_update.o \ t_rlp.o t_conv.o el737hpdriv.o dornier2.o el734hp.o \ el737hpv2driv.o swmotor2.o tricssupport.o amorcomp.o \ - $(MZOBJ) amordrive.o amorset.o sinqhttp.o\ + $(MZOBJ) amordrive.o amorset.o sinqhttp.o slsecho.o\ dgrambroadcast.o sinq.o tabledrive.o julcho.o \ ritastorage.o poldizug.o audinelib.o delcam.o el737hpdrivsps.o \ rebin.o sanslirebin.o lmd200.o slsvme.o julprot.o sinqhttpprot.o \ diff --git a/phytron.c b/phytron.c index 2c7255f..db4433a 100644 --- a/phytron.c +++ b/phytron.c @@ -65,7 +65,7 @@ static int PhytronHandler(Ascon * a) } break; default: - return AsconStdHandler(a); + return AsconStdHandler(a); } return 1; } diff --git a/psi.c b/psi.c index a7f2249..e3d5d98 100644 --- a/psi.c +++ b/psi.c @@ -20,8 +20,8 @@ Markus Zolliker, Jan 2010 -----------------------------------------------------------------------*/ #include -#include "site.h" #include "sics.h" +#include "site.h" #include #include #include "ecbdriv.h" @@ -69,6 +69,7 @@ void SiteInit(void) INIT(AddPfeifferProtocoll); INIT(AddTermProtocoll); INIT(AddPhytronProtocoll); + INIT(AddSLSEchoProtocoll); } diff --git a/slsecho.c b/slsecho.c new file mode 100644 index 0000000..7752b83 --- /dev/null +++ b/slsecho.c @@ -0,0 +1,288 @@ +/* + * slsecho.c + * This is a protocol adapter for yet another driver for the + * SLS DPC magnet controller. This one is for the shiny, silvery + * little box with TCP/IP, 8 fibre optic magnet couplers and + * two wires for power. The current assumption is that this + * device communicates with 64 byte message going back and forth, + * 8 bytes for each channel. Where the first two seem to be unused + * and the last 6 being the DSP codes as described elsewhere. + * + * The expected input for this is: + * + * channel:mode:code:val:convertflag + * + * with mode being either r or w and convertflag being one of + * none,read,write + * + * Created on: Mar 18, 2010 + * Author: Mark Koennecke + */ +#include +#include +#include +#include +#include + +#define NONE 0 +#define READ 1 +#define WRITE 2 + +typedef struct { + char sendbuf[64]; + char readbuf[64]; + int convertflag; /* which conversions need to be done */ + int channel; /* magnet channel number, starting at 0 */ +} SLSEcho, *pSLSEcho; + +/* + packet header codes +*/ +#define DSPWRITE 0x80 +#define DSPREAD 0x0 + +/*------------------------------------------------------------------------- + Code for data conversion from Lukas Tanner. The ULONG is system dependent, + it MUST describe a 32 bit value. + -------------------------------------------------------------------------*/ +#define ULONG int +/***********************************************************************/ +static ULONG double2DSPfloat(double input) +/***********************************************************************/ +/* Konvertiert eine normale double Variable in ein 32bit DSP Float Format. */ +{ + ULONG output; + double mantissa; + int exponent; + + if (input == 0) { + output = 0; + } else { + mantissa = 2 * (frexp(fabs(input), &exponent)); + mantissa = ldexp(mantissa, 23); + exponent = exponent - 1 + 127; + if (exponent < 0) { + exponent = 0; + } else if (exponent > 0xFE) { + exponent = 0xFE; + } + exponent = exponent << 23; + output = ((ULONG) mantissa) & 0x7FFFFF; + output = output | ((ULONG) (exponent)); + if (input < 0) { + output = output | 0x80000000; + } + } + return output; +} + +/***********************************************************************/ +static double DSPfloat2double(ULONG input) +/***********************************************************************/ +/* Konvertiert eine Variable inder ein 32bit DSP Float Wert abgelegt ist, + in ein normales double Format + 32bit IEEE Float => SEEEEEEEEMMMMMMMMMMMMMMMMMMMMMMM; S = Sign Bit + E = Exponent + M = Mantissa (23)*/ +{ + double output; + if ((input & 0x7FFFFF) == 0 && ((input >> 23) & 0xFF) == 0) { + output = 0; + } else { + output = ldexp(input & 0x7FFFFF, -23) + 1; + output = output * pow(-1, ((input >> 31) & 1)); + output = output * ldexp(1, (((input >> 23) & 0xFF) - 127)); + } + return output; +} +/*----------------------------------------------------------------------------*/ +static int EchoPack(Ascon *a) +{ + pSLSEcho echo = NULL; + char *pMessage = NULL, *pPtr = NULL; + char token[20], parseString[255]; + int code, val; + double dval; + + echo = a->private; + assert(echo != NULL); + + memset(echo->sendbuf,0,sizeof(echo->sendbuf)); + memset(echo->readbuf,0,sizeof(echo->readbuf)); + + assert(GetDynStringLength(a->wrBuffer) < 255); + memset(parseString,0,sizeof(parseString)); + strcpy(parseString,GetCharArray(a->wrBuffer)); + pPtr = parseString; + + /* read num */ + pPtr = stptok(pPtr,token,sizeof(token),":"); + echo->channel = atoi(token); + pMessage = echo->sendbuf + echo->channel*8; + + /* read mode */ + pPtr = stptok(pPtr,token,sizeof(token),":"); + if(token[0] == 'r'){ + pMessage[2] = DSPREAD; + } else if(token[0] == 'w'){ + pMessage[2] = DSPWRITE; + } else { + a->state = AsconFailed; + DynStringConcat(a->errmsg,"ERROR: bad mode token "); + DynStringConcat(a->errmsg,token); + return 0; + } + + /* read code */ + pPtr = stptok(pPtr,token,sizeof(token),":"); + sscanf(token,"%x", &code); + pMessage[3] = code; + + /* read value */ + pPtr = stptok(pPtr,token,sizeof(token),":"); + dval = atof(token); + val = dval; + + /* read convertflag */ + pPtr = stptok(pPtr,token,sizeof(token),":"); + if(strcmp(token,"none") == 0){ + echo->convertflag = NONE; + } else if(strcmp(token,"read") == 0){ + echo->convertflag = READ; + } else if(strcmp(token,"write") == 0){ + echo->convertflag = WRITE; + } else { + a->state = AsconFailed; + DynStringConcat(a->errmsg,"ERROR: bad convertflag "); + DynStringConcat(a->errmsg,token); + return 0; + } + if(echo->convertflag == WRITE){ + val = double2DSPfloat(dval); + } + memcpy(pMessage+4,&val,4); + + /* printf("Sending code: 0x%x, val = %d\n", code, val); */ + return 1; +} +/*-------------------------------------------------------------------*/ +static int EchoUnpack(Ascon *a) +{ + pSLSEcho echo = a->private; + char *pMessage = NULL, printbuf[30]; + int code, val; + double dval; + + + DynStringClear(a->rdBuffer); + pMessage = echo->readbuf + echo->channel*8; + + code = pMessage[3]; + snprintf(printbuf,sizeof(printbuf),"%x", code); + DynStringConcat(a->rdBuffer,printbuf); + DynStringConcatChar(a->rdBuffer,':'); + + memcpy(&val,pMessage+4, 4); + dval = (double)val; + + if(echo->convertflag == READ){ + dval = DSPfloat2double(val); + } + snprintf(printbuf,sizeof(printbuf),"%f", dval); + DynStringConcat(a->rdBuffer,printbuf); + /* printf("Reading code: 0x%x, val = %d\n", code, val);*/ + return 1; +} +/*----------------------------------------------------------------------*/ +static int SLSEchoHandler(Ascon *a) +{ + pSLSEcho echo = a->private; + int written, read; + char c; + + assert(echo != NULL); + + switch(a->state){ + case AsconWriteStart: + EchoPack(a); + a->wrPos = 0; + a->state = AsconWriting; + break; + case AsconWriting: + AsconReadGarbage(a->fd); + written = send(a->fd, echo->sendbuf+a->wrPos,64-a->wrPos, 0); + if(written > 0){ + a->wrPos += written; + if(a->wrPos >= 64){ + a->state = AsconWriteDone; + break; + } + } else if (written == 0) { + errno = ECONNRESET; + return ASCON_DISCONNECTED; + } else { + return ASCON_SEND_ERROR; + } + break; + case AsconReadStart: + a->start = DoubleTime(); + a->state = AsconReading; + a->wrPos = 0; /* reusing wrPos for character count received*/ + break; + case AsconReading: + read = AsconReadChar(a->fd,&c); + if (read <= 0) { + if (read < 0) { + AsconError(a, "ASC5", errno); + return 0; + } + if (a->timeout > 0) { + if (DoubleTime() - a->start > a->timeout) { + AsconError(a, "no response", 0); + a->state = AsconTimeout; + } + } + return 0; + } + echo->readbuf[a->wrPos] = c; + a->wrPos++; + if(a->wrPos >= 64){ + a->state = AsconReadDone; + EchoUnpack(a); + } + break; + default: + return AsconStdHandler(a); + } + return 1; +} +/*------------------------------------------------------------------------*/ +static int SLSEchoInit(Ascon * a, SConnection * con, int argc, char *argv[]) +{ + a->fd = -1; + a->state = AsconConnectStart; + a->reconnectInterval = 10; + a->hostport = strdup(argv[1]); + if (argc > 2) { + a->timeout = atof(argv[2]); + } else { + a->timeout = 2.0; /* sec */ + } + a->private = malloc(sizeof(SLSEcho)); + if(a->private == NULL){ + return 0; + } + a->killPrivate = free; + return 1; +} +/*------------------------------------------------------------------------*/ +void AddSLSEchoProtocoll() +{ + AsconProtocol *prot = NULL; + + prot = calloc(sizeof(AsconProtocol), 1); + prot->name = strdup("slsecho"); + prot->init = SLSEchoInit; + prot->handler = SLSEchoHandler; + AsconInsertProtocol(prot); +} diff --git a/sps.c b/sps.c index 590fca0..8c82ed3 100644 --- a/sps.c +++ b/sps.c @@ -67,7 +67,7 @@ static int init(pSPS self) return 0; } setRS232SendTerminator(rs232,"\r\n"); - setRS232ReplyTerminator(rs232,"\n"); + setRS232ReplyTerminator(rs232,"\r\n"); setRS232Timeout(rs232,10000); /* setRS232Debug(rs232,1); */