Index: configs/common/emc.nml =================================================================== RCS file: /cvs/emc2/configs/common/emc.nml,v retrieving revision 1.6 diff -u -8 -p -r1.6 emc.nml --- configs/common/emc.nml 6 Oct 2006 02:14:47 -0000 1.6 +++ configs/common/emc.nml 9 Oct 2007 23:35:40 -0000 @@ -2,44 +2,48 @@ # Note: emcsvr is the master for all NML channels, and therefore is the # first to start. # Buffers # Name Type Host size neut? (old) buffer# MP --- # Top-level buffers to EMC B emcCommand SHMEM localhost 8192 0 0 1 16 1001 TCP=5005 xdr +B druidCommand SHMEM localhost 8192 0 0 6 16 1006 TCP=5005 xdr B emcStatus SHMEM localhost 10240 0 0 2 16 1002 TCP=5005 xdr B emcError SHMEM localhost 8192 0 0 3 16 1003 TCP=5005 xdr queue # These are for the IO controller, EMCIO B toolCmd SHMEM localhost 1024 0 0 4 16 1004 TCP=5005 xdr B toolSts SHMEM localhost 4096 0 0 5 16 1005 TCP=5005 xdr # Processes # Name Buffer Type Host Ops server? timeout master? cnum P emc emcCommand LOCAL localhost RW 0 1.0 0 0 +P emc druidCommand LOCAL localhost RW 0 1.0 0 0 P emc emcStatus LOCAL localhost W 0 1.0 0 0 P emc emcError LOCAL localhost W 0 1.0 0 0 P emc toolCmd LOCAL localhost W 0 1.0 0 0 P emc toolSts LOCAL localhost R 0 1.0 0 0 P emcsvr emcCommand LOCAL localhost W 1 1.0 1 2 +P emcsvr druidCommand LOCAL localhost W 1 1.0 1 2 P emcsvr emcStatus LOCAL localhost R 1 1.0 1 2 P emcsvr emcError LOCAL localhost R 1 1.0 1 2 P emcsvr toolCmd LOCAL localhost W 1 1.0 1 2 P emcsvr toolSts LOCAL localhost R 1 1.0 1 2 P emcsvr default LOCAL localhost RW 1 1.0 1 2 P tool emcError LOCAL localhost W 0 1.0 0 3 P tool toolCmd LOCAL localhost RW 0 1.0 0 3 P tool toolSts LOCAL localhost W 0 1.0 0 3 P xemc emcCommand LOCAL localhost W 0 10.0 0 10 +P xemc druidCommand LOCAL localhost W 0 10.0 0 10 P xemc emcStatus LOCAL localhost R 0 10.0 0 10 P xemc emcError LOCAL localhost R 0 10.0 0 10 P xemc toolCmd LOCAL localhost W 0 10.0 0 10 P xemc toolSts LOCAL localhost R 0 10.0 0 10 P keystick emcCommand LOCAL localhost W 0 10.0 0 10 P keystick emcStatus LOCAL localhost R 0 10.0 0 10 P keystick emcError LOCAL localhost R 0 10.0 0 10 Index: src/emc/nml_intf/emc.hh =================================================================== RCS file: /cvs/emc2/src/emc/nml_intf/emc.hh,v retrieving revision 1.45 diff -u -8 -p -r1.45 emc.hh --- src/emc/nml_intf/emc.hh 5 Nov 2006 19:34:44 -0000 1.45 +++ src/emc/nml_intf/emc.hh 9 Oct 2007 23:35:42 -0000 @@ -2823,16 +2823,17 @@ class EMC_STAT:public EMC_STAT_MSG { // the top-level EMC_TASK status class EMC_TASK_STAT task; // subordinate status classes EMC_MOTION_STAT motion; EMC_IO_STAT io; int debug; // copy of EMC_DEBUG global + int originator_msqid; // MG the msqid of the originator of the command }; /* Declarations of EMC status class implementations, for major subsystems. These are defined in the appropriate main() files, and referenced by code in other files to get EMC status. */ Index: src/emc/nml_intf/emcops.cc =================================================================== RCS file: /cvs/emc2/src/emc/nml_intf/emcops.cc,v retrieving revision 1.9 diff -u -8 -p -r1.9 emcops.cc --- src/emc/nml_intf/emcops.cc 28 Sep 2006 20:44:20 -0000 1.9 +++ src/emc/nml_intf/emcops.cc 9 Oct 2007 23:35:42 -0000 @@ -252,9 +252,10 @@ EMC_AUX_STAT EMC_AUX_STAT::operator =(EM ain[t] = s.ain[t]; } return s; } EMC_STAT::EMC_STAT():EMC_STAT_MSG(EMC_STAT_TYPE, sizeof(EMC_STAT)) { + originator_msqid = 0; // MG } Index: src/emc/task/emcsvr.cc =================================================================== RCS file: /cvs/emc2/src/emc/task/emcsvr.cc,v retrieving revision 1.11 diff -u -8 -p -r1.11 emcsvr.cc --- src/emc/task/emcsvr.cc 27 Sep 2006 20:53:24 -0000 1.11 +++ src/emc/task/emcsvr.cc 9 Oct 2007 23:35:42 -0000 @@ -60,16 +60,17 @@ static int iniLoad(const char *filename) // close it inifile.close(); return 0; } static RCS_CMD_CHANNEL *emcCommandChannel = NULL; +static RCS_CMD_CHANNEL *druidCommandChannel = NULL; // MG static RCS_STAT_CHANNEL *emcStatusChannel = NULL; static NML *emcErrorChannel = NULL; static RCS_CMD_CHANNEL *toolCommandChannel = NULL; static RCS_STAT_CHANNEL *toolStatusChannel = NULL; void nice_exit(int ignore) { _exit(0); } @@ -90,25 +91,32 @@ int main(int argc, char *argv[]) rcs_print("after iniLoad()\n"); start_time = etime(); while (fabs(etime() - start_time) < 10.0 && (emcCommandChannel == NULL || emcStatusChannel == NULL + || druidCommandChannel == NULL || toolCommandChannel == NULL || toolStatusChannel == NULL || emcErrorChannel == NULL) ) { if (NULL == emcCommandChannel) { rcs_print("emcCommandChannel==NULL, attempt to create\n"); emcCommandChannel = new RCS_CMD_CHANNEL(emcFormat, "emcCommand", "emcsvr", EMC_NMLFILE); } + if (NULL == druidCommandChannel) { + rcs_print("druidCommandChannel==NULL, attempt to create\n"); + druidCommandChannel = + new RCS_CMD_CHANNEL(emcFormat, "druidCommand", "emcsvr", + EMC_NMLFILE); + } if (NULL == emcStatusChannel) { rcs_print("emcStatusChannel==NULL, attempt to create\n"); emcStatusChannel = new RCS_STAT_CHANNEL(emcFormat, "emcStatus", "emcsvr", EMC_NMLFILE); } if (NULL == emcErrorChannel) { emcErrorChannel = @@ -125,16 +133,20 @@ int main(int argc, char *argv[]) EMC_NMLFILE); } if (!emcCommandChannel->valid()) { delete emcCommandChannel; emcCommandChannel = NULL; } + if (!druidCommandChannel->valid()) { + delete druidCommandChannel; + druidCommandChannel = NULL; + } if (!emcStatusChannel->valid()) { delete emcStatusChannel; emcStatusChannel = NULL; } if (!emcErrorChannel->valid()) { delete emcErrorChannel; emcErrorChannel = NULL; } @@ -152,16 +164,21 @@ int main(int argc, char *argv[]) set_rcs_print_destination(RCS_PRINT_TO_STDERR); if (NULL == emcCommandChannel) { emcCommandChannel = new RCS_CMD_CHANNEL(emcFormat, "emcCommand", "emcsvr", EMC_NMLFILE); } + if (NULL == druidCommandChannel) { + druidCommandChannel = + new RCS_CMD_CHANNEL(emcFormat, "druidCommand", "emcsvr", + EMC_NMLFILE); + } if (NULL == emcStatusChannel) { emcStatusChannel = new RCS_STAT_CHANNEL(emcFormat, "emcStatus", "emcsvr", EMC_NMLFILE); } if (NULL == emcErrorChannel) { emcErrorChannel = new NML(nmlErrorFormat, "emcError", "emcsvr", EMC_NMLFILE); Index: src/emc/task/emctaskmain.cc =================================================================== RCS file: /cvs/emc2/src/emc/task/emctaskmain.cc,v retrieving revision 1.76.4.2 diff -u -8 -p -r1.76.4.2 emctaskmain.cc --- src/emc/task/emctaskmain.cc 29 Aug 2007 14:11:12 -0000 1.76.4.2 +++ src/emc/task/emctaskmain.cc 9 Oct 2007 23:35:43 -0000 @@ -53,17 +53,37 @@ extern "C" { #include // exit() #include // signal(), SIGINT #include // DBL_MAX #include // pid_t #include // fork() #include // waitpid(), WNOHANG, WIFEXITED #include // isspace() #include +// For semaphores +#include +#include +#include +#include +#include + +#if defined(__GNU_LIBRARY__) && !defined(_SEM_SEMUN_UNDEFINED) +/* union semun is defined by including */ +#else +/* according to X/OPEN we have to define it ourselves */ +union semun { + int val; /* value for SETVAL */ + struct semid_ds *buf; /* buffer for IPC_STAT, IPC_SET */ + unsigned short *array; /* array for GETALL, SETALL */ + /* Linux specific part: */ + struct seminfo *__buf; /* buffer for IPC_INFO */ +}; +#endif } + #include "rcs.hh" // NML classes, nmlErrorFormat() #include "emc.hh" // EMC NML #include "canon.hh" // CANON_TOOL_TABLE stuff #include "inifile.hh" // INIFILE #include "interpl.hh" // NML_INTERP_LIST, interp_list #include "emcglb.h" // EMC_INIFILE,NMLFILE, EMC_TASK_CYCLE_TIME #include "interp_return.hh" // public interpreter return values #ifdef USE_NLS @@ -72,28 +92,42 @@ extern "C" { #define _(string) (string) #endif // command line args-- global so that other modules can access int Argc; char **Argv; // NML channels static RCS_CMD_CHANNEL *emcCommandBuffer = 0; +static RCS_CMD_CHANNEL *druidCommandBuffer = 0; static RCS_STAT_CHANNEL *emcStatusBuffer = 0; static NML *emcErrorBuffer = 0; // NML command channel data pointer static RCS_CMD_MSG *emcCommand = 0; +static RCS_CMD_MSG *druidCommand = 0; +static RCS_CMD_MSG *taskCommand = 0; // global EMC status EMC_STAT *emcStatus = 0; // timer stuff static RCS_TIMER *timer = 0; +// Druid semaphores +static int cmdSemId = -1; + +// The message queue status buffer +// An extension of the struct msgbuf +struct msgstatbuf { + long mtype; + int serial_number; + int status; +}; + // flag signifying that ini file [TASK] CYCLE_TIME is <= 0.0, so // we should not delay at all between cycles. This means also that // the EMC_TASK_CYCLE_TIME global will be set to the measured cycle // time each cycle, in case other code references this. static int emcTaskNoDelay = 0; // flag signifying that on the next loop, there should be no delay. // this is set when transferring trajectory data from userspace to kernel // space, annd reset otherwise. @@ -485,19 +519,19 @@ static int emcTaskPlan(void) { NMLTYPE type; static char errstring[200]; int retval = 0; int readRetval; int execRetval; // check for new command - if (emcCommand->serial_number != emcStatus->echo_serial_number) { + if (taskCommand->serial_number != emcStatus->echo_serial_number) { // flag it here locally as a new command - type = emcCommand->type; + type = taskCommand->type; } else { // no new command-- reset local flag type = 0; } // handle any new command switch (emcStatus->task.state) { case EMC_TASK_STATE_OFF: @@ -542,31 +576,31 @@ static int emcTaskPlan(void) case EMC_TASK_PLAN_OPEN_TYPE: case EMC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE: case EMC_TASK_PLAN_SET_BLOCK_DELETE_TYPE: case EMC_TASK_ABORT_TYPE: case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: case EMC_TRAJ_PROBE_TYPE: case EMC_TRAJ_SET_TELEOP_ENABLE_TYPE: case EMC_SET_DEBUG_TYPE: - retval = emcTaskIssueCommand(emcCommand); + retval = emcTaskIssueCommand(taskCommand); break; // one case where we need to be in manual mode case EMC_AXIS_OVERRIDE_LIMITS_TYPE: retval = 0; if (emcStatus->task.mode == EMC_TASK_MODE_MANUAL) { - retval = emcTaskIssueCommand(emcCommand); + retval = emcTaskIssueCommand(taskCommand); } break; case EMC_TOOL_LOAD_TOOL_TABLE_TYPE: case EMC_TOOL_SET_OFFSET_TYPE: // send to IO - emcTaskQueueCommand(emcCommand); + emcTaskQueueCommand(taskCommand); // signify no more reading emcTaskPlanSetWait(); if (EMC_DEBUG & EMC_DEBUG_INTERP) { rcs_print("emcTaskPlanSetWait() called\n"); } // then resynch interpreter emcTaskQueueCommand(&taskPlanSynchCmd); break; @@ -650,33 +684,33 @@ static int emcTaskPlan(void) case EMC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE: case EMC_TASK_PLAN_SET_BLOCK_DELETE_TYPE: case EMC_TASK_PLAN_OPTIONAL_STOP_TYPE: case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: case EMC_TRAJ_PROBE_TYPE: case EMC_TRAJ_SET_TELEOP_ENABLE_TYPE: case EMC_TRAJ_SET_TELEOP_VECTOR_TYPE: case EMC_SET_DEBUG_TYPE: - retval = emcTaskIssueCommand(emcCommand); + retval = emcTaskIssueCommand(taskCommand); break; // queued commands case EMC_TASK_PLAN_EXECUTE_TYPE: // resynch the interpreter, since we may have moved // externally emcTaskIssueCommand(&taskPlanSynchCmd); // and now call for interpreter execute - retval = emcTaskIssueCommand(emcCommand); + retval = emcTaskIssueCommand(taskCommand); break; case EMC_TOOL_LOAD_TOOL_TABLE_TYPE: case EMC_TOOL_SET_OFFSET_TYPE: // send to IO - emcTaskQueueCommand(emcCommand); + emcTaskQueueCommand(taskCommand); // signify no more reading emcTaskPlanSetWait(); if (EMC_DEBUG & EMC_DEBUG_INTERP) { rcs_print("emcTaskPlanSetWait() called\n"); } // then resynch interpreter emcTaskQueueCommand(&taskPlanSynchCmd); break; @@ -741,17 +775,17 @@ static int emcTaskPlan(void) case EMC_TASK_PLAN_PAUSE_TYPE: case EMC_TASK_PLAN_RESUME_TYPE: case EMC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE: case EMC_TASK_PLAN_SET_BLOCK_DELETE_TYPE: case EMC_TASK_PLAN_OPTIONAL_STOP_TYPE: case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: case EMC_TRAJ_PROBE_TYPE: case EMC_SET_DEBUG_TYPE: - retval = emcTaskIssueCommand(emcCommand); + retval = emcTaskIssueCommand(taskCommand); break; case EMC_TASK_PLAN_STEP_TYPE: // handles case where first action is to step the program taskPlanRunCmd.line = 1; // run from start /*! \todo FIXME-- can have GUI set this; send a run instead of a step */ retval = emcTaskIssueCommand(&taskPlanRunCmd); @@ -760,17 +794,17 @@ static int emcTaskPlan(void) // flag-- reset it here stepping = 1; // set step flag steppingWait = 0; // don't wait for first one break; case EMC_TOOL_LOAD_TOOL_TABLE_TYPE: case EMC_TOOL_SET_OFFSET_TYPE: // send to IO - emcTaskQueueCommand(emcCommand); + emcTaskQueueCommand(taskCommand); // signify no more reading emcTaskPlanSetWait(); if (EMC_DEBUG & EMC_DEBUG_INTERP) { rcs_print("emcTaskPlanSetWait() called\n"); } // then resynch interpreter emcTaskQueueCommand(&taskPlanSynchCmd); break; @@ -821,17 +855,17 @@ static int emcTaskPlan(void) case EMC_TASK_PLAN_SET_BLOCK_DELETE_TYPE: case EMC_TASK_PLAN_OPTIONAL_STOP_TYPE: case EMC_TASK_SET_MODE_TYPE: case EMC_TASK_SET_STATE_TYPE: case EMC_TASK_ABORT_TYPE: case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: case EMC_TRAJ_PROBE_TYPE: case EMC_SET_DEBUG_TYPE: - retval = emcTaskIssueCommand(emcCommand); + retval = emcTaskIssueCommand(taskCommand); return retval; break; case EMC_TASK_PLAN_STEP_TYPE: stepping = 1; // set stepping mode in case it's not steppingWait = 0; // clear the wait break; @@ -1021,17 +1055,17 @@ interpret_again: case EMC_TASK_PLAN_PAUSE_TYPE: case EMC_TASK_PLAN_RESUME_TYPE: case EMC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE: case EMC_TASK_PLAN_SET_BLOCK_DELETE_TYPE: case EMC_TASK_PLAN_OPTIONAL_STOP_TYPE: case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: case EMC_TRAJ_PROBE_TYPE: case EMC_SET_DEBUG_TYPE: - retval = emcTaskIssueCommand(emcCommand); + retval = emcTaskIssueCommand(taskCommand); break; case EMC_TASK_PLAN_STEP_TYPE: if (emcStatus->motion.traj.paused && emcStatus->motion.traj.queue > 0) { // there are pending motions paused; step them emcTrajStep(); } @@ -1086,17 +1120,17 @@ interpret_again: case EMC_TASK_PLAN_SET_BLOCK_DELETE_TYPE: case EMC_TASK_PLAN_OPTIONAL_STOP_TYPE: case EMC_TASK_SET_MODE_TYPE: case EMC_TASK_SET_STATE_TYPE: case EMC_TASK_ABORT_TYPE: case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: case EMC_TRAJ_PROBE_TYPE: case EMC_SET_DEBUG_TYPE: - retval = emcTaskIssueCommand(emcCommand); + retval = emcTaskIssueCommand(taskCommand); break; case EMC_TASK_PLAN_STEP_TYPE: stepping = 1; // set stepping mode in case it's not steppingWait = 0; // clear the wait break; // otherwise we can't handle it @@ -1192,23 +1226,23 @@ interpret_again: case EMC_TASK_PLAN_SET_OPTIONAL_STOP_TYPE: case EMC_TASK_PLAN_SET_BLOCK_DELETE_TYPE: case EMC_TASK_PLAN_RESUME_TYPE: case EMC_TASK_PLAN_OPTIONAL_STOP_TYPE: case EMC_TASK_ABORT_TYPE: case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: case EMC_TRAJ_PROBE_TYPE: case EMC_SET_DEBUG_TYPE: - retval = emcTaskIssueCommand(emcCommand); + retval = emcTaskIssueCommand(taskCommand); break; case EMC_TOOL_LOAD_TOOL_TABLE_TYPE: case EMC_TOOL_SET_OFFSET_TYPE: // send to IO - emcTaskQueueCommand(emcCommand); + emcTaskQueueCommand(taskCommand); // signify no more reading emcTaskPlanSetWait(); if (EMC_DEBUG & EMC_DEBUG_INTERP) { rcs_print("emcTaskPlanSetWait() called\n"); } // then resynch interpreter emcTaskQueueCommand(&taskPlanSynchCmd); break; @@ -2394,16 +2428,51 @@ static int emctask_startup() set_rcs_print_destination(RCS_PRINT_TO_STDOUT); // restore diag // messages if (!good) { rcs_print_error("can't get emcCommand buffer\n"); return -1; } // get our command data structure emcCommand = emcCommandBuffer->get_address(); + taskCommand = emcCommand; // Ensure taskCommand is initialized at beginning + + // get the NML druidcommand buffer + if (!(EMC_DEBUG & EMC_DEBUG_NML)) { + set_rcs_print_destination(RCS_PRINT_TO_NULL); // inhibit diag + // messages + } + end = RETRY_TIME; + good = 0; + do { + if (NULL != druidCommandBuffer) { + delete druidCommandBuffer; + } + druidCommandBuffer = + new RCS_CMD_CHANNEL(emcFormat, "druidCommand", "emc", + EMC_NMLFILE); + if (druidCommandBuffer->valid()) { + good = 1; + break; + } + esleep(RETRY_INTERVAL); + end -= RETRY_INTERVAL; + if (done) { + emctask_shutdown(); + exit(1); + } + } while (end > 0.0); + set_rcs_print_destination(RCS_PRINT_TO_STDOUT); // restore diag + // messages + if (!good) { + rcs_print_error("can't get druidCommand buffer\n"); + return -1; + } + // get our command data structure + druidCommand = druidCommandBuffer->get_address(); // get the NML status buffer if (!(EMC_DEBUG & EMC_DEBUG_NML)) { set_rcs_print_destination(RCS_PRINT_TO_NULL); // inhibit diag // messages } end = RETRY_TIME; good = 0; @@ -2600,30 +2669,80 @@ static int emctask_shutdown(void) } if (0 != emcCommandBuffer) { delete emcCommandBuffer; emcCommandBuffer = 0; emcCommand = 0; } + if (0 != druidCommandBuffer) { + delete druidCommandBuffer; + druidCommandBuffer = 0; + druidCommand = 0; + } + if (0 != emcStatus) { delete emcStatus; emcStatus = 0; } return 0; } +// Gets or create semaphore +// returns 0 if OK, -1 if error +static int sem_init(const char *inifile) +{ + key_t key; + union semun su; + + // Get command sem key + if((key = ftok(inifile, 0)) == -1){ + printf("Can't ftok(%s): errno=%d (%s)\n", inifile, errno, strerror(errno)); + goto error; + } + // Get command sem + if((cmdSemId = semget(key, 1, 0)) == -1){ + // Sem do not exist, so create, allow access to all + if((cmdSemId = semget(key, 1, IPC_CREAT | IPC_EXCL | 0666)) < 0){ + printf("Can't create command semaphore(%d), errno=%d (%s)\n", key, errno, strerror(errno)); + goto error; + } + if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE) printf("Command semaphore created\n"); + } + // When the main interface starts, always set sem to 1 + su.val = 1; + if(semctl(cmdSemId, 0, SETVAL, su) == -1){ + printf("Can't semctl command semaphore(%d), errno=%d (%s)\n", key, errno, strerror(errno)); + goto error; + } + if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE){ + int val; + su.val = -1; + if((val = semctl(cmdSemId, 0, GETVAL)) == -1){ + printf("Can't semctl semaphore(%d), errno=%d (%s)\n", key, errno, strerror(errno)); + goto error; + } + printf("Found semaphore(%s) val:%d\n", inifile, val); + } + return(0); +error: + printf("Can't init semaphores, errno=%d (%s)\n", errno, strerror(errno)); + return(-1); +} + static int iniLoad(const char *filename) { Inifile inifile; const char *inistring; char version[LINELEN], machine[LINELEN]; double saveDouble; + // Open semaphore + if(sem_init(filename))return(-1); // open it if (inifile.open(filename) == false) { return -1; } if (NULL != (inistring = inifile.find("DEBUG", "EMC"))) { // copy to global if (1 != sscanf(inistring, "%i", &EMC_DEBUG)) { @@ -2695,27 +2814,54 @@ static int iniLoad(const char *filename) } // close it inifile.close(); return 0; } +static int send_status_to_druid(int msqid, int serial_number, int status) +{ + int retries, done; + struct msgstatbuf sbuf; + + if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE){ + printf("Returning status#%d to druid, cmd#%d originator_msqid=%d\n", status, serial_number, msqid); + } + // Create the message + sbuf.serial_number = serial_number; + sbuf.status = status; + // And send it + for(retries = 0, done = 0; retries < 10; retries++){ + if(msgsnd(msqid, (struct msgbuf *)&sbuf, + sizeof(msgstatbuf), IPC_NOWAIT) == 0){ + done = 1; + break; + } + } + if(done)return(true); + printf("%s: msgsnd(%d) failed after %d retries, errno=%d (%s)\n", + __FUNCTION__, emcStatus->originator_msqid, + retries, errno, strerror(errno)); + return(false); +} + static EMC_STAT last_emc_status; /* syntax: a.out {-d -ini } {-nml } {-shm } */ int main(int argc, char *argv[]) { int taskAborted = 0; // flag to prevent flurry of task aborts int taskPlanError = 0; int taskExecuteError = 0; double startTime; + int inDruidMode; // true if a druid transaction is running double minTime, maxTime; // copy command line args Argc = argc; Argv = argv; // loop until done @@ -2732,52 +2878,109 @@ int main(int argc, char *argv[]) rcs_print_error("error in argument list\n"); exit(1); } if (done) { emctask_shutdown(); exit(1); } + if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE) printf("emctask:main: initializing globals...\n"); // initialize globals emcInitGlobals(); if (done) { emctask_shutdown(); exit(1); } + if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE) printf("emctask:main: iniLoad...\n"); // get configuration information iniLoad(EMC_INIFILE); if (done) { emctask_shutdown(); exit(1); } // initialize everything + if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE) printf("emctask:main: emctask_startup...\n"); if (0 != emctask_startup()) { emctask_shutdown(); exit(1); } // set the default startup modes emcTaskSetState(EMC_TASK_STATE_ESTOP); emcTaskSetMode(EMC_TASK_MODE_MANUAL); // reflect the initial value of EMC_DEBUG in emcStatus->debug emcStatus->debug = EMC_DEBUG; startTime = etime(); // set start time before entering loop; // it will be set at end of loop from now on minTime = DBL_MAX; // set to value that can never be exceeded maxTime = 0.0; // set to value that can never be underset + inDruidMode = false; + if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE) printf("emctask:main: main loop...\n"); while (!done) { // read command - if (0 != emcCommandBuffer->peek()) { - // got a new command, so clear out errors + // We first try to get a druid command + if (0 != druidCommandBuffer->peek()) { + // got a new druid command + inDruidMode = true; // Enter druid mode + // clear out errors taskPlanError = 0; taskExecuteError = 0; + taskCommand = druidCommand; + if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE) printf("Got druid command#%d, originator_msqid=%d\n", + druidCommand->serial_number, druidCommand->originator_msqid); + } + else { + int semValue; + + if (inDruidMode == true){ + // Check if sem value = 0, and otherwise exit druid mode + if((semValue = semctl(cmdSemId, 0, GETVAL)) == -1){ + printf("Can't GETVAL semaphore(%d), errno=%d (%s)\n", + cmdSemId, errno, strerror(errno)); + inDruidMode = false; // accept normal command + } + else if(semValue > 0){ + // Druid has released semaphore, so be ready to accept command + inDruidMode = false; + } + } + } + // Check if a normal command is also coming + if (0 != emcCommandBuffer->peek()) { + if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE) printf("Got normal command#%d, inDruidMode=%d\n", + emcCommand->serial_number, inDruidMode); + if(inDruidMode == true){ + // if it is an ESTOP, abort current druid command + if((emcCommand->type == EMC_TASK_SET_STATE_TYPE) + && (((EMC_TASK_SET_STATE *)emcCommand)->state == EMC_TASK_STATE_ESTOP)){ + (void)send_status_to_druid(emcStatus->originator_msqid, + emcStatus->echo_serial_number, emcStatus->status); + taskCommand = emcCommand; + } + else { + // Otherwise, refuse normal command + if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE) printf("Refusing normal command#%d\n", + emcCommand->serial_number); + emcStatus->echo_serial_number = emcCommand->serial_number; + emcStatus->originator_msqid = emcCommand->originator_msqid; + emcStatus->status = RCS_ERROR; + emcStatusBuffer->write(emcStatus); + } + } + else { + // Accept it + // clear out errors + taskPlanError = 0; + taskExecuteError = 0; + taskCommand = emcCommand; + } } // run control cycle if (0 != emcTaskPlan()) { taskPlanError = 1; } if (0 != emcTaskExecute()) { taskExecuteError = 1; } @@ -2850,22 +3053,23 @@ int main(int argc, char *argv[]) // update task-specific status emcTaskUpdate(&emcStatus->task); // handle RCS_STAT_MSG base class members explicitly, since this // is not an NML_MODULE and they won't be set automatically // do task - emcStatus->task.command_type = emcCommand->type; - emcStatus->task.echo_serial_number = emcCommand->serial_number; + emcStatus->task.command_type = taskCommand->type; + emcStatus->task.echo_serial_number = taskCommand->serial_number; // do top level - emcStatus->command_type = emcCommand->type; - emcStatus->echo_serial_number = emcCommand->serial_number; + emcStatus->command_type = taskCommand->type; + emcStatus->echo_serial_number = taskCommand->serial_number; + emcStatus->originator_msqid = taskCommand->originator_msqid; if (taskPlanError || taskExecuteError || emcStatus->task.execState == EMC_TASK_EXEC_ERROR || emcStatus->motion.status == RCS_ERROR || emcStatus->io.status == RCS_ERROR) { emcStatus->status = RCS_ERROR; emcStatus->task.status = RCS_ERROR; } else if (!taskPlanError && !taskExecuteError && @@ -2907,16 +3111,43 @@ int main(int argc, char *argv[]) } last_emc_status.motion.axis[i].maxSoftLimit = emcStatus->motion.axis[i].maxSoftLimit; } // write it // since emcStatus was passed to the WM init functions, it // will be updated in the _update() functions above. There's // no need to call the individual functions on all WM items. + { + static int oldStatus = -2; + static int oldSerial = -1; + + if(inDruidMode && (emcStatus->originator_msqid != -1) + && ((oldStatus != emcStatus->status) + || (oldSerial != emcStatus->echo_serial_number))){ + // Command comes from a druid, so write ack to msgqueue + if(send_status_to_druid(emcStatus->originator_msqid, + emcStatus->echo_serial_number, emcStatus->status) == true){ + oldStatus = emcStatus->status; + oldSerial = emcStatus->echo_serial_number; + } + } + } + + // Write regular status for others + { + static int old_serial, old_status; + if((emcStatus->echo_serial_number != old_serial) + || (emcStatus->status != old_status)){ + if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE) printf("Sending status#%d, cmd#%d originator_msqid=%d\n", + emcStatus->status, emcStatus->echo_serial_number, emcStatus->originator_msqid); + old_serial = emcStatus->echo_serial_number; + old_status = emcStatus->status; + } + } emcStatusBuffer->write(emcStatus); // wait on timer cycle, if specified, or calculate actual // interval if ini file says to run full out via // [TASK] CYCLE_TIME <= 0.0 // emcTaskEager = 0; if ((emcTaskNoDelay) || (emcTaskEager)) { emcTaskEager = 0; @@ -2926,14 +3157,14 @@ int main(int argc, char *argv[]) } // end of while (! done) // clean up everything emctask_shutdown(); /* debugging */ if (emcTaskNoDelay) { if (EMC_DEBUG & EMC_DEBUG_INTERP) { printf("cycle times (seconds): %f min, %f max\n", minTime, - maxTime); + maxTime); } } // and leave exit(0); } Index: src/libnml/nml/cmd_msg.cc =================================================================== RCS file: /cvs/emc2/src/libnml/nml/cmd_msg.cc,v retrieving revision 1.6 diff -u -8 -p -r1.6 cmd_msg.cc --- src/libnml/nml/cmd_msg.cc 23 May 2005 16:34:14 -0000 1.6 +++ src/libnml/nml/cmd_msg.cc 9 Oct 2007 23:35:43 -0000 @@ -27,16 +27,17 @@ extern "C" { #include "cms.hh" NMLTYPE nmltype; #include "cmd_msg.hh" RCS_CMD_MSG::RCS_CMD_MSG(NMLTYPE t, long sz):NMLmsg(t, sz) { serial_number = 0; + originator_msqid = -1; } int RCS_CMD_MSG_format(NMLTYPE t, void *buf, CMS * cms) { cms->update(((RCS_CMD_MSG *) buf)->serial_number); // printf(" RCS_CMD_MSG_format: called.\n"); switch (t) { Index: src/libnml/nml/cmd_msg.hh =================================================================== RCS file: /cvs/emc2/src/libnml/nml/cmd_msg.hh,v retrieving revision 1.5 diff -u -8 -p -r1.5 cmd_msg.hh --- src/libnml/nml/cmd_msg.hh 23 May 2005 16:34:14 -0000 1.5 +++ src/libnml/nml/cmd_msg.hh 9 Oct 2007 23:35:43 -0000 @@ -19,16 +19,17 @@ #include "nml.hh" #include "nmlmsg.hh" class RCS_CMD_MSG:public NMLmsg { public: RCS_CMD_MSG(NMLTYPE t, long sz); int serial_number; + int originator_msqid; // MG }; extern int RCS_CMD_MSG_format(NMLTYPE, void *, CMS *); class RCS_CMD_CHANNEL:public NML { public: RCS_CMD_CHANNEL(NML_FORMAT_PTR, char *, char *, char *, int set_to_server = 0);