hal_ppmc.c

Source code for an Numeric Cmputer

        /* check for - to + transition */
        if ((oldpos.byte.b2 & 0xc0) == 0xc0 && (pos.byte.b2 == 0))
            pos.byte.b3++;
        else
            if ((oldpos.byte.b2 == 0) && (pos.byte.b2 & 0xc0) == 0xc0)
                pos.byte.b3--;
	*(slot->encoder[i].delta) = pos.l - slot->encoder[i].oldreading;
	slot->encoder[i].oldreading = pos.l;
	*(slot->encoder[i].count) = pos.l;
	if (slot->encoder[i].scale < 0.0) {
	    if (slot->encoder[i].scale > -EPSILON)
		slot->encoder[i].scale = -1.0;
	} else {
	    if (slot->encoder[i].scale < EPSILON)
		slot->encoder[i].scale = 1.0;
	}
	*(slot->encoder[i].position) = pos.l / slot->encoder[i].scale;
	*(slot->encoder[i].index) = (((indextemp & mask) == mask) ? 1 : 0);
	mask <<= 1;
    }
}

static void write_stepgens(slot_data_t *slot)
{
    int n, reverse, run;
    unsigned int divisor;
    stepgen_t *sg;
    double bd_max_freq, ch_max_freq, abs_scale, freq;
    unsigned char control_byte;

    /* pulse width cannot be less than 2 (HW limitation) */
    if ( slot->stepgen->pulse_width < 2 ) {
	slot->stepgen->pulse_width = 2;
    }
    /* write it to the cache, inverted */
    slot->wr_buf[RATE_WIDTH_0] = 256 - slot->stepgen->pulse_width;
    /* pulse space cannot be less than 3 (HW limitation) */
    if ( slot->stepgen->pulse_space < 3 ) {
	slot->stepgen->pulse_space = 3;
    }
    /* setup time cannot be less than 2 (HW limit) */
    if ( slot->stepgen->setup_time < 2 ) {
	slot->stepgen->setup_time = 2;
    }
    /* write it to the cache, inverted */
    slot->wr_buf[RATE_SETUP_0] = 256 - slot->stepgen->setup_time;
    /* calculate the max frequency, varies with pulse width and
       min pulse spacing */
    bd_max_freq = 10000000.0 / 
	(slot->stepgen->pulse_width + slot->stepgen->pulse_space);
    /* now do the four individual stepgens */
    control_byte = 0;
    for ( n = 0 ; n < 4 ; n++ ) {
	/* point to the specific stepgen */
	sg = &(slot->stepgen->sg[n]);
	/* validate the scale value */
	if ( sg->scale < 0.0 ) {
	    if ( sg->scale > -EPSILON ) {
		/* too small, divide by zero is bad */
		sg->scale = -1.0;
	    }
	    abs_scale = -sg->scale;
	} else {
	    if ( sg->scale < EPSILON ) {
		sg->scale = 1.0;
	    }
	    abs_scale = sg->scale;
	}
	ch_max_freq = bd_max_freq;
	/* check for user specified max velocity */
	if (sg->max_vel <= 0.0) {
	    /* set to zero if negative, and ignore if zero */
	    sg->max_vel = 0.0;
	} else {
	    /* 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 */
	period = (10000000.0 / freq) + 0.5;
	/* 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;
}

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

/* these functions are used to register a runtime function to be called
   by either read_all or write_all.  'min_addr' and 'max_addr' define
   the range of EPP addresses that the function needs.  All addresses
   needed by all functions associated with the slot woll be sequentially
   be 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,
			int min_addr, int max_addr )
{
    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;
    if ( slot->first_rd > min_addr ) {
	slot->first_rd = min_addr;
    }
    if ( slot->last_rd < max_addr ) {
	slot->last_rd = max_addr;
    }
    return 0;
}

static int add_wr_funct(slot_funct_t *funct, slot_data_t *slot,
			int min_addr, int max_addr )
{
    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;
    if ( slot->first_wr > min_addr ) {
	slot->first_wr = min_addr;
    }
    if ( slot->last_wr < max_addr ) {
	slot->last_wr = max_addr;
    }
    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 digital inputs\n");

    /* do hardware init */

    /* allocate shared memory for the digital input data */
    slot->digin = hal_malloc(16 * sizeof(din_t));
    if (slot->digin == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "PPMC: ERROR: hal_malloc() failed\n");
	return -1;
    }
    for ( n = 0 ; n < 16 ; n++ ) {
	/* export pins for input data */
	rtapi_snprintf(buf, HAL_NAME_LEN, "ppmc.%d.din.%02d.in",
	    bus->busnum, bus->last_digin);
	retval = hal_pin_bit_new(buf, HAL_WR, 
	    &(slot->digin[n].data), comp_id);
	if (retval != 0) {
	    return retval;
	}
	rtapi_snprintf(buf, HAL_NAME_LEN, "ppmc.%d.din.%02d.in-not",
	    bus->busnum, bus->last_digin);
	retval = hal_pin_bit_new(buf, HAL_WR, 
	    &(slot->digin[n].data_not), comp_id);
	if (retval != 0) {
	    return retval;
	}
	/* increment number to prepare for next output */
	bus->last_digin++;
    }
    add_rd_funct(read_digins, slot, UxC_DINA, UxC_DINB);
    return 0;
}

static int export_UxC_digout(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 digital outputs\n");

    /* do hardware init */
    /* turn off all outputs */
    SelWrt(0, slot->slot_base+UxC_DOUTA, slot->port_addr);

    /* allocate shared memory for the digital output data */
    slot->digout = hal_malloc(8 * sizeof(dout_t));
    if (slot->digout == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "PPMC: ERROR: hal_malloc() failed\n");
	return -1;
    }
    for ( n = 0 ; n < 8 ; n++ ) {
	/* export pin for output data */