From 3cf41c851c62f56076ebe29c8534ee1aa758cf7c Mon Sep 17 00:00:00 2001 From: Ron Sluiter Date: Fri, 16 Feb 2007 20:21:47 +0000 Subject: [PATCH] Bug fix for overwriting PID parameter fields during normalization calculation. --- motorApp/OmsSrc/devOmsCom.cc | 643 ++++++++++++++++++----------------- 1 file changed, 322 insertions(+), 321 deletions(-) diff --git a/motorApp/OmsSrc/devOmsCom.cc b/motorApp/OmsSrc/devOmsCom.cc index a5b8ffe4..29c66580 100644 --- a/motorApp/OmsSrc/devOmsCom.cc +++ b/motorApp/OmsSrc/devOmsCom.cc @@ -1,17 +1,17 @@ /* -FILENAME... devOmsCom.cc +FILENAME... devOmsCom.cc USAGE... Data and functions common to all OMS device level support. -Version: $Revision: 1.10 $ -Modified By: $Author: sluiter $ -Last Modified: $Date: 2006-08-18 21:13:31 $ +Version: $Revision: 1.11 $ +Modified By: $Author: sluiter $ +Last Modified: $Date: 2007-02-16 20:21:47 $ */ /* * Original Author: Jim Kowalkowski * Previous Author: Joe Sullivan * Date: 11/14/94 - * Current Author: Ron Sluiter + * Current Author: Ron Sluiter * * Experimental Physics and Industrial Control System (EPICS) * @@ -23,16 +23,16 @@ Last Modified: $Date: 2006-08-18 21:13:31 $ * and (W-31-109-ENG-38) at Argonne National Laboratory. * * Initial development by: - * The Controls and Automation Group (AT-8) - * Ground Test Accelerator - * Accelerator Technology Division - * Los Alamos National Laboratory + * The Controls and Automation Group (AT-8) + * Ground Test Accelerator + * Accelerator Technology Division + * Los Alamos National Laboratory * * Co-developed with - * The Controls and Computing Group - * Accelerator Systems Division - * Advanced Photon Source - * Argonne National Laboratory + * The Controls and Computing Group + * Accelerator Systems Division + * Advanced Photon Source + * Argonne National Laboratory * * Modification Log: * ----------------- @@ -42,13 +42,13 @@ Last Modified: $Date: 2006-08-18 21:13:31 $ * .04 06-20-96 jps allow for bumpless-reboot on position * .04a 02-19-97 tmm fixed for EPICS 3.13 * .05 05-14-01 rls Support for jog velocity and acceleration commands. - * Added "CA" to home and jog commands so JVEL does not - * see done flag from previous operation. + * Added "CA" to home and jog commands so JVEL does not + * see done flag from previous operation. * .06 03-27-02 rls Eliminated RES. All input positions, velocities and - * accelerations are in motor steps. + * accelerations are in motor steps. * .07 07-05-02 rls Work around for OMS bug that ignores MR+/-1. * Use OMS UU command to support reference is always in - * motor steps. + * motor steps. * .08 06-04-03 rls Convert to R3.14.x. * .09 06-04-03 rls extended device directive support PREM and POST. * .10 06-16-03 rls Converted to R3.14.x. @@ -57,6 +57,8 @@ Last Modified: $Date: 2006-08-18 21:13:31 $ * .13 03-21-05 rls OSI - built for solaris and linux hosts. * .14 03-23-05 rls restrict acceleration to valid values. * .15 08-18-06 rls Output "slew <= base" error message only one time. + * .16 02-16-07 rls Bug fix for overwriting PID parameter fields during + * normalization calculation. */ #include @@ -84,28 +86,28 @@ struct motor_table struct motor_table const oms_table[] = { - {MOTION, " MA", 1}, /* MOVE_ABS */ - {MOTION, " MR", 1}, /* MOVE_REL */ - {MOTION, " CA HM", 1}, /* HOME_FOR */ - {MOTION, " CA HR", 1}, /* HOME_REV */ - {IMMEDIATE, " LP", 1}, /* LOAD_POS */ - {IMMEDIATE, " VB", 1}, /* SET_VEL_BASE */ - {IMMEDIATE, " VL", 1}, /* SET_VELO */ - {IMMEDIATE, " AC", 1}, /* SET_ACCEL */ - {IMMEDIATE, " GD", 0}, /* jps: from GO to GD */ - {IMMEDIATE, " ER", 2}, /* SET_ENC_RATIO */ - {INFO, " ", 0}, /* GET_INFO */ - {MOVE_TERM, " ST", 0}, /* STOP_AXIS */ - {VELOCITY, " CA JG", 1}, /* JOG */ - {IMMEDIATE," KP", 1}, /* SET_PGAIN */ - {IMMEDIATE," KI", 1}, /* SET_IGAIN */ - {IMMEDIATE," KD", 1}, /* SET_DGAIN */ - {IMMEDIATE," HN", 0}, /* ENABLE_TORQUE */ - {IMMEDIATE," HF", 0}, /* DISABL_TORQUE */ - {IMMEDIATE, "", 0}, /* PRIMITIVE */ - {IMMEDIATE, "", 0}, /* SET_HIGH_LIMIT */ - {IMMEDIATE, "", 0}, /* SET_LOW_LIMIT */ - {VELOCITY, " JG", 1}, /* JOG_VELOCITY */ + {MOTION, " MA", 1}, /* MOVE_ABS */ + {MOTION, " MR", 1}, /* MOVE_REL */ + {MOTION, " CA HM", 1}, /* HOME_FOR */ + {MOTION, " CA HR", 1}, /* HOME_REV */ + {IMMEDIATE, " LP", 1}, /* LOAD_POS */ + {IMMEDIATE, " VB", 1}, /* SET_VEL_BASE */ + {IMMEDIATE, " VL", 1}, /* SET_VELO */ + {IMMEDIATE, " AC", 1}, /* SET_ACCEL */ + {IMMEDIATE, " GD", 0}, /* jps: from GO to GD */ + {IMMEDIATE, " ER", 2}, /* SET_ENC_RATIO */ + {INFO, " ", 0}, /* GET_INFO */ + {MOVE_TERM, " ST", 0}, /* STOP_AXIS */ + {VELOCITY, " CA JG", 1}, /* JOG */ + {IMMEDIATE," KP", 1}, /* SET_PGAIN */ + {IMMEDIATE," KI", 1}, /* SET_IGAIN */ + {IMMEDIATE," KD", 1}, /* SET_DGAIN */ + {IMMEDIATE," HN", 0}, /* ENABLE_TORQUE */ + {IMMEDIATE," HF", 0}, /* DISABL_TORQUE */ + {IMMEDIATE, "", 0}, /* PRIMITIVE */ + {IMMEDIATE, "", 0}, /* SET_HIGH_LIMIT */ + {IMMEDIATE, "", 0}, /* SET_LOW_LIMIT */ + {VELOCITY, " JG", 1}, /* JOG_VELOCITY */ }; @@ -113,68 +115,68 @@ struct motor_table const oms_table[] = FUNCTION... long oms_build_trans(motor_cmnd, double *, struct motorRecord *) USAGE... Add a part to the transaction. -INPUT... command - index into oms_table[]. - *parms - one or more input parameters; determined by - "num_parms" in oms_table. - *mr - motor record pointer. +INPUT... command - index into oms_table[]. + *parms - one or more input parameters; determined by + "num_parms" in oms_table. + *mr - motor record pointer. LOGIC... Initialize pointers; return value to OK. Set highest command type priority. IF transaction is NOT initialized (i.e., state != BUILD_STATE) - ERROR RETURN. + ERROR RETURN. ENDIF IF input command is a PRIMITIVE command, AND, the INIT field is not empty. - IF input command is a "device directive". - Find terminating '@'. - IF valid "device directive" (i.e, directive bracketed by '@'s). - IF "Driver Power Monitoring ON" directive. - Flush response buffer. - Read motor controller's GPIO configuration. - IF bit corresponding to current axis is configured as an - output bit. - Set "Driver Power Monitoring" indicator true. - ELSE - Output ERROR message. - ENDIF - ENDIF - Strip device directive and copy rest of INIT field to local buffer. - ELSE - Copy INIT field to local buffer. - ENDIF - ELSE - Copy INIT field to local buffer. - ENDIF + IF input command is a "device directive". + Find terminating '@'. + IF valid "device directive" (i.e, directive bracketed by '@'s). + IF "Driver Power Monitoring ON" directive. + Flush response buffer. + Read motor controller's GPIO configuration. + IF bit corresponding to current axis is configured as an + output bit. + Set "Driver Power Monitoring" indicator true. + ELSE + Output ERROR message. + ENDIF + ENDIF + Strip device directive and copy rest of INIT field to local buffer. + ELSE + Copy INIT field to local buffer. + ENDIF + ELSE + Copy INIT field to local buffer. + ENDIF ELSE - IF input command is set HIGH or LOW travel limit. - Null command. Set transaction state to IDLE. - ELSE - IF input command is Proportional Gain, AND, gain < valid minimum. - Set the gain and the PCOF field to the minimum. - Set return indicator to ERROR. - ELSE IF input command is STOP_AXIS. - NOTE: This logic is here as a workaround for an OMS firmware - error. When a STOP command was issued during the target move, - the backlash acceleration would go into effect. - - Prefix command with ACCL field acceleration. - ENDIF - IF command type is MOTION or VELOCITY. - Process PREM and POST fields. - ENDIF - IF Overtravel status indicator true, AND, input command is a MOVE. - NOTE: This logic is here as a workaround for the "Moving off a - limit switch" OMS firmware error. + IF input command is set HIGH or LOW travel limit. + Null command. Set transaction state to IDLE. + ELSE + IF input command is Proportional Gain, AND, gain < valid minimum. + Set the gain and the PCOF field to the minimum. + Set return indicator to ERROR. + ELSE IF input command is STOP_AXIS. + NOTE: This logic is here as a workaround for an OMS firmware + error. When a STOP command was issued during the target move, + the backlash acceleration would go into effect. + + Prefix command with ACCL field acceleration. + ENDIF + IF command type is MOTION or VELOCITY. + Process PREM and POST fields. + ENDIF + IF Overtravel status indicator true, AND, input command is a MOVE. + NOTE: This logic is here as a workaround for the "Moving off a + limit switch" OMS firmware error. - IF incremental distance to target position is positive. - Set OMS controller "direction logic" positive. - ELSE - Set OMS controller "direction logic" negative. - ENDIF - ENDIF - Copy input command into output message. - Copy input command parameters into output message. - ENDIF + IF incremental distance to target position is positive. + Set OMS controller "direction logic" positive. + ELSE + Set OMS controller "direction logic" negative. + ENDIF + ENDIF + Copy input command into output message. + Copy input command parameters into output message. + ENDIF ENDIF */ @@ -192,276 +194,275 @@ RTN_STATUS oms_build_trans(motor_cmnd command, double *parms, struct motorRecord cmnd_type = oms_table[command].type; if (cmnd_type > motor_call->type) - motor_call->type = cmnd_type; + motor_call->type = cmnd_type; /* concatenate onto the dpvt message field */ if (trans->state != BUILD_STATE) - return(rtnind = ERROR); + return(rtnind = ERROR); if ((command == PRIMITIVE) && (mr->init != NULL) && - (strlen(mr->init) != 0)) + (strlen(mr->init) != 0)) { - extern struct driver_table oms58_access; - /* Test for a "device directive" in the Initialization string. */ - if ((mr->init[0] == '@') && (trans->tabptr == &oms58_access)) - { - char *end = strrchr(&mr->init[1], '@'); - if (end != NULL) - { - struct driver_table *tabptr = trans->tabptr; - int size = (end - &mr->init[0]) + 1; - strncpy(buffer, mr->init, size); - buffer[size] = (char) NULL; - if (strcmp(buffer, "@DPM_ON@") == 0) - { - int response, bitselect; - char respbuf[10]; + extern struct driver_table oms58_access; + /* Test for a "device directive" in the Initialization string. */ + if ((mr->init[0] == '@') && (trans->tabptr == &oms58_access)) + { + char *end = strrchr(&mr->init[1], '@'); + if (end != NULL) + { + struct driver_table *tabptr = trans->tabptr; + int size = (end - &mr->init[0]) + 1; + strncpy(buffer, mr->init, size); + buffer[size] = (char) NULL; + if (strcmp(buffer, "@DPM_ON@") == 0) + { + int response, bitselect; + char respbuf[10]; - (*tabptr->getmsg)(motor_call->card, respbuf, -1); - (*tabptr->sendmsg)(motor_call->card, "RB\r", - (char) NULL); - (*tabptr->getmsg)(motor_call->card, respbuf, 1); - if (sscanf(respbuf, "%x", &response) == 0) - response = 0; /* Force an error. */ - bitselect = (1 << motor_call->signal); - if ((response & bitselect) == 0) - trans->dpm = true; - else - errPrintf(0, __FILE__, __LINE__, - "Invalid VME58 configuration; RB = 0x%x\n", response); - } - end++; - strcpy(buffer, end); - } - else - strcpy(buffer, mr->init); - } - else - strcpy(buffer, mr->init); + (*tabptr->getmsg)(motor_call->card, respbuf, -1); + (*tabptr->sendmsg)(motor_call->card, "RB\r", + (char) NULL); + (*tabptr->getmsg)(motor_call->card, respbuf, 1); + if (sscanf(respbuf, "%x", &response) == 0) + response = 0; /* Force an error. */ + bitselect = (1 << motor_call->signal); + if ((response & bitselect) == 0) + trans->dpm = true; + else + errPrintf(0, __FILE__, __LINE__, + "Invalid VME58 configuration; RB = 0x%x\n", response); + } + end++; + strcpy(buffer, end); + } + else + strcpy(buffer, mr->init); + } + else + strcpy(buffer, mr->init); - strcat(motor_call->message, " "); - strcat(motor_call->message, buffer); + strcat(motor_call->message, " "); + strcat(motor_call->message, buffer); } else { - int first_one, itera; + int first_one, itera; - if (command == SET_HIGH_LIMIT || command == SET_LOW_LIMIT) - trans->state = IDLE_STATE; /* No command sent to the controller. */ - else - { - msta_field msta; - - /* Silently enforce minimum range on KP command. */ - if (command == SET_PGAIN && *parms < 0.00005) - { - *parms = 0.00005; - mr->pcof = 0.00005; - rtnind = ERROR; - } - else if (command == STOP_AXIS) - { - double acc = (mr->velo / fabs(mr->mres)) / mr->accl; - - /* Put in acceleration. */ - strcat(motor_call->message, oms_table[SET_ACCEL].command); - sprintf(buffer, "%ld", NINT(acc)); - strcat(motor_call->message, buffer); - } + if (command == SET_HIGH_LIMIT || command == SET_LOW_LIMIT) + trans->state = IDLE_STATE; /* No command sent to the controller. */ + else + { + msta_field msta; + + /* Silently enforce minimum range on KP command. */ + if (command == SET_PGAIN && *parms < 0.00005) + { + *parms = 0.00005; + mr->pcof = 0.00005; + rtnind = ERROR; + } + else if (command == STOP_AXIS) + { + double acc = (mr->velo / fabs(mr->mres)) / mr->accl; + + /* Put in acceleration. */ + strcat(motor_call->message, oms_table[SET_ACCEL].command); + sprintf(buffer, "%ld", NINT(acc)); + strcat(motor_call->message, buffer); + } - if (cmnd_type == MOTION || cmnd_type == VELOCITY) - { - if (strlen(mr->prem) != 0) - { - char prem_buff[40]; + if (cmnd_type == MOTION || cmnd_type == VELOCITY) + { + if (strlen(mr->prem) != 0) + { + char prem_buff[40]; - /* Test for a "device directive" in the PREM string. */ - if (mr->prem[0] == '@') - { - bool errind = false; - char *end = strchr(&mr->prem[1], '@'); - if (end == NULL) - errind = true; - else - { - DBADDR addr; - double delay; - char *start, *tail; - int size = (end - &mr->prem[0]) + 1; + /* Test for a "device directive" in the PREM string. */ + if (mr->prem[0] == '@') + { + bool errind = false; + char *end = strchr(&mr->prem[1], '@'); + if (end == NULL) + errind = true; + else + { + DBADDR addr; + double delay; + char *start, *tail; + int size = (end - &mr->prem[0]) + 1; - /* Copy device directive to prem_buff. */ - strncpy(prem_buff, mr->prem, size); - prem_buff[size] = (char) NULL; + /* Copy device directive to prem_buff. */ + strncpy(prem_buff, mr->prem, size); + prem_buff[size] = (char) NULL; - if (strncmp(prem_buff, "@PUT(", 5) != 0) - goto errorexit; + if (strncmp(prem_buff, "@PUT(", 5) != 0) + goto errorexit; - /* Point "start" to PV name argument. */ - tail = NULL; - start = strtok_r(&prem_buff[5], ",", &tail); - if (tail == NULL) - goto errorexit; + /* Point "start" to PV name argument. */ + tail = NULL; + start = strtok_r(&prem_buff[5], ",", &tail); + if (tail == NULL) + goto errorexit; - if (dbNameToAddr(start, &addr)) /* Get address of PV. */ - { - errPrintf(-1, __FILE__, __LINE__, "Invalid PV name: %s", start); - goto errorexit; - } + if (dbNameToAddr(start, &addr)) /* Get address of PV. */ + { + errPrintf(-1, __FILE__, __LINE__, "Invalid PV name: %s", start); + goto errorexit; + } - /* Point "start" to PV value argument. */ - start = strtok_r(NULL, ",", &tail); - if (tail == NULL) - { - delay = 0.0; - tail = start; - start = strtok_r(NULL, ")", &tail); - if (tail == NULL) - goto errorexit; - } - else - { - char *last; + /* Point "start" to PV value argument. */ + start = strtok_r(NULL, ",", &tail); + if (tail == NULL) + { + delay = 0.0; + tail = start; + start = strtok_r(NULL, ")", &tail); + if (tail == NULL) + goto errorexit; + } + else + { + char *last; - last = strtok_r(NULL, ")", &tail); - if (last == NULL) - goto errorexit; - delay = atof(last); - } - if (dbPutField(&addr, DBR_STRING, start, 1L)) - { - errPrintf(-1, __FILE__, __LINE__, "invalid value: %s", start); - goto errorexit; - } - if (delay != 0.0) - { - if (delay > 10.0) - delay = 10.0; - epicsThreadSleep(delay); - } - } + last = strtok_r(NULL, ")", &tail); + if (last == NULL) + goto errorexit; + delay = atof(last); + } + if (dbPutField(&addr, DBR_STRING, start, 1L)) + { + errPrintf(-1, __FILE__, __LINE__, "invalid value: %s", start); + goto errorexit; + } + if (delay != 0.0) + { + if (delay > 10.0) + delay = 10.0; + epicsThreadSleep(delay); + } + } - if (errind == true) -errorexit: errMessage(-1, "Invalid device directive"); - end++; - strcpy(prem_buff, end); - } - else - strcpy(prem_buff, mr->prem); + if (errind == true) +errorexit: errMessage(-1, "Invalid device directive"); + end++; + strcpy(prem_buff, end); + } + else + strcpy(prem_buff, mr->prem); - strcat(motor_call->message, " "); - strcat(motor_call->message, prem_buff); - strcat(motor_call->message, " "); - } - if (strlen(mr->post) != 0) - motor_call->postmsgptr = (char *) &mr->post; - } - - /* Code to hide Oms58 moving off limit switch problem. */ - msta.All = mr->msta; - if ((msta.Bits.RA_PLUS_LS || msta.Bits.RA_MINUS_LS) && - (command == MOVE_ABS || command == MOVE_REL)) - { - if (mr->rdif >= 0) - strcat(motor_call->message, " MP"); - else - strcat(motor_call->message, " MM"); - } + strcat(motor_call->message, " "); + strcat(motor_call->message, prem_buff); + strcat(motor_call->message, " "); + } + if (strlen(mr->post) != 0) + motor_call->postmsgptr = (char *) &mr->post; + } + + /* Code to hide Oms58 moving off limit switch problem. */ + msta.All = mr->msta; + if ((msta.Bits.RA_PLUS_LS || msta.Bits.RA_MINUS_LS) && + (command == MOVE_ABS || command == MOVE_REL)) + { + if (mr->rdif >= 0) + strcat(motor_call->message, " MP"); + else + strcat(motor_call->message, " MM"); + } - /* put in command */ - strcat(motor_call->message, oms_table[command].command); + /* put in command */ + strcat(motor_call->message, oms_table[command].command); - /* put in parameters */ - for (first_one = YES, itera = 0; itera < oms_table[command].num_parms; - itera++) - { - if (first_one == YES) - first_one = NO; - else - strcat(motor_call->message, ","); + /* put in parameters */ + for (first_one = YES, itera = 0; itera < oms_table[command].num_parms; + itera++) + { + if (first_one == YES) + first_one = NO; + else + strcat(motor_call->message, ","); - switch (command) - { - case SET_PGAIN: - case SET_IGAIN: - case SET_DGAIN: - *parms *= 1999.9; - sprintf(buffer, "%.1f", parms[itera]); - break; + switch (command) + { + case SET_PGAIN: + case SET_IGAIN: + case SET_DGAIN: + sprintf(buffer, "%.1f", (parms[itera] * 1999.9)); + break; - case SET_VELOCITY: /* OMS errors if VB = VL. */ - { - long vbase, vel; + case SET_VELOCITY: /* OMS errors if VB = VL. */ + { + long vbase, vel; - vbase = NINT(mr->vbas / fabs(mr->mres)); - vel = NINT(parms[itera]); + vbase = NINT(mr->vbas / fabs(mr->mres)); + vel = NINT(parms[itera]); - if (vel <= vbase) - { + if (vel <= vbase) + { if (invalid_velmsg_latch == false) { invalid_velmsg_latch = true; /* Ouput msg. one time. */ - errPrintf(-1, __FILE__, __LINE__, - "Overriding invalid velocity; slew <= base.\n"); + errPrintf(-1, __FILE__, __LINE__, + "Overriding invalid velocity; slew <= base.\n"); } - vel = vbase + 1; - } - sprintf(buffer, "%ld", vel); - } - break; + vel = vbase + 1; + } + sprintf(buffer, "%ld", vel); + } + break; - case MOVE_REL: - { - /* Code around OMS bug that ignores MR+/-1. */ - long relmove; + case MOVE_REL: + { + /* Code around OMS bug that ignores MR+/-1. */ + long relmove; - relmove = NINT(parms[itera]); - if (relmove == 1 || relmove == -1) - sprintf(buffer, "%ld.5", relmove); - else - sprintf(buffer, "%ld", relmove); - } - break; + relmove = NINT(parms[itera]); + if (relmove == 1 || relmove == -1) + sprintf(buffer, "%ld.5", relmove); + else + sprintf(buffer, "%ld", relmove); + } + break; - case SET_ACCEL: /* Prevent invalid acceleration values. */ - { - long valid_acc = NINT(parms[itera]); + case SET_ACCEL: /* Prevent invalid acceleration values. */ + { + long valid_acc = NINT(parms[itera]); - if (valid_acc < 1 || valid_acc > 1000000000) - { - errPrintf(-1, __FILE__, __LINE__, - "Overriding invalid acceleration.\n"); + if (valid_acc < 1 || valid_acc > 1000000000) + { + errPrintf(-1, __FILE__, __LINE__, + "Overriding invalid acceleration.\n"); - if (valid_acc < 1) - valid_acc = 1; - else - valid_acc = 1000000000; - } - sprintf(buffer, "%ld", valid_acc); - } - break; + if (valid_acc < 1) + valid_acc = 1; + else + valid_acc = 1000000000; + } + sprintf(buffer, "%ld", valid_acc); + } + break; - default: - sprintf(buffer, "%ld", NINT(parms[itera])); - } - strcat(motor_call->message, buffer); - } + default: + sprintf(buffer, "%ld", NINT(parms[itera])); + } + strcat(motor_call->message, buffer); + } - switch (command) - { - case SET_ENC_RATIO: - sprintf(buffer, " UU%f", parms[0]/parms[1]); - strcat(motor_call->message, buffer); - break; + switch (command) + { + case SET_ENC_RATIO: + sprintf(buffer, " UU%f", parms[0]/parms[1]); + strcat(motor_call->message, buffer); + break; - case LOAD_POS: - case JOG: - case JOG_VELOCITY: - strcat(motor_call->message, ";"); + case LOAD_POS: + case JOG: + case JOG_VELOCITY: + strcat(motor_call->message, ";"); - default: - break; - } - } + default: + break; + } + } } return(rtnind); }