623 lines
15 KiB
C
623 lines
15 KiB
C
/*----------------------------------------------------------------------------
|
|
This is a single counter implemented on top of the Risoe ECB electronic
|
|
|
|
copyright: see file COPYRIGHT
|
|
|
|
Mark Koennecke, January-February 2003
|
|
---------------------------------------------------------------------------*/
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include <errno.h>
|
|
#include <tcl.h>
|
|
#include <math.h>
|
|
#include <unistd.h>
|
|
#include "fortify.h"
|
|
#include "sics.h"
|
|
#include "status.h"
|
|
#include "psi/ecb.h"
|
|
#include "countdriv.h"
|
|
|
|
/*------------------ our private data structure ------------------------*/
|
|
typedef struct {
|
|
pECB ecb; /* the ECB system we talk to */
|
|
unsigned char prescaler[8]; /* an array for the prescaler values */
|
|
int tfreq; /* timer frequency */
|
|
unsigned char control; /* marks the control monitor */
|
|
int state; /* current counting state */
|
|
} ECBCounter, *pECBCounter;
|
|
|
|
/*----------------- private defines ------------------------------------*/
|
|
#define STFRD 137
|
|
#define STREAD 138
|
|
#define STOPS 136
|
|
#define STCLEA 134
|
|
#define PRELOA 139
|
|
#define STLOAD 156
|
|
#define STCPRE 133
|
|
#define STARTS 135
|
|
#define SPCSTA 169
|
|
|
|
/*------------------ state codes --------------------------------------*/
|
|
#define IDLE 0
|
|
#define COUNT 2
|
|
#define NOBEAM 3
|
|
/*--------------------------------------------------------------------*/
|
|
#define MAX_COUNT 4294967295.0
|
|
/*------------------ error codes --------------------------------------*/
|
|
#define COMMERROR -300
|
|
#define TOMANYCOUNTS -301
|
|
#define NOSEND -302
|
|
#define INVALIDCOUNTER -304
|
|
#define INVALIDPRESCALER -305
|
|
#define BADFREQ -306
|
|
/*======================================================================*/
|
|
static int readScaler(pECBCounter pPriv, int scaler, int *count)
|
|
{
|
|
int status;
|
|
Z80_reg in, out;
|
|
Ecb_pack data;
|
|
|
|
in.c = (unsigned char) scaler;
|
|
status = ecbExecute(pPriv->ecb, STREAD, in, &out);
|
|
if (status != 1) {
|
|
return COMMERROR;
|
|
}
|
|
|
|
data.b.byt3 = out.c;
|
|
data.b.byt2 = out.b;
|
|
data.b.byt1 = out.d;
|
|
data.b.byt0 = out.e;
|
|
if (scaler == 0) {
|
|
*count = data.result / pPriv->tfreq;
|
|
} else {
|
|
*count = data.result;
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------*/
|
|
static int check4Beam(struct __COUNTER *pCter, int *beam)
|
|
{
|
|
Z80_reg in, out;
|
|
pECBCounter self = NULL;
|
|
int status;
|
|
|
|
self = (pECBCounter) pCter->pData;
|
|
assert(self);
|
|
|
|
in.c = 1;
|
|
status = ecbExecute(self->ecb, SPCSTA, in, &out);
|
|
if (status != 1) {
|
|
pCter->iErrorCode = COMMERROR;
|
|
return HWFault;
|
|
}
|
|
*beam = (int) out.d;
|
|
return 1;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------*/
|
|
static int stopScalers(pECBCounter self)
|
|
{
|
|
int status;
|
|
Z80_reg in, out;
|
|
|
|
status = ecbExecute(self->ecb, STOPS, in, &out);
|
|
if (status != 1) {
|
|
return COMMERROR;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*========================================================================
|
|
These two functions currently rely on the idea that the ECB stops
|
|
and starts without clearing counters in between. The sequence of
|
|
things necessary to start it, suggests this. If this is not the case then
|
|
this will not work.
|
|
===========================================================================*/
|
|
static int ECBPause(struct __COUNTER *self)
|
|
{
|
|
int status;
|
|
pECBCounter pPriv = NULL;
|
|
|
|
assert(self);
|
|
pPriv = (pECBCounter) self->pData;
|
|
assert(pPriv);
|
|
|
|
if ((status = stopScalers(pPriv)) <= 0) {
|
|
self->iErrorCode = status;
|
|
return HWFault;
|
|
}
|
|
return OKOK;
|
|
}
|
|
|
|
/*=======================================================================*/
|
|
static int ECBContinue(struct __COUNTER *self)
|
|
{
|
|
int status;
|
|
pECBCounter pPriv = NULL;
|
|
Z80_reg in, out;
|
|
|
|
assert(self);
|
|
pPriv = (pECBCounter) self->pData;
|
|
assert(pPriv);
|
|
|
|
status = ecbExecute(pPriv->ecb, STARTS, in, &out);
|
|
if (status != 1) {
|
|
self->iErrorCode = status;
|
|
return HWFault;
|
|
}
|
|
|
|
return OKOK;
|
|
}
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
static int ECBGetStatus(struct __COUNTER *self, float *fControl)
|
|
{
|
|
pECBCounter pPriv = (pECBCounter) self->pData;
|
|
int status, result, scaler;
|
|
Z80_reg in, out;
|
|
int count, beam;
|
|
|
|
assert(pPriv);
|
|
|
|
/*
|
|
This can happen after a stop
|
|
*/
|
|
if (pPriv->state == IDLE) {
|
|
ECBTransfer(self);
|
|
return HWIdle;
|
|
}
|
|
|
|
/*
|
|
read status bit
|
|
*/
|
|
status = ecbExecute(pPriv->ecb, STFRD, in, &out);
|
|
if (status != 1) {
|
|
self->iErrorCode = COMMERROR;
|
|
pPriv->state = IDLE;
|
|
return HWFault;
|
|
}
|
|
/*
|
|
read beam status
|
|
*/
|
|
status = check4Beam(self, &beam);
|
|
if (status != 1) {
|
|
self->iErrorCode = COMMERROR;
|
|
return HWFault;
|
|
}
|
|
beam &= 1;
|
|
|
|
/*
|
|
sophisticated logic in order to keep track of the various states
|
|
the thing can be in. Complicated by the fact that the status becomes
|
|
idle (out.d = 0) when the measurement is paused due to the lack of
|
|
beam.
|
|
*/
|
|
if (pPriv->state == COUNT && beam == 1) {
|
|
ECBPause(self);
|
|
pPriv->state = NOBEAM;
|
|
SetStatus(eOutOfBeam);
|
|
result = HWNoBeam;
|
|
}
|
|
if (pPriv->state == NOBEAM && beam == 0) {
|
|
ECBContinue(self);
|
|
pPriv->state = COUNT;
|
|
SetStatus(eCounting);
|
|
return HWBusy;
|
|
}
|
|
if (pPriv->state == NOBEAM && beam == 1) {
|
|
return HWNoBeam;
|
|
}
|
|
if (out.d == 0 && pPriv->state == COUNT) {
|
|
result = HWIdle;
|
|
ECBTransfer(self);
|
|
pPriv->state = IDLE;
|
|
} else {
|
|
result = HWBusy;
|
|
}
|
|
|
|
|
|
/*
|
|
select which scaler to read
|
|
*/
|
|
if (self->eMode == eTimer) {
|
|
scaler = 0;
|
|
} else {
|
|
scaler = pPriv->control;
|
|
}
|
|
|
|
readScaler(pPriv, scaler, &count);
|
|
/*
|
|
ignore errors on this one
|
|
*/
|
|
*fControl = (float) count;
|
|
|
|
return result;
|
|
}exit
|
|
|
|
|
|
/*=====================================================================*/
|
|
static int clearScalers(pECBCounter self)
|
|
{
|
|
int status;
|
|
Z80_reg in, out;
|
|
|
|
status = ecbExecute(self->ecb, STCLEA, in, &out);
|
|
if (status != 1) {
|
|
return COMMERROR;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------*/
|
|
static int loadPrescalers(pECBCounter self)
|
|
{
|
|
Z80_reg in, out;
|
|
int status, i;
|
|
|
|
for (i = 0; i < 8; i++) {
|
|
in.c = (unsigned char) i;
|
|
in.d = self->prescaler[i];
|
|
status = ecbExecute(self->ecb, PRELOA, in, &out);
|
|
if (status != 1) {
|
|
return COMMERROR;
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------*/
|
|
static int loadPreset(pECBCounter self, int preset, unsigned char control)
|
|
{
|
|
Z80_reg in, out;
|
|
Ecb_pack data;
|
|
int status, i;
|
|
|
|
data.result = preset;
|
|
|
|
in.c = data.b.byt3;
|
|
in.b = data.b.byt2;
|
|
in.e = data.b.byt1;
|
|
in.d = data.b.byt0;
|
|
status = ecbExecute(self->ecb, STLOAD, in, &out);
|
|
if (status != 1) {
|
|
return COMMERROR;
|
|
}
|
|
|
|
in.b = data.b.byt2;
|
|
in.e = data.b.byt1;
|
|
in.d = data.b.byt0;
|
|
in.c = 4 * control;
|
|
status = ecbExecute(self->ecb, STCPRE, in, &out);
|
|
if (status != 1) {
|
|
return COMMERROR;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
static int ECBStart(struct __COUNTER *self)
|
|
{
|
|
pECBCounter pPriv = NULL;
|
|
int preset, status, controlUnit;
|
|
Z80_reg in, out;
|
|
|
|
assert(self);
|
|
pPriv = (pECBCounter) self->pData;
|
|
assert(pPriv);
|
|
|
|
/*
|
|
check if the preset is permissible
|
|
*/
|
|
preset = (int) rint(self->fPreset);
|
|
if (preset > MAX_COUNT) {
|
|
self->iErrorCode = TOMANYCOUNTS;
|
|
return HWFault;
|
|
}
|
|
if (self->eMode == eTimer) {
|
|
controlUnit = 0;
|
|
preset *= pPriv->tfreq;
|
|
if (preset > MAX_COUNT) {
|
|
self->iErrorCode = TOMANYCOUNTS;
|
|
return HWFault;
|
|
}
|
|
} else {
|
|
controlUnit = pPriv->control;
|
|
}
|
|
|
|
if ((status = stopScalers(pPriv)) <= 0) {
|
|
self->iErrorCode = status;
|
|
return HWFault;
|
|
}
|
|
|
|
if ((status = clearScalers(pPriv)) <= 0) {
|
|
self->iErrorCode = status;
|
|
return HWFault;
|
|
}
|
|
|
|
if ((status = loadPrescalers(pPriv)) <= 0) {
|
|
self->iErrorCode = status;
|
|
return HWFault;
|
|
}
|
|
|
|
if ((status =
|
|
loadPreset(pPriv, preset, (unsigned char) controlUnit)) <= 0) {
|
|
self->iErrorCode = status;
|
|
return HWFault;
|
|
}
|
|
|
|
status = ecbExecute(pPriv->ecb, STARTS, in, &out);
|
|
if (status != 1) {
|
|
self->iErrorCode = status;
|
|
return HWFault;
|
|
}
|
|
|
|
pPriv->state = COUNT;
|
|
return OKOK;
|
|
}
|
|
|
|
/*=======================================================================*/
|
|
static int ECBHalt(struct __COUNTER *self)
|
|
{
|
|
int status;
|
|
pECBCounter pPriv = NULL;
|
|
|
|
assert(self);
|
|
pPriv = (pECBCounter) self->pData;
|
|
assert(pPriv);
|
|
|
|
pPriv->state = IDLE;
|
|
if ((status = stopScalers(pPriv)) <= 0) {
|
|
self->iErrorCode = status;
|
|
return HWFault;
|
|
}
|
|
return OKOK;
|
|
}
|
|
|
|
/*=======================================================================*/
|
|
static int ECBTransfer(struct __COUNTER *self)
|
|
{
|
|
int status, count, i;
|
|
pECBCounter pPriv = NULL;
|
|
|
|
assert(self);
|
|
pPriv = (pECBCounter) self->pData;
|
|
assert(pPriv);
|
|
|
|
/*
|
|
read time
|
|
*/
|
|
status = readScaler(pPriv, 0, &count);
|
|
if (status <= 0) {
|
|
self->iErrorCode = COMMERROR;
|
|
return HWFault;
|
|
}
|
|
self->fTime = (float) count;
|
|
|
|
/*
|
|
read other scalers
|
|
*/
|
|
for (i = 1; i < 8; i++) {
|
|
status = readScaler(pPriv, i, &count);
|
|
if (status <= 0) {
|
|
self->iErrorCode = COMMERROR;
|
|
return HWFault;
|
|
}
|
|
self->lCounts[i - 1] = count;
|
|
}
|
|
return OKOK;
|
|
}
|
|
|
|
/*======================================================================*/
|
|
static int ECBGetError(struct __COUNTER *self, int *iCode,
|
|
char *errorText, int errlen)
|
|
{
|
|
char pBueffel[132];
|
|
|
|
*iCode = self->iErrorCode;
|
|
switch (self->iErrorCode) {
|
|
case COMMERROR:
|
|
strlcpy(errorText, "Communication error with ECB", errlen);
|
|
break;
|
|
case TOMANYCOUNTS:
|
|
strlcpy(errorText, "Preset is to high!", errlen);
|
|
break;
|
|
case NOSEND:
|
|
strlcpy(errorText, "Cannot send naked data to ECB", errlen);
|
|
break;
|
|
case UNKNOWNPAR:
|
|
strlcpy(errorText, "parameter unknown", errlen);
|
|
break;
|
|
case INVALIDCOUNTER:
|
|
strlcpy(errorText, "Invalid counter number requested, 0-7 allowed",
|
|
errlen);
|
|
break;
|
|
case INVALIDPRESCALER:
|
|
strlcpy(errorText, "Invalid prescaler value, allowed 1 or 10", errlen);
|
|
break;
|
|
case BADFREQ:
|
|
strlcpy(errorText, "Bad timer frequency: 10 or 1000 allowed", errlen);
|
|
break;
|
|
default:
|
|
sprintf(pBueffel, "Unknown error code %d", self->iErrorCode);
|
|
strlcpy(errorText, pBueffel, errlen);
|
|
break;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*=======================================================================*/
|
|
static int ECBFixIt(struct __COUNTER *self, int iCode)
|
|
{
|
|
return COTERM;
|
|
}
|
|
|
|
/*======================================================================*/
|
|
|
|
/*******************************************************************************
|
|
* Load the parameters 'dot' and 'divide' for a motor or an encoder.
|
|
* 'dot' specifies the placement of a punctuation mark on the display
|
|
* of f.ex a motor position. 'divide' specifies how many times the po-
|
|
* sition is to be divided by two before it is displayed.
|
|
******************************************************************************/
|
|
static void Dot_divide(int device, int data, pECB ecb)
|
|
{
|
|
int function, dot, divide;
|
|
Z80_reg x_inreg, out;
|
|
|
|
if (data == 0) /* If zero, dont send dot/divide) */
|
|
return;
|
|
|
|
dot = 0;
|
|
while ((data % 10) == 0) {
|
|
dot++;
|
|
data /= 10;
|
|
}
|
|
divide = 0;
|
|
while ((data % 2) == 0) {
|
|
divide++;
|
|
data /= 2;
|
|
}
|
|
if (data != 1) /* If != 1, not a binary No. */
|
|
return;
|
|
if (dot > 0)
|
|
dot = 8 - dot;
|
|
x_inreg.c = 0; /* Specify input */
|
|
x_inreg.b = (unsigned char) device;
|
|
x_inreg.d = (unsigned char) dot; /* Dot position */
|
|
x_inreg.e = (unsigned char) divide; /* No. of times to divide by 2 */
|
|
|
|
ecbExecute(ecb, 170, x_inreg, &out);
|
|
return;
|
|
}
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
static int ECBSet(struct __COUNTER *self, char *name,
|
|
int iCter, float fVal)
|
|
{
|
|
pECBCounter pPriv = NULL;
|
|
int iVal;
|
|
|
|
assert(self);
|
|
pPriv = (pECBCounter) self->pData;
|
|
assert(pPriv);
|
|
|
|
iVal = (int) rint(fVal);
|
|
|
|
if (strcmp(name, "prescaler") == 0) {
|
|
if (iCter < 0 || iCter > 7) {
|
|
self->iErrorCode = INVALIDCOUNTER;
|
|
return HWFault;
|
|
}
|
|
if (iVal != 1 && iVal != 10) {
|
|
self->iErrorCode = INVALIDPRESCALER;
|
|
return HWFault;
|
|
}
|
|
pPriv->prescaler[iCter] = (unsigned char) iVal;
|
|
return OKOK;
|
|
} else if (strcmp(name, "tfreq") == 0) {
|
|
if (fVal == 1000) {
|
|
pPriv->prescaler[0] = 1;
|
|
pPriv->tfreq = 1000;
|
|
Dot_divide(64, 1000, pPriv->ecb);
|
|
return OKOK;
|
|
} else if (fVal == 10) {
|
|
pPriv->tfreq = 10;
|
|
pPriv->prescaler[0] = 10;
|
|
Dot_divide(64, 10, pPriv->ecb);
|
|
return OKOK;
|
|
} else {
|
|
self->iErrorCode = BADFREQ;
|
|
return HWFault;
|
|
}
|
|
} else {
|
|
self->iErrorCode = UNKNOWNPAR;
|
|
return HWFault;
|
|
}
|
|
}
|
|
|
|
/*===================================================================*/
|
|
static int ECBGet(struct __COUNTER *self, char *name,
|
|
int iCter, float *fVal)
|
|
{
|
|
pECBCounter pPriv = NULL;
|
|
|
|
assert(self);
|
|
pPriv = (pECBCounter) self->pData;
|
|
assert(pPriv);
|
|
|
|
if (strcmp(name, "prescaler") == 0) {
|
|
*fVal = (float) pPriv->prescaler[iCter];
|
|
return OKOK;
|
|
} else if (strcmp(name, "tfreq") == 0) {
|
|
*fVal = (float) pPriv->tfreq;
|
|
return OKOK;
|
|
} else {
|
|
self->iErrorCode = UNKNOWNPAR;
|
|
return HWFault;
|
|
}
|
|
}
|
|
|
|
/*=====================================================================*/
|
|
static int ECBSend(struct __COUNTER *self, char *text,
|
|
char *reply, int replylen)
|
|
{
|
|
strlcpy(reply, "ECB does not feast on ASCII strings, refused!",
|
|
replylen);
|
|
return OKOK;
|
|
}
|
|
|
|
/*====================================================================*/
|
|
pCounterDriver MakeECBCounter(char *ecb)
|
|
{
|
|
pECBCounter pPriv = NULL;
|
|
pCounterDriver self = NULL;
|
|
int i;
|
|
|
|
/*
|
|
memory for everybody
|
|
*/
|
|
self = CreateCounterDriver("ecb", "ecb");
|
|
pPriv = (pECBCounter) malloc(sizeof(ECBCounter));
|
|
if (self == NULL || pPriv == NULL) {
|
|
return NULL;
|
|
}
|
|
memset(pPriv, 0, sizeof(ECBCounter));
|
|
|
|
/*
|
|
initialize private data structure
|
|
*/
|
|
pPriv->ecb = (pECB) FindCommandData(pServ->pSics, ecb, "ECB");
|
|
if (pPriv->ecb == NULL) {
|
|
DeleteCounterDriver(self);
|
|
free(pPriv);
|
|
return NULL;
|
|
}
|
|
for (i = 0; i < 8; i++) {
|
|
pPriv->prescaler[i] = 1;
|
|
}
|
|
pPriv->tfreq = 1000;
|
|
pPriv->control = 1;
|
|
|
|
|
|
/*
|
|
assign function pointers
|
|
*/
|
|
self->GetStatus = ECBGetStatus;
|
|
self->Start = ECBStart;
|
|
self->Pause = ECBPause;
|
|
self->Continue = ECBContinue;
|
|
self->Halt = ECBHalt;
|
|
self->ReadValues = ECBTransfer;
|
|
self->GetError = ECBGetError;
|
|
self->TryAndFixIt = ECBFixIt;
|
|
self->Set = ECBSet;
|
|
self->Get = ECBGet;
|
|
self->Send = ECBSend;
|
|
self->KillPrivate = NULL;
|
|
|
|
self->pData = pPriv;
|
|
return self;
|
|
}
|