Added modbus protocol handler and Fermi chopper support for Pelican
r3359 | ffr | 2012-01-30 11:09:25 +1100 (Mon, 30 Jan 2012) | 2 lines
This commit is contained in:
Ferdi Franceschini
committed by
Douglas Clowes
Douglas Clowes
parent
790fb6a062
commit
20ce9381bb
@@ -20,6 +20,7 @@ HOBJ += geterrno.o
|
||||
HOBJ += strjoin.o
|
||||
HOBJ += chopper.o
|
||||
HOBJ += modbustcp.o
|
||||
HOBJ += sct_tcpmodbus.o
|
||||
HOBJ += sct_galilprot.o
|
||||
HOBJ += sct_modbusprot.o
|
||||
HOBJ += sct_oxfordprot.o
|
||||
|
||||
515
site_ansto/hardsup/sct_tcpmodbus.c
Normal file
515
site_ansto/hardsup/sct_tcpmodbus.c
Normal file
@@ -0,0 +1,515 @@
|
||||
/** @file TCP Modbus protocol handler for script-context based controllers.
|
||||
*
|
||||
*/
|
||||
#include <errno.h>
|
||||
#include <ascon.h>
|
||||
#include <ascon.i>
|
||||
#include <dynstring.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
#include <fupa.h>
|
||||
|
||||
#define ERRLEN 128
|
||||
static int dbgprintf(char* fmtstr, ...);
|
||||
static int dbgprintx(const char* msg, const unsigned char* cp, int blen);
|
||||
static unsigned int debug_modbus = 1;
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
/* IEEE code copied from psi/modbus.c */
|
||||
static void double2ieee(double input, char ieee[4]) {
|
||||
|
||||
/* convert double to IEEE 32 bit floating number (denormalized numbers are considered as zero) */
|
||||
|
||||
long mantissa;
|
||||
int exponent;
|
||||
|
||||
if (input == 0) {
|
||||
ieee[0]= 0;
|
||||
ieee[1]= 0;
|
||||
ieee[2]= 0;
|
||||
ieee[3]= 0;
|
||||
} else {
|
||||
/* frexp -> 0.f * 2^23 */
|
||||
/* 2^24 * fract instead of 2^23 because we get 0.(f/2) not 1.f from frexp and exponent = e + 1*/
|
||||
mantissa = 0x1000000 * (frexp(fabs(input), &exponent));
|
||||
exponent = exponent - 1 + 127;
|
||||
if (exponent < 0) {
|
||||
exponent = 0;
|
||||
} else if (exponent > 0xFE) {
|
||||
exponent = 0xFE;
|
||||
}
|
||||
if (input < 0) {
|
||||
ieee[0] = 0x80 | (exponent >> 1);
|
||||
} else {
|
||||
ieee[0] = exponent >> 1;
|
||||
}
|
||||
ieee[1] = (exponent & 1) << 7 | ((mantissa & 0x7F0000) >> 16);
|
||||
ieee[2] = (mantissa & 0xFF00) >> 8;
|
||||
ieee[3] = mantissa & 0xFF;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static double ieee2double(unsigned char ieee[4]) {
|
||||
|
||||
/* IEEE 32 bit floating number to double (denormalized numbers are considered as zero) */
|
||||
|
||||
long mantissa;
|
||||
double output, norm = 1;
|
||||
int exponent, bias = 127;
|
||||
|
||||
mantissa = ((ieee[1] << 16) & 0x7F0000)
|
||||
| ((ieee[2] << 8) & 0xFF00)
|
||||
| ((ieee[3] ) & 0xFF);
|
||||
|
||||
exponent = (ieee[0] & 0x7F) * 2 + ((ieee[1] >> 7) & 1); /* raw exponent */
|
||||
if (exponent == 255) {
|
||||
if (mantissa == 0) {
|
||||
/*TODO INF */
|
||||
} else {
|
||||
/*TODO NAN */
|
||||
}
|
||||
}
|
||||
if (exponent == 0) {
|
||||
if (mantissa == 0) {
|
||||
return 0.0;
|
||||
} else {
|
||||
bias = 126;
|
||||
norm = 0;
|
||||
}
|
||||
}
|
||||
output = ldexp(mantissa, -23) + norm;
|
||||
if (ieee[0] & 0x80) {
|
||||
output = -output;
|
||||
}
|
||||
return output * ldexp(1, exponent - bias);
|
||||
}
|
||||
/*-------------------------------------------------------------------------*/
|
||||
|
||||
|
||||
/*
|
||||
NumVal = number of values to read or write, ie number of coils, input/outputs or registers.
|
||||
Number of bytes = NumVal * (number of bytes in Dtype)
|
||||
*/
|
||||
enum mbDtypes {U16, U32, F32};
|
||||
struct cmdPar {
|
||||
unsigned int MBAddr, Fcode, SAddr, NumVal, NumBytes, NumRd, NumWr;
|
||||
enum mbDtypes Dtype;
|
||||
double MBdata[2000];
|
||||
};
|
||||
|
||||
enum MBFCODES { RdCoil=1, RdInp=2, RdHldReg=3, RdInpReg=4, WrCoil=5, WrReg=6, WrMCoils=15, WrMRegs=16 };
|
||||
static struct cmdPar MBcmd;
|
||||
static int BO[4];
|
||||
|
||||
/** @brief Parse a command string of the form "MBAddr:Fcode:SAddr:Dtype:MBdata"
|
||||
Dtype U16, U32, F32 with optional byte order array eg U32[3,2,1,0]
|
||||
*/
|
||||
void parseCmd(char *cmdLine, struct cmdPar *MBC, int *BO) {
|
||||
int len;
|
||||
unsigned int i;
|
||||
char *nextPar, dataType[4];
|
||||
double dVal;
|
||||
|
||||
MBC->MBAddr = strtoul(cmdLine, &nextPar, 0);
|
||||
cmdLine = nextPar+1;
|
||||
MBC->Fcode = strtoul(cmdLine, &nextPar, 0);
|
||||
cmdLine = nextPar+1;
|
||||
MBC->SAddr = strtoul(cmdLine, &nextPar, 0);
|
||||
cmdLine = nextPar+1;
|
||||
if (MBC->Fcode != WrCoil) {
|
||||
MBC->NumVal = strtod(cmdLine, &nextPar);
|
||||
cmdLine = nextPar + 1;
|
||||
strncpy(dataType,cmdLine,3);
|
||||
dataType[3] = '\0';
|
||||
cmdLine += 3;
|
||||
nextPar = cmdLine;
|
||||
if (strcmp(dataType, "U16") == 0) {
|
||||
MBC->Dtype = U16;
|
||||
} else if (strcmp(dataType, "U32") == 0) {
|
||||
MBC->Dtype = U32;
|
||||
} else if (strcmp(dataType, "F32") == 0) {
|
||||
MBC->Dtype = F32;
|
||||
}
|
||||
if (*cmdLine == '[') {
|
||||
cmdLine++;
|
||||
BO[0] = strtol(cmdLine, &nextPar, 10);
|
||||
cmdLine = nextPar + 1;
|
||||
BO[1] = strtol(cmdLine, &nextPar, 10);
|
||||
cmdLine = nextPar + 1;
|
||||
BO[2] = strtol(cmdLine, &nextPar, 10);
|
||||
cmdLine = nextPar + 1;
|
||||
BO[3] = strtol(cmdLine, &nextPar, 10);
|
||||
}
|
||||
cmdLine = nextPar + 1;
|
||||
}
|
||||
switch (MBC->Fcode) {
|
||||
case RdCoil:
|
||||
/* Coils are numbered from 1 but addressed from 0 */
|
||||
MBC->SAddr--;
|
||||
MBC->NumBytes = MBC->NumVal;
|
||||
MBC->NumRd = MBC->NumVal;
|
||||
break;
|
||||
case WrCoil:
|
||||
MBC->SAddr--;
|
||||
dVal = strtod(cmdLine, &nextPar);
|
||||
if (dVal == 1) {
|
||||
MBC->MBdata[0] = 255;
|
||||
} else if (dVal == 0) {
|
||||
MBC->MBdata[0] = 0;
|
||||
} else {
|
||||
/* TODO ERROR */
|
||||
}
|
||||
break;
|
||||
case RdHldReg:
|
||||
if (MBC->Dtype == F32 || MBC->Dtype == U32) {
|
||||
MBC->NumRd = MBC->NumVal * 2;
|
||||
} else if (MBC->Dtype == U16) {
|
||||
MBC->NumRd = MBC->NumVal;
|
||||
} else {
|
||||
/* TODO ERROR */
|
||||
}
|
||||
break;
|
||||
case WrMRegs:
|
||||
for (i=0; i < MBC->NumVal; i++) {
|
||||
MBC->MBdata[i] = strtod(cmdLine, &nextPar);
|
||||
cmdLine = nextPar + 1;
|
||||
}
|
||||
if (MBC->Dtype == F32 || MBC->Dtype == U32) {
|
||||
MBC->NumWr = MBC->NumVal * 2;
|
||||
MBC->NumBytes = MBC->NumVal * 4;
|
||||
} else if (MBC->Dtype == U16) {
|
||||
MBC->NumWr = MBC->NumVal;
|
||||
MBC->NumBytes = MBC->NumVal * 2;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
/** @brief encode modbus request
|
||||
*/
|
||||
int TCPMBWriteStart(Ascon *a) {
|
||||
int ADUlen, PDUlenPlusUID;
|
||||
unsigned int i, j;
|
||||
char *cmdLine;
|
||||
unsigned char ADU[32], ieee[4];
|
||||
|
||||
|
||||
cmdLine = GetCharArray(a->wrBuffer);
|
||||
for (i=0; i < 4; i++) BO[i] = i;
|
||||
parseCmd(cmdLine, &MBcmd, BO);
|
||||
|
||||
ADU[0] = 0;
|
||||
ADU[1] = 0;
|
||||
ADU[2] = 0;
|
||||
ADU[3] = 0;
|
||||
ADU[6] = MBcmd.MBAddr;
|
||||
ADU[7] = MBcmd.Fcode;
|
||||
switch (MBcmd.Fcode) {
|
||||
case RdCoil:
|
||||
case RdHldReg:
|
||||
/* TODO Max NumRd = 125 */
|
||||
ADU[8] = (MBcmd.SAddr & 0xFF00) >> 8;
|
||||
ADU[9] = MBcmd.SAddr & 0xFF;
|
||||
ADU[10] = (MBcmd.NumRd & 0xFF00) >> 8;
|
||||
ADU[11] = MBcmd.NumRd & 0xFF;
|
||||
ADUlen = 12;
|
||||
break;
|
||||
case WrCoil:
|
||||
ADU[8] = (MBcmd.SAddr & 0xFF00) >> 8;
|
||||
ADU[9] = MBcmd.SAddr & 0xFF;
|
||||
ADU[10] = MBcmd.MBdata[0];
|
||||
ADU[11] = 0;
|
||||
ADUlen = 12;
|
||||
break;
|
||||
case WrMRegs:
|
||||
ADU[8] = (MBcmd.SAddr & 0xFF00) >> 8;
|
||||
ADU[9] = MBcmd.SAddr & 0xFF;
|
||||
ADU[10] = (MBcmd.NumWr & 0xFF00) >> 8;
|
||||
ADU[11] = MBcmd.NumWr & 0xFF;
|
||||
ADU[12] = MBcmd.NumBytes;
|
||||
switch (MBcmd.Dtype) {
|
||||
case U16:
|
||||
for (i=0, j=13; i < MBcmd.NumVal; i++, j+=4) {
|
||||
ADU[j + BO[0]] = ((unsigned int)MBcmd.MBdata[i] & 0xFF00) >> 8;
|
||||
ADU[j + BO[1]] = (unsigned int)MBcmd.MBdata[i] & 0xFF;
|
||||
}
|
||||
break;
|
||||
case U32:
|
||||
for (i=0, j=13; i < MBcmd.NumVal; i++, j+=4) {
|
||||
ADU[j + BO[0]] = ((unsigned int)MBcmd.MBdata[i] & 0xFF000000) >> 24;
|
||||
ADU[j + BO[1]] = ((unsigned int)MBcmd.MBdata[i] & 0xFF0000) >> 16;
|
||||
ADU[j + BO[2]] = ((unsigned int)MBcmd.MBdata[i] & 0xFF00) >> 8;
|
||||
ADU[j + BO[3]] = (unsigned int)MBcmd.MBdata[i] & 0xFF;
|
||||
}
|
||||
break;
|
||||
case F32:
|
||||
for (i=0, j=13; i < MBcmd.NumVal; i++, j+=4) {
|
||||
double2ieee(MBcmd.MBdata[i], ieee);
|
||||
ADU[j + BO[0]] = ieee[0];
|
||||
ADU[j + BO[1]] = ieee[1];
|
||||
ADU[j + BO[2]] = ieee[2];
|
||||
ADU[j + BO[3]] = ieee[3];
|
||||
}
|
||||
break;
|
||||
}
|
||||
ADUlen = 13 + MBcmd.NumBytes;
|
||||
break;
|
||||
default:
|
||||
// Not Implemented
|
||||
break;
|
||||
}
|
||||
/* Length field in MBAP includes byte 7 of the MBAP, so Length = ADU length - 6. */
|
||||
PDUlenPlusUID = ADUlen - 6;
|
||||
ADU[4] = (PDUlenPlusUID & 0xFF00) >> 8;
|
||||
ADU[5] = PDUlenPlusUID & 0xFF;
|
||||
DynStringReplaceWithLen(a->wrBuffer, ADU, 0, ADUlen);
|
||||
a->state = AsconWriting;
|
||||
a->wrPos = 0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
/** @brief decode modbus response
|
||||
*/
|
||||
/* ADU[4] = MSB of DatLen, ADU[5] = LSB of DatLen */
|
||||
static int aduLen = 0, RespLen = 0, pduLen = 0, DatLen = 0;
|
||||
/* NOTE: Min aduLen is 9. The header (7 bytes) + an Error code and Exception code */
|
||||
#define ADUSIZE 17
|
||||
static const int TIDidx=0, PIDidx=2, LENidx=4, UIDidx=6, CODEidx=7, DATLENidx=8, DATAidx=9;
|
||||
static unsigned char ADU[ADUSIZE]; /* Allows upto 8 bytes if data */
|
||||
int TCPMBReading(Ascon *a) {
|
||||
const int MBAPLen = 7;
|
||||
int i, ret, rlen, byteCnt, allRead = 0;
|
||||
char chr, temp[64], errMsg[ERRLEN];
|
||||
unsigned char ieee[4];
|
||||
double dval;
|
||||
long int lival;
|
||||
|
||||
ret = AsconReadChar(a->fd, &chr);
|
||||
/* TODO Check for timeout while reading and call AsconError()
|
||||
Eg if (DoubleTime() > a->lastReconnect + a->reconnectInterval) then AsconError
|
||||
*/
|
||||
while (ret > 0) {
|
||||
a->start = DoubleTime();
|
||||
ADU[aduLen++] = chr;
|
||||
ret = AsconReadChar(a->fd, &chr);
|
||||
}
|
||||
if (ret < 0) {
|
||||
AsconError(a, "AsconReadChar failed:", errno);
|
||||
return 1;
|
||||
}
|
||||
if (RespLen == 0 && aduLen > CODEidx) {
|
||||
RespLen = (ADU[LENidx] << 8) + ADU[LENidx+1];
|
||||
}
|
||||
if (allRead == 0 && aduLen >= (6 + RespLen)) {
|
||||
/* all of response has been read */
|
||||
allRead = 1;
|
||||
}
|
||||
if (allRead) {
|
||||
if (ADU[CODEidx] > 0x80) {
|
||||
a->state = AsconReadDone;
|
||||
snprintf(errMsg, ERRLEN, "TCPMODBUS: Function code %d exception %d:", ADU[CODEidx] & 0x0F, ADU[8] &0x0F);
|
||||
AsconError(a, errMsg, 0);
|
||||
/*TODO This hack stops ascon.c:AsconTask() from needlessly closing the connection. Remove this when it's no longer needed */
|
||||
a->lastReconnect = DoubleTime();
|
||||
} else if (ADU[CODEidx] != MBcmd.Fcode) {
|
||||
a->state = AsconReadDone;
|
||||
snprintf(errMsg, ERRLEN, "TCPMODBUS: Requested function %d but got response for %d:", MBcmd.Fcode, ADU[CODEidx]);
|
||||
AsconError(a, errMsg, 0);
|
||||
/*TODO This hack stops ascon.c:AsconTask() from needlessly closing the connection. Remove this when it's no longer needed */
|
||||
a->lastReconnect = DoubleTime();
|
||||
} else {
|
||||
DynStringClear(a->rdBuffer);
|
||||
switch (MBcmd.Fcode) {
|
||||
case RdCoil:
|
||||
byteCnt = ADU[8];
|
||||
for (i=0; i < byteCnt; i++) {
|
||||
rlen = snprintf(temp, 64, "%ld", ADU[9+i]);
|
||||
DynStringConcat(a->rdBuffer, temp);
|
||||
DynStringConcatChar(a->rdBuffer, ' ');
|
||||
}
|
||||
a->state = AsconReadDone;
|
||||
break;
|
||||
case RdHldReg:
|
||||
byteCnt = ADU[8];
|
||||
switch (MBcmd.Dtype) {
|
||||
case U16:
|
||||
for (i=0; i < byteCnt/2; i++) {
|
||||
ieee[ BO[0] ] = ADU[9+i*2];
|
||||
ieee[ BO[1] ] = ADU[10+i*2];
|
||||
lival = (ieee[0] << 8) | ieee[1];
|
||||
rlen = snprintf(temp, 64, "%ld", lival);
|
||||
DynStringConcat(a->rdBuffer, temp);
|
||||
DynStringConcatChar(a->rdBuffer, ' ');
|
||||
}
|
||||
break;
|
||||
case U32:
|
||||
case F32:
|
||||
for (i=0; i < byteCnt/4; i++) {
|
||||
ieee[ BO[0] ] = ADU[9+i*4];
|
||||
ieee[ BO[1] ] = ADU[10+i*4];
|
||||
ieee[ BO[2] ] = ADU[11+i*4];
|
||||
ieee[ BO[3] ] = ADU[12+i*4];
|
||||
if (MBcmd.Dtype == F32) {
|
||||
dval = ieee2double(ieee);
|
||||
rlen = snprintf(temp, 64, "%g", dval);
|
||||
} else if (MBcmd.Dtype == U32) {
|
||||
lival = (ieee[0] << 24) | (ieee[1] << 16) | (ieee[2] << 8) | ieee[3];
|
||||
rlen = snprintf(temp, 64, "%ld", lival);
|
||||
}
|
||||
DynStringConcat(a->rdBuffer, temp);
|
||||
DynStringConcatChar(a->rdBuffer, ' ');
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
a->state = AsconReadDone;
|
||||
break;
|
||||
case WrCoil:
|
||||
if (ADU[7] == MBcmd.Fcode && (ADU[8] << 8 | ADU[9]) == MBcmd.SAddr &&
|
||||
ADU[10] == MBcmd.MBdata[0]) {
|
||||
DynStringReplace(a->rdBuffer, "OK", 0);
|
||||
} else {
|
||||
DynStringReplace(a->rdBuffer, "ASCERR: Response doesn't match set request", 0);
|
||||
}
|
||||
a->state = AsconReadDone;
|
||||
break;
|
||||
case WrMRegs:
|
||||
if (ADU[7] == MBcmd.Fcode && (ADU[8] << 8 | ADU[9]) == MBcmd.SAddr &&
|
||||
(ADU[10] << 8 | ADU[11]) == MBcmd.NumWr) {
|
||||
DynStringReplace(a->rdBuffer, "OK", 0);
|
||||
} else {
|
||||
DynStringReplace(a->rdBuffer, "ASCERR: Response doesn't match set request", 0);
|
||||
}
|
||||
a->state = AsconReadDone;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
aduLen = 0, DatLen = 0, RespLen = 0;
|
||||
memset(ADU, 0, ADUSIZE);
|
||||
if (a->state != AsconReadDone) {
|
||||
if (a->timeout > 0) {
|
||||
if (DoubleTime() - a->start > a->timeout) {
|
||||
AsconError(a, "read timeout", 0);
|
||||
a->state = AsconTimeout;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
int TCPMBUtil(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]) {
|
||||
char cmdLine[512], msg[512];
|
||||
unsigned long ul;
|
||||
unsigned char ieee[4];
|
||||
double dval;
|
||||
int iRet, iNumCmds = 3;
|
||||
FuPaResult PaRes;
|
||||
FuncTemplate CommandTemplate[] = {
|
||||
{"help",0,{0,0}},
|
||||
{"ieee2double",1,{FUPATEXT}},
|
||||
{"double2ieee",1,{FUPAFLOAT}},
|
||||
{NULL}
|
||||
};
|
||||
|
||||
Arg2Text(argc, argv, cmdLine, 511);
|
||||
iRet = EvaluateFuPa((pFuncTemplate)&CommandTemplate,iNumCmds,argc-1,&argv[1],&PaRes);
|
||||
switch(iRet) {
|
||||
case 1:
|
||||
ul = 0xFFFFFFFF & strtoul(argv[2],NULL,16);
|
||||
ieee[3] = ul & 0xFF;
|
||||
ieee[2] = (ul & 0xFF00) >> 8;
|
||||
ieee[1] = (ul & 0xFF0000) >> 16;
|
||||
ieee[0] = (ul & 0xFF000000) >> 24;
|
||||
dval = ieee2double(ieee);
|
||||
snprintf(msg, 512, "%s -> %f", argv[1], dval);
|
||||
SCWrite(pCon, msg, eValue);
|
||||
break;
|
||||
case 2:
|
||||
break;
|
||||
case 0:
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/** @brief Modbus TCP protocol handler.
|
||||
* This handler encodes commands and decodes responses
|
||||
* for the Modbus TCP protocol
|
||||
*/
|
||||
int TCPMBProtHandler(Ascon *a) {
|
||||
int ret;
|
||||
|
||||
switch(a->state){
|
||||
case AsconWriteStart:
|
||||
ret = TCPMBWriteStart(a);
|
||||
return ret;
|
||||
break;
|
||||
case AsconReadStart:
|
||||
a->start = DoubleTime();
|
||||
ret = AsconStdHandler(a);
|
||||
return ret;
|
||||
break;
|
||||
case AsconReading:
|
||||
ret = TCPMBReading(a);
|
||||
return ret;
|
||||
break;
|
||||
default:
|
||||
ret = AsconStdHandler(a);
|
||||
return ret;
|
||||
break;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
void AddTCPMBProtocol(){
|
||||
AsconProtocol *prot = NULL;
|
||||
|
||||
AddCommand(pServ->pSics, "fermi", TCPMBUtil, NULL, NULL);
|
||||
prot = calloc(sizeof(AsconProtocol), 1);
|
||||
prot->name = strdup("tcpmodbus");
|
||||
prot->init = AsconStdInit;
|
||||
prot->handler = TCPMBProtHandler;
|
||||
AsconInsertProtocol(prot);
|
||||
}
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
static int dbgprintf(char* fmtstr, ...) {
|
||||
if (debug_modbus > 0) {
|
||||
FILE* fp = NULL;
|
||||
int ret = 0;
|
||||
fp = fopen("/tmp/fermiskfMB.txt", "a");
|
||||
if (fp != NULL) {
|
||||
va_list ap;
|
||||
va_start(ap, fmtstr);
|
||||
ret = vfprintf(fp, fmtstr, ap);
|
||||
va_end(ap);
|
||||
fclose(fp);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
static int dbgprintx(const char* msg, const unsigned char* cp, int blen) {
|
||||
if (debug_modbus > 0) {
|
||||
char tmp[128];
|
||||
int i, j;
|
||||
const char hex[] = "0123456789ABCDEF";
|
||||
for (i = 0, j = 0; i < blen && j < 126; ++i) {
|
||||
tmp[j++] = hex[(cp[i] >> 4) & 0xF];
|
||||
tmp[j++] = hex[(cp[i] ) & 0xF];
|
||||
}
|
||||
tmp[j++] = '\0';
|
||||
return dbgprintf("%s: %s\n", msg, tmp);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -27,7 +27,7 @@ set instrument_dictionary [subst {
|
||||
property {data true control true nxsave false klass @none type graphset}
|
||||
}
|
||||
instrument {
|
||||
sobj {@any instrument @any NXvelocity_selector @any NXaperture @any NXcollimator @any NXdetector @any NXdisk_chopper}
|
||||
sobj {@any instrument @any NXvelocity_selector @any NXaperture @any NXcollimator @any NXdetector @any NXdisk_chopper @any NXfermi_chopper}
|
||||
privilege spy
|
||||
datatype @none
|
||||
property {data true control true nxsave false klass NXinstrument type instrument}
|
||||
@@ -73,12 +73,6 @@ set instrument_dictionary [subst {
|
||||
datatype @none
|
||||
property {data true control true nxsave false klass NXcrystal type part}
|
||||
}
|
||||
instrument/fermi_chopper {
|
||||
privilege spy
|
||||
sobj {@any fermi_chopper}
|
||||
datatype @none
|
||||
property {data true control true nxsave false klass NXfermi_chopper type part}
|
||||
}
|
||||
instrument/filter {
|
||||
privilege spy
|
||||
sobj {@any filter}
|
||||
|
||||
@@ -0,0 +1,247 @@
|
||||
|
||||
namespace eval ::scobj::chopper {
|
||||
|
||||
# MakeSICSObj disk_chopper SCT_OBJECT user int
|
||||
# sicslist setatt disk_chopper klass NXdisk_chopper
|
||||
# sicslist setatt disk_chopper long_name disk_chopper
|
||||
|
||||
proc sndMBquery {rdScript cmd nextReq} {
|
||||
set chN [sct chopper]
|
||||
sct nextSndReq $nextReq
|
||||
sct send "$chN:$cmd"
|
||||
return $rdScript
|
||||
}
|
||||
|
||||
proc sndMBset {root cmd} {
|
||||
set val [sct target]
|
||||
#TODO convert val to appropriate units, if [convfact] then val = val * [sct convfact]
|
||||
sct send "$cmd:$val"
|
||||
sct target 0
|
||||
return RDSETREPLY
|
||||
}
|
||||
proc rdStatus {root statnode fields} {
|
||||
set currCh [sct chopper]
|
||||
set chPath $root/ch$currCh/$statnode
|
||||
set oldval [hgetpropval $chPath oldval]
|
||||
set val [sct result]
|
||||
if {[string match "ASCERR:*" $val]} {
|
||||
sct geterror $val
|
||||
return idle
|
||||
}
|
||||
if {$val != $oldval} {
|
||||
hsetprop $chPath oldval $val
|
||||
binary scan [binary format c $val] B* binNum
|
||||
set bitfield [split $binNum ""]
|
||||
foreach n $fields b $bitfield {
|
||||
hset $chPath/$n $b
|
||||
}
|
||||
}
|
||||
return [sct nextSndReq]
|
||||
}
|
||||
proc rdSysStatus {root nodes} {
|
||||
# Read status byte and set nodes for each bit
|
||||
set currCh [sct chopper]
|
||||
set chPath $root/ch$currCh/system_status
|
||||
set oldval [hgetpropval $chPath oldval]
|
||||
set val [sct result]
|
||||
if {[string match "ASCERR:*" $val]} {
|
||||
sct geterror $val
|
||||
return idle
|
||||
}
|
||||
if {$val != $oldval} {
|
||||
hsetprop $chPath oldval $val
|
||||
binary scan [binary format c $val] B* binNum
|
||||
set bitfield [split $binNum ""]
|
||||
binary scan [binary format c $oldval] B* oldbinNum
|
||||
set oldbitfield [split $oldbinNum ""]
|
||||
foreach n $nodes b $bitfield ob $oldbitfield {
|
||||
if {$b != $ob} {
|
||||
hset $chPath/$n $b
|
||||
}
|
||||
}
|
||||
}
|
||||
return [sct nextSndReq]
|
||||
}
|
||||
proc rdIntLkStatus {root nodes} {
|
||||
# Read status byte and set nodes for each bit
|
||||
set currCh [sct chopper]
|
||||
set chPath $root/ch$currCh/intlck_status
|
||||
set oldval [hgetpropval $chPath oldval]
|
||||
set val [sct result]
|
||||
if {[string match "ASCERR:*" $val]} {
|
||||
sct geterror $val
|
||||
return idle
|
||||
}
|
||||
if {$val != $oldval} {
|
||||
hsetprop $chPath oldval $val
|
||||
binary scan [binary format c $val] B* binNum
|
||||
set bitfield [split $binNum ""]
|
||||
binary scan [binary format c $oldval] B* oldbinNum
|
||||
set oldbitfield [split $oldbinNum ""]
|
||||
foreach n $nodes b $bitfield ob $oldbitfield {
|
||||
if {$b != $ob} {
|
||||
hset $chPath/$n $b
|
||||
}
|
||||
}
|
||||
}
|
||||
return [sct nextSndReq]
|
||||
}
|
||||
##
|
||||
# Reads a list of values in the result and assigns them to the nodes in the args list
|
||||
proc rdVal {root nodes} {
|
||||
set currCh [sct chopper]
|
||||
set values [sct result]
|
||||
set chPath $root/ch$currCh
|
||||
if {[string match "ASCERR:*" $values]} {
|
||||
sct geterror $values
|
||||
hset $chPath/device_error $values
|
||||
return idle
|
||||
}
|
||||
if {[hval $chPath/device_error] != ""} {
|
||||
hset $chPath/device_error ""
|
||||
}
|
||||
foreach n $nodes v $values {
|
||||
set oldval [hval $chPath/$n]
|
||||
# if {$n == "speed_setpt"} {
|
||||
# broadcast rdVal,$n $chPath/$n: oldval=$oldval, v=$v
|
||||
# }
|
||||
if {$v != $oldval} {
|
||||
# if {$n == "speed_setpt"} {
|
||||
# broadcast rdVal,$n: hset $chPath/$n $v
|
||||
# }
|
||||
hset $chPath/$n $v
|
||||
}
|
||||
}
|
||||
set nextReq [sct nextSndReq]
|
||||
if {$nextReq == "NXTCHOPPER"} {
|
||||
if {$currCh < [sct chMax] } {
|
||||
incr currCh
|
||||
sct chopper $currCh
|
||||
} else {
|
||||
sct chopper 1
|
||||
return idle
|
||||
}
|
||||
}
|
||||
return $nextReq
|
||||
}
|
||||
|
||||
proc rdSetCmdReply {root chPath} {
|
||||
set reply [sct result]
|
||||
if {[string match "ASCERR:*" $reply]} {
|
||||
sct geterror $reply
|
||||
hset $chPath/device_error $reply
|
||||
return idle
|
||||
}
|
||||
if {[hval $chPath/device_error] != ""} {
|
||||
hset $chPath/device_error ""
|
||||
}
|
||||
sct_fermi queue $root progress read
|
||||
return idle
|
||||
}
|
||||
# Create chopper control
|
||||
set scobjNS ::scobj::chopper
|
||||
set sim_mode false
|
||||
set pollrate 5
|
||||
MakeSICSObj fermi_chopper SCT_OBJECT user int
|
||||
sicslist setatt fermi_chopper klass NXfermi_chopper
|
||||
set fermiPath /sics/fermi_chopper
|
||||
::scobj::hinitprops fermi_chopper
|
||||
if {$sim_mode == "false"} {
|
||||
makesctcontroller sct_fermi tcpmodbus 137.157.202.213:502
|
||||
}
|
||||
hsetprop $fermiPath chopper 1
|
||||
|
||||
set intlck_fields {test_mode cc_shutdown_req dsp_summ_shtdwn cooling_loss spd_sensor_loss ref_sig_loss over_temp vac_fail overspeed_or_breakfail cc_wd_fail ext_fault ups_fail emerg_stop pos_alarm osc_fail dsp_wd_fail}
|
||||
|
||||
set chMax 1
|
||||
hsetprop $fermiPath chMax $chMax
|
||||
for {set chN 1} {$chN <= $chMax} {incr chN} {
|
||||
set chname ch$chN
|
||||
sicslist setatt fermi_chopper klass NXfermi_chopper
|
||||
sicslist setatt fermi_chopper long_name fermi_chopper
|
||||
hfactory $fermiPath/$chname plain user float
|
||||
set chPath $fermiPath/$chname
|
||||
hfactory $chPath/device_error plain user text
|
||||
|
||||
foreach field {
|
||||
system_status intlck_status rotation_speed
|
||||
phase_veto_count phase_nonveto_count phase_acc
|
||||
phase_rep phase_ok vetowin100ns
|
||||
vetowin50ns mode speed_setpt
|
||||
prop_gain int_gain phase_gain
|
||||
ref_delay ref_period sync_srce motdir idle_toggle
|
||||
} {
|
||||
hfactory $fermiPath/$chname/$field plain user float
|
||||
}
|
||||
hsetprop $chPath/system_status oldval "UNKNOWN"
|
||||
hsetprop $chPath/intlck_status oldval "UNKNOWN"
|
||||
|
||||
foreach field {
|
||||
avc_on motdir phase_locked lev_complete alarm run up_to_speed ok
|
||||
} {
|
||||
hfactory $fermiPath/$chname/system_status/$field plain user int
|
||||
}
|
||||
foreach field $intlck_fields {
|
||||
hfactory $fermiPath/$chname/intlck_status/$field plain user int
|
||||
}
|
||||
|
||||
|
||||
hfactory $chPath/control plain user none
|
||||
hfactory $chPath/control/device_error plain user text
|
||||
|
||||
foreach {n cmd} [subst -nocommands {
|
||||
set_rotspeed "$chN:16:1000:1:U32"
|
||||
set_prop_gain "$chN:16:1004:1:F32"
|
||||
set_int_gain "$chN:16:1006:1:F32"
|
||||
set_phase_gain "$chN:16:1008:1:F32"
|
||||
set_ref_delay "$chN:16:1010:1:U32"
|
||||
set_ref_period "$chN:16:1012:1:U32"
|
||||
set_sync_source "$chN:16:1014:1:U32"
|
||||
set_motor_dir "$chN:16:1016:1:U32"
|
||||
start "$chN:5:1"
|
||||
stop "$chN:5:2"
|
||||
idle_toggle "$chN:5:3"
|
||||
reset "$chN:5:4"
|
||||
}] {
|
||||
hfactory $chPath/control/$n plain user float
|
||||
hsetprop $chPath/control/$n write ${scobjNS}::sndMBset $chPath $cmd
|
||||
hsetprop $chPath/control/$n RDSETREPLY ${scobjNS}::rdSetCmdReply $fermiPath $chPath/control
|
||||
if {$sim_mode == "false"} {
|
||||
sct_fermi write $chPath/control/$n
|
||||
}
|
||||
}
|
||||
}
|
||||
# Each Req chains to a rdVal proc which chains to the next req
|
||||
# Add a next state property
|
||||
#TODO Read idle_toggle status
|
||||
hsetprop $fermiPath nextSndReq "UNKNOWN"
|
||||
hsetprop $fermiPath read ${scobjNS}::sndMBquery "RDSYSSTAT" "3:10:1:U16" "INTLKREQ"
|
||||
hsetprop $fermiPath "NXTCHOPPER" ${scobjNS}::sndMBquery "RDSYSSTAT" "3:10:1:U16" "INTLKREQ"
|
||||
hsetprop $fermiPath "INTLKREQ" ${scobjNS}::sndMBquery "RDINTLKSTAT" "3:12:1:U16" "ROTSPDREQ"
|
||||
hsetprop $fermiPath "ROTSPDREQ" ${scobjNS}::sndMBquery "RDROTSPD" "3:14:1:U16" "VETOINF"
|
||||
hsetprop $fermiPath "VETOINF" ${scobjNS}::sndMBquery "RDVETOINF" "3:18:2:U32" "PHASEINF"
|
||||
hsetprop $fermiPath "PHASEINF" ${scobjNS}::sndMBquery "RDPHASEINF" "3:24:3:F32" "VETO"
|
||||
hsetprop $fermiPath "VETO" ${scobjNS}::sndMBquery "RDVETO" "3:30:3:U32" "ROTSPSET"
|
||||
hsetprop $fermiPath "ROTSPSET" ${scobjNS}::sndMBquery "RDROTSPSET" "3:1000:1:U32" "GAINPHASE"
|
||||
hsetprop $fermiPath "GAINPHASE" ${scobjNS}::sndMBquery "RDGAINPHASE" "3:1004:3:F32" "SYNMOTDIR"
|
||||
hsetprop $fermiPath "SYNMOTDIR" ${scobjNS}::sndMBquery "RDSYNMOTDIR" "3:1010:4:U32" "IDLETOGGLE"
|
||||
hsetprop $fermiPath "IDLETOGGLE" ${scobjNS}::sndMBquery "RDIDLE" "1:3:1" "NXTCHOPPER"
|
||||
|
||||
|
||||
hsetprop $fermiPath "RDSYSSTAT" ${scobjNS}::rdStatus $fermiPath system_status {avc_on motdir phase_locked lev_complete alarm run up_to_speed ok}
|
||||
hsetprop $fermiPath "RDINTLKSTAT" ${scobjNS}::rdStatus $fermiPath intlck_status $intlck_fields
|
||||
hsetprop $fermiPath "RDROTSPD" ${scobjNS}::rdVal $fermiPath rotation_speed
|
||||
hsetprop $fermiPath "RDVETOINF" ${scobjNS}::rdVal $fermiPath {phase_veto_count phase_nonveto_count}
|
||||
hsetprop $fermiPath "RDPHASEINF" ${scobjNS}::rdVal $fermiPath {phase_acc phase_rep phase_ok}
|
||||
hsetprop $fermiPath "RDVETO" ${scobjNS}::rdVal $fermiPath {vetowin100ns vetowin50ns mode}
|
||||
hsetprop $fermiPath "RDROTSPSET" ${scobjNS}::rdVal $fermiPath speed_setpt
|
||||
hsetprop $fermiPath "RDGAINPHASE" ${scobjNS}::rdVal $fermiPath {prop_gain int_gain phase_gain}
|
||||
hsetprop $fermiPath "RDSYNMOTDIR" ${scobjNS}::rdVal $fermiPath {ref_delay ref_period sync_srce motdir}
|
||||
hsetprop $fermiPath "RDIDLE" ${scobjNS}::rdVal $fermiPath idle_toggle
|
||||
|
||||
|
||||
if {$sim_mode == "false"} {
|
||||
sct_fermi poll $fermiPath $pollrate
|
||||
sct_fermi queue $fermiPath progress read
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user