hal_stg.c

CNC 的开放码,EMC2 V2.2.8版

    rtapi_print_msg(RTAPI_MSG_INFO,
	"STG: installed %d digital inputs\n", inputpinnum);

    retval = hal_export_funct("stg.do-write", stg_do_write,
	stg_driver, 0, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "STG: ERROR: stg.do-write funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    rtapi_print_msg(RTAPI_MSG_INFO,
	"STG: installed %d digital outputs\n", outpinnum);

    hal_ready(comp_id);
    return 0;
}

void rtapi_app_exit(void)
{
    hal_exit(comp_id);
}

/***********************************************************************
*            REALTIME ENCODER COUNTING AND UPDATE FUNCTIONS            *
************************************************************************/

static void stg_counter_capture(void *arg, long period)
{
    stg_struct *stg;
    int n;

    stg = arg;
    for (n = 0; n < num_chan; n++) {
	/* check for index-enable = high, set from the outside, we need to reset position based on index pulse */
	if (*(stg->index_enable[n])) {
		// if check_index[n] == 0 set up polling for index on axis n
		if (stg->check_index[n] == 0) {
			/* select current axis to be reset by index */
			stg_select_index_axis(arg, n);
			/* remember axis beeing polled */
			stg->check_index[n] = 1;
		} 
		if (stg->check_index[n] != 0) { // otherwise, we already did that, and we need to see if the index arrived
			if (stg_get_index_pulse_latch(arg, n)) {
				//index arrived, we need to clear the index, and substract the position
				stg->check_index[n] = 0;
				stg->offset[n] = stg_counter_read(n);	// read the value without latching, latching was done on index
														// remember this as an offset, it will be substracted from nominal
				/* set index-enable false, so outside knows we found the index, and reset the position */
				*(stg->index_enable[n]) = 0;
			}
		}
		
	}
	/* capture raw counts to latches */
	stg_counter_latch(n);
	/* read raw count, and substract the offset (determined by indexed homing) */
	*(stg->count[n]) = stg_counter_read(n) - stg->offset[n]; 
	/* make sure scale isn't zero or tiny to avoid divide error */
	if (stg->pos_scale[n] < 0.0) {
	    if (stg->pos_scale[n] > -EPSILON)
		stg->pos_scale[n] = -1.0;
	} else {
	    if (stg->pos_scale[n] < EPSILON)
		stg->pos_scale[n] = 1.0;
	}
	/* scale count to make floating point position */
	*(stg->pos[n]) = *(stg->count[n]) / stg->pos_scale[n];
    }
    /* done */
}

/* stg_dacs_write() - writes all dac's to the board
    - calls stg_dac_write() */
static void stg_dacs_write(void *arg, long period)
{    
    stg_struct *stg;
    float volts;
    short ncounts, i;

    stg=arg;
    for (i=0;i < num_chan; i++) {
	/* scale the voltage to be written based on offset and gain */
	volts = (*(stg->dac_value[i]) - stg->dac_offset[i]) * stg->dac_gain[i];
        /* clamp the scaled voltage value to the -10V to 10V output range of the STG */
        if (volts < -10.0)
                volts = -10.0;
        if (volts > 10.0)
                volts = 10.0;
	/* compute the value for the DAC, the extra - in there is STG specific */
	ncounts = (short) ((-10.0 - volts) / 20.0 * 0x1FFF);
	/* write it to the card */	
	stg_dac_write(i, ncounts);	
    }
}

/* stg_adcs_read() - reads one adc at a time from the board to hal
    - calls stg_adc_read() */

/* long description :

    Because the conversion takes a while (first mux to the right channel ~5usecs,
    then start the conversion, and wait for it to finish ~16-20 usecs) for all the
    8 channels, it would be too much to start the conversion, wait for the result
    for all the 8 axes.
    Instead a different approach is chosen:
    - on the beginning of the function the conversion should already be done
    - it gets read and sent to HAL
    - the new channel gets mux'ed
    - and at the end of the function the new conversion is started, so that the data
      will be available at the next run.
    This way 8 periods are needed to read 8 ADC's. It is possible to set the board
    to do faster conversions (AZ bit on INTC off), but that would make it less 
    reliable (autozero takes care of temp. errors).*/
/*! \todo STG_ADC_Improvement (if any user requests it).
    Another improvement might be to let the user chose what channels he would like
    for ADC (having only 2 channels might speed things up considerably).
*/
static void stg_adcs_read(void *arg, long period)
{    
    stg_struct *stg;
    float volts;
    short ncounts;
    int i;

    stg = arg;
    i = stg->adc_current_chan;
    if ((i >= 0) && (i < num_chan)) { 
	/* we should have the conversion done for adc_num_chan */
	ncounts = stg_adc_read(stg,i);
	volts = ncounts * 10.0 / 4096;
	*(stg->adc_value[i]) = volts * stg->adc_gain[i] - stg->adc_offset[i];
    }
    /* if adc_num_chan < 0, it's the first time this routine runs
       thus we don't have any ready data, we simply start the next conversion */
    if (stg->adc_current_chan++ >= num_chan) 
	stg->adc_current_chan=0; //increase the channel, and roll back to 0 after all chans are done

    /* select the current channel with the mux, and start the conversion */
    stg_adc_start(stg,stg->adc_current_chan);
    /* the next time this function runs, the result should be available */
}


