hal_stg.c
1 /******************************************************************** 2 * Description: hal_stg.c 3 * This is the driver for Servo-To-Go Model I & II board. 4 * 5 * Author: Alex Joni 6 * License: GPL Version 2 7 * 8 * Copyright (c) 2004 All rights reserved. 9 * see below for aditional notes 10 * 11 * Last change: 12 ********************************************************************/ 13 14 /** This is the driver for Servo-To-Go Model I & II board. 15 The board includes 8 channels of quadrature encoder input, 16 8 channels of analog input and output, 32 bits digital I/O, 17 and an interval timer with interrupt. 18 19 Installation of the driver only realtime: 20 21 insmod hal_stg num_chan=8 dio="IIOO" 22 - autodetects the address 23 or 24 25 insmod hal_stg base=0x200 num_chan=8 dio="IIOO" 26 27 Check your Hardware manual for your base address. 28 29 The digital inputs/outputs configuration is determined by a 30 config string passed to insmod when loading the module. 31 The format consists by a four character string that sets the 32 direction of each group of pins. Each character of the direction 33 string is either "I" or "O". The first character sets the 34 direction of port A (Port A - DIO.0-7), the next sets 35 port B (Port B - DIO.8-15), the next sets port C (Port C - DIO.16-23), 36 and the fourth sets port D (Port D - DIO.24-31). 37 38 The following items are exported to the HAL. 39 40 Encoders: 41 Parameters: 42 float stg.<channel>.position-scale (counts per unit) 43 44 Pins: 45 s32 stg.<channel>.counts 46 float stg.<channel>.position 47 48 /todo bit stg.<channel>.index-enable 49 /todo bit stg.<channel>.enc-reset-count 50 51 Functions: 52 void stg.<channel>.capture_position 53 54 55 DACs: 56 Parameters: 57 float stg.<channel>.dac-offset 58 float stg.<channel>.dac-gain 59 60 Pins: 61 float stg.<channel>.dac-value 62 63 Functions: 64 void stg.<channel>.dac-write 65 66 67 ADC: 68 Parameters: 69 float stg.<channel>.adc-offset 70 float stg.<channel>.adc-gain 71 72 Pins: 73 float stg.<channel>.adc-value 74 75 Functions: 76 void stg.<channel>.adc-read 77 78 79 Digital In: 80 Pins: 81 bit stg.in-<pinnum> 82 bit stg.in-<pinnum>-not 83 84 Functions: 85 void stg.digital-in-read 86 87 88 Digital Out: 89 Parameters: 90 bit stg.out-<pinnum>-invert 91 92 Pins: 93 bit stg.out-<pinnum> 94 95 Functions: 96 void stg.digital-out-write 97 98 */ 99 100 /** Copyright (C) 2004 Alex Joni 101 <alex DOT joni AT robcon DOT ro> 102 */ 103 104 /** Copyright (C) 2003 John Kasunich 105 <jmkasunich AT users DOT sourceforge DOT net> 106 */ 107 108 /* Based on STGMEMBS.CPP from the Servo To Go Windows drivers 109 - Copyright (c) 1996 Servo To Go Inc and released under GPL Version 2 */ 110 /* Also relates to the EMC1 code (very similar to STGMEMBS.CPP) 111 work done by Fred Proctor, Will Shackleford */ 112 113 /** This program is free software; you can redistribute it and/or 114 modify it under the terms of version 2 of the GNU General 115 Public License as published by the Free Software Foundation. 116 This library is distributed in the hope that it will be useful, 117 but WITHOUT ANY WARRANTY; without even the implied warranty of 118 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 119 GNU General Public License for more details. 120 121 You should have received a copy of the GNU General Public 122 License along with this library; if not, write to the Free Software 123 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 124 125 THE AUTHORS OF THIS LIBRARY ACCEPT ABSOLUTELY NO LIABILITY FOR 126 ANY HARM OR LOSS RESULTING FROM ITS USE. IT IS _EXTREMELY_ UNWISE 127 TO RELY ON SOFTWARE ALONE FOR SAFETY. Any machinery capable of 128 harming persons must have provisions for completely removing power 129 from all motors, etc, before persons enter any danger area. All 130 machinery must be designed to comply with local and national safety 131 codes, and the authors of this software can not, and do not, take 132 any responsibility for such compliance. 133 134 This code was written as part of the EMC HAL project. For more 135 information, go to www.linuxcnc.org. 136 */ 137 138 #include <asm/io.h> 139 #include "rtapi.h" /* RTAPI realtime OS API */ 140 #include "rtapi_app.h" /* RTAPI realtime module decls */ 141 #include "hal.h" /* HAL public API decls */ 142 #include "hal_stg.h" /* STG related defines */ 143 144 /* module information */ 145 MODULE_AUTHOR("Alex Joni"); 146 MODULE_DESCRIPTION("Driver for Servo-to-Go Model I & II for EMC HAL"); 147 MODULE_LICENSE("GPL"); 148 static int base = 0x00; /* board base address, 0 means autodetect */ 149 RTAPI_MP_INT(base, "board base address, don't use for autodetect"); 150 static int model = 0; /* board model, leave empty and autodetect */ 151 RTAPI_MP_INT(model, "board model, use with caution. it overrides the detected model"); 152 static int num_chan = MAX_CHANS; /* number of channels - default = 8 */ 153 RTAPI_MP_INT(num_chan, "number of channels"); 154 static char *dio = "IIOO"; /* dio config - default = port A&B inputs, port C&D outputs */ 155 RTAPI_MP_STRING(dio, "dio config string - expects something like IIOO"); 156 157 #define EPSILON 1e-20 158 159 /*********************************************************************** 160 * STRUCTURES AND GLOBAL VARIABLES * 161 ************************************************************************/ 162 163 typedef struct { 164 hal_bit_t *data; /* basic pin for input or output */ 165 union { 166 hal_bit_t *not; /* pin for inverted data (input only) */ 167 hal_bit_t invert; /* param for inversion (output only) */ 168 } io; 169 } io_pin; 170 171 typedef struct { 172 /* counter data */ 173 hal_s32_t *count[MAX_CHANS]; /* captured binary count value */ 174 hal_s32_t offset[MAX_CHANS]; /* offset to hold latched position from index pulse */ 175 hal_float_t *pos[MAX_CHANS]; /* scaled position (floating point) */ 176 hal_float_t pos_scale[MAX_CHANS]; /* parameter: scaling factor for pos */ 177 hal_bit_t *index_enable[MAX_CHANS]; /* pins for index homing */ 178 hal_bit_t *index_latch[MAX_CHANS]; /* value of the index latch for the axis */ 179 // hal_s32_t check_index[MAX_CHANS]; /* internal marker for two stage index pulse check */ 180 hal_bit_t *index_polarity[MAX_CHANS]; /* Polarity of index pulse */ 181 182 /* dac data */ 183 hal_float_t *dac_value[MAX_CHANS]; /* value to be written to dac */ 184 hal_float_t dac_offset[MAX_CHANS]; /* offset value for DAC */ 185 hal_float_t dac_gain[MAX_CHANS]; /* gain to be applied */ 186 187 /* adc data */ 188 hal_float_t *adc_value[MAX_CHANS]; /* value to be read from adc */ 189 hal_float_t adc_offset[MAX_CHANS]; /* offset value for ADC */ 190 hal_float_t adc_gain[MAX_CHANS]; /* gain to be applied */ 191 int adc_current_chan; /* holds the currently converting channel */ 192 193 /* dio data */ 194 io_pin port[4][8]; /* holds 4 ports each 8 pins, either input or output */ 195 unsigned char dir_bits; /* remembers config (which port is input which is output) */ 196 197 unsigned char model; 198 199 } stg_struct; 200 201 static stg_struct *stg_driver; 202 203 /* other globals */ 204 static int comp_id; /* component ID */ 205 static int outpinnum=0, inputpinnum=0; 206 //const int STG_MSG_LEVEL = RTAPI_MSG_ALL; 207 const int STG_MSG_LEVEL = RTAPI_MSG_INFO; 208 209 #define DATA(x) (base + (2 * x) - (x % 2)) /* Address of Data register 0 */ 210 #define CTRL(x) (base + (2 * (x+1)) - (x % 2)) /* Address of Control register 0 */ 211 212 /*********************************************************************** 213 * LOCAL FUNCTION DECLARATIONS * 214 ************************************************************************/ 215 /* helper functions, to export HAL pins & co. */ 216 static int export_counter(int num, stg_struct * addr); 217 static int export_dac(int num, stg_struct * addr); 218 static int export_adc(int num, stg_struct * addr); 219 static int export_pins(int num, int dir, stg_struct * addr); 220 static int export_input_pin(int pinnum, io_pin * pin); 221 static int export_output_pin(int pinnum, io_pin * pin); 222 223 /* Board specific functions */ 224 225 /* initializes the STG, takes care of autodetection, all initialisations */ 226 static int stg_init_card(void); 227 /* sets up interrupt to be used */ 228 static int stg_set_interrupt(short interrupt); 229 /* scans possible addresses for STG cards */ 230 static unsigned short stg_autodetect(void); 231 232 /* counter related functions */ 233 static int stg_counter_init(int ch); 234 static long stg_counter_read(int i); 235 static void stg_counter_latch(int i); 236 static void stg1_select_index_axis(void *arg, unsigned int chan); 237 static void stg1_reset_index_latch(void *arg, unsigned int chan); 238 static unsigned short stg1_get_index_pulse_latch(void *arg, unsigned int chan); 239 240 static void stg2_reset_all_index_latches( void *arg ); 241 static void stg2_select_index_axes( void *arg, unsigned char mask ); 242 static unsigned char stg2_get_all_index_pulse_latches( void *arg ); 243 244 /* dac related functions */ 245 static int stg_dac_init(int ch); 246 static int stg_dac_write(int ch, short value); 247 248 /* adc related functions */ 249 static int stg_adc_init(int ch); 250 static int stg_adc_start(void *arg, unsigned short wAxis); 251 static short stg_adc_read(void *arg, int ch); 252 253 /* dio related functions */ 254 static int stg_dio_init(void); 255 256 /* periodic functions registered to HAL */ 257 static void stg_adcs_read(void *arg, long period); //reads adc data from the board, check long description at the beginning of the function 258 static void stg_dacs_write(void *arg, long period); //writes dac's to the STG 259 static void stg_counter_capture(void *arg, long period); //captures encoder counters 260 static void stg_di_read(void *arg, long period); //reads digital inputs from the STG 261 static void stg_do_write(void *arg, long period); //writes digital outputs to the STG 262 //static void stg_debug_print( void *, long ); 263 264 /*********************************************************************** 265 * INIT AND EXIT CODE * 266 ************************************************************************/ 267 268 #define MAX_CHAN 8 269 270 int rtapi_app_main(void) 271 { 272 int n, retval, mask, m; 273 unsigned char dir_bits; 274 int msg; 275 276 msg = rtapi_get_msg_level(); 277 rtapi_set_msg_level( STG_MSG_LEVEL ); 278 279 /* test for number of channels */ 280 if ((num_chan <= 0) || (num_chan > MAX_CHAN)) { 281 rtapi_print_msg(RTAPI_MSG_ERR, 282 "STG: ERROR: invalid num_chan: %d\n", num_chan); 283 return -1; 284 } 285 286 /* test for config string */ 287 if ((dio == 0) || (dio[0] == '\0')) { 288 rtapi_print_msg(RTAPI_MSG_ERR, "STG: ERROR: no dio config string\n"); 289 return -1; 290 } 291 292 /* have good config info, connect to the HAL */ 293 comp_id = hal_init("hal_stg"); 294 if (comp_id < 0) { 295 rtapi_print_msg(RTAPI_MSG_ERR, "STG: ERROR: hal_init() failed\n"); 296 return -1; 297 } 298 299 /* allocate shared memory for stg data */ 300 stg_driver = hal_malloc(num_chan * sizeof(stg_struct)); 301 if (stg_driver == 0) { 302 rtapi_print_msg(RTAPI_MSG_ERR, "STG: ERROR: hal_malloc() failed\n"); 303 hal_exit(comp_id); 304 return -1; 305 } 306 307 /* takes care of all initialisations, also autodetection and model if necessary */ 308 if ((retval=stg_init_card()) != 0) { 309 rtapi_print_msg(RTAPI_MSG_ERR, 310 "STG: ERROR: stg_init_card() failed\n"); 311 hal_exit(comp_id); 312 return retval; 313 } 314 315 /* dio should be a string of 4 'I" or "O" characters */ 316 dir_bits = 0; 317 mask = 0x01; 318 for ( m = 0 ; m < 4 ; m++ ) { 319 /* test character and set/clear bit */ 320 if ((dio[m] == 'i') || (dio[m] == 'I')) { 321 /* input, set mask bit to zero */ 322 dir_bits &= ~mask; 323 } else if ((dio[m] == 'o') || (dio[m] == 'O')) { 324 /* output, set mask bit to one */ 325 dir_bits |= mask; 326 } else { 327 rtapi_print_msg(RTAPI_MSG_ERR, 328 "STG: ERROR: bad config info for port %d\n", m); 329 return -1; 330 } 331 /* shift mask for next bit */ 332 mask <<= 1; 333 } 334 335 /* we now should have directions figured out, next is exporting the pins based on that */ 336 mask = 0x01; 337 for ( m = 0 ; m < 4 ; m++ ) { 338 339 /* port, direction, driver */ 340 export_pins(m, (dir_bits & mask), stg_driver); 341 342 /* shift mask for next bit */ 343 mask <<= 1; 344 } 345 stg_driver->dir_bits = dir_bits; /* remember direction of each port, will be used in the write / read functions */ 346 347 stg_dio_init(); 348 349 /* export all the variables for each counter, dac */ 350 for (n = 0; n < num_chan; n++) { 351 /* export all vars */ 352 retval = export_counter(n, stg_driver); 353 if (retval != 0) { 354 rtapi_print_msg(RTAPI_MSG_ERR, 355 "STG: ERROR: counter %d var export failed\n", n + 1); 356 hal_exit(comp_id); 357 return -1; 358 } 359 /* init counter */ 360 *(stg_driver->count[n]) = 0; 361 stg_driver->offset[n] = 0; 362 *(stg_driver->pos[n]) = 0.0; 363 364 /* By default the index pulse is not processed/used */ 365 *(stg_driver->index_enable[n]) = 0; 366 367 /* Default polarity for the index pulse is active high */ 368 if( stg_driver->model == 1 ) 369 { 370 *(stg_driver->index_polarity[n]) = 1; 371 } 372 373 /* Default value for the index latch output is false */ 374 *(stg_driver->index_latch[n]) = 0; 375 376 stg_driver->pos_scale[n] = 1.0; 377 378 /* init counter chip */ 379 stg_counter_init(n); 380 381 retval = export_dac(n, stg_driver); 382 if (retval != 0) { 383 rtapi_print_msg(RTAPI_MSG_ERR, 384 "STG: ERROR: dac %d var export failed\n", n + 1); 385 hal_exit(comp_id); 386 return -1; 387 } 388 /* init counter */ 389 *(stg_driver->dac_value[n]) = 0; 390 stg_driver->dac_offset[n] = 0.0; 391 stg_driver->dac_gain[n] = 1.0; 392 393 /* init dac chip */ 394 stg_dac_init(n); 395 396 retval = export_adc(n, stg_driver); 397 if (retval != 0) { 398 rtapi_print_msg(RTAPI_MSG_ERR, 399 "STG: ERROR: adc %d var export failed\n", n + 1); 400 hal_exit(comp_id); 401 return -1; 402 } 403 /* init counter */ 404 *(stg_driver->adc_value[n]) = 0; 405 stg_driver->adc_offset[n] = 0.0; 406 stg_driver->adc_gain[n] = 1.0; 407 408 stg_driver->adc_current_chan = -1; /* notify that no conversion has been started yet */ 409 410 /* init adc chip */ 411 stg_adc_init(n); 412 } 413 414 /* export functions */ 415 retval = hal_export_funct("stg.capture-position", stg_counter_capture, 416 stg_driver, 1, 0, comp_id); 417 if (retval != 0) { 418 rtapi_print_msg(RTAPI_MSG_ERR, 419 "STG: ERROR: stg.counter-capture funct export failed\n"); 420 hal_exit(comp_id); 421 return -1; 422 } 423 rtapi_print_msg(RTAPI_MSG_INFO, 424 "STG: installed %d encoder counters\n", num_chan); 425 426 retval = hal_export_funct("stg.write-dacs", stg_dacs_write, 427 stg_driver, 1, 0, comp_id); 428 if (retval != 0) { 429 rtapi_print_msg(RTAPI_MSG_ERR, 430 "STG: ERROR: stg.write-dacs funct export failed\n"); 431 hal_exit(comp_id); 432 return -1; 433 } 434 rtapi_print_msg(RTAPI_MSG_INFO, 435 "STG: installed %d dacs\n", num_chan); 436 437 retval = hal_export_funct("stg.read-adcs", stg_adcs_read, 438 stg_driver, 1, 0, comp_id); 439 if (retval != 0) { 440 rtapi_print_msg(RTAPI_MSG_ERR, 441 "STG: ERROR: stg.read-adcs funct export failed\n"); 442 hal_exit(comp_id); 443 return -1; 444 } 445 rtapi_print_msg(RTAPI_MSG_INFO, 446 "STG: installed %d adcs\n", num_chan); 447 448 retval = hal_export_funct("stg.di-read", stg_di_read, 449 stg_driver, 0, 0, comp_id); 450 if (retval != 0) { 451 rtapi_print_msg(RTAPI_MSG_ERR, 452 "STG: ERROR: stg.di-read funct export failed\n"); 453 hal_exit(comp_id); 454 return -1; 455 } 456 457 rtapi_print_msg(RTAPI_MSG_INFO, 458 "STG: installed %d digital inputs\n", inputpinnum); 459 460 retval = hal_export_funct("stg.do-write", stg_do_write, 461 stg_driver, 0, 0, comp_id); 462 if (retval != 0) { 463 rtapi_print_msg(RTAPI_MSG_ERR, 464 "STG: ERROR: stg.do-write funct export failed\n"); 465 hal_exit(comp_id); 466 return -1; 467 } 468 rtapi_print_msg(RTAPI_MSG_INFO, 469 "STG: installed %d digital outputs\n", outpinnum); 470 471 /* 472 retval = hal_export_funct("stg.debug_print", stg_debug_print, stg_driver, 0, 0, comp_id); 473 if (retval != 0) 474 { 475 rtapi_print_msg(RTAPI_MSG_ERR, 476 "STG: ERROR: stg.debug_print funct export failed\n"); 477 hal_exit(comp_id); 478 return -1; 479 } 480 rtapi_print_msg(RTAPI_MSG_INFO, 481 "STG: installed periodic debug print\n"); 482 */ 483 hal_ready(comp_id); 484 485 /* restore saved message level */ 486 rtapi_set_msg_level(msg); 487 488 return 0; 489 } 490 491 void rtapi_app_exit(void) 492 { 493 hal_exit(comp_id); 494 // FIXME, check for return code ? 495 return; 496 } 497 498 /*********************************************************************** 499 * REALTIME ENCODER COUNTING AND UPDATE FUNCTIONS * 500 ************************************************************************/ 501 502 static void stg_counter_capture(void *arg, long period) 503 { 504 stg_struct *stg = arg; 505 int n; 506 // int msg; 507 unsigned char mask; 508 unsigned char index_pulse_latches; 509 510 if( stg->model == 1 ) 511 { 512 /* 513 * STG Model 1, stg1 514 */ 515 for( n = 0; n < num_chan; n++ ) 516 { 517 /* reset and then select current axes pair to be reset by index 518 * Because they index polarity is configurable for the stg2 card be select 519 * even for the odd axes */ 520 stg1_select_index_axis(arg, n); 521 522 if (stg1_get_index_pulse_latch(arg, n)) 523 { 524 *(stg->index_latch[n]) = 1; 525 526 if ( *(stg->index_enable[n]) == 1 ) 527 { 528 // read the value without latching, latching was done on index 529 // remember this as an offset, it will be substracted from nominal 530 stg->offset[n] = stg_counter_read(n); 531 /* set index-enable false, so outside knows we found the index, and reset the position */ 532 *(stg->index_enable[n]) = 0; 533 534 /* 535 msg = rtapi_get_msg_level(); 536 rtapi_set_msg_level( STG_MSG_LEVEL ); 537 rtapi_print_msg(RTAPI_MSG_DBG, "STG: Index pulse detected on channel %1d\n", n ); 538 rtapi_set_msg_level(msg); 539 */ 540 } else { 541 /* NOP, no action needed, since the selection of an index pair is just valid until the next 542 * pair is selected */ 543 } 544 } else { 545 *(stg->index_latch[n]) = 0; 546 } 547 548 } 549 } else if( stg->model == 2 ) 550 { 551 /* 552 * STG Model 2, stg2 553 */ 554 555 // Set IDLEN 556 for( mask = 0, n = 0; n < num_chan; n++ ) 557 { 558 if( *(stg->index_enable[n]) == 1 ) 559 { 560 mask |= ( 1<<n ); 561 } 562 } 563 stg2_select_index_axes( arg, mask ); 564 565 // Read all latches 566 index_pulse_latches = stg2_get_all_index_pulse_latches( arg ); 567 568 // set or reset index_latch 569 // index-enable reset if needed 570 for( n = 0; n < num_chan; n++ ) 571 { 572 if( index_pulse_latches & (1<<n) ) 573 { 574 *(stg->index_latch[n]) = 1; 575 576 if ( *(stg->index_enable[n]) == 1 ) 577 { 578 // read the value without latching, latching was done on index 579 // remember this as an offset, it will be substracted from nominal 580 stg->offset[n] = stg_counter_read(n); 581 /* set index-enable false, so outside knows we found the index, and reset the position */ 582 *(stg->index_enable[n]) = 0; 583 584 /* 585 msg = rtapi_get_msg_level(); 586 rtapi_set_msg_level( STG_MSG_LEVEL ); 587 rtapi_print_msg(RTAPI_MSG_DBG, "STG: Index pulse detected on channel %1d\n", n ); 588 rtapi_set_msg_level(msg); 589 */ 590 } else { 591 /* NOP, no action needed, since all index latches will be clearer for the next iteration anyway */ 592 } 593 } else { 594 *(stg->index_latch[n]) = 0; 595 } 596 } 597 // Reset all latches 598 stg2_reset_all_index_latches( arg ); 599 600 } else { 601 // NOP, only models stg1 and stg2, thus should never be reached */ 602 } 603 604 for (n = 0; n < num_chan; n++) 605 { 606 /* capture raw counts to latches */ 607 stg_counter_latch(n); 608 /* read raw count, and substract the offset (determined by indexed homing) */ 609 *(stg->count[n]) = stg_counter_read(n) - stg->offset[n]; 610 /* make sure scale isn't zero or tiny to avoid divide error */ 611 if (stg->pos_scale[n] < 0.0) { 612 if (stg->pos_scale[n] > -EPSILON) 613 stg->pos_scale[n] = -1.0; 614 } else { 615 if (stg->pos_scale[n] < EPSILON) 616 stg->pos_scale[n] = 1.0; 617 } 618 /* scale count to make floating point position */ 619 *(stg->pos[n]) = *(stg->count[n]) / stg->pos_scale[n]; 620 } 621 /* done */ 622 return; 623 } 624 625 /* stg_debug_print 626 * run this function from a very slow, 627 * e.g. 1sec thread and it will give some information 628 */ 629 /* 630 static void stg_debug_print( void *arg, long period ) 631 { 632 stg_struct *stg=arg; 633 int msg; 634 static int counter; 635 636 // model 2 encoder index registers 637 unsigned char idlen_reg; 638 unsigned char seldi_reg; 639 640 //model 1 encoder index registers 641 unsigned char intc_reg; 642 643 msg = rtapi_get_msg_level(); 644 rtapi_set_msg_level( STG_MSG_LEVEL ); 645 646 if( stg->model == 1 ) 647 { 648 intc_reg = inb(base + INTC); 649 rtapi_print_msg(RTAPI_MSG_DBG, "STG: %04d: IXS1 is %s\n", counter, ( intc_reg & IXS1 ) ? "TRUE" : "FALSE" ); 650 rtapi_print_msg(RTAPI_MSG_DBG, "STG: %04d: IXS0 is %s\n", counter, ( intc_reg & IXS0 ) ? "TRUE" : "FALSE" ); 651 rtapi_print_msg(RTAPI_MSG_DBG, "STG: %04d: IXLVL is active %s\n", counter, ( intc_reg & IXLVL ) ? "TRUE" : "FALSE" ); 652 653 } else if (stg->model == 2 ) 654 { 655 idlen_reg = inb( base + IDLEN ); 656 seldi_reg = inb( base + SELDI ); 657 658 rtapi_print_msg(RTAPI_MSG_DBG, "STG: %04d: IDLEN is 0x%02x\n", counter, idlen_reg ); 659 rtapi_print_msg(RTAPI_MSG_DBG, "STG: %04d: SELDI is 0x%02x\n", counter, seldi_reg ); 660 661 } else { 662 // NOP, should never be reached 663 } 664 665 // restore saved message level 666 rtapi_set_msg_level(msg); 667 668 counter++; 669 670 return; 671 } 672 */ 673 674 /* stg_dacs_write() - writes all dac's to the board 675 - calls stg_dac_write() */ 676 static void stg_dacs_write(void *arg, long period) 677 { 678 stg_struct *stg; 679 double volts; 680 short ncounts, i; 681 682 stg=arg; 683 for (i=0;i < num_chan; i++) { 684 /* scale the voltage to be written based on offset and gain */ 685 volts = (*(stg->dac_value[i]) - stg->dac_offset[i]) * stg->dac_gain[i]; 686 /* clamp the scaled voltage value to the -10V to 10V output range of the STG */ 687 if (volts < -10.0) 688 volts = -10.0; 689 if (volts > 10.0) 690 volts = 10.0; 691 /* compute the value for the DAC, the extra - in there is STG specific */ 692 ncounts = (short) ((((-10.0 - volts) * 0x1FFF) / 20.0) - 1 ); 693 /* write it to the card */ 694 stg_dac_write(i, ncounts); 695 } 696 return; 697 } 698 699 /* stg_adcs_read() - reads one adc at a time from the board to hal 700 - calls stg_adc_read() */ 701 702 /* long description : 703 704 Because the conversion takes a while (first mux to the right channel ~5usecs, 705 then start the conversion, and wait for it to finish ~16-20 usecs) for all the 706 8 channels, it would be too much to start the conversion, wait for the result 707 for all the 8 axes. 708 Instead a different approach is chosen: 709 - on the beginning of the function the conversion should already be done 710 - it gets read and sent to HAL 711 - the new channel gets mux'ed 712 - and at the end of the function the new conversion is started, so that the data 713 will be available at the next run. 714 This way 8 periods are needed to read 8 ADC's. It is possible to set the board 715 to do faster conversions (AZ bit on INTC off), but that would make it less 716 reliable (autozero takes care of temp. errors).*/ 717 /*! \todo STG_ADC_Improvement (if any user requests it). 718 Another improvement might be to let the user chose what channels he would like 719 for ADC (having only 2 channels might speed things up considerably). 720 */ 721 static void stg_adcs_read(void *arg, long period) 722 { 723 stg_struct *stg; 724 double volts; 725 short ncounts; 726 int i; 727 728 stg = arg; 729 i = stg->adc_current_chan; 730 if ((i >= 0) && (i < num_chan)) { 731 /* we should have the conversion done for adc_num_chan */ 732 ncounts = stg_adc_read(stg,i); 733 volts = ncounts * 10.0 / 4096; 734 *(stg->adc_value[i]) = volts * stg->adc_gain[i] - stg->adc_offset[i]; 735 } 736 /* if adc_num_chan < 0, it's the first time this routine runs 737 thus we don't have any ready data, we simply start the next conversion */ 738 if (stg->adc_current_chan++ >= num_chan) 739 stg->adc_current_chan=0; //increase the channel, and roll back to 0 after all chans are done 740 741 /* select the current channel with the mux, and start the conversion */ 742 stg_adc_start(stg,stg->adc_current_chan); 743 /* the next time this function runs, the result should be available */ 744 return; 745 } 746 747 748 // helper function to extract the data out of a char and place it into HAL data 749 // written by JMK 750 static void split_input(unsigned char data, io_pin *dest, int num) 751 { 752 int b; 753 unsigned char mask; 754 755 /* splits a byte into 'num' HAL pins (and their NOTs) */ 756 mask = 0x01; 757 for (b = 0 ; b < num ; b++ ) { 758 if ( data & mask ) { 759 /* input high, which means FALSE (active low) */ 760 *(dest->data) = 0; 761 *(dest->io.not) = 1; 762 } else { 763 /* input low, which means TRUE */ 764 *(dest->data) = 1; 765 *(dest->io.not) = 0; 766 } 767 mask <<= 1; 768 dest++; 769 } 770 return; 771 } 772 773 // helper function to extract the data out of HAL and place it into a char 774 // written by JMK 775 unsigned char build_output(io_pin *src, int num) 776 { 777 int b; 778 unsigned char data, mask; 779 780 data = 0x00; 781 mask = 0x01; 782 /* assemble output byte for data port from 'num' source variables */ 783 for (b = 0; b < num; b++) { 784 /* get the data, add to output byte */ 785 if ( *(src->data) ) { 786 if ( !(src->io.invert) ) { 787 data |= mask; 788 } 789 } else { 790 if ( (src->io.invert) ) { 791 data |= mask; 792 } 793 } 794 mask <<= 1; 795 src++; 796 } 797 return data; 798 } 799 800 801 static void stg_di_read(void *arg, long period) //reads digital inputs from the STG 802 { 803 stg_struct *stg; 804 unsigned char val; 805 stg=arg; 806 807 if ( (stg->dir_bits & 0x01) == 0) { // if port A is set as input, read the bits 808 if (stg->model == 1) 809 val = inb(base + DIO_A); 810 else 811 val = inb(base + PORT_A); 812 split_input(val, &(stg->port[0][0]), 8); 813 } 814 if ( (stg->dir_bits & 0x02) == 0) { // if port B is set as input, read the bits 815 if (stg->model == 1) 816 val = inb(base + DIO_B); 817 else 818 val = inb(base + PORT_B); 819 split_input(val, &(stg->port[1][0]), 8); 820 } 821 if ( (stg->dir_bits & 0x04) == 0) { // if port C is set as input, read the bits 822 if (stg->model == 1) 823 val = inb(base + DIO_C); 824 else 825 val = inb(base + PORT_C); 826 split_input(val, &(stg->port[2][0]), 8); 827 } 828 if ( (stg->dir_bits & 0x08) == 0) { // if port D is set as input, read the bits 829 if (stg->model == 1) 830 val = inb(base + DIO_D); 831 else 832 val = inb(base + PORT_D); 833 split_input(val, &(stg->port[3][0]), 8); 834 } 835 } 836 837 static void stg_do_write(void *arg, long period) //writes digital outputs to the STG 838 { 839 stg_struct *stg; 840 unsigned char val; 841 stg=arg; 842 843 if ( (stg->dir_bits & 0x01) != 0) { // if port A is set as output, write the bits 844 val = build_output(&(stg->port[0][0]), 8); 845 if (stg->model == 1) 846 outb(val, base + DIO_A); 847 else 848 outb(val, base + PORT_A); 849 } 850 if ( (stg->dir_bits & 0x02) != 0) { // if port B is set as output, write the bits 851 val = build_output(&(stg->port[1][0]), 8); 852 if (stg->model == 1) 853 outb(val, base + DIO_B); 854 else 855 outb(val, base + PORT_B); 856 } 857 if ( (stg->dir_bits & 0x04) != 0) { // if port C is set as output, write the bits 858 val = build_output(&(stg->port[2][0]), 8); 859 if (stg->model == 1) 860 outb(val, base + DIO_C); 861 else 862 outb(val, base + PORT_C); 863 } 864 if ( (stg->dir_bits & 0x08) != 0) { // if port D is set as output, write the bits 865 val = build_output(&(stg->port[3][0]), 8); 866 if (stg->model == 1) 867 outb(val, base + DIO_D); 868 else 869 outb(val, base + PORT_D); 870 } 871 } 872 873 /*********************************************************************** 874 * BOARD SPECIFIC FUNCTIONS * 875 * execute board related things (write/read to/from the stg) * 876 ************************************************************************/ 877 878 879 /*********************************************************************** 880 * INIT FUNCTIONS * 881 ************************************************************************/ 882 883 /* 884 stg_counter_init() - Initializes the channel 885 886 works the same for both cards (STG & STG2) 887 */ 888 static int stg_counter_init(int ch) 889 { 890 /* Set Counter Command Register - Master Control, Master Reset (MRST), */ 891 /* and Reset address pointer (RADR). */ 892 outb(0x23, CTRL(ch)); 893 894 /* Set Counter Command Register - Input Control, OL Load (P3), */ 895 /* and Enable Inputs A and B (INA/B). */ 896 outb(0x68, CTRL(ch)); 897 898 /* Set Counter Command Register - Output Control */ 899 outb(0x80, CTRL(ch)); 900 901 /* Set Counter Command Register - Quadrature */ 902 outb(0xC3, CTRL(ch)); 903 return 0; 904 } 905 906 /* 907 stg_dac_init() - Initializes the dac channel 908 909 works the same for both cards (STG & STG2) 910 */ 911 static int stg_dac_init(int ch) 912 { 913 int i; 914 915 /* set all DAC's to 0 on startup */ 916 for (i=0; i < num_chan; i++) { 917 stg_dac_write(i, 0x1000); //by Xuecheng, 0x1000 coresponds to 0V 918 } 919 return 0; 920 } 921 922 923 /* 924 stg_adc_init() - Initializes the adc channel 925 */ 926 static int stg_adc_init(int ch) 927 { 928 929 /* not much to setup for the ADC's */ 930 /* only select the mode of operation we will work with AutoZero */ 931 if (stg_driver->model == 1) 932 outb(0x0f, base + MIO_2); // the second 82C55 is already configured (by running stg_dio_init) 933 // we only set bit 8 (AZ) to 1 to enable it 934 return 0; 935 } 936 937 938 /* 939 stg_dio_init() - Initializes the dio's 940 */ 941 static int stg_dio_init(void) 942 { 943 /* we will select the directions of each port */ 944 unsigned char control, tempINTC, tempIMR, tempCtrl0, tempCtrl1; 945 946 control = 0x80; //set up |1|0|0|A|CH|0|B|CL| 947 if ( (stg_driver->dir_bits & 0x01) == 0) // if port A is set as input, set bit accordingly 948 control |= 0x10; 949 if ( (stg_driver->dir_bits & 0x02) == 0) // if port B is set as input, set bit accordingly 950 control |= 0x02; 951 if ( (stg_driver->dir_bits & 0x04) == 0) // if port C is set as input, set bits accordingly 952 control |= 0x09; 953 954 if (stg_driver->model == 1) { 955 // write the computed control to MIO_1 956 outb(control, base+MIO_1); 957 } else { //model STG2 958 // write port A,B,C direction to ABC_DIR 959 outb(control, base+ABC_DIR); 960 } 961 962 tempINTC = inb(base + INTC); 963 964 if (stg_driver->model == 1) { 965 // next compute the directions for port D, located on the second 82C55 966 control = 0x82; 967 968 if ( (stg_driver->dir_bits & 0x08) == 0)// if port D is set as input, set bits accordingly 969 control = 0x92; 970 971 tempIMR = inb(base + IMR); // get the current interrupt mask 972 973 outb(0xff, base + OCW1); //mask off all interrupts 974 975 // write the computed control to MIO_2 976 outb(control, base+MIO_2); 977 978 outb(tempINTC, base + INTC); //restore interrupt control reg. 979 980 outb(tempIMR, base+ OCW1); //restore int mask 981 982 } else { //model STG2 983 984 // save contents of CNTRL0, it will get reinitialized 985 tempCtrl0 = inb(base+CNTRL0); 986 tempCtrl1 = inb(base+CNTRL1); 987 988 // CNTRL0 output, BRDTST input, D output 989 control = 0x82; 990 991 if ( (stg_driver->dir_bits & 0x08) == 0)// if port D is set as input, set bits accordingly 992 control = 0x8b; 993 994 outb(0xff, base + CNTRL1); // disable interrupts 995 996 outb(control, base + D_DIR); // set port D direction, also resets CNTRL0 997 998 outb(tempCtrl0, base + CNTRL0); 999 outb( (tempCtrl1 & 0x0f) | 0xf0, base + CNTRL1); 1000 } 1001 1002 return 0; 1003 } 1004 1005 1006 /*********************************************************************** 1007 * ACTION FUNCTIONS * 1008 * these do the actual data exchange with the board * 1009 ************************************************************************/ 1010 1011 static void stg_counter_latch(int i) 1012 { 1013 outb(0x03, CTRL(i)); 1014 } 1015 1016 1017 /* 1018 stg_counter_read() - reads one channel 1019 FIXME - todo, extend to 32 bits in software 1020 1021 works the same for both cards (STG & STG2) 1022 */ 1023 static long stg_counter_read(int i) 1024 { 1025 union pos_tag { 1026 long l; 1027 struct byte_tag { 1028 char b0; 1029 char b1; 1030 char b2; 1031 char b3; 1032 } byte; 1033 } pos; 1034 1035 pos.byte.b0 = inb(DATA(i)); 1036 pos.byte.b1 = inb(DATA(i)); 1037 pos.byte.b2 = inb(DATA(i)); 1038 if (pos.byte.b2 < 0) { 1039 pos.byte.b3 = -1; 1040 } else { 1041 pos.byte.b3 = 0; 1042 } 1043 return pos.l; 1044 } 1045 1046 static void stg1_select_index_axis(void *arg, unsigned int channel) 1047 { 1048 stg_struct *stg = arg; 1049 unsigned char byIntc,byAxis; 1050 unsigned char byPol = 1; 1051 1052 if (stg->model == 1) 1053 { 1054 /* 1055 * Set polarity to low active if that is requested 1056 */ 1057 if( *(stg->index_polarity[channel]) == 0 ) 1058 { 1059 byPol = 0; 1060 } 1061 1062 /* Stg manual p. 21: "The bits are level triggered and cannot be reset if they are active" 1063 * So it is save to reset them and only those which are really active will remain */ 1064 stg1_reset_index_latch(arg, channel); 1065 1066 // routine for Model 1 1067 // initialize stuff to poll index pulse 1068 byAxis = channel; 1069 1070 byAxis &= 0x6; // ignore low bit, we check 2 axes at a time 1071 byAxis <<= 3; // shift into position for IXS1, IXS0 1072 byIntc = inb(base + INTC); // get a copy of INTC, we'll change 1073 // some bits in it, not all 1074 byIntc &= ~(IXLVL | IXS1 | IXS0); // zero bits for axis and polarity 1075 byIntc |= byAxis; // put axes address in INTC 1076 if (byPol != 0) // is index pulse active high? 1077 byIntc |= IXLVL; 1078 outb(byIntc, base + INTC); 1079 } 1080 } 1081 1082 static void stg2_select_index_axes( void *arg, unsigned char mask ) 1083 { 1084 /* stg2 manual, p.21 1085 * writing 0 to the corresponding bit disables the index pulse 1086 * writing 1 enables it 1087 */ 1088 outb( mask, base + IDLEN ); 1089 return; 1090 } 1091 1092 /* Note that for stg1 cards this function will clear the index pulse latches for the two selected axes, 1093 */ 1094 static void stg1_reset_index_latch(void *arg, unsigned int channel) 1095 { 1096 stg_struct *stg = arg; 1097 1098 if (stg->model == 1) 1099 { // routine for Model 1 1100 inb(base + ODDRST); //reset index pulse latch for ODD axis 1101 inb(base + BRDTST); //reset index pulse latch for EVEN axis 1102 } 1103 return; 1104 } 1105 1106 static void stg2_reset_all_index_latches( void *arg ) 1107 { 1108 stg_struct *stg = arg; 1109 if( stg->model == 2 ) 1110 { 1111 /* 1112 * stg2 manual p.22, 1113 * writing 0 to IDL resets the index latch, 1114 * writing 1 has no effect 1115 */ 1116 outb( 0x00, base + IDL); 1117 } 1118 return; 1119 } 1120 1121 unsigned char stg1_get_current_IRR(void) 1122 { 1123 outb(base + OCW3, 0x0a); // IRR on next read 1124 return inb(base + IRR); 1125 } 1126 1127 static unsigned short stg1_get_index_pulse_latch(void *arg, unsigned int chan) 1128 { 1129 // routine for Model 1 board 1130 1131 stg_struct *stg = arg; 1132 unsigned char byIRR, byAxisMask; 1133 1134 if (stg->model == 1){ // routine for Model 1 1135 byIRR = stg1_get_current_IRR(); 1136 byAxisMask = (chan & 1) ? LIXODD : LIXEVN; // even or odd axis? 1137 if (byIRR & byAxisMask) // check latched index pulse 1138 return 1; 1139 return 0; 1140 } else if (stg->model == 2) { 1141 //FIXME: return if latched index pulse is there 1142 return 0; 1143 } 1144 return 0; 1145 } 1146 1147 static unsigned char stg2_get_all_index_pulse_latches( void *arg ) 1148 { 1149 stg_struct *stg = arg; 1150 unsigned char indexRegister = 0; 1151 1152 if( stg-> model == 2 ) 1153 indexRegister = inb( base + IDL ); 1154 return indexRegister; 1155 } 1156 1157 1158 /* 1159 stg_dac_write() - writes a dac channel 1160 1161 works the same for both cards (STG & STG2) 1162 */ 1163 static int stg_dac_write(int ch, short value) 1164 { 1165 /* write the DAC */ 1166 outw(value, base + DAC_0 + (ch << 1)); 1167 1168 return 0; 1169 } 1170 1171 1172 1173 int stg_adc_start(void *arg, unsigned short wAxis) 1174 { 1175 stg_struct *stg; 1176 unsigned char tempCtrl0; 1177 1178 stg = arg; 1179 1180 if (stg->model == 1) { 1181 /* do a dummy read from the ADC, just to set the input multiplexer to 1182 the right channel */ 1183 inw(base + ADC_0 + (wAxis << 1)); 1184 1185 /* wait 4 uS for settling time on the multiplexer and ADC. You probably 1186 shouldn't really have a delay in a driver */ 1187 outb(0, 0x80); 1188 outb(0, 0x80); 1189 outb(0, 0x80); 1190 outb(0, 0x80); 1191 1192 /* now start conversion */ 1193 outw(0, base + ADC_0 + (wAxis << 1)); 1194 } else { //model STG2 1195 1196 tempCtrl0 = inb(base+CNTRL0) & 0x07; // save IRQ 1197 tempCtrl0 |= (wAxis << 4) | 0x88; //autozero & cal cycle 1198 outb(tempCtrl0, base + CNTRL0); // select channel 1199 1200 /* wait 4 uS for settling time on the multiplexer and ADC. You probably 1201 shouldn't really have a delay in a driver */ 1202 outb(0, 0x80); 1203 outb(0, 0x80); 1204 outb(0, 0x80); 1205 outb(0, 0x80); 1206 1207 /* now start conversion */ 1208 outw(0, base + ADC_0); 1209 } 1210 return 0; 1211 }; 1212 1213 static short stg_adc_read(void *arg, int axis) 1214 { 1215 short j; 1216 stg_struct *stg; 1217 1218 stg = arg; 1219 1220 /* 1221 there must have been a delay between stg_adc_start() and 1222 stg_adc_read(), of 19 usec if autozeroing (we are), 4 usecs 1223 otherwise. In code that calls this, make sure you split these 1224 calls up with some intervening code 1225 */ 1226 1227 1228 if (stg->model == 1) { 1229 1230 /* make sure conversion is done, assume polling delay is done. 1231 EOC (End Of Conversion) is bit 0x08 in IIR (Interrupt Request 1232 Register) of Interrupt Controller. Don't wait forever though 1233 bail out eventually. */ 1234 1235 for (j = 0; !(inb(base + IRR) & 0x08) && (j < 1000); j++); 1236 1237 j = inw(base + ADC_0 + (axis << 1)); 1238 1239 } else { //model 2 1240 1241 for (j = 0; (inb(base + BRDTST) & 0x08) && (j < 1000); j++); 1242 1243 j = inw(base + ADC_0 + (axis << 1)); 1244 1245 } 1246 1247 1248 if (j & 0x1000) /* is sign bit negative? */ 1249 j |= 0xf000; /* sign extend */ 1250 else 1251 j &= 0xfff; /* make sure high order bits are zero. */ 1252 1253 return j; 1254 }; 1255 1256 /*********************************************************************** 1257 * BOARD INIT FUNCTIONS * 1258 * functions for autodetec, irq init * 1259 ************************************************************************/ 1260 1261 static int stg_set_interrupt(short interrupt) 1262 { 1263 unsigned char tempINTC; 1264 1265 if (stg_driver->model == 1) 1266 tempINTC=0x80; 1267 else 1268 tempINTC=0x88;//also CAL low, don|t want ADC to calibrate 1269 1270 switch (interrupt) { 1271 case 3: break; 1272 case 5: tempINTC |= 4;break; 1273 case 7: tempINTC |= 2;break; 1274 case 9: tempINTC |= 6;break; 1275 case 10: tempINTC |= 5;break; 1276 case 11: tempINTC |= 7;break; 1277 case 12: tempINTC |= 3;break; 1278 case 15: tempINTC |= 1;break; 1279 default: tempINTC |= 4;break; 1280 } 1281 if (stg_driver->model == 1) 1282 outb(tempINTC, base + INTC); 1283 else 1284 outb(tempINTC, base + CNTRL0); 1285 1286 return 0; 1287 } 1288 1289 static int stg_init_card() 1290 { 1291 int msg; 1292 1293 msg = rtapi_get_msg_level(); 1294 rtapi_set_msg_level( STG_MSG_LEVEL ); 1295 1296 /* 1297 * If both stg card model and base address are set 1298 * then no autodetecting is necessary. 1299 * Else we need to autodetect 1300 */ 1301 if ( (model != 0) && (base != 0) ) 1302 { 1303 stg_driver->model = model; 1304 } else { 1305 base = stg_autodetect(); 1306 } 1307 1308 /* 1309 * Now check if the settings for a card a ok 1310 */ 1311 if ( (base == 0x00) || (stg_driver->model == 0) ) 1312 { 1313 rtapi_print_msg(RTAPI_MSG_ERR, "STG: ERROR: no stg1 or stg2 card could be initialised\n"); 1314 return -ENODEV; 1315 } 1316 1317 if (stg_driver->model == 1) { 1318 /* 1319 * STG1 1320 */ 1321 // initialize INTC as output 1322 outb(0x92, base + MIO_2); 1323 1324 stg_set_interrupt(5); // initialize it to smthg, we won't use it anyways 1325 1326 outb(0x1a, base + ICW1); // initialize the 82C59 as single chip (STG docs say so:) 1327 outb(0x00, base + ICW2); // ICW2 not used, must init it to 0 1328 outb(0xff, base + OCW1); // mask off all interrupts 1329 rtapi_print_msg(RTAPI_MSG_INFO, 1330 "STG: Initialised stg%1d card at address %x\n", stg_driver->model, base); 1331 } else if (stg_driver->model == 2 ) { 1332 /* 1333 * STG2 1334 */ 1335 outb(0x8b, base + D_DIR); // initialize CONTRL0 output, BRDTST input 1336 1337 /* stg2 manual, p.21 1338 * writing 0 to the corresponding bit disables the index pulse 1339 * writing 1 enables it 1340 */ 1341 outb( 0x00, base + IDLEN ); 1342 1343 /* stg2 manual, p.21 1344 * writing 0 to the corresponding bit selects the index pulse to latch the counter 1345 * writing 1 to the corresponding bit selects EXLATCH to latch the counter 1346 */ 1347 outb( 0x00, base + SELDI ); 1348 1349 stg_set_interrupt(5); // initialize it to something, we won't use it anyways 1350 rtapi_print_msg(RTAPI_MSG_INFO, 1351 "STG: Initialised stg%1d card at address %x\n", stg_driver->model, base); 1352 } else { 1353 rtapi_print_msg(RTAPI_MSG_ERR, "STG: ERROR: The model stg%1d is not correct\n", stg_driver->model ); 1354 return -ENODEV; 1355 } 1356 1357 /* restore saved message level */ 1358 rtapi_set_msg_level(msg); 1359 1360 // all ok 1361 return 0; 1362 } 1363 1364 /* scans possible addresses for STG cards */ 1365 unsigned short stg_autodetect() 1366 { 1367 1368 short i, j, k, ofs; 1369 unsigned short address; 1370 unsigned short retval = 0; 1371 int msg; 1372 1373 msg = rtapi_get_msg_level(); 1374 rtapi_set_msg_level( STG_MSG_LEVEL ); 1375 1376 /* search all possible addresses */ 1377 for (i = 15; i >= 0; i--) { 1378 address = i * 0x20 + 0x200; 1379 1380 /* does jumper = i? */ 1381 //if ((inb(address + BRDTST) & 0x0f) == i) { // by Xuecheng, not necessary 1382 k = 0; // var for getting the serial 1383 for (j = 0; j < 8; j++) { 1384 ofs = (inb(address + BRDTST) >> 4); 1385 1386 if (ofs & 8) { /* is SER set? */ 1387 ofs = ofs & 7; /* mask for Q2,Q1,Q0 */ 1388 k += (1 << ofs); /* shift bit into position specified 1389 by Q2, Q1, Q0 */ 1390 } 1391 } 1392 1393 if (k == 0x75) { 1394 rtapi_print_msg(RTAPI_MSG_INFO, 1395 "STG: Autodetected stg1 card at address %x\n", address); 1396 stg_driver->model=1; 1397 retval = address; /* SER sequence is 01110101 */ 1398 break; 1399 } 1400 1401 if (k == 0x74) { 1402 rtapi_print_msg(RTAPI_MSG_INFO, 1403 "STG: Autodetected stg2 card at address %x\n", address); 1404 stg_driver->model=2; 1405 retval = address; 1406 break; 1407 } 1408 //} 1409 } 1410 1411 if ( ( retval == 0 ) || ( stg_driver->model == 0 ) ) 1412 { 1413 rtapi_print_msg(RTAPI_MSG_ERR, 1414 "STG: stg_autodetect() did not find any stg1 or stg2 card\n"); 1415 } 1416 1417 /* restore saved message level */ 1418 rtapi_set_msg_level(msg); 1419 1420 return retval; 1421 } 1422 1423 /*********************************************************************** 1424 * LOCAL FUNCTION DEFINITIONS * 1425 * these are functions used for exporting various HAL pins/prams * 1426 ************************************************************************/ 1427 static int export_counter(int num, stg_struct *addr) 1428 { 1429 int retval, msg; 1430 1431 /* This function exports a lot of stuff, which results in a lot of 1432 logging if msg_level is at INFO or ALL. So we save the current value 1433 of msg_level and restore it later. If you actually need to log this 1434 function's actions, change the second line below */ 1435 msg = rtapi_get_msg_level(); 1436 rtapi_set_msg_level( STG_MSG_LEVEL ); 1437 1438 /* export pin for counts captured by update() */ 1439 retval = hal_pin_s32_newf(HAL_OUT, &addr->count[num], 1440 comp_id, "stg.%d.counts", num); 1441 if (retval != 0) { 1442 return retval; 1443 } 1444 /* export pin for scaled position captured by update() */ 1445 retval = hal_pin_float_newf(HAL_OUT, &addr->pos[num], 1446 comp_id, "stg.%d.position", num); 1447 if (retval != 0) { 1448 return retval; 1449 } 1450 /* export parameter for scaling */ 1451 retval = hal_param_float_newf(HAL_RW, &addr->pos_scale[num], 1452 comp_id, "stg.%d.position-scale", num); 1453 if (retval != 0) { 1454 return retval; 1455 } 1456 1457 /* export pin for index homing */ 1458 retval = hal_pin_bit_newf(HAL_IO, &addr->index_enable[num], 1459 comp_id, "stg.%d.index-enable", num); 1460 if (retval != 0) { 1461 return retval; 1462 } 1463 1464 /* export pin for reading the index latch */ 1465 retval = hal_pin_bit_newf(HAL_OUT, &addr->index_latch[num], 1466 comp_id, "stg.%d.index-latch", num); 1467 if (retval != 0) { 1468 return retval; 1469 } 1470 1471 1472 /* 1473 * The index polarity is configurable for the stg1 cards only, 1474 * but not for the stg2 cards 1475 */ 1476 if( addr->model == 1 ) 1477 { 1478 /* export read only HAL pin for index pulse polarity */ 1479 retval = hal_pin_bit_newf(HAL_IN, &addr->index_polarity[num], 1480 comp_id, "stg.%d.index-polarity", num); 1481 if (retval != 0) 1482 { 1483 return retval; 1484 } 1485 } 1486 1487 /* restore saved message level */ 1488 rtapi_set_msg_level(msg); 1489 1490 return 0; 1491 } 1492 1493 static int export_dac(int num, stg_struct *addr) 1494 { 1495 int retval, msg; 1496 1497 /* This function exports a lot of stuff, which results in a lot of 1498 logging if msg_level is at INFO or ALL. So we save the current value 1499 of msg_level and restore it later. If you actually need to log this 1500 function's actions, change the second line below */ 1501 msg = rtapi_get_msg_level(); 1502 rtapi_set_msg_level( STG_MSG_LEVEL ); 1503 1504 /* export pin for voltage received by the board() */ 1505 retval = hal_pin_float_newf(HAL_IN, &addr->dac_value[num], 1506 comp_id, "stg.%d.dac-value", num); 1507 if (retval != 0) { 1508 return retval; 1509 } 1510 /* export parameter for offset */ 1511 retval = hal_param_float_newf(HAL_RW, &addr->dac_offset[num], 1512 comp_id, "stg.%d.dac-offset", num); 1513 if (retval != 0) { 1514 return retval; 1515 } 1516 /* export parameter for gain */ 1517 retval = hal_param_float_newf(HAL_RW, &addr->dac_gain[num], 1518 comp_id, "stg.%d.dac-gain", num); 1519 if (retval != 0) { 1520 return retval; 1521 } 1522 1523 /* restore saved message level */ 1524 rtapi_set_msg_level(msg); 1525 return 0; 1526 } 1527 1528 static int export_adc(int num, stg_struct *addr) 1529 { 1530 int retval, msg; 1531 1532 /* This function exports a lot of stuff, which results in a lot of 1533 logging if msg_level is at INFO or ALL. So we save the current value 1534 of msg_level and restore it later. If you actually need to log this 1535 function's actions, change the second line below */ 1536 msg = rtapi_get_msg_level(); 1537 rtapi_set_msg_level( STG_MSG_LEVEL ); 1538 1539 /* export pin for voltage received by the board() */ 1540 retval = hal_pin_float_newf(HAL_OUT, &addr->adc_value[num], 1541 comp_id, "stg.%d.adc-value", num); 1542 if (retval != 0) { 1543 return retval; 1544 } 1545 /* export parameter for offset */ 1546 retval = hal_param_float_newf(HAL_RW, &addr->adc_offset[num], 1547 comp_id, "stg.%d.adc-offset", num); 1548 if (retval != 0) { 1549 return retval; 1550 } 1551 /* export parameter for gain */ 1552 retval = hal_param_float_newf(HAL_RW, &addr->adc_gain[num], 1553 comp_id, "stg.%d.adc-gain", num); 1554 if (retval != 0) { 1555 return retval; 1556 } 1557 /* restore saved message level */ 1558 rtapi_set_msg_level(msg); 1559 return 0; 1560 } 1561 1562 static int export_pins(int num, int dir, stg_struct *addr) 1563 { 1564 int retval, msg, i; 1565 1566 /* This function exports a lot of stuff, which results in a lot of 1567 logging if msg_level is at INFO or ALL. So we save the current value 1568 of msg_level and restore it later. If you actually need to log this 1569 function's actions, change the second line below */ 1570 msg = rtapi_get_msg_level(); 1571 rtapi_set_msg_level( STG_MSG_LEVEL ); 1572 1573 for (i=0; i<8; i++) { 1574 1575 if (dir != 0) 1576 retval=export_output_pin(outpinnum++, &(addr->port[num][i]) ); 1577 else 1578 retval=export_input_pin(inputpinnum++, &(addr->port[num][i]) ); 1579 1580 if (retval != 0) { 1581 return retval; 1582 } 1583 } 1584 /* restore saved message level */ 1585 rtapi_set_msg_level(msg); 1586 return 0; 1587 } 1588 1589 static int export_input_pin(int pinnum, io_pin * pin) 1590 { 1591 int retval; 1592 int msg; 1593 1594 /* This function exports a lot of stuff, which results in a lot of 1595 logging if msg_level is at INFO or ALL. So we save the current value 1596 of msg_level and restore it later. If you actually need to log this 1597 function's actions, change the second line below */ 1598 msg = rtapi_get_msg_level(); 1599 rtapi_set_msg_level( STG_MSG_LEVEL ); 1600 1601 /* export read only HAL pin for input data */ 1602 retval = hal_pin_bit_newf(HAL_OUT, &(pin->data), comp_id, 1603 "stg.in-%02d", pinnum); 1604 if (retval != 0) { 1605 return retval; 1606 } 1607 /* export additional pin for inverted input data */ 1608 retval = hal_pin_bit_newf(HAL_OUT, &(pin->io.not), comp_id, 1609 "stg.in-%02d-not", pinnum); 1610 /* initialize HAL pins */ 1611 *(pin->data) = 0; 1612 *(pin->io.not) = 1; 1613 1614 /* restore saved message level */ 1615 rtapi_set_msg_level(msg); 1616 1617 return retval; 1618 } 1619 1620 static int export_output_pin(int pinnum, io_pin * pin) 1621 { 1622 int retval; 1623 int msg; 1624 1625 /* 1626 * This function exports a lot of stuff, which results in a lot of 1627 * logging if msg_level is at INFO or ALL. So we save the current value 1628 * of msg_level and restore it later. If you actually need to log this 1629 * function's actions, change the second line below 1630 */ 1631 msg = rtapi_get_msg_level(); 1632 rtapi_set_msg_level( STG_MSG_LEVEL ); 1633 1634 /* export read only HAL pin for output data */ 1635 retval = hal_pin_bit_newf(HAL_IN, &(pin->data), 1636 comp_id, "stg.out-%02d", pinnum); 1637 if (retval != 0) { 1638 return retval; 1639 } 1640 /* export parameter for polarity */ 1641 retval = hal_param_bit_newf(HAL_RW, &(pin->io.invert), 1642 comp_id, "stg.out-%02d-invert", pinnum); 1643 /* initialize HAL pin and param */ 1644 *(pin->data) = 0; 1645 pin->io.invert = 0; 1646 1647 1648 /* restore saved message level */ 1649 rtapi_set_msg_level(msg); 1650 1651 return retval; 1652 }