hal_ppmc.c

来自「CNC 的开放码,EMC2 V2.2.8版」· C语言 代码 · 共 1,961 行 · 第 1/5 页

	    /* parameter is non-zero and positive, compare to max_freq */
	    if ( (sg->max_vel * abs_scale) > ch_max_freq) {
		/* parameter is too high, lower it */
		sg->max_vel = ch_max_freq / abs_scale;
	    } else {
		/* lower max_freq to match parameter */
		ch_max_freq = sg->max_vel * abs_scale;
	    }
	}
	/* calculate desired frequency */
	freq = *(sg->vel) * sg->scale;
	/* should we be running? */
	if ( *(sg->enable) != 0 ) {
	    run = 1;
	} else {
	    run = 0;
	}
	/* deal with special cases - negative and very low frequency */
	reverse = 0;
	if ( freq < 0.0 ) {
	    /* negative */
	    freq = -freq;
	    reverse = 1;
	}
	/* apply limits */
	if ( freq > ch_max_freq ) {
	    freq = ch_max_freq;
	    divisor = 10000000.0 / freq;
	} else if ( freq < (10000000.0/16777215.0) ) {
	    /* frequency would result in a divisor greater than 2^24-1 */
	    freq = 0.0;
	    divisor = 16777215;
	    /* only way to get zero is to turn it off */
	    run = 0;
	} else {
	    /* calculate divisor, round to nearest instead of truncating */
	    divisor = ( 10000000.0 / freq ) + 0.5;
	    /* calculate actual frequency (due to divisor roundoff) */
	    freq = 10000000.0 / divisor;
	}
	/* set run bit in the control byte */
	control_byte >>= 2;
	if ( run ) {
	    control_byte |= 0x80;
	}
	/* set dir bit in the control byte, and save the frequency */
	if ( reverse ) {
	    sg->freq = -freq;
	} else {
	    sg->freq = freq;
	    control_byte |= 0x40;
	}
	/* correct for an offset of 4 in the hardware */
	divisor -= 4;
	/* write divisor to the cache */
	slot->wr_buf[RATE_GEN_0+(n*3)] = divisor & 0xff;
	divisor >>= 8;
	slot->wr_buf[RATE_GEN_0+(n*3)+1] = divisor & 0xff;
	divisor >>= 8;
	slot->wr_buf[RATE_GEN_0+(n*3)+2] = divisor & 0xff;
    }
    /* write control byte to cache */
    slot->wr_buf[RATE_CTRL_0] = control_byte;
}