// helper function to extract the data out of a char and place it into HAL data
// written by JMK
static void split_input(unsigned char data, io_pin *dest, int num)
{
    int b;
    unsigned char mask;

    /* splits a byte into 'num' HAL pins (and their NOTs) */
    mask = 0x01;
    for (b = 0 ; b < num ; b++ ) {
	if ( data & mask ) {
	    /* input high, which means FALSE (active low) */
	    *(dest->data) = 0;
	    *(dest->io.not) = 1;
	} else {
	    /* input low, which means TRUE */
	    *(dest->data) = 1;
	    *(dest->io.not) = 0;
	}
	mask <<= 1;
	dest++;
    }
}    

// helper function to extract the data out of HAL and place it into a char
// written by JMK
unsigned char build_output(io_pin *src, int num)
{
    int b;
    unsigned char data, mask;

    data = 0x00;
    mask = 0x01;
    /* assemble output byte for data port from 'num' source variables */
    for (b = 0; b < num; b++) {
	/* get the data, add to output byte */
	if ( *(src->data) ) {
	    if ( !(src->io.invert) ) {
		data |= mask;
	    }
	} else {
	    if ( (src->io.invert) ) {
		data |= mask;
	    }
	}
	mask <<= 1;
	src++;
    }
    return data;
}


static void stg_di_read(void *arg, long period) //reads digital inputs from the STG
{
    stg_struct *stg;
    unsigned char val;
    stg=arg;
    
    if ( (stg->dir_bits & 0x01) == 0) { // if port A is set as input, read the bits
	if (stg->model == 1)
	    val = inb(base + DIO_A);
	else
	    val = inb(base + PORT_A);
	split_input(val, &(stg->port[0][0]), 8);
    }
    if ( (stg->dir_bits & 0x02) == 0) { // if port B is set as input, read the bits
	if (stg->model == 1)
    	    val = inb(base + DIO_B);
	else
	    val = inb(base + PORT_B);
	split_input(val, &(stg->port[1][0]), 8);
    }
    if ( (stg->dir_bits & 0x04) == 0) { // if port C is set as input, read the bits
	if (stg->model == 1)
	    val = inb(base + DIO_C);
	else
	    val = inb(base + PORT_C);
	split_input(val, &(stg->port[2][0]), 8);
    }
    if ( (stg->dir_bits & 0x08) == 0) { // if port D is set as input, read the bits
	if (stg->model == 1)
    	    val = inb(base + DIO_D);
	else
	    val = inb(base + PORT_D);
	split_input(val, &(stg->port[3][0]), 8);
    }
}

static void stg_do_write(void *arg, long period) //writes digital outputs to the STG
{
    stg_struct *stg;
    unsigned char val;
    stg=arg;

    if ( (stg->dir_bits & 0x01) != 0) { // if port A is set as output, write the bits
	val = build_output(&(stg->port[0][0]), 8);
	if (stg->model == 1)
	    outb(val, base + DIO_A);
	else
	    outb(val, base + PORT_A);
    }
    if ( (stg->dir_bits & 0x02) != 0) { // if port B is set as output, write the bits
	val = build_output(&(stg->port[1][0]), 8);
	if (stg->model == 1)
	    outb(val, base + DIO_B);
	else
	    outb(val, base + PORT_B);
    }
    if ( (stg->dir_bits & 0x04) != 0) { // if port C is set as output, write the bits
	val = build_output(&(stg->port[2][0]), 8);
	if (stg->model == 1)
	    outb(val, base + DIO_C);
	else
	    outb(val, base + PORT_C);
    }
    if ( (stg->dir_bits & 0x08) != 0) { // if port D is set as output, write the bits
	val = build_output(&(stg->port[3][0]), 8);
	if (stg->model == 1)
	    outb(val, base + DIO_D);
	else
	    outb(val, base + PORT_D);
    }
}

/***********************************************************************
*                      BOARD SPECIFIC FUNCTIONS                        *
*       execute board related things (write/read to/from the stg)      *
************************************************************************/


/***********************************************************************
*                            INIT FUNCTIONS                            *
************************************************************************/

