- Adapted indenation to new agreed upon system
- Added support for second generation scriptcontext based counter
This commit is contained in:
koennecke
@ -24,40 +24,45 @@
|
||||
#include "scriptcontext.h"
|
||||
/*---------------------------------------------------------------*/
|
||||
typedef struct {
|
||||
pObjectDescriptor pDes;
|
||||
pIDrivable pDriv;
|
||||
pHdb node;
|
||||
SctController *c;
|
||||
}SctDrive, *pSctDrive;
|
||||
pObjectDescriptor pDes;
|
||||
pIDrivable pDriv;
|
||||
pHdb node;
|
||||
SctController *c;
|
||||
} SctDrive, *pSctDrive;
|
||||
/*---------------------------------------------------------------*/
|
||||
static void *SCTDRIVGetInterface(void *data, int iD){
|
||||
pSctDrive self = NULL;
|
||||
|
||||
self = (pSctDrive)data;
|
||||
if(self != NULL && iD == DRIVEID){
|
||||
if (self->node == NULL) return NULL;
|
||||
return self->pDriv;
|
||||
} else {
|
||||
return NULL;
|
||||
}
|
||||
return NULL;
|
||||
static void *SCTDRIVGetInterface(void *data, int iD)
|
||||
{
|
||||
pSctDrive self = NULL;
|
||||
|
||||
self = (pSctDrive) data;
|
||||
if (self != NULL && iD == DRIVEID) {
|
||||
if (self->node == NULL)
|
||||
return NULL;
|
||||
return self->pDriv;
|
||||
} else {
|
||||
return NULL;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------
|
||||
This routine can return either OKOK or HWFault when thing
|
||||
go wrong. However, the return value of Halt is usually ignored!
|
||||
------------------------------------------------------------------*/
|
||||
static int SCTDRIVHalt(void *data) {
|
||||
pSctDrive self = NULL;
|
||||
char dummy[16];
|
||||
|
||||
self = (pSctDrive)data;
|
||||
if (GetHdbProperty(self->node,"halt", dummy, sizeof dummy)) {
|
||||
SctQueueNode(self->c, self->node, HaltPRIO, "halt", NULL);
|
||||
} else if (GetHdbProperty(self->node, "status", dummy, sizeof dummy)) {
|
||||
SetHdbProperty(self->node, "status", "idle");
|
||||
}
|
||||
return OKOK;
|
||||
static int SCTDRIVHalt(void *data)
|
||||
{
|
||||
pSctDrive self = NULL;
|
||||
char dummy[16];
|
||||
|
||||
self = (pSctDrive) data;
|
||||
if (GetHdbProperty(self->node, "halt", dummy, sizeof dummy)) {
|
||||
SctQueueNode(self->c, self->node, HaltPRIO, "halt", NULL);
|
||||
} else if (GetHdbProperty(self->node, "status", dummy, sizeof dummy)) {
|
||||
SetHdbProperty(self->node, "status", "idle");
|
||||
}
|
||||
return OKOK;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------
|
||||
This routine can return either 1 or 0. 1 means the position can
|
||||
be reached, 0 NOT
|
||||
@ -65,30 +70,32 @@ static int SCTDRIVHalt(void *data) {
|
||||
about which limit was violated
|
||||
------------------------------------------------------------------*/
|
||||
static int SCTDRIVCheckLimits(void *data, float val,
|
||||
char *error, int errlen){
|
||||
pSctDrive self = NULL;
|
||||
char script[1024];
|
||||
int status;
|
||||
Tcl_Interp *pTcl = NULL;
|
||||
char *result;
|
||||
|
||||
self = (pSctDrive)data;
|
||||
snprintf(script,1024,"%f", val);
|
||||
SetHdbProperty(self->node,"target", script);
|
||||
if(GetHdbProperty(self->node,"checklimits",script,1024)){
|
||||
status = SctCallInContext(pServ->dummyCon, script,
|
||||
self->node, self->c, &result);
|
||||
if(SctVerbose(self->c)){
|
||||
SCPrintf(pServ->dummyCon, eWarning, "script %s called with result %s\n ",
|
||||
script, result);
|
||||
}
|
||||
if(status == 0){
|
||||
strncpy(error,result,errlen);
|
||||
return 0;
|
||||
}
|
||||
char *error, int errlen)
|
||||
{
|
||||
pSctDrive self = NULL;
|
||||
char script[1024];
|
||||
int status;
|
||||
Tcl_Interp *pTcl = NULL;
|
||||
char *result;
|
||||
|
||||
self = (pSctDrive) data;
|
||||
snprintf(script, 1024, "%f", val);
|
||||
SetHdbProperty(self->node, "target", script);
|
||||
if (GetHdbProperty(self->node, "checklimits", script, 1024)) {
|
||||
status = SctCallInContext(pServ->dummyCon, script,
|
||||
self->node, self->c, &result);
|
||||
if (SctVerbose(self->c)) {
|
||||
SCPrintf(pServ->dummyCon, eWarning,
|
||||
"script %s called with result %s\n ", script, result);
|
||||
}
|
||||
return 1;
|
||||
if (status == 0) {
|
||||
strncpy(error, result, errlen);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------
|
||||
This routine can return 0 when a limit problem occurred
|
||||
OKOK when the motor was successfully started
|
||||
@ -98,22 +105,24 @@ static int SCTDRIVCheckLimits(void *data, float val,
|
||||
to start the motor in question
|
||||
val is the value to drive the motor too
|
||||
------------------------------------------------------------------*/
|
||||
static long SCTDRIVSetValue(void *data, SConnection *pCon, float val){
|
||||
pSctDrive self = NULL;
|
||||
int status;
|
||||
hdbValue v;
|
||||
|
||||
self = (pSctDrive)data;
|
||||
v.dataType = HIPFLOAT;
|
||||
v.v.doubleValue = (double)val;
|
||||
SetHdbProperty(self->node,"writestatus", "start");
|
||||
status = SetHipadabaPar(self->node, v, pCon);
|
||||
if(status == 1){
|
||||
return OKOK;
|
||||
} else {
|
||||
return HWFault;
|
||||
}
|
||||
static long SCTDRIVSetValue(void *data, SConnection * pCon, float val)
|
||||
{
|
||||
pSctDrive self = NULL;
|
||||
int status;
|
||||
hdbValue v;
|
||||
|
||||
self = (pSctDrive) data;
|
||||
v.dataType = HIPFLOAT;
|
||||
v.v.doubleValue = (double) val;
|
||||
SetHdbProperty(self->node, "writestatus", "start");
|
||||
status = SetHipadabaPar(self->node, v, pCon);
|
||||
if (status == 1) {
|
||||
return OKOK;
|
||||
} else {
|
||||
return HWFault;
|
||||
}
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------
|
||||
Checks the status of a running motor. Possible return values
|
||||
HWBusy The motor is still running
|
||||
@ -124,199 +133,214 @@ static long SCTDRIVSetValue(void *data, SConnection *pCon, float val){
|
||||
For real motors CheckStatus again shall try hard to fix any
|
||||
issues with the motor
|
||||
------------------------------------------------------------------*/
|
||||
static int SCTDRIVCheckStatus(void *data, SConnection *pCon){
|
||||
pSctDrive self = NULL;
|
||||
char script[1024];
|
||||
int status;
|
||||
Tcl_Interp *pTcl = NULL;
|
||||
char *result;
|
||||
SConnection *con;
|
||||
|
||||
self = (pSctDrive)data;
|
||||
|
||||
/*
|
||||
* check if the write command has gone through
|
||||
*/
|
||||
if(GetHdbProperty(self->node,"writestatus", script,1024)){
|
||||
if(strcmp(script,"start") == 0){
|
||||
return HWBusy;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* run the checkstatus script
|
||||
*/
|
||||
if(!GetHdbProperty(self->node,"checkstatus",script,1024)){
|
||||
if (!GetHdbProperty(self->node,"status",script,1024)){
|
||||
SCWrite(pCon,
|
||||
"ERROR: configuration problem: no checkstatus script!", eError);
|
||||
return HWFault;
|
||||
}
|
||||
result = script;
|
||||
} else {
|
||||
status = SctCallInContext(pCon,script, self->node,
|
||||
self->c, &result);
|
||||
if (status == 0) {
|
||||
SCPrintf(pCon,eError," script %s returned %s",
|
||||
script, result);
|
||||
return HWFault;
|
||||
}
|
||||
if(SctVerbose(self->c)){
|
||||
SCPrintf(pCon,eError," script %s returned %s",
|
||||
script, result);
|
||||
}
|
||||
}
|
||||
if(strstr(result,"busy") != NULL){
|
||||
static int SCTDRIVCheckStatus(void *data, SConnection * pCon)
|
||||
{
|
||||
pSctDrive self = NULL;
|
||||
char script[1024];
|
||||
int status;
|
||||
Tcl_Interp *pTcl = NULL;
|
||||
char *result;
|
||||
SConnection *con;
|
||||
|
||||
self = (pSctDrive) data;
|
||||
|
||||
/*
|
||||
* check if the write command has gone through
|
||||
*/
|
||||
if (GetHdbProperty(self->node, "writestatus", script, 1024)) {
|
||||
if (strcmp(script, "start") == 0) {
|
||||
return HWBusy;
|
||||
} else if(strstr(result,"posfault") != NULL){
|
||||
return HWPosFault;
|
||||
} else if(strstr(result,"fault") != NULL){
|
||||
return HWFault;
|
||||
} else if(strstr(result,"idle") != NULL){
|
||||
return HWIdle;
|
||||
} else {
|
||||
SCPrintf(pCon,eError,
|
||||
"ERROR: invalid status code %s returned from checkstatus script",
|
||||
result);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* run the checkstatus script
|
||||
*/
|
||||
if (!GetHdbProperty(self->node, "checkstatus", script, 1024)) {
|
||||
if (!GetHdbProperty(self->node, "status", script, 1024)) {
|
||||
SCWrite(pCon,
|
||||
"ERROR: configuration problem: no checkstatus script!",
|
||||
eError);
|
||||
return HWFault;
|
||||
}
|
||||
result = script;
|
||||
} else {
|
||||
status = SctCallInContext(pCon, script, self->node, self->c, &result);
|
||||
if (status == 0) {
|
||||
SCPrintf(pCon, eError, " script %s returned %s", script, result);
|
||||
return HWFault;
|
||||
}
|
||||
if (SctVerbose(self->c)) {
|
||||
SCPrintf(pCon, eError, " script %s returned %s", script, result);
|
||||
}
|
||||
}
|
||||
if (strstr(result, "busy") != NULL) {
|
||||
return HWBusy;
|
||||
} else if (strstr(result, "posfault") != NULL) {
|
||||
return HWPosFault;
|
||||
} else if (strstr(result, "fault") != NULL) {
|
||||
return HWFault;
|
||||
} else if (strstr(result, "idle") != NULL) {
|
||||
return HWIdle;
|
||||
} else {
|
||||
SCPrintf(pCon, eError,
|
||||
"ERROR: invalid status code %s returned from checkstatus script",
|
||||
result);
|
||||
return HWFault;
|
||||
}
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------
|
||||
GetValue is supposed to read a motor position
|
||||
On errors, -99999999.99 is returned and messages printed to pCon
|
||||
------------------------------------------------------------------*/
|
||||
static float SCTDRIVGetValue(void *data, SConnection *pCon){
|
||||
pSctDrive self = NULL;
|
||||
float val = -99999999.99;
|
||||
int status;
|
||||
char error[256];
|
||||
hdbValue v;
|
||||
|
||||
self = (pSctDrive)data;
|
||||
if(GetHdbProperty(self->node,"geterror", error, 256)){
|
||||
SCWrite(pCon,error, eError);
|
||||
return val;
|
||||
}
|
||||
return (float)self->node->value.v.doubleValue;
|
||||
static float SCTDRIVGetValue(void *data, SConnection * pCon)
|
||||
{
|
||||
pSctDrive self = NULL;
|
||||
float val = -99999999.99;
|
||||
int status;
|
||||
char error[256];
|
||||
hdbValue v;
|
||||
|
||||
self = (pSctDrive) data;
|
||||
if (GetHdbProperty(self->node, "geterror", error, 256)) {
|
||||
SCWrite(pCon, error, eError);
|
||||
return val;
|
||||
}
|
||||
return (float) self->node->value.v.doubleValue;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------
|
||||
returns NULL on failure, a new datastructure else
|
||||
------------------------------------------------------------------*/
|
||||
static pSctDrive SCTDRIVMakeObject(){
|
||||
pSctDrive self = NULL;
|
||||
static pSctDrive SCTDRIVMakeObject()
|
||||
{
|
||||
pSctDrive self = NULL;
|
||||
|
||||
self = calloc(sizeof(SctDrive),1);
|
||||
if(self == NULL){
|
||||
return NULL;
|
||||
}
|
||||
self->pDes = CreateDescriptor("SctDriveAdapter");
|
||||
self->pDriv = CreateDrivableInterface();
|
||||
if(self->pDes == NULL || self->pDriv == NULL){
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
self->pDes->GetInterface = SCTDRIVGetInterface;
|
||||
self->pDriv->Halt = SCTDRIVHalt;
|
||||
self->pDriv->CheckLimits = SCTDRIVCheckLimits;
|
||||
self->pDriv->SetValue = SCTDRIVSetValue;
|
||||
self->pDriv->CheckStatus = SCTDRIVCheckStatus;
|
||||
self->pDriv->GetValue = SCTDRIVGetValue;
|
||||
self = calloc(sizeof(SctDrive), 1);
|
||||
if (self == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
self->pDes = CreateDescriptor("SctDriveAdapter");
|
||||
self->pDriv = CreateDrivableInterface();
|
||||
if (self->pDes == NULL || self->pDriv == NULL) {
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
return self;
|
||||
self->pDes->GetInterface = SCTDRIVGetInterface;
|
||||
self->pDriv->Halt = SCTDRIVHalt;
|
||||
self->pDriv->CheckLimits = SCTDRIVCheckLimits;
|
||||
self->pDriv->SetValue = SCTDRIVSetValue;
|
||||
self->pDriv->CheckStatus = SCTDRIVCheckStatus;
|
||||
self->pDriv->GetValue = SCTDRIVGetValue;
|
||||
|
||||
return self;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------*/
|
||||
static int SctDriveCommand(SConnection *pCon, SicsInterp *sics, void *object,
|
||||
int argc, char *argv[]) {
|
||||
pSctDrive self = (pSctDrive)object;
|
||||
float val;
|
||||
|
||||
assert(self != NULL);
|
||||
|
||||
if (self->node == NULL) {
|
||||
SCWrite(pCon, "ERROR: defunct object", eError);
|
||||
return 0;
|
||||
}
|
||||
/*
|
||||
* only action: print value
|
||||
*/
|
||||
val = self->pDriv->GetValue(self,pCon);
|
||||
SCPrintf(pCon,eValue,"%s = %f", argv[0], val);
|
||||
return 1;
|
||||
static int SctDriveCommand(SConnection * pCon, SicsInterp * sics,
|
||||
void *object, int argc, char *argv[])
|
||||
{
|
||||
pSctDrive self = (pSctDrive) object;
|
||||
float val;
|
||||
|
||||
assert(self != NULL);
|
||||
|
||||
if (self->node == NULL) {
|
||||
SCWrite(pCon, "ERROR: defunct object", eError);
|
||||
return 0;
|
||||
}
|
||||
/*
|
||||
* only action: print value
|
||||
*/
|
||||
val = self->pDriv->GetValue(self, pCon);
|
||||
SCPrintf(pCon, eValue, "%s = %f", argv[0], val);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------*/
|
||||
static void SctDriveKill(void *data){
|
||||
pSctDrive self = (pSctDrive)data;
|
||||
|
||||
if(self == NULL){
|
||||
return;
|
||||
}
|
||||
if(self->pDriv != NULL){
|
||||
free(self->pDriv);
|
||||
}
|
||||
if(self->pDes != NULL){
|
||||
DeleteDescriptor(self->pDes);
|
||||
}
|
||||
free(self);
|
||||
static void SctDriveKill(void *data)
|
||||
{
|
||||
pSctDrive self = (pSctDrive) data;
|
||||
|
||||
if (self == NULL) {
|
||||
return;
|
||||
}
|
||||
if (self->pDriv != NULL) {
|
||||
free(self->pDriv);
|
||||
}
|
||||
if (self->pDes != NULL) {
|
||||
DeleteDescriptor(self->pDes);
|
||||
}
|
||||
free(self);
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------*/
|
||||
static hdbCallbackReturn SctDummyCallback(Hdb *node, void *userData,
|
||||
hdbMessage *msg) {
|
||||
static hdbCallbackReturn SctDummyCallback(Hdb * node, void *userData,
|
||||
hdbMessage * msg)
|
||||
{
|
||||
return hdbContinue;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------*/
|
||||
static void SctDriveDeleteNode(void *data) {
|
||||
pSctDrive self = (pSctDrive)data;
|
||||
static void SctDriveDeleteNode(void *data)
|
||||
{
|
||||
pSctDrive self = (pSctDrive) data;
|
||||
self->node = NULL;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------*/
|
||||
int SctMakeDriveAdapter(SConnection *pCon, SicsInterp *pSics, void *object,
|
||||
int argc, char *argv[]) {
|
||||
pSctDrive pNew = NULL;
|
||||
pSICSOBJ obj = NULL;
|
||||
int SctMakeDriveAdapter(SConnection * pCon, SicsInterp * pSics,
|
||||
void *object, int argc, char *argv[])
|
||||
{
|
||||
pSctDrive pNew = NULL;
|
||||
pSICSOBJ obj = NULL;
|
||||
hdbCallback *cb;
|
||||
|
||||
pNew = SCTDRIVMakeObject();
|
||||
if(pNew == NULL){
|
||||
SCWrite(pCon,"ERROR: out of memory in SctMakeDriveAdapter",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(argc < 4){
|
||||
SCWrite(pCon,"ERROR: not enough arguments for SctMakeDriveAdapter", eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
pNew->node = FindHdbNode(NULL,argv[2], pCon);
|
||||
obj = FindCommandData(pSics,argv[3], "SctController");
|
||||
if(pNew->node == NULL || obj == NULL){
|
||||
SCWrite(pCon,"ERROR: node or controller not found", eError);
|
||||
SctDriveKill(pNew);
|
||||
return 0;
|
||||
}
|
||||
pNew->c =(SctController *)obj->pPrivate;
|
||||
|
||||
if (strcasecmp(argv[0],"dynsctdrive") == 0) {
|
||||
|
||||
pNew = SCTDRIVMakeObject();
|
||||
if (pNew == NULL) {
|
||||
SCWrite(pCon, "ERROR: out of memory in SctMakeDriveAdapter", eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (argc < 4) {
|
||||
SCWrite(pCon, "ERROR: not enough arguments for SctMakeDriveAdapter",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
pNew->node = FindHdbNode(NULL, argv[2], pCon);
|
||||
obj = FindCommandData(pSics, argv[3], "SctController");
|
||||
if (pNew->node == NULL || obj == NULL) {
|
||||
SCWrite(pCon, "ERROR: node or controller not found", eError);
|
||||
SctDriveKill(pNew);
|
||||
return 0;
|
||||
}
|
||||
pNew->c = (SctController *) obj->pPrivate;
|
||||
|
||||
if (strcasecmp(argv[0], "dynsctdrive") == 0) {
|
||||
/* make object dynamic by defining a descriptor command */
|
||||
SetDescriptorKey(pNew->pDes, "creationCommand", "0");
|
||||
}
|
||||
AddCommand(pSics, argv[1], SctDriveCommand, SctDriveKill, pNew);
|
||||
SetHdbProperty(pNew->node,"sicsdev",argv[1]);
|
||||
|
||||
AddCommand(pSics, argv[1], SctDriveCommand, SctDriveKill, pNew);
|
||||
SetHdbProperty(pNew->node, "sicsdev", argv[1]);
|
||||
|
||||
cb = MakeHipadabaCallback(SctDummyCallback, pNew, SctDriveDeleteNode);
|
||||
assert(cb);
|
||||
AppendHipadabaCallback(pNew->node, cb);
|
||||
|
||||
return 1;
|
||||
}
|
||||
/*---------------------------------------------------------------*/
|
||||
int SctMakeDriveObject(SConnection *pCon, SicsInterp *pSics, void *object,
|
||||
int argc, char *argv[]);
|
||||
|
||||
void SctDriveInit(void) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------*/
|
||||
int SctMakeDriveObject(SConnection * pCon, SicsInterp * pSics,
|
||||
void *object, int argc, char *argv[]);
|
||||
|
||||
void SctDriveInit(void)
|
||||
{
|
||||
AddCmd("makesctdrive", SctMakeDriveAdapter);
|
||||
AddCmd("dynsctdrive", SctMakeDriveAdapter);
|
||||
AddCmd("makesctdriveobj",SctMakeDriveObject);
|
||||
AddCmd("makesctdriveobj", SctMakeDriveObject);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user