102 lines
2.3 KiB
C
102 lines
2.3 KiB
C
/**
|
|
* This is an asynchronous protocol driver for the protocol of
|
|
* the Phytron MCC-2 motor controller.
|
|
* The send format is:
|
|
* <STX> DATA <ETX>
|
|
* the reply format is:
|
|
* <STX> NACK | ACK |& REPLY <ETX>
|
|
*
|
|
* copyright: see file COPYRIGHT
|
|
*
|
|
* Mark Koennecke, June 2009
|
|
*/
|
|
#include <errno.h>
|
|
#include <ascon.h>
|
|
#include <ascon.i>
|
|
#include <dynstring.h>
|
|
|
|
#define STX '\2'
|
|
#define ETX '\3'
|
|
#define NACK '\5'
|
|
#define ACK '\6'
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
static int PhytronHandler(Ascon * a)
|
|
{
|
|
char *data = NULL;
|
|
int ret, l;
|
|
char chr;
|
|
|
|
|
|
switch (a->state) {
|
|
case AsconWriteStart:
|
|
DynStringInsert(a->wrBuffer,"\2",0);
|
|
DynStringConcatChar(a->wrBuffer,ETX);
|
|
a->state = AsconWriting;
|
|
a->wrPos = 0;
|
|
break;
|
|
case AsconReading:
|
|
ret = AsconReadChar(a->fd, &chr);
|
|
if (ret < 0) {
|
|
/* EINTR means we must retry */
|
|
if (errno != EINTR && errno != EAGAIN) {
|
|
AsconError(a, "AsconReadChar failed:", errno);
|
|
}
|
|
return 1;
|
|
} else if (ret > 0) {
|
|
if(chr == STX){
|
|
DynStringClear(a->rdBuffer);
|
|
} else if (chr == ACK){
|
|
DynStringConcat(a->rdBuffer,"ACK");
|
|
} else if(chr == NACK){
|
|
DynStringConcat(a->rdBuffer,"NACK");
|
|
} else if(chr == ETX){
|
|
a->state = AsconReadDone;
|
|
} else {
|
|
DynStringConcatChar(a->rdBuffer,chr);
|
|
}
|
|
} else if (ret == 0) {
|
|
if (a->timeout > 0) {
|
|
if (DoubleTime() - a->start > a->timeout) {
|
|
AsconError(a, "read timeout", 0);
|
|
a->state = AsconTimeout;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
break;
|
|
default:
|
|
return AsconStdHandler(a);
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------*/
|
|
static int PhytronInit(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->killPrivate = free;
|
|
return 1;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------*/
|
|
void AddPhytronProtocoll()
|
|
{
|
|
AsconProtocol *prot = NULL;
|
|
|
|
prot = calloc(sizeof(AsconProtocol), 1);
|
|
prot->name = strdup("phytron");
|
|
prot->init = PhytronInit;
|
|
prot->handler = PhytronHandler;
|
|
AsconInsertProtocol(prot);
|
|
}
|
|
|