/*
  stg_counter_init() - Initializes the channel
  
  works the same for both cards (STG & STG2)
*/
static int stg_counter_init(int ch)
{
    /* Set Counter Command Register - Master Control, Master Reset (MRST), */
    /* and Reset address pointer (RADR). */
    outb(0x23, CTRL(ch));

    /* Set Counter Command Register - Input Control, OL Load (P3), */
    /* and Enable Inputs A and B (INA/B). */
    outb(0x68, CTRL(ch));

    /* Set Counter Command Register - Output Control */
    outb(0x80, CTRL(ch));

    /* Set Counter Command Register - Quadrature */
    outb(0xC3, CTRL(ch));
    return 0;
}

/*
  stg_dac_init() - Initializes the dac channel

  works the same for both cards (STG & STG2)
*/
static int stg_dac_init(int ch)
{
    int i;
    
    /* set all DAC's to 0 on startup */
    for (i=0; i < num_chan; i++) {
	stg_dac_write(i, 0x1000); //by Xuecheng, 0x1000 coresponds to 0V 
    }
    return 0;
}


/*
  stg_adc_init() - Initializes the adc channel
*/
static int stg_adc_init(int ch)
{

    /* not much to setup for the ADC's */
    /* only select the mode of operation we will work with AutoZero */
    if (stg_driver->model == 1)
	outb(0x0f, base + MIO_2);	// the second 82C55 is already configured (by running stg_dio_init)
					// we only set bit 8 (AZ) to 1 to enable it
    return 0;
}


/*
  stg_dio_init() - Initializes the dio's
*/
static int stg_dio_init(void)
{
    /* we will select the directions of each port */
    unsigned char control, tempINTC, tempIMR, tempCtrl0, tempCtrl1;

    control = 0x80; //set up |1|0|0|A|CH|0|B|CL|
    if ( (stg_driver->dir_bits & 0x01) == 0) // if port A is set as input, set bit accordingly
	control |= 0x10;
    if ( (stg_driver->dir_bits & 0x02) == 0) // if port B is set as input, set bit accordingly
	control |= 0x02;
    if ( (stg_driver->dir_bits & 0x04) == 0) // if port C is set as input, set bits accordingly
	control |= 0x09;
    
    if (stg_driver->model == 1) {
	// write the computed control to MIO_1
	outb(control, base+MIO_1);
    } else { //model STG2
	// write port A,B,C direction to ABC_DIR
	outb(control, base+ABC_DIR);
    }
    
	tempINTC = inb(base + INTC);
    
    if (stg_driver->model == 1) {
	// next compute the directions for port D, located on the second 82C55
	control = 0x82;
    
	if ( (stg_driver->dir_bits & 0x08) == 0)// if port D is set as input, set bits accordingly
	    control = 0x92;

	tempIMR = inb(base + IMR); // get the current interrupt mask
        
	outb(0xff, base + OCW1); //mask off all interrupts
    
	// write the computed control to MIO_2
	outb(control, base+MIO_2);
    
	outb(tempINTC, base + INTC); //restore interrupt control reg.
    
	outb(tempIMR, base+ OCW1); //restore int mask

    } else { //model STG2
    
	// save contents of CNTRL0, it will get reinitialized
	tempCtrl0 = inb(base+CNTRL0);
	tempCtrl1 = inb(base+CNTRL1);
    
	// CNTRL0 output, BRDTST input, D output
	control = 0x82;

	if ( (stg_driver->dir_bits & 0x08) == 0)// if port D is set as input, set bits accordingly
	    control = 0x8b;
	
	outb(0xff, base + CNTRL1); // disable interrupts
	
	outb(control, base + D_DIR); // set port D direction, also resets CNTRL0
	
	outb(tempCtrl0, base + CNTRL0);
	outb( (tempCtrl1 & 0x0f) | 0xf0, base + CNTRL1);
    }
    
    return 0;
}


/***********************************************************************
*                          ACTION FUNCTIONS                            *
*            these do the actual data exchange with the board          *
************************************************************************/

static void stg_counter_latch(int i) 
{
    outb(0x03, CTRL(i));
}


/*
  stg_counter_read() - reads one channel
  FIXME - todo, extend to 32 bits in software

  works the same for both cards (STG & STG2)
*/
static long stg_counter_read(int i)
{
    union pos_tag {
	long l;
	struct byte_tag {
	    char b0;
	    char b1;
	    char b2;
	    char b3;
	} byte;
    } pos;

    pos.byte.b0 = inb(DATA(i));
    pos.byte.b1 = inb(DATA(i));
    pos.byte.b2 = inb(DATA(i));
    if (pos.byte.b2 < 0) {
	pos.byte.b3 = -1;
    } else {
	pos.byte.b3 = 0;
    }
    return pos.l;
}

static void stg_select_index_axis(void *arg, unsigned int chan)
{
    stg_struct *stg = arg;
    unsigned char byIntc,byAxis;