00001
00002
00003 #include "rcs.hh"
00004
00005 extern "C" {
00006 extern double gstrip_nml_get(void *);
00007 extern void * gstrip_nml_open(const char *);
00008 extern int nml_new_data();
00009 extern int emcIniLoad(const char *filename);
00010 }
00011
00012 #include <stdlib.h>
00013 #include <stdio.h>
00014 #include <string.h>
00015 #include <math.h>
00016 #include "emc.hh"
00017 #include "emcglb.h"
00018
00019
00020 int emcIniLoad(const char *filename)
00021 {
00022 INIFILE inifile;
00023 const char *inistring;
00024
00025
00026 if (-1 == inifile.open(filename)) {
00027 return -1;
00028 }
00029
00030 if (NULL != (inistring = inifile.find("DEBUG", "EMC"))) {
00031
00032 if (1 != sscanf(inistring, "%i", &EMC_DEBUG)) {
00033 EMC_DEBUG = 0;
00034 }
00035 }
00036 else {
00037
00038 EMC_DEBUG = 0;
00039 }
00040
00041 if (NULL != (inistring = inifile.find("NML_FILE", "EMC"))) {
00042
00043 strcpy(EMC_NMLFILE, inistring);
00044 }
00045 else {
00046
00047 }
00048
00049
00050 inifile.close();
00051
00052 return 0;
00053 }
00054
00055
00056
00057 static int first_nml_variable = 1;
00058 RCS_STAT_CHANNEL *statChannel=NULL;
00059 static int no_nml_stat_channel=0;
00060 EMC_STAT *statMsg;
00061 int nml_new_data_var = 0;
00062
00063 int nml_new_data()
00064 {
00065 return nml_new_data_var;
00066 }
00067
00068 struct nml_data
00069 {
00070 int variable_number;
00071 int array_index;
00072 int first;
00073 };
00074
00075 double get_variable_value(int varnum, int array_index);
00076
00077 double gstrip_nml_get(void *p)
00078 {
00079 if(((nml_data *)p)->first)
00080 {
00081 if(statChannel->read() == EMC_STAT_TYPE)
00082 {
00083 statMsg = (EMC_STAT *) statChannel->get_address();
00084 nml_new_data_var=1;
00085 }
00086 else
00087 {
00088 nml_new_data_var = 0;
00089 }
00090 }
00091 return get_variable_value(((nml_data *)p)->variable_number,((nml_data *)p)->array_index);
00092 }
00093
00094 int get_task_varnum(const char *str);
00095 int get_motion_varnum(const char *str, int *array_index_ptr);
00096 int get_io_varnum(const char *str, int *array_index_ptr);
00097
00098 void * gstrip_nml_open(const char *str)
00099 {
00100 struct nml_data *nd = NULL;
00101 int varnum=-1;
00102
00103 if(no_nml_stat_channel)
00104 {
00105 return NULL;
00106 }
00107
00108 if(NULL == statChannel)
00109 {
00110 statChannel = new RCS_STAT_CHANNEL(emcFormat,"emcStatus","xemc",EMC_NMLFILE);
00111 if(!statChannel->valid())
00112 {
00113 no_nml_stat_channel = 1;
00114 delete statChannel;
00115 statChannel = NULL;
00116 return NULL;
00117 }
00118 else
00119 {
00120 statMsg = (EMC_STAT *) statChannel->get_address();
00121 }
00122 }
00123
00124
00125 int array_index=0;
00126 if(!strncmp(str,"task.",5))
00127 {
00128 varnum = get_task_varnum(str+5);
00129 }
00130 else if (!strncmp(str,"motion.",7))
00131 {
00132 varnum = get_motion_varnum(str+7, &array_index);
00133 }
00134 else if (!strncmp(str,"io.",3))
00135 {
00136 varnum = get_motion_varnum(str+3, &array_index);
00137 }
00138 else
00139 {
00140 fprintf(stderr,"Bad variable name: %s\n",str);
00141 return NULL;
00142 }
00143 if(varnum > 0)
00144 {
00145 nd = (struct nml_data *) malloc(sizeof(struct nml_data));
00146 nd->variable_number = varnum;
00147 nd->array_index = array_index;
00148 nd->first = first_nml_variable;
00149 first_nml_variable = 0;
00150 return nd;
00151 }
00152 fprintf(stderr,"Bad variable name: %s\n",str);
00153 return NULL;
00154 }
00155
00156
00157 int get_task_varnum(const char *str)
00158 {
00159 if(!strcmp("heartbeat",str))
00160 {
00161 return 1;
00162 }
00163 else if(!strcmp("mode",str))
00164 {
00165 return 2;
00166 }
00167 else if(!strcmp("state",str))
00168 {
00169 return 3;
00170 }
00171 else if(!strcmp("execState",str))
00172 {
00173 return 4;
00174 }
00175 else if(!strcmp("interpState",str))
00176 {
00177 return 5;
00178 }
00179 else if(!strcmp("motionLine",str))
00180 {
00181 return 6;
00182 }
00183 else if(!strcmp("currentLine",str))
00184 {
00185 return 7;
00186 }
00187 else if(!strcmp("readLine",str))
00188 {
00189 return 8;
00190 }
00191 return -1;
00192 }
00193
00194 int get_traj_varnum(const char *str)
00195 {
00196 if(!strcmp(str,"linearUnits"))
00197 {
00198 return 11;
00199 }
00200 if(!strcmp(str,"angularUnits"))
00201 {
00202 return 12;
00203 }
00204 if(!strcmp(str,"cycleTime"))
00205 {
00206 return 13;
00207 }
00208 if(!strcmp(str,"axes"))
00209 {
00210 return 14;
00211 }
00212 if(!strcmp(str,"mode"))
00213 {
00214 return 15;
00215 }
00216 if(!strcmp(str,"enabled"))
00217 {
00218 return 16;
00219 }
00220 if(!strcmp(str,"inpos"))
00221 {
00222 return 17;
00223 }
00224 if(!strcmp(str,"queue"))
00225 {
00226 return 18;
00227 }
00228 if(!strcmp(str,"activeQueue"))
00229 {
00230 return 19;
00231 }
00232 if(!strcmp(str,"queueFull"))
00233 {
00234 return 20;
00235 }
00236 if(!strcmp(str,"id"))
00237 {
00238 return 21;
00239 }
00240 if(!strcmp(str,"paused"))
00241 {
00242 return 22;
00243 }
00244 if(!strcmp(str,"scale"))
00245 {
00246 return 23;
00247 }
00248 if(!strcmp(str,"position.tran.x"))
00249 {
00250 return 24;
00251 }
00252 if(!strcmp(str,"position.tran.y"))
00253 {
00254 return 25;
00255 }
00256 if(!strcmp(str,"position.tran.z"))
00257 {
00258 return 26;
00259 }
00260 if(!strcmp(str,"actualPosition.tran.x"))
00261 {
00262 return 27;
00263 }
00264 if(!strcmp(str,"actualPosition.tran.y"))
00265 {
00266 return 28;
00267 }
00268 if(!strcmp(str,"actualPosition.tran.z"))
00269 {
00270 return 29;
00271 }
00272 if(!strcmp(str,"velocity"))
00273 {
00274 return 30;
00275 }
00276 if(!strcmp(str,"acceleration"))
00277 {
00278 return 31;
00279 }
00280 if(!strcmp(str,"maxVelocity"))
00281 {
00282 return 32;
00283 }
00284 if(!strcmp(str,"maxAcceleration"))
00285 {
00286 return 33;
00287 }
00288 return -1;
00289 }
00290
00291 int get_axis_varnum(const char *str)
00292 {
00293 int varnum = -1;
00294 if(!strcmp("p",str))
00295 {
00296 varnum = 1;
00297 }
00298 else if(!strcmp("i",str))
00299 {
00300 varnum = 2;
00301 }
00302 else if(!strcmp("d",str))
00303 {
00304 varnum = 3;
00305 }
00306 else if(!strcmp("ff0",str))
00307 {
00308 varnum = 4;
00309 }
00310 else if(!strcmp("ff1",str))
00311 {
00312 varnum = 5;
00313 }
00314 else if(!strcmp("ff2",str))
00315 {
00316 varnum = 6;
00317 }
00318 else if(!strcmp("backlash",str))
00319 {
00320 varnum = 7;
00321 }
00322 else if(!strcmp("bias",str))
00323 {
00324 varnum = 8;
00325 }
00326 else if(!strcmp("maxError",str))
00327 {
00328 varnum = 9;
00329 }
00330 else if(!strcmp("deadband",str))
00331 {
00332 varnum = 10;
00333 }
00334 else if(!strcmp("cycleTime",str))
00335 {
00336 varnum = 11;
00337 }
00338 else if(!strcmp("inputScale",str))
00339 {
00340 varnum = 12;
00341 }
00342 else if(!strcmp("inputOffset",str))
00343 {
00344 varnum = 13;
00345 }
00346 else if(!strcmp("outputScale",str))
00347 {
00348 varnum = 14;
00349 }
00350 else if(!strcmp("outputOffset",str))
00351 {
00352 varnum = 15;
00353 }
00354 else if(!strcmp("minPositionLimit",str))
00355 {
00356 varnum = 16;
00357 }
00358 else if(!strcmp("maxPositionLimit",str))
00359 {
00360 varnum = 17;
00361 }
00362 else if(!strcmp("minOutputLimit",str))
00363 {
00364 varnum = 18;
00365 }
00366 else if(!strcmp("maxOutputLimit",str))
00367 {
00368 varnum = 19;
00369 }
00370 else if(!strcmp("maxFerror",str))
00371 {
00372 varnum = 20;
00373 }
00374 else if(!strcmp("minFerror",str))
00375 {
00376 varnum = 21;
00377 }
00378 else if(!strcmp("homingVel",str))
00379 {
00380 varnum = 22;
00381 }
00382 else if(!strcmp("homeOffset",str))
00383 {
00384 varnum = 23;
00385 }
00386 else if(!strcmp("enablePolarity",str))
00387 {
00388 varnum = 24;
00389 }
00390 else if(!strcmp("minLimitSwitchPolarity",str))
00391 {
00392 varnum = 25;
00393 }
00394 else if(!strcmp("maxLimitSwitchPolarity",str))
00395 {
00396 varnum = 26;
00397 }
00398 else if(!strcmp("homeSwitchPolarity",str))
00399 {
00400 varnum = 27;
00401 }
00402 else if(!strcmp("homingPolarity",str))
00403 {
00404 varnum = 28;
00405 }
00406 else if(!strcmp("faultPolarity",str))
00407 {
00408 varnum = 29;
00409 }
00410 else if(!strcmp("setpoint",str))
00411 {
00412 varnum = 30;
00413 }
00414 else if(!strcmp("ferrorCurrent",str))
00415 {
00416 varnum = 31;
00417 }
00418 else if(!strcmp("output",str))
00419 {
00420 varnum = 32;
00421 }
00422 else if(!strcmp("input",str))
00423 {
00424 varnum = 33;
00425 }
00426 else if(!strcmp("inpos",str))
00427 {
00428 varnum = 34;
00429 }
00430 else if(!strcmp("homing",str))
00431 {
00432 varnum = 35;
00433 }
00434 else if(!strcmp("homed",str))
00435 {
00436 varnum = 36;
00437 }
00438 else if(!strcmp("fault",str))
00439 {
00440 varnum = 37;
00441 }
00442 else if(!strcmp("enabled",str))
00443 {
00444 varnum = 38;
00445 }
00446 else if(!strcmp("minSoftLimit",str))
00447 {
00448 varnum = 39;
00449 }
00450 else if(!strcmp("maxSoftLimit",str))
00451 {
00452 varnum = 40;
00453 }
00454 else if(!strcmp("minHardLimit",str))
00455 {
00456 varnum = 41;
00457 }
00458 else if(!strcmp("maxHardLimit",str))
00459 {
00460 varnum = 42;
00461 }
00462 else if(!strcmp("overrideLimits",str))
00463 {
00464 varnum = 43;
00465 }
00466 else if(!strcmp("scale",str))
00467 {
00468 varnum = 44;
00469 }
00470 else if(!strcmp("ferrorHighMark",str))
00471 {
00472 varnum = 45;
00473 }
00474 if(varnum == -1)
00475 {
00476 return -1;
00477 }
00478 return 50+varnum;
00479 }
00480
00481 int get_motion_varnum(const char *str, int *array_index_ptr)
00482 {
00483 if(!strcmp("heartbeat",str))
00484 {
00485 return 10;
00486 }
00487 else if(!strncmp("traj.",str,5))
00488 {
00489 return get_traj_varnum(str+5);
00490 }
00491 else if(!strncmp("axis[",str,5))
00492 {
00493 int axis = str[5]-'0';
00494 if(NULL != array_index_ptr && axis >= 0 && axis < EMC_AXIS_MAX)
00495 {
00496 *array_index_ptr = axis;
00497 }
00498 else
00499 {
00500 return -1;
00501 }
00502 return get_axis_varnum(str+8);
00503 }
00504 return -1;
00505 }
00506
00507
00508
00509
00510 double get_variable_value(int varnum, int array_index)
00511 {
00512 switch(varnum)
00513 {
00514 case 1:
00515 return statMsg->task.heartbeat;
00516
00517 case 2:
00518 return statMsg->task.mode;
00519
00520 case 3:
00521 return statMsg->task.state;
00522
00523 case 4:
00524 return statMsg->task.execState;
00525
00526 case 5:
00527 return statMsg->task.interpState;
00528
00529 case 6:
00530 return statMsg->task.motionLine;
00531
00532 case 7:
00533 return statMsg->task.currentLine;
00534
00535 case 8:
00536 return statMsg->task.readLine;
00537
00538 case 10:
00539 return statMsg->motion.heartbeat;
00540
00541 case 11:
00542 return statMsg->motion.traj.linearUnits;
00543
00544 case 12:
00545 return statMsg->motion.traj.angularUnits;
00546
00547 case 13:
00548 return statMsg->motion.traj.cycleTime;
00549
00550 case 14:
00551 return statMsg->motion.traj.axes;
00552
00553 case 15:
00554 return statMsg->motion.traj.mode;
00555
00556 case 16:
00557 return statMsg->motion.traj.enabled;
00558
00559 case 17:
00560 return statMsg->motion.traj.inpos;
00561
00562 case 18:
00563 return statMsg->motion.traj.queue;
00564
00565 case 19:
00566 return statMsg->motion.traj.activeQueue;
00567
00568 case 20:
00569 return statMsg->motion.traj.queueFull;
00570
00571 case 21:
00572 return statMsg->motion.traj.id;
00573
00574 case 22:
00575 return statMsg->motion.traj.paused;
00576
00577 case 23:
00578 return statMsg->motion.traj.scale;
00579
00580 case 24:
00581 return statMsg->motion.traj.position.tran.x;
00582
00583 case 25:
00584 return statMsg->motion.traj.position.tran.y;
00585
00586 case 26:
00587 return statMsg->motion.traj.position.tran.z;
00588
00589 case 27:
00590 return statMsg->motion.traj.actualPosition.tran.x;
00591
00592 case 28:
00593 return statMsg->motion.traj.actualPosition.tran.y;
00594
00595 case 29:
00596 return statMsg->motion.traj.actualPosition.tran.z;
00597
00598 case 30:
00599 return statMsg->motion.traj.velocity;
00600
00601 case 31:
00602 return statMsg->motion.traj.acceleration;
00603
00604 case 32:
00605 return statMsg->motion.traj.maxVelocity;
00606
00607 case 33:
00608 return statMsg->motion.traj.maxAcceleration;
00609
00610 case 50:
00611 return statMsg->motion.axis[array_index].units;
00612 case 51:
00613 return statMsg->motion.axis[array_index].p;
00614 case 52:
00615 return statMsg->motion.axis[array_index].i;
00616 case 53:
00617 return statMsg->motion.axis[array_index].d;
00618 case 54:
00619 return statMsg->motion.axis[array_index].ff0;
00620 case 55:
00621 return statMsg->motion.axis[array_index].ff1;
00622 case 56:
00623 return statMsg->motion.axis[array_index].ff2;
00624 case 57:
00625 return statMsg->motion.axis[array_index].backlash;
00626 case 58:
00627 return statMsg->motion.axis[array_index].bias;
00628 case 59:
00629 return statMsg->motion.axis[array_index].maxError;
00630 case 60:
00631 return statMsg->motion.axis[array_index].deadband;
00632 case 61:
00633 return statMsg->motion.axis[array_index].cycleTime;
00634 case 62:
00635 return statMsg->motion.axis[array_index].inputScale;
00636 case 63:
00637 return statMsg->motion.axis[array_index].inputOffset;
00638 case 64:
00639 return statMsg->motion.axis[array_index].outputScale;
00640 case 65:
00641 return statMsg->motion.axis[array_index].outputOffset;
00642 case 66:
00643 return statMsg->motion.axis[array_index].minPositionLimit;
00644 case 67:
00645 return statMsg->motion.axis[array_index].maxPositionLimit;
00646 case 68:
00647 return statMsg->motion.axis[array_index].minOutputLimit;
00648 case 69:
00649 return statMsg->motion.axis[array_index].maxOutputLimit;
00650 case 70:
00651 return statMsg->motion.axis[array_index].maxFerror;
00652 case 71:
00653 return statMsg->motion.axis[array_index].minFerror;
00654 case 72:
00655 return statMsg->motion.axis[array_index].homingVel;
00656 case 73:
00657 return statMsg->motion.axis[array_index].homeOffset;
00658 case 74:
00659 return statMsg->motion.axis[array_index].enablePolarity;
00660 case 75:
00661 return statMsg->motion.axis[array_index].minLimitSwitchPolarity;
00662 case 76:
00663 return statMsg->motion.axis[array_index].maxLimitSwitchPolarity;
00664 case 77:
00665 return statMsg->motion.axis[array_index].homeSwitchPolarity;
00666 case 78:
00667 return statMsg->motion.axis[array_index].homingPolarity;
00668 case 79:
00669 return statMsg->motion.axis[array_index].faultPolarity;
00670 case 80:
00671 return statMsg->motion.axis[array_index].setpoint;
00672 case 81:
00673 return statMsg->motion.axis[array_index].ferrorCurrent;
00674 case 82:
00675 return statMsg->motion.axis[array_index].output;
00676 case 83:
00677 return statMsg->motion.axis[array_index].input;
00678 case 84:
00679 return statMsg->motion.axis[array_index].inpos;
00680 case 85:
00681 return statMsg->motion.axis[array_index].homing;
00682 case 86:
00683 return statMsg->motion.axis[array_index].homed;
00684 case 87:
00685 return statMsg->motion.axis[array_index].fault;
00686 case 88:
00687 return statMsg->motion.axis[array_index].enabled;
00688 case 89:
00689 return statMsg->motion.axis[array_index].minSoftLimit;
00690 case 90:
00691 return statMsg->motion.axis[array_index].maxSoftLimit;
00692 case 91:
00693 return statMsg->motion.axis[array_index].minHardLimit;
00694 case 92:
00695 return statMsg->motion.axis[array_index].maxHardLimit;
00696 case 93:
00697 return statMsg->motion.axis[array_index].overrideLimits;
00698 case 94:
00699 return statMsg->motion.axis[array_index].scale;
00700 case 95:
00701 return statMsg->motion.axis[array_index].ferrorHighMark;
00702
00703
00704 case 300:
00705 return statMsg->io.heartbeat;
00706 case 301:
00707 return statMsg->io.cycleTime;
00708
00709 case 310:
00710 return statMsg->io.tool.toolPrepped;
00711 case 311:
00712 return statMsg->io.tool.toolInSpindle;
00713
00714 case 320:
00715 return statMsg->io.spindle.speed;
00716 case 321:
00717 return statMsg->io.spindle.direction;
00718 case 322:
00719 return statMsg->io.spindle.brake;
00720 case 323:
00721 return statMsg->io.spindle.increasing;
00722 case 324:
00723 return statMsg->io.spindle.enabled;
00724
00725 case 330:
00726 return statMsg->io.coolant.mist;
00727 case 331:
00728 return statMsg->io.coolant.flood;
00729
00730 case 340:
00731 return statMsg->io.lube.on;
00732 case 341:
00733 return statMsg->io.lube.level;
00734
00735 case 350:
00736 return statMsg->io.aux.estop;
00737 case 351:
00738 return statMsg->io.aux.estopIn;
00739 case 352:
00740 return statMsg->io.aux.dout[array_index];
00741 case 353:
00742 return statMsg->io.aux.din[array_index];
00743 case 354:
00744 return statMsg->io.aux.aout[array_index];
00745 case 355:
00746 return statMsg->io.aux.ain[array_index];
00747
00748 default:
00749 return 0.0;
00750 }
00751 }
00752
00753 int get_tool_varnum(const char *str);
00754 int get_spindle_varnum(const char *str);
00755 int get_coolant_varnum(const char *str);
00756 int get_lube_varnum(const char *str);
00757 int get_aux_varnum(const char *str, int *array_index_ptr);
00758
00759 int get_io_varnum(const char *str, int *array_index_ptr)
00760 {
00761 if(!strcmp("heartbeat",str))
00762 {
00763 return 300;
00764 }
00765 else if(!strcmp("cycleTime",str))
00766 {
00767 return 301;
00768 }
00769 else if(!strncmp("tool.",str,5))
00770 {
00771 return get_tool_varnum(str+5);
00772 }
00773 else if(!strncmp("spindle.",str,8))
00774 {
00775 return get_spindle_varnum(str+8);
00776 }
00777 else if(!strncmp("coolant.",str,8))
00778 {
00779 return get_coolant_varnum(str+8);
00780 }
00781 else if(!strncmp("aux.",str,4))
00782 {
00783 return get_aux_varnum(str+4, array_index_ptr);
00784 }
00785 else if(!strncmp("lube.",str,5))
00786 {
00787 return get_lube_varnum(str+5);
00788 }
00789 return -1;
00790 }
00791
00792 int get_tool_varnum(const char *str)
00793 {
00794 if(!strcmp("toolPrepped",str))
00795 {
00796 return 310;
00797 }
00798 else if(!strcmp("toolInSpindle",str))
00799 {
00800 return 311;
00801 }
00802 return -1;
00803 }
00804
00805 int get_spindle_varnum(const char *str)
00806 {
00807 if(!strcmp("speed",str))
00808 {
00809 return 320;
00810 }
00811 else if(!strcmp("direction",str))
00812 {
00813 return 321;
00814 }
00815 else if(!strcmp("brake",str))
00816 {
00817 return 322;
00818 }
00819 else if(!strcmp("increasing",str))
00820 {
00821 return 323;
00822 }
00823 else if(!strcmp("enabled",str))
00824 {
00825 return 324;
00826 }
00827 return -1;
00828 }
00829
00830 int get_coolant_varnum(const char *str)
00831 {
00832 if(!strcmp("mist",str))
00833 {
00834 return 330;
00835 }
00836 else if(!strcmp("flood",str))
00837 {
00838 return 331;
00839 }
00840 return -1;
00841 }
00842
00843 int get_lube_varnum(const char *str)
00844 {
00845 if(!strcmp("on",str))
00846 {
00847 return 340;
00848 }
00849 else if(!strcmp("level",str))
00850 {
00851 return 341;
00852 }
00853 return -1;
00854 }
00855
00856 int get_aux_varnum(const char *str, int *array_index_ptr)
00857 {
00858
00859 if(!strcmp("estop",str))
00860 {
00861 return 350;
00862 }
00863 else if(!strcmp("estopIn",str))
00864 {
00865 return 351;
00866 }
00867 else if(!strncmp("dout[",str,5))
00868 {
00869 char *end_index;
00870 int i = strtol(str+5,&end_index,0);
00871 if(i >= 0 && i < EMC_AUX_MAX_DOUT && NULL != array_index_ptr)
00872 {
00873 *array_index_ptr = i;
00874 return 352;
00875 }
00876 else
00877 {
00878 return -1;
00879 }
00880 }
00881 else if(!strncmp("din[",str,4))
00882 {
00883 char *end_index;
00884 int i = strtol(str+4,&end_index,0);
00885 if(i >= 0 && i < EMC_AUX_MAX_DIN && NULL != array_index_ptr)
00886 {
00887 *array_index_ptr = i;
00888 return 353;
00889 }
00890 else
00891 {
00892 return -1;
00893 }
00894 }
00895 else if(!strncmp("aout[",str,5))
00896 {
00897 char *end_index;
00898 int i = strtol(str+5,&end_index,0);
00899 if(i >= 0 && i < EMC_AUX_MAX_AOUT && NULL != array_index_ptr)
00900 {
00901 *array_index_ptr = i;
00902 return 354;
00903 }
00904 else
00905 {
00906 return -1;
00907 }
00908 }
00909 else if(!strncmp("ain[",str,4))
00910 {
00911 char *end_index;
00912 int i = strtol(str+4,&end_index,0);
00913 if(i >= 0 && i < EMC_AUX_MAX_AIN && NULL != array_index_ptr)
00914 {
00915 *array_index_ptr = i;
00916 return 355;
00917 }
00918 else
00919 {
00920 return -1;
00921 }
00922 }
00923 return -1;
00924 }