static void write_pwmgens(slot_data_t *slot)
{
    int n, reverse;
    unsigned int period, start, len, stop;
    pwmgen_t *pg;
    double freq, dc, abs_dc;
    unsigned char control_byte;

    /* zero frequency is a special case, turn off everything */
    if ( slot->pwmgen->freq == 0.0 ) {
	slot->pwmgen->old_freq = slot->pwmgen->freq;
	/* write control byte to cache */
	slot->wr_buf[PWM_CTRL_0] = 0;
	/* done */
	return;
    }
    /* check for new frequency setting */
    if ( slot->pwmgen->freq != slot->pwmgen->old_freq ) {
	/* process new frequency value */
	freq = slot->pwmgen->freq;
	/* frequency must be between 153Hz and 500KHz */
	if ( freq < 153.0 ) {
	    freq = 153.0;
	}
	if ( freq > 500000.0 ) {
	    freq = 500000.0;
	}
	/* calculate divisor */
	if (slot->ver >= 3)  // accomodate 3.1 and newer boards with 40MHz clk
	  period = (40000000.0 / freq) + 0.5;  // 40 MHz clock on ver 3 and up
	else
	  period = (10000000.0 / freq) + 0.5;  // 10 MHz on lower version
	/* calculate actual frequency (after rounding, etc) */
	freq = 10000000.0 / period;
	/* save values */
	slot->pwmgen->freq = freq;
	slot->pwmgen->old_freq = freq;
	slot->pwmgen->period = period;
	slot->pwmgen->period_recip = 1.0 / period;
    }
    /* calculate counter start value */
    start = 65536 - slot->pwmgen->period;
    /* write to cache */
    slot->wr_buf[PWM_FREQ_LO] = start & 0xFF;
    slot->wr_buf[PWM_FREQ_HI] = (start >> 8) & 0xFF;
    /* now do the four individual pwmgens */
    control_byte = 0;
    for ( n = 0 ; n < 4 ; n++ ) {
	/* point to the specific pwm generator */
	pg = &(slot->pwmgen->pg[n]);
	/* validate the scale value */
	if ( pg->scale < 0.0 ) {
	    if ( pg->scale > -EPSILON ) {
		/* too small, divide by zero is bad */
		pg->scale = -1.0;
	    }
	} else {
	    if ( pg->scale < EPSILON ) {
		pg->scale = 1.0;
	    }
	}
	/* calculate desired duty cycle */
	dc = *(pg->value) / pg->scale;
	/* Special code to deal with the requirements of the Pico PWM
	   amps.  They need at least one PWM pulse in each direction 
	   every time you enable the amps.  So we override the commanded
	   duty cycle with +5%, then -5%, for one thread execution time 
	   each, when we see a rising edge on enable.
	*/
	if ( pg->bootstrap != 0 ) {
	    /* check for rising edge on enable */
	    if (( *(pg->enable) != 0 ) && ( pg->old_enable == 0 )) {
		/* kick off state machine */
		pg->boot_state = BOOT_FWD;
	    }
	    pg->old_enable = *(pg->enable);
	    /* now execute a state machine */
	    switch(pg->boot_state) {
	    case BOOT_NORMAL:
		break;
	    case BOOT_REV:
		dc = -0.05;
		pg->boot_state = BOOT_NORMAL;
		break;
	    case BOOT_FWD:
		dc =  0.05;
		pg->boot_state = BOOT_REV;
		break;
	    default:
		pg->boot_state = BOOT_NORMAL;
		break;
	    }
	}
	/* deal with negative values */
	reverse = 0;
	if ( dc < 0.0 ) {
	    reverse = 1;
	    abs_dc = -dc;
	} else {
	    abs_dc = dc;
	}
	/* reset any illegal duty cycle limits */
	if (( pg->min_dc > 1.0 ) || ( pg->min_dc < 0.0 )) {
	    pg->min_dc = 0.0;
	} 
	if (( pg->max_dc > 1.0 ) || ( pg->max_dc < 0.0 )) {
	    pg->max_dc = 1.0;
	} 
	if ( pg->min_dc >= pg->max_dc ) {
	    pg->min_dc = 0.0;
	    pg->max_dc = 1.0;
	}
	/* apply limits */
	if ( abs_dc > pg->max_dc ) {
	    abs_dc = pg->max_dc;
	} else if ( abs_dc < pg->min_dc ) {
	    abs_dc = pg->min_dc;
	}
	/* calculate length of PWM pulse in clocks */
	len = ( abs_dc * slot->pwmgen->period ) + 0.5;
	/* calculate actual duty cycle (after rounding) */
	abs_dc = len * slot->pwmgen->period_recip;
	/* set run bit in the control byte */
	control_byte >>= 2;
	if ( *(pg->enable) != 0 ) {
	    control_byte |= 0x80;
	}
	/* set dir bit in the control byte, and save the duty cycle */
	if ( reverse ) {
	    pg->duty_cycle = -abs_dc;
	} else {
	    pg->duty_cycle = abs_dc;
	    control_byte |= 0x40;
	}
	/* calculate count at which to turn off output */
	stop = 65535 - len;
	/* write count to the cache */
	slot->wr_buf[PWM_GEN_0+(n*2)] = stop & 0xff;
	stop >>= 8;
	slot->wr_buf[PWM_GEN_0+(n*2)+1] = stop & 0xff;
    }
    /* write control byte to cache */
    slot->wr_buf[PWM_CTRL_0] = control_byte;
}

static void write_DACs(slot_data_t *slot)
{
    int n;
    DAC_t *pg;
    long dc;
    float volts;

    //    rtapi_print_msg(RTAPI_MSG_INFO, "enter write_DACs()\n");
    /* now do the four individual DACs */
    for ( n = 0 ; n < 4 ; n++ ) {
      /* point to the specific DAC */
      pg = &(slot->DAC->pg[n]);
      /* validate the scale value */
      if ( pg->scale < 0.0 ) {
	if ( pg->scale > -EPSILON ) {
	  /* too small, divide by zero is bad */
	  pg->scale = -1.0;
	}
      } else {
	if ( pg->scale < EPSILON ) {
	  pg->scale = 1.0;
	}
      }
      /* calculate desired output voltage */
      volts = *(pg->value) / pg->scale;
      /* output to DAC word works like:
	 0xFFFF -> +10 V
	 0x8000 ->   0 V
	 0x0000 -> -10 V   */
      dc = (long) (((volts / 10.0) * 0x7FFF)+0x8000);
      if (dc > 0xffff)
	{
	  dc = 0xffff;
	}
      if (dc < 0 )
	{
	  dc = 0;
	}
      slot->wr_buf[DAC_0+(n*2)] = dc & 0xff;  // put low byte in cache
      dc >>= 8;
      slot->wr_buf[DAC_0+(n*2)+1] = dc & 0xff;  // put high byte in cache
    }
}


static void write_extraDAC(slot_data_t *slot)
{
    DAC_t *pg;
    long dc;
    float volts;

    /* point to the DAC */
    pg = &(slot->extra->dac);
    /* validate the scale value */
    if ( pg->scale < 0.0 ) {
	if ( pg->scale > -EPSILON ) {
	    /* too small, divide by zero is bad */
	    pg->scale = -1.0;
	}
    } else {
	if ( pg->scale < EPSILON ) {
	    pg->scale = 1.0;
	}
    }
    /* calculate desired output voltage */
//    volts = *(pg->value) / pg->scale;
    /* this DAC is sign-magnitude, and the sign enable
       bits are controlled by digital output bits.
       driving P8 pin 12 to ground enables + output.
       driving P8 pin 11 to ground enables - output.
       leaving both pins high or floating forces output to 0 V.
       don't drive both outputs low at the same time - undefined.
       UPC and USC boards sold with the spindle DAC option
       have SSR1 set up for + output, and SSR2 for - output.  */

    volts = abs(*(pg->value)) / pg->scale;
    /* output to DAC word works like:
	0xFF -> +10 V
	0x00 ->   0 V */
    /* there is NO negative.  You can invert the signal with
	an external signal into the post-DAC amplifier, but
	the software is completely unaware of that */
    dc = (long) ((volts / 10.0) * 0xff);
    if (dc > 0xff) {
	dc = 0xff;
    }
    if (dc < 0 ) {
	dc = 0;
    }
    slot->wr_buf[UxC_EXTRA] = dc & 0xff;  // put in cache
}

static void write_extra_dout(slot_data_t *slot)
{
    dout_t *pg;
    int b;
    unsigned char outdata, mask;

    //    rtapi_print_msg(RTAPI_MSG_INFO, "enter write_extra_dout()\n");
    outdata = 0x00;
    mask = 0x01;
    /* assemble output byte from 8 source variables */
    for (b = 0; b < 8; b++) {
      pg = &(slot->extra->douts[b]);
	/* get the data, add to output byte */
	if ((*(pg->data)) && (!pg->invert)) {
	    outdata |= mask;
	}
	if ((!*(pg->data)) && (pg->invert)) {
	    outdata |= mask;
	}
	mask <<= 1;
    }
    slot->wr_buf[UxC_EXTRA] = outdata;  // put in cache
}


/***********************************************************************
*                   LOCAL FUNCTION DEFINITIONS                         *
************************************************************************/

/* this function converts a range of EPP addresses into a bitmap
   to be passed to add_rd_funct() or add_wr_funct()
*/

static __u32 block(int min, int max)
{
    int n;
    __u32 mask;

    mask = 0;
    for ( n = min ; n <= max ; n++ ) {
	mask |= ( 1 << n );
    }
    return mask;
}

/* these functions are used to register a runtime function to be called
   by either read_all or write_all.  'cache_bitmap' defines the EPP
   addresses that the function needs.  All addresses needed by all 
   functions associated with the slot will be sequentially read into
   the rd_buf cache (or written from the wr_buf cache) by read_all 
   or write_all respectively, to minimize the number of slow inb
   and outb operations needed.
*/

static int add_rd_funct(slot_funct_t *funct, slot_data_t *slot,
			__u32 cache_bitmap )
{
    if ( slot->num_rd_functs >= MAX_FUNCT ) {
	rtapi_print_msg(RTAPI_MSG_ERR, 
	    "PPMC: ERROR: too many read functions\n");
	return -1;
    }
    slot->rd_functs[slot->num_rd_functs++] = funct;
    slot->read_bitmap |= cache_bitmap;
    return 0;
}

static int add_wr_funct(slot_funct_t *funct, slot_data_t *slot,
			__u32 cache_bitmap )
{
    if ( slot->num_wr_functs >= MAX_FUNCT ) {
	rtapi_print_msg(RTAPI_MSG_ERR, 
	    "PPMC: ERROR: too many write functions\n");
	return -1;
    }
    slot->wr_functs[slot->num_wr_functs++] = funct;
    slot->write_bitmap |= cache_bitmap;
    return 0;
}

static int export_UxC_digin(slot_data_t *slot, bus_data_t *bus)
{
    int retval, n;
    char buf[HAL_NAME_LEN + 2];

    rtapi_print_msg(RTAPI_MSG_INFO, "PPMC:  exporting UxC digital inputs\n");

    /* do hardware init */

    /* allocate shared memory for the digital input data */