00001
00058 #include <stddef.h>
00059 #include <stdlib.h>
00060 #include <stdarg.h>
00061 #include <math.h>
00062 #include <stdio.h>
00063 #include <string.h>
00064 #include <errno.h>
00065
00066 #include "paramLib.h"
00067
00068 #include "epicsFindSymbol.h"
00069 #include "epicsTime.h"
00070 #include "epicsThread.h"
00071 #include "epicsEvent.h"
00072 #include "epicsMutex.h"
00073 #include "ellLib.h"
00074
00075 #include "drvSup.h"
00076 #include "epicsExport.h"
00077 #define DEFINE_MOTOR_PROTOTYPES 1
00078 #include "motor_interface.h"
00079 #include "asynDriver.h"
00080 #include "asynOctetSyncIO.h"
00081 #include "pmacAsynCoord.h"
00082
00083 motorAxisDrvSET_t pmacAsynCoord = {
00084 15,
00085 motorAxisReport,
00086 motorAxisInit,
00087 motorAxisSetLog,
00088 motorAxisOpen,
00089 motorAxisClose,
00090 motorAxisSetCallback,
00091 motorAxisSetDouble,
00092 motorAxisSetInteger,
00093 motorAxisGetDouble,
00094 motorAxisGetInteger,
00095 motorAxisHome,
00096 motorAxisMove,
00097 motorAxisVelocityMove,
00098 motorAxisStop,
00099 motorAxisforceCallback,
00100 };
00101
00102 epicsExportAddress(drvet, pmacAsynCoord);
00103
00104 typedef struct drvPmac * PMACDRV_ID;
00105 typedef struct drvPmac
00106 {
00107 PMACDRV_ID pNext;
00108 asynUser * pasynUser;
00109 int ref;
00110 int cs;
00111 AXIS_HDL axis;
00112 epicsThreadId motorThread;
00113 epicsTimeStamp now;
00114 int movesDeferred;
00115 double movingPollPeriod;
00116 double idlePollPeriod;
00117 epicsEventId pollEventId;
00118 epicsMutexId controllerMutexId;
00119 } drvPmac_t;
00120
00121
00122 static const int defaultMovingPollPeriod = 100;
00123 static const int defaultIdlePollPeriod = 500;
00124
00125 typedef struct motorAxisHandle
00126 {
00127 PMACDRV_ID pDrv;
00128 int coord_system;
00129 int axis;
00130 int program;
00131 asynUser * pasynUser;
00132 PARAMS params;
00133 motorAxisLogFunc print;
00134 void * logParam;
00135 epicsMutexId axisMutex;
00136 int deferred_move;
00137 double stepSize;
00138 } motorAxis;
00139
00140 const char axisName[] = {' ', 'A','B','C','U','V','W','X','Y','Z'};
00141
00142 static PMACDRV_ID pFirstDrv = NULL;
00143
00144 static int drvPmacLogMsg( void * param, const motorAxisLogMask_t logMask, const char *pFormat, ...);
00145 static motorAxisLogFunc drvPrint = drvPmacLogMsg;
00146 static motorAxisLogFunc drvPrintParam = NULL;
00147
00148 #define TRACE_FLOW motorAxisTraceFlow
00149 #define TRACE_DRIVER motorAxisTraceIODriver
00150 #define TRACE_ERROR motorAxisTraceError
00151
00152 #define MAX(a,b) ((a)>(b)? (a): (b))
00153 #define MIN(a,b) ((a)<(b)? (a): (b))
00154
00155
00156 #define NAXES 9
00157 #define NAME(pAxis) (axisName[(pAxis->axis)])
00158 #define CSVAR(pAxis) (pAxis->coord_system + 50)
00159
00160 #define DEMAND "Q7%d"
00161
00162 #define READBACK "Q8%d"
00163
00164 static void motorAxisReportAxis( AXIS_HDL pAxis, int level )
00165 {
00166 printf("axis %c:\n", NAME(pAxis));
00167 if (level > 0) printf( "drvPmac->axisMutex = %p\n", pAxis->axisMutex );
00168
00169 if (level > 1) {
00170 motorParam->dump( pAxis->params );
00171 }
00172 }
00173
00174 static void motorAxisReport( int level )
00175 {
00176 PMACDRV_ID pDrv;
00177
00178 for (pDrv = pFirstDrv; pDrv != NULL; pDrv = pDrv->pNext) {
00179 int i;
00180 const char *name;
00181
00182 pasynManager->getPortName(pDrv->pasynUser, &name);
00183 printf( "Found driver for PMAC C.S. %d via %s (ref %d)\n", pDrv->cs, name, pDrv->ref );
00184 for ( i = 0; i < NAXES; i++ ) {
00185 if (level > 0) {
00186 motorAxisReportAxis( &(pDrv->axis[i]), level );
00187 }
00188 }
00189 }
00190 }
00191
00192
00193 static int motorAxisInit( void )
00194 {
00195 return MOTOR_AXIS_OK;
00196 }
00197
00198 static int motorAxisSetLog( AXIS_HDL pAxis, motorAxisLogFunc logFunc, void * param )
00199 {
00200 if (pAxis == NULL) {
00201 if (logFunc == NULL) {
00202 drvPrint=drvPmacLogMsg;
00203 drvPrintParam = NULL;
00204 } else {
00205 drvPrint=logFunc;
00206 drvPrintParam = param;
00207 }
00208 } else {
00209 if (logFunc == NULL) {
00210 pAxis->print=drvPmacLogMsg;
00211 pAxis->logParam = NULL;
00212 } else {
00213 pAxis->print=logFunc;
00214 pAxis->logParam = param;
00215 }
00216 }
00217 return MOTOR_AXIS_OK;
00218 }
00219
00220 static AXIS_HDL motorAxisOpen( int ref, int axis, char * param )
00221 {
00222 PMACDRV_ID pDrv;
00223 AXIS_HDL pAxis = NULL;
00224
00225 for ( pDrv=pFirstDrv; pDrv != NULL && (ref != pDrv->ref); pDrv = pDrv->pNext){}
00226
00227 if (pDrv != NULL) {
00228 if (axis >= 0 && axis < NAXES) pAxis = &(pDrv->axis[axis]);
00229 }
00230
00231 return pAxis;
00232 }
00233
00234 static int motorAxisClose( AXIS_HDL pAxis )
00235 {
00236 return MOTOR_AXIS_OK;
00237 }
00238
00239 static int motorAxisGetInteger( AXIS_HDL pAxis, motorAxisParam_t function, int * value )
00240 {
00241 if (pAxis == NULL) {
00242 return MOTOR_AXIS_ERROR;
00243 } else {
00244 switch (function) {
00245 case motorAxisDeferMoves:
00246 *value = pAxis->pDrv->movesDeferred;
00247 return MOTOR_AXIS_OK;
00248 break;
00249 default:
00250 return motorParam->getInteger( pAxis->params, (paramIndex) function, value );
00251 }
00252 }
00253 }
00254
00255 static int motorAxisGetDouble( AXIS_HDL pAxis, motorAxisParam_t function, double * value )
00256 {
00257 if (pAxis == NULL) {
00258 return MOTOR_AXIS_ERROR;
00259 } else {
00260 switch(function) {
00261 case motorAxisDeferMoves:
00262 *value = (double)pAxis->pDrv->movesDeferred;
00263 return MOTOR_AXIS_OK;
00264 break;
00265 default:
00266 return motorParam->getDouble( pAxis->params, (paramIndex) function, value );
00267 }
00268 }
00269 }
00270
00271 static int motorAxisSetCallback( AXIS_HDL pAxis, motorAxisCallbackFunc callback, void * param )
00272 {
00273 if (pAxis == NULL) {
00274 return MOTOR_AXIS_ERROR;
00275 } else {
00276 return motorParam->setCallback( pAxis->params, callback, param );
00277 }
00278 }
00279
00280 static int motorAxisAsynConnect( const char * port, int addr, asynUser ** ppasynUser, char * inputEos, char * outputEos )
00281 {
00282 asynStatus status;
00283
00284 status = pasynOctetSyncIO->connect( port, addr, ppasynUser, NULL);
00285 if (status) {
00286 drvPrint( drvPrintParam, TRACE_ERROR,
00287 "drvPmacCreate: unable to connect to port %s\n",
00288 port);
00289 return MOTOR_AXIS_ERROR;
00290 }
00291
00292 status = pasynOctetSyncIO->setInputEos(*ppasynUser, inputEos, strlen(inputEos) );
00293 if (status) {
00294 asynPrint(*ppasynUser, ASYN_TRACE_ERROR,
00295 "drvPmacCreate: unable to set input EOS on %s: %s\n",
00296 port, (*ppasynUser)->errorMessage);
00297 pasynOctetSyncIO->disconnect(*ppasynUser);
00298 return MOTOR_AXIS_ERROR;
00299 }
00300
00301 status = pasynOctetSyncIO->setOutputEos(*ppasynUser, outputEos, strlen(outputEos));
00302 if (status) {
00303 asynPrint(*ppasynUser, ASYN_TRACE_ERROR,
00304 "drvPmacCreate: unable to set output EOS on %s: %s\n",
00305 port, (*ppasynUser)->errorMessage);
00306 pasynOctetSyncIO->disconnect(*ppasynUser);
00307 return MOTOR_AXIS_ERROR;
00308 }
00309
00310 return MOTOR_AXIS_OK;
00311 }
00312
00313 static int motorAxisWriteRead( AXIS_HDL pAxis, char * command, size_t reply_buff_size, char * response, int logGlobal )
00314 {
00315 asynStatus status;
00316 const double timeout=6.0;
00317 size_t nwrite, nread;
00318 int eomReason;
00319 asynUser * pasynUser = (logGlobal? pAxis->pDrv->pasynUser: pAxis->pasynUser);
00320
00321 if ( !logGlobal ) {
00322 pAxis->print( pAxis->logParam, TRACE_DRIVER, "Sending to PMAC C.S. ref %d command : %s\n",pAxis->pDrv->ref, command );
00323 }
00324
00325 status = pasynOctetSyncIO->writeRead( pasynUser,
00326 command, strlen(command),
00327 response, reply_buff_size,
00328 timeout,
00329 &nwrite, &nread, &eomReason );
00330
00331 if ( !logGlobal && nread != 0 ) {
00332 pAxis->print( pAxis->logParam, TRACE_DRIVER, "Recvd from PMAC C.S. ref %d response: %s\n",pAxis->pDrv->ref, response );
00333 }
00334
00335 if (status) {
00336 motorParam->setInteger( pAxis->params, motorAxisCommError, 1 );
00337 asynPrint( pasynUser,
00338 ASYN_TRACE_ERROR,
00339 "Read/write error to PMAC C.S. ref %d, axis %c command %s. Status=%d, Error=%s\n",
00340 pAxis->pDrv->ref, NAME(pAxis), command,
00341 status, pasynUser->errorMessage);
00342 return MOTOR_AXIS_ERROR;
00343 }
00344
00345 motorParam->setInteger( pAxis->params, motorAxisCommError, 0 );
00346 return MOTOR_AXIS_OK;
00347 }
00348
00349 static int processDeferredMoves(drvPmac_t *pDrv)
00350 {
00351 int i;
00352 int status = MOTOR_AXIS_ERROR;
00353 char command[128], response[32];
00354
00355
00356 sprintf(command, "&%dA", pDrv->cs);
00357 status = motorAxisWriteRead(&pDrv->axis[0], command, sizeof(response), response, 0);
00358
00359
00360 sprintf(command, "&%dAB%dR", pDrv->cs, pDrv->axis[0].program);
00361 status = motorAxisWriteRead(&pDrv->axis[0], command, sizeof(response), response, 0);
00362
00363
00364 for (i = 0; i < NAXES; i++) {
00365 pDrv->axis[i].deferred_move = 0;
00366 }
00367
00368 return status;
00369 }
00370
00371 static int motorAxisSetDouble( AXIS_HDL pAxis, motorAxisParam_t function, double value )
00372 {
00373 int status = MOTOR_AXIS_OK;
00374
00375 if (pAxis == NULL) {
00376 return MOTOR_AXIS_ERROR;
00377 } else {
00378 char command[64]="\0";
00379 char response[64];
00380
00381 if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) {
00382 switch (function) {
00383 case motorAxisPosition: {
00384 int position = (int) floor(value + 0.5);
00385
00386
00387
00388 sprintf(command, "&%d"DEMAND"=%f",pAxis->coord_system, pAxis->axis, position*(pAxis->stepSize));
00389 pAxis->print( pAxis->logParam, TRACE_FLOW,
00390 "Set ref %d, axis %c to position %f\n",
00391 pAxis->pDrv->ref, NAME(pAxis), value );
00392 break;
00393 }
00394 case motorAxisEncoderRatio: {
00395 pAxis->print( pAxis->logParam, TRACE_FLOW,
00396 "Cannot set PMAC ref %d, cs %d, axis %c encoder ratio (%f)\n",
00397 pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis), value );
00398 break;
00399 }
00400 case motorAxisLowLimit: {
00401 pAxis->print( pAxis->logParam, TRACE_FLOW,
00402 "Ignoring PMAC ref %d, cs %d, axis %c low limit (%f)\n",
00403 pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis), value );
00404 break;
00405 }
00406 case motorAxisHighLimit: {
00407 pAxis->print( pAxis->logParam, TRACE_FLOW,
00408 "Ignoring PMAC ref %d, cs %d, axis %c high limit (%f)\n",
00409 pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis), value );
00410 break;
00411 }
00412 case motorAxisPGain: {
00413 pAxis->print( pAxis->logParam, TRACE_FLOW,
00414 "Ignoring PMAC ref %d, cs %d, axis %c pgain (%f)\n",
00415 pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis), value );
00416 break;
00417 }
00418 case motorAxisIGain: {
00419 pAxis->print( pAxis->logParam, TRACE_FLOW,
00420 "Ignoring PMAC ref %d, cs %d, axis %c igain (%f)\n",
00421 pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis), value );
00422 break;
00423 }
00424 case motorAxisDGain: {
00425 pAxis->print( pAxis->logParam, TRACE_FLOW,
00426 "Ignoring PMAC ref %d, cs %d, axis %c dgain (%f)\n",
00427 pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis), value );
00428 break;
00429 }
00430 case motorAxisClosedLoop: {
00431 pAxis->print( pAxis->logParam, TRACE_FLOW,
00432 "Cannot set PMAC ref %d, axis %c closed loop (%s)\n",
00433 pAxis->pDrv->ref, NAME(pAxis), (value!=0?"ON":"OFF") );
00434 break;
00435 }
00436 case motorAxisDeferMoves: {
00437 pAxis->print( pAxis->logParam, TRACE_FLOW,
00438 "%sing Deferred Move flag on PMAC ref %d, cs %d\n",
00439 value != 0.0?"Sett":"Clear", pAxis->pDrv->ref, pAxis->coord_system);
00440 if (value == 0.0 && pAxis->pDrv->movesDeferred != 0) {
00441 processDeferredMoves(pAxis->pDrv);
00442 }
00443 pAxis->pDrv->movesDeferred = (int)value;
00444 break;
00445 }
00446 default:
00447 status = MOTOR_AXIS_ERROR;
00448 break;
00449 }
00450
00451 if ( command[0] != 0 && status == MOTOR_AXIS_OK) {
00452 status = motorAxisWriteRead( pAxis, command, sizeof(response), response, 0 );
00453 }
00454
00455 if (status == MOTOR_AXIS_OK ) {
00456 motorParam->setDouble( pAxis->params, function, value );
00457 motorParam->callCallback( pAxis->params );
00458 }
00459 epicsMutexUnlock( pAxis->axisMutex );
00460 }
00461 }
00462 return status;
00463 }
00464
00465 static int motorAxisSetInteger( AXIS_HDL pAxis, motorAxisParam_t function, int value )
00466 {
00467 int status = MOTOR_AXIS_OK;
00468
00469 if (pAxis == NULL) {
00470 return MOTOR_AXIS_ERROR;
00471 } else {
00472 status = motorAxisSetDouble( pAxis, function, (double) value );
00473 }
00474 return status;
00475 }
00476
00477
00478 static int motorAxisMove( AXIS_HDL pAxis, double position, int relative, double min_velocity, double max_velocity, double acceleration )
00479 {
00480 int status = MOTOR_AXIS_ERROR;
00481
00482 if (pAxis != NULL) {
00483 char acc_buff[32]="";
00484 char vel_buff[32]="";
00485 char buff[128];
00486 char command[256];
00487 char commandtemp[128];
00488 char response[256];
00489 double deviceUnits = 0.0;
00490
00491 if (max_velocity != 0) {
00492
00493 sprintf(vel_buff, "I%d89=%f ", CSVAR(pAxis), max_velocity * pAxis->stepSize);
00494 }
00495 if (acceleration != 0) {
00496 if (max_velocity != 0) {
00497
00498 sprintf(acc_buff, "I%d87=%f ", CSVAR(pAxis),
00499 (fabs(max_velocity/acceleration) * 1000.0));
00500 }
00501 }
00502
00503 deviceUnits = position * pAxis->stepSize;
00504 sprintf( command, "&%d%s%s"DEMAND"=%.12f", pAxis->coord_system, vel_buff, acc_buff, pAxis->axis, deviceUnits );
00505
00506 if (pAxis->pDrv->movesDeferred) {
00507 pAxis->deferred_move = 1;
00508 }
00509 else if (pAxis->program != 0) {
00510
00511 sprintf(commandtemp, "&%dA", pAxis->coord_system);
00512 status = motorAxisWriteRead(pAxis, commandtemp, sizeof(response), response, 0);
00513
00514
00515
00516
00517 sprintf(buff, "B%dR", pAxis->program);
00518 strcat(command, buff);
00519 }
00520 if (epicsMutexLock( pAxis->axisMutex ) == epicsMutexLockOK) {
00521 status = motorAxisWriteRead( pAxis, command, sizeof(response), response, 0 );
00522 motorParam->setInteger( pAxis->params, motorAxisDone, 0 );
00523 motorParam->callCallback( pAxis->params );
00524 epicsMutexUnlock( pAxis->axisMutex );
00525 }
00526
00527 epicsEventSignal(pAxis->pDrv->pollEventId);
00528 }
00529 return status;
00530 }
00531
00532 static int motorAxisHome( AXIS_HDL pAxis, double min_velocity, double max_velocity, double acceleration, int forwards )
00533 {
00534 asynPrint(pAxis->pasynUser, ASYN_TRACE_ERROR, "motorAxisHome: not implemented for CS axes\n");
00535 return MOTOR_AXIS_ERROR;
00536 }
00537
00538
00539 static int motorAxisVelocityMove( AXIS_HDL pAxis, double min_velocity, double velocity, double acceleration )
00540 {
00541 asynPrint(pAxis->pasynUser, ASYN_TRACE_ERROR, "motorAxisVelocityMove: not implemented for CS axes\n");
00542 return MOTOR_AXIS_ERROR;
00543 }
00544
00545
00546 static int motorAxisStop( AXIS_HDL pAxis, double acceleration )
00547 {
00548 int status = MOTOR_AXIS_ERROR;
00549
00550 if (pAxis != NULL) {
00551 char acc_buff[32]="\0";
00552 char command[128];
00553 char response[32];
00554
00555 sprintf( command, "&%d%sA "DEMAND"="READBACK, pAxis->coord_system, acc_buff,
00556 pAxis->axis, pAxis->axis);
00557 pAxis->deferred_move = 0;
00558
00559 status = motorAxisWriteRead( pAxis, command, sizeof(response), response, 0 );
00560 }
00561 return status;
00562 }
00563
00564 static int motorAxisforceCallback(AXIS_HDL pAxis)
00565 {
00566 if (pAxis == NULL) return MOTOR_AXIS_ERROR;
00567
00568 pAxis->print(pAxis->logParam, TRACE_FLOW, "motorAxisforceCallback: request ref %d, cs %d, axis %c status update\n",
00569 pAxis->pDrv->ref, pAxis->coord_system, NAME(pAxis));
00570
00571 motorParam->forceCallback(pAxis->params);
00572
00573 return (MOTOR_AXIS_OK);
00574 }
00575
00576 static int drvPmacGetCoordStatus(AXIS_HDL pAxis, asynUser *pasynUser,
00577 epicsUInt32 *status)
00578 {
00579 char command[32];
00580 char response[128]="";
00581 int nvals, cmdStatus;
00582 int retval = 0;
00583
00584
00585 sprintf( command, "&%d??", pAxis->coord_system );
00586 cmdStatus = motorAxisWriteRead( pAxis, command, sizeof(response), response, 1 );
00587 nvals = sscanf( response, "%6x%6x%6x", &status[0], &status[1], &status[2]);
00588
00589 if ( cmdStatus || nvals != 3) {
00590 asynPrint(pasynUser, ASYN_TRACE_ERROR,
00591 "drvPmacAxisGetStatus: not all status values returned. Status: %d\nCommand :%s\nResponse:%s",
00592 cmdStatus, command, response );
00593 } else {
00594 retval = 1;
00595 }
00596 return retval;
00597 }
00598
00599 static void drvPmacGetAxesStatus( PMACDRV_ID pDrv, epicsUInt32 *status)
00600 {
00601 char command[128];
00602 char commandtemp[128];
00603 char pos_response[128];
00604 char *pos_parse = pos_response;
00605 int cmdStatus, done;
00606 double position, oldposition, error;
00607 int i;
00608 asynUser *pasynUser = pDrv->pasynUser;
00609 AXIS_HDL first_axis, last_axis, pAxis;
00610 int direction;
00611 int homeSignal;
00612
00613
00614 for (i = 0; i < NAXES; i++) {
00615 pAxis = &pDrv->axis[i];
00616 epicsMutexLock( pAxis->axisMutex );
00617 }
00618
00619
00620 error = 0;
00621 first_axis = &pDrv->axis[0];
00622 last_axis = &pDrv->axis[NAXES-1];
00623
00624
00625 sprintf( command, "&%d", first_axis->coord_system);
00626 for (i = first_axis->axis; i <= last_axis->axis; i++) {
00627 sprintf( commandtemp, READBACK, i);
00628 strcat(command, commandtemp);
00629 }
00630
00631 cmdStatus = motorAxisWriteRead( first_axis, command, sizeof(pos_response), pos_response, 1 );
00632
00633
00634 drvPmacGetCoordStatus(first_axis, pDrv->pasynUser, status);
00635
00636 for (i = 0; i < NAXES; i++) {
00637 pAxis = &pDrv->axis[i];
00638
00639
00640 homeSignal = 0;
00641 errno = 0;
00642 position = strtod(pos_parse, &pos_parse);
00643 if (position == 0 && errno != 0) {
00644 asynPrint(pasynUser, ASYN_TRACE_ERROR,
00645 "drvPmacAxisGetStatus: not all status values returned. Status: %d\nCommand :%s\nResponse1:%s\n",
00646 cmdStatus, command, pos_response );
00647 }
00648
00649 position = position / pAxis->stepSize;
00650 motorParam->getDouble( pAxis->params, motorAxisPosition, &oldposition );
00651 oldposition = oldposition * pAxis->stepSize;
00652 motorParam->setDouble( pAxis->params, motorAxisPosition, (position+error) );
00653 motorParam->setDouble( pAxis->params, motorAxisEncoderPosn, position );
00654
00655 motorParam->getInteger( pAxis->params, motorAxisDirection, &direction );
00656 motorParam->setInteger( pAxis->params, motorAxisDirection, ((position > oldposition) || (position == oldposition && direction)) );
00657 motorParam->setInteger( pAxis->params, motorAxisHighHardLimit, ((status[2] & CS_STATUS3_LIMIT) != 0) );
00658 motorParam->setInteger( pAxis->params, motorAxisHomeSignal, homeSignal );
00659 motorParam->setInteger( pAxis->params, motorAxisMoving, ((status[1] & CS_STATUS2_IN_POSITION) == 0) );
00660 if (pAxis->deferred_move) {
00661 done = 0;
00662 } else {
00663 done = ((status[0] & CS_STATUS1_RUNNING_PROG) == 0)&&((status[1] & CS_STATUS2_IN_POSITION) != 0);
00664 }
00665 motorParam->setInteger( pAxis->params, motorAxisDone, done);
00666 motorParam->setInteger( pAxis->params, motorAxisLowHardLimit, ((status[2] & CS_STATUS3_LIMIT)!=0) );
00667
00668 motorParam->setInteger( pAxis->params, motorAxisFollowingError,((status[1] & CS_STATUS2_FOLLOW_ERR)!=0) );
00669 motorParam->setInteger( pAxis->params, motorAxisProblem, ((status[1] & CS_STATUS2_AMP_FAULT) != 0) );
00670 motorParam->setInteger( pAxis->params, motorAxisProblem, ((status[1] & CS_STATUS2_RUNTIME_ERR) != 0) );
00671 motorParam->callCallback( pAxis->params );
00672 }
00673
00674
00675 for (i = 0; i < NAXES; i++) {
00676 pAxis = &pDrv->axis[i];
00677 epicsMutexUnlock( pAxis->axisMutex );
00678 }
00679 }
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694 static void drvPmacTask( PMACDRV_ID pDrv )
00695 {
00696 int i = 0;
00697 int done = 0;
00698 int eventStatus = 0;
00699 float timeout = 0.0;
00700 epicsUInt32 status[3];
00701
00702 while ( 1 )
00703 {
00704
00705 drvPmacGetAxesStatus( pDrv, status);
00706
00707 if (epicsMutexLock(pDrv->controllerMutexId) == epicsMutexLockOK) {
00708 timeout = pDrv->idlePollPeriod;
00709 }
00710 else {
00711 drvPrint(drvPrintParam, TRACE_ERROR, "drvPmacTask: Failed to get controllerMutexId lock.\n");
00712 }
00713
00714 for ( i = 0; i < NAXES; i++ )
00715 {
00716 AXIS_HDL pAxis = &(pDrv->axis[i]);
00717
00718 epicsMutexLock( pAxis->axisMutex );
00719 motorParam->getInteger( pAxis->params, motorAxisDone, &done );
00720 epicsMutexUnlock( pAxis->axisMutex );
00721 if ((eventStatus == epicsEventWaitOK) || (done == 0))
00722 {
00723
00724 timeout = pDrv->movingPollPeriod;
00725 break;
00726 }
00727 }
00728 epicsMutexUnlock(pDrv->controllerMutexId);
00729 eventStatus = epicsEventWaitWithTimeout(pDrv->pollEventId, timeout);
00730 }
00731 }
00732
00733
00738 int asynMatch(asynUser *pasynUser, char *checkport, int checkaddr)
00739 {
00740 const char *port;
00741 int addr;
00742
00743 pasynManager->getPortName(pasynUser, &port);
00744 pasynManager->getAddr(pasynUser, &addr);
00745 if (strncmp(port, checkport, strlen(port)) != 0 || addr != checkaddr) {
00746 return 0;
00747 }
00748 return 1;
00749 }
00750
00760 int pmacAsynCoordCreate( char *port, int addr, int cs, int ref, int program )
00761 {
00762 int i;
00763 int status = MOTOR_AXIS_OK;
00764 PMACDRV_ID pDrv;
00765 PMACDRV_ID * ppLast = &(pFirstDrv);
00766
00767 if ((cs < 1) || (cs > 16)) {
00768 drvPrint( drvPrintParam, TRACE_ERROR, "Invalid co-ordinate system number: %d\n", cs);
00769 return MOTOR_AXIS_ERROR;
00770 }
00771
00772 for ( pDrv = pFirstDrv;
00773 pDrv != NULL && (pDrv->ref != ref) && (!asynMatch(pDrv->pasynUser, port, addr) || pDrv->cs != cs);
00774 pDrv = pDrv->pNext )
00775 {
00776 ppLast = &(pDrv->pNext);
00777 }
00778
00779 if ( pDrv == NULL) {
00780 drvPrint( drvPrintParam, TRACE_FLOW,
00781 "Creating PMAC co-ordinate system driver on port %s, address %d, cs: %d (ref = %d)\n",
00782 port, addr, cs, ref );
00783
00784 pDrv = (PMACDRV_ID) calloc( 1, sizeof(drvPmac_t) );
00785
00786 if (pDrv != NULL) {
00787 pDrv->axis = (AXIS_HDL) calloc( NAXES, sizeof( motorAxis ) );
00788
00789 if (pDrv->axis != NULL ) {
00790 pDrv->cs = cs;
00791 pDrv->ref = ref;
00792
00793 pDrv->movingPollPeriod = (double)defaultMovingPollPeriod / 1000.0;
00794 pDrv->idlePollPeriod = (double)defaultIdlePollPeriod / 1000.0;
00795
00796 pDrv->pollEventId = epicsEventMustCreate(epicsEventEmpty);
00797
00798 if ((pDrv->controllerMutexId = epicsMutexCreate()) == NULL) {
00799 drvPrint( drvPrintParam, TRACE_ERROR, "pmacAsynMotorCreate: Could not create controllerMutexId.\n");
00800 }
00801
00802 status = motorAxisAsynConnect( port, addr, &(pDrv->pasynUser), "\006", "\r" );
00803
00804 for (i=0; i<NAXES && status == MOTOR_AXIS_OK; i++ ) {
00805 if ((pDrv->axis[i].params = motorParam->create( 0, MOTOR_AXIS_NUM_PARAMS )) != NULL &&
00806 (pDrv->axis[i].axisMutex = epicsMutexCreate( )) != NULL ) {
00807 pDrv->axis[i].pDrv = pDrv;
00808 pDrv->axis[i].coord_system = cs;
00809 pDrv->axis[i].program = program;
00810 pDrv->axis[i].axis = i + 1;
00811 pDrv->axis[i].logParam = pDrv->pasynUser;
00812 pDrv->axis[i].pasynUser = pDrv->pasynUser;
00813 pDrv->axis[i].stepSize = 0.0001;
00814
00815 asynPrint( pDrv->pasynUser, ASYN_TRACE_FLOW, "Created motor for ref %d, C.S. %d, signal %d OK\n", ref, cs, i );
00816 } else {
00817 asynPrint(pDrv->pasynUser, ASYN_TRACE_ERROR,
00818 "drvPmacCreate: unable to set create axis %d on %s: insufficient memory\n",
00819 i, port );
00820 status = MOTOR_AXIS_ERROR;
00821 }
00822 }
00823
00824 if ( status == MOTOR_AXIS_ERROR ) {
00825 for (i=0; i<NAXES; i++ ) {
00826 if (pDrv->axis[i].params != NULL) motorParam->destroy( pDrv->axis[i].params );
00827 if (pDrv->axis[i].axisMutex != NULL) epicsMutexDestroy( pDrv->axis[i].axisMutex );
00828 }
00829 free ( pDrv );
00830 }
00831 } else {
00832 free ( pDrv );
00833 status = MOTOR_AXIS_ERROR;
00834 }
00835 } else {
00836 drvPrint( drvPrintParam, TRACE_ERROR,
00837 "drvPmacCreate: unable to create driver for port %s: insufficient memory\n",
00838 port );
00839 status = MOTOR_AXIS_ERROR;
00840 }
00841
00842 if ( status == MOTOR_AXIS_OK ) {
00843 *ppLast = pDrv;
00844 }
00845 } else {
00846 drvPrint( drvPrintParam, TRACE_ERROR, "C.S. reference %d already exists\n", ref );
00847 status = MOTOR_AXIS_ERROR;
00848 }
00849
00850 if (status == MOTOR_AXIS_OK) {
00851 epicsUInt32 status[3];
00852
00853
00854 drvPmacGetAxesStatus( pDrv, status );
00855
00856 pDrv->motorThread = epicsThreadCreate( "drvPmacCSThread",
00857 epicsThreadPriorityLow,
00858 epicsThreadGetStackSize(epicsThreadStackMedium),
00859 (EPICSTHREADFUNC) drvPmacTask, (void *) pDrv );
00860 if (pDrv->motorThread == NULL) {
00861 asynPrint(pDrv->pasynUser, ASYN_TRACE_ERROR, "Cannot start motor polling thread\n" );
00862 return MOTOR_AXIS_ERROR;
00863 }
00864 }
00865
00866 return status;
00867 }
00868
00869
00870
00876 int pmacSetDefaultCoordSteps(double defaultSteps)
00877 {
00878 int status = -1;
00879 PMACDRV_ID pDrv = NULL;
00880 AXIS_HDL pAxis = NULL;
00881 int axis = 0;
00882
00883 if (defaultSteps <= 0.0) {
00884 drvPrint(drvPrintParam, TRACE_ERROR, "ERROR: Scaling factor has to be positive.\n" );
00885 return status;
00886 }
00887
00888 if (pFirstDrv != NULL) {
00889 for (pDrv = pFirstDrv; pDrv != NULL; pDrv = pDrv->pNext) {
00890 if (epicsMutexLock( pDrv->controllerMutexId ) == epicsMutexLockOK) {
00891 for (axis = 0; axis < NAXES; axis++) {
00892 pAxis = &pDrv->axis[axis];
00893 pAxis->stepSize = 1.0 / defaultSteps;
00894 }
00895 epicsMutexUnlock( pDrv->controllerMutexId );
00896 drvPrint(drvPrintParam, TRACE_FLOW, " Set scaling factor to %f for CS ref %d.\n", defaultSteps, pDrv->ref);
00897 status = 0;
00898 } else {
00899 drvPrint(drvPrintParam, TRACE_ERROR, "ERROR: Could not lock axis mutex.\n");
00900 return status;
00901 }
00902 }
00903 }
00904
00905 return status;
00906 }
00907
00918 int pmacSetCoordStepsPerUnit(int ref, int axis, double stepsPerUnit)
00919 {
00920 int status = -1;
00921 PMACDRV_ID pDrv = NULL;
00922
00923 if (stepsPerUnit <= 0.0) {
00924 drvPrint(drvPrintParam, TRACE_ERROR, "ERROR: Scaling factor has to be positive.\n" );
00925 return status;
00926 }
00927
00928 if ((axis < 0) | (axis >= NAXES)) {
00929 drvPrint(drvPrintParam, TRACE_ERROR, "ERROR: axis number has to be between 0 and %d.\n", NAXES-1);
00930 return status;
00931 }
00932
00933 if (pFirstDrv != NULL) {
00934 for (pDrv = pFirstDrv; pDrv != NULL; pDrv = pDrv->pNext) {
00935 if (ref == pDrv->ref) {
00936 if (epicsMutexLock( pDrv->controllerMutexId ) == epicsMutexLockOK) {
00937 pDrv->axis[axis].stepSize = 1.0 / stepsPerUnit;
00938 epicsMutexUnlock( pDrv->controllerMutexId );
00939 status = 0;
00940 drvPrint(drvPrintParam, TRACE_FLOW, " Setting axis %d scaling factor to %f.\n", axis, stepsPerUnit);
00941 } else {
00942 drvPrint(drvPrintParam, TRACE_ERROR, "ERROR: Could not lock axis mutex.\n");
00943 return status;
00944 }
00945 }
00946 }
00947 }
00948
00949 return status;
00950 }
00951
00952
00953 static int drvPmacLogMsg( void * param, const motorAxisLogMask_t mask, const char *pFormat, ...)
00954 {
00955 va_list pvar;
00956 int nchar=0;
00957 asynUser * pasynUser = (asynUser *) param;
00958 int reason = (int) mask;
00959
00960 if ( pasynUser == NULL ) {
00961 va_start(pvar, pFormat);
00962 vprintf( pFormat, pvar );
00963 va_end (pvar);
00964 } else if ( pasynTrace->getTraceMask(pasynUser) & reason ) {
00965 va_start(pvar, pFormat);
00966 nchar = pasynTrace->vprint( pasynUser, reason, pFormat, pvar );
00967 va_end (pvar);
00968 }
00969
00970 return(nchar);
00971 }
00972
00973
00981 int pmacSetCoordMovingPollPeriod(int ref, int movingPollPeriod)
00982 {
00983 int status = 1;
00984 PMACDRV_ID pDrv = NULL;
00985
00986 if (pFirstDrv != NULL) {
00987 for (pDrv = pFirstDrv; pDrv != NULL; pDrv = pDrv->pNext) {
00988 drvPrint(drvPrintParam, TRACE_FLOW, "** Setting moving poll period of %dms on ref %d\n", movingPollPeriod, pDrv->ref);
00989 if (ref == pDrv->ref) {
00990 if (epicsMutexLock( pDrv->controllerMutexId ) == epicsMutexLockOK) {
00991 pDrv->movingPollPeriod = (double)movingPollPeriod / 1000.0;
00992 epicsMutexUnlock( pDrv->controllerMutexId );
00993 epicsEventSignal( pDrv->pollEventId );
00994 status = 0;
00995 } else {
00996 drvPrint(drvPrintParam, TRACE_ERROR, "pmacSetCoordMovingPollPeriod: could not access pDrv to set polling period.\n" );
00997 }
00998 }
00999 }
01000 }
01001
01002 return status;
01003 }
01004
01005
01013 int pmacSetCoordIdlePollPeriod(int ref, int idlePollPeriod)
01014 {
01015 int status = 1;
01016 PMACDRV_ID pDrv = NULL;
01017
01018 if (pFirstDrv != NULL) {
01019 for (pDrv = pFirstDrv; pDrv!=NULL; pDrv = pDrv->pNext) {
01020 drvPrint(drvPrintParam, TRACE_FLOW, "** Setting idle poll period of %dms on ref %d\n", idlePollPeriod, pDrv->ref);
01021 if (ref == pDrv->ref) {
01022 if (epicsMutexLock( pDrv->controllerMutexId ) == epicsMutexLockOK) {
01023 pDrv->idlePollPeriod = (double)idlePollPeriod / 1000.0;
01024 epicsMutexUnlock( pDrv->controllerMutexId );
01025 epicsEventSignal( pDrv->pollEventId );
01026 status = 0;
01027 } else {
01028 drvPrint(drvPrintParam, TRACE_ERROR, "pmacSetMovingPollPeriod: could not access pDrv to set polling period.\n" );
01029 }
01030 }
01031 }
01032 }
01033
01034 return status;
01035 }