diff --git a/site_ansto/hardsup/makefile b/site_ansto/hardsup/makefile index a11642a5..b544fe86 100644 --- a/site_ansto/hardsup/makefile +++ b/site_ansto/hardsup/makefile @@ -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 diff --git a/site_ansto/hardsup/sct_tcpmodbus.c b/site_ansto/hardsup/sct_tcpmodbus.c new file mode 100644 index 00000000..96ab19e5 --- /dev/null +++ b/site_ansto/hardsup/sct_tcpmodbus.c @@ -0,0 +1,515 @@ +/** @file TCP Modbus protocol handler for script-context based controllers. +* +*/ +#include +#include +#include +#include +#include +#include +#include + +#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 +#include + +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; +} diff --git a/site_ansto/instrument/config/hipadaba/common_instrument_dictionary.tcl b/site_ansto/instrument/config/hipadaba/common_instrument_dictionary.tcl index e06f6b0f..243baf9e 100644 --- a/site_ansto/instrument/config/hipadaba/common_instrument_dictionary.tcl +++ b/site_ansto/instrument/config/hipadaba/common_instrument_dictionary.tcl @@ -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} diff --git a/site_ansto/instrument/pelican/config/chopper/sct_fermichopper.tcl b/site_ansto/instrument/pelican/config/chopper/sct_fermichopper.tcl new file mode 100644 index 00000000..4a46d647 --- /dev/null +++ b/site_ansto/instrument/pelican/config/chopper/sct_fermichopper.tcl @@ -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 + } +}