hal_ppmc.c

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

    /* loop thru all slots */
    for ( slotnum = 0 ; slotnum < NUM_SLOTS ; slotnum++ ) {
	/* check for anthing in slot */
	if ( bus->slot_valid[slotnum] ) {
	    /* point at slot data */
	    slot = &(bus->slot_data[slotnum]);
	    /* We only need to send a latch strobe on the master encoder */
	    if ( slot->strobe == 1 ) {
	      /* set the strobe bit, slave mode */
	      SelWrt(0x20, slot->slot_base + ENCRATE, slot->port_addr);
	      /* repeat to guarantee at least 2uS */
	      SelWrt(0x20, slot->slot_base + ENCRATE, slot->port_addr);
	      /* end of strobe pulse, stay in slave mode */
	      SelWrt(0x00, slot->slot_base + ENCRATE, slot->port_addr);
	    }
	    /* fetch data from EPP to cache */
	    addr_ok = 0;
	    bitmap = slot->read_bitmap;
	    n = 0;
	    while ( bitmap ) {
		if ( bitmap & 1 ) {
		    /* need to read register 'n' */
		    if ( addr_ok ) {
			/* auto-increment address is usable */
			slot->rd_buf[n] = ReadMore(slot->port_addr);
		    } else {
			/* need to specify address */
			eppaddr = slot->slot_base + n;
			/* send address and read byte */
			slot->rd_buf[n] = SelRead(eppaddr, slot->port_addr);
			/* mark auto-incrementing address as valid */
			addr_ok = 1;
		    }
		} else {
		    /* don't need to read this register */
		    /* mark auto-incrementing address as invalid */
		    addr_ok = 0;
		}
		/* next register */
		n++;
		bitmap >>= 1;
	    }
	    /* loop thru all functions associated with slot */
	    for ( functnum = 0 ; functnum < slot->num_rd_functs ; functnum++ ) {
		/* call function */
		(slot->rd_functs[functnum])(slot);
	    }
	}
    }
}

static void write_all(void *arg, long period)
{
    bus_data_t *bus;
    slot_data_t *slot;
    int slotnum, functnum, addr_ok;
    unsigned char n, eppaddr;
    __u32 bitmap;

    /* get pointer to bus data structure */
    bus = *(bus_data_t **)(arg);
    /* test to make sure it hasn't been freed */
    if ( bus == NULL ) {
	return;
    }
    /* loop thru all slots */
    for ( slotnum = 0 ; slotnum < NUM_SLOTS ; slotnum++ ) {
	/* check for anthing in slot */
	if ( bus->slot_valid[slotnum] ) {
	    /* point at slot data */
	    slot = &(bus->slot_data[slotnum]);
	    /* loop thru all functions associated with slot */
	    for ( functnum = 0 ; functnum < slot->num_wr_functs ; functnum++ ) {
		/* call function */
		(slot->wr_functs[functnum])(slot);
	    }
	    /* write data from cache to EPP */
	    addr_ok = 0;
	    bitmap = slot->write_bitmap;
	    n = 0;
	    while ( bitmap ) {
		if ( bitmap & 1 ) {
		    /* need to write data register 'n' */
		    if ( addr_ok ) {
			/* auto-increment address is usable */
			WrtMore(slot->wr_buf[n], slot->port_addr);
		    } else {
			/* need to specify address */
			eppaddr = slot->slot_base + n;
			/* send address and write byte */
			SelWrt(slot->wr_buf[n], eppaddr, slot->port_addr);
			/* mark auto-incrementing address as valid */
			addr_ok = 1;
		    }
		} else {
		    /* don't need to write this one */
		    /* mark auto-incrementing address as invalid */
		    addr_ok = 0;
		}
		/* next register */
		n++;
		bitmap >>= 1;
	    }
	}
    }
}

static void read_digins(slot_data_t *slot)
{
    int b;
    unsigned char indata, mask;

    /* read the first 8 inputs */
    indata = slot->rd_buf[UxC_DINA];
    /* split the bits into 16 variables (8 regular, 8 inverted) */
    b = 0;
    mask = 0x01;
    while ( b < 8 ) {
	*(slot->digin[b].data) = indata & mask;
	*(slot->digin[b].data_not) = !(indata & mask);
	mask <<= 1;
	b++;
    }
    /* read the next 8 inputs */
    indata = slot->rd_buf[UxC_DINB];
    /* and split them too */
    mask = 0x01;
    while ( b < 16 ) {
	*(slot->digin[b].data) = indata & mask;
	*(slot->digin[b].data_not) = !(indata & mask);
	mask <<= 1;
	b++;
    }
}

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

    outdata = 0x00;
    mask = 0x01;
    /* assemble output byte from 8 source variables */
    for (b = 0; b < 8; b++) {
	/* get the data, add to output byte */
	if ((*(slot->digout[b].data)) && (!slot->digout[b].invert)) {
	    outdata |= mask;
	}
	if ((!*(slot->digout[b].data)) && (slot->digout[b].invert)) {
	    outdata |= mask;
	}
	mask <<= 1;
    }
    /* write it to the hardware (cache) */
    slot->wr_buf[UxC_DOUTA] = outdata;
}

static void read_PPMC_digins(slot_data_t *slot)
{
    int b;
    unsigned char indata, mask;

    //    rtapi_print_msg(RTAPI_MSG_INFO, "enter read_digins()\n");
    /* read the first 8 inputs */
    indata = slot->rd_buf[DIO_DINA];
    /* split the bits into 16 variables (8 regular, 8 inverted) */
    b = 0;
    mask = 0x01;
    while ( b < 8 ) {
	*(slot->digin[b].data) = indata & mask;
	*(slot->digin[b].data_not) = !(indata & mask);
	mask <<= 1;
	b++;
    }
    /* read the next 8 inputs */
    indata = slot->rd_buf[DIO_DINB];
    /* and split them too */
    mask = 0x01;
    while ( b < 16 ) {
	*(slot->digin[b].data) = indata & mask;
	*(slot->digin[b].data_not) = !(indata & mask);
	mask <<= 1;
	b++;
    }
    if (slot->digin[b].data != NULL) {
      /* read the 2 Estop-related inputs */
      indata = slot->rd_buf[DIO_ESTOP_IN];
      /* and split them too */
      mask = 0x01;
      while ( b < 18 ) {
	*(slot->digin[b].data) = indata & mask;
	*(slot->digin[b].data_not) = !(indata & mask);
	mask <<= 1;
	b++;
      }
    }
}

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

    //    rtapi_print_msg(RTAPI_MSG_INFO, "enter write_PPMC_digouts()\n");
    outdata = 0x00;
    mask = 0x01;
    /* assemble output byte from 8 source variables */
    for (b = 0; b < 8; b++) {
	/* get the data, add to output byte */
	if ((*(slot->digout[b].data)) && (!slot->digout[b].invert)) {
	    outdata |= mask;
	}
	if ((!*(slot->digout[b].data)) && (slot->digout[b].invert)) {
	    outdata |= mask;
	}
	mask <<= 1;
    }
    /* write it to the hardware (cache) */
    slot->wr_buf[DIO_DOUTA] = outdata;
    if (slot->digout[8].data != NULL) {  // no estop funct on slave boards - hal pin doesn't exist
      outdata = 0;  // now process estop bit
      if ((*(slot->digout[8].data)) && (!slot->digout[8].invert)) {
	outdata =1;
      }
      if ((!*(slot->digout[8].data)) && (slot->digout[8].invert)) {
	outdata |= 1;
      }
      slot->wr_buf[DIO_ESTOP_OUT] = outdata;
    }
    else slot->wr_buf[DIO_ESTOP_OUT] = 2;  // force 2 to set additional boards to slave
}

static void read_encoders(slot_data_t *slot)
{
    int i, byteindex;
    union pos_tag {
        signed long l;
        struct byte_tag {
            signed char b0;
            signed char b1;
            signed char b2;
            signed char b3;
        } byte;
    } pos, oldpos;

    byteindex = ENCCNT0;        /* first encoder count register */
    for (i = 0; i < 4; i++) {
      slot->encoder[i].indrescnt++;  /* increment counter each servo cycle */
        oldpos.l = slot->encoder[i].oldreading;
	pos.byte.b0 = (signed char)slot->rd_buf[byteindex++];
	pos.byte.b1 = (signed char)slot->rd_buf[byteindex++];
	pos.byte.b2 = (signed char)slot->rd_buf[byteindex++];
        pos.byte.b3 = oldpos.byte.b3;
        /* 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;
	/* index processing */
	if ( (slot->rd_buf[ENCISR] & ( 1 << i )) != 0 ) {
	  //	  rtapi_print_msg(RTAPI_MSG_INFO, "index seen for axis %d",i);
	  //	  rtapi_print_msg(RTAPI_MSG_INFO, "indrescnt %d\n",slot->encoder[i].indrescnt);
	    /* index edge occurred since last time this code ran */
	    *(slot->encoder[i].index) = 1;
	    /* index-enable only works on version 2 and up */
	    if (slot->ver >= 2) {
		/* were we looking for an index edge? */
		if ( ((slot->encoder[0].indres & ( 1 << i )) != 0) &&
		     (slot->encoder[i].indrescnt > 3)) {
		    /* yes, clear index-enable to announce that we found it */
		    *(slot->encoder[i].index_enable) = 0;
    /* need to properly set the 24->32 bit extension byte */
    if ( pos.byte.b2 < 0 ) {
      /* going backwards */
      pos.byte.b3 = 0xFF;
    } else {
      pos.byte.b3 = 0;
    }
    oldpos.byte.b3 = pos.byte.b3;
		}
	    }
	} else {
	  /* no index edge since last check */
	  *(slot->encoder[i].index) = 0;
	}
	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;
    }
}

/* I can see the puzzled look on your face now.  Why do we need
   a write function for encoders?  You don't write to encoders...
   Well, you do write to the index latching hardware.
*/

static void write_encoders(slot_data_t *slot)
{
    int i;

    if ( slot->ver < 2 ) {
	/* no index support in old boards */
	return;
    }
    for (i = 0; i < 4; i++) {
	if ( *(slot->encoder[i].index_enable) ) {
	    /* all 4 control bits are packed into the same register */
	  if ((slot->encoder[0].indres & (1 << i)) == 0) {
	    slot->encoder[i].indrescnt = 0; /* clear counter first time only */
	    /* set bit to force reset on index pulse */
	    (slot->encoder[0].indres) |= (1 << i);
	  }
	} else {
	    /* clear bit to ignore index pulses */
	    (slot->encoder[0].indres) &= ~(1 << i);
	}
    }
    /* put the control bits in cache for write to hardware */
    slot->wr_buf[ENCINDX] = slot->encoder[0].indres;
}


/* fetch a time parameter (in nS), make sure it is a multiple
   of 100nS, and is between min_ns and 25.4uS, and return the
   value in 10MHz clock pulses. */
static unsigned int ns2cp( hal_u32_t *pns, unsigned int min_ns )
{
    int ns, cp;

    ns = *pns;
    if ( ns < min_ns ) ns = min_ns;
    if ( ns > 25400 ) ns = 25400;
    cp = ns / 100;
    ns = cp * 100;
    *pns = ns;
    return cp;
}


static void write_stepgens(slot_data_t *slot)
{
    int n, reverse, run, pulse_width, pulse_space, setup_time;
    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 200nS (HW limit) */
    pulse_width = ns2cp(&(slot->stepgen->pulse_width_ns), 200);
    /* write pulse width to the cache, inverted */
    slot->wr_buf[RATE_WIDTH_0] = 256 - pulse_width;
    /* pulse space cannot be less than 300nS (HW limitation) */
    pulse_space = ns2cp(&(slot->stepgen->pulse_space_ns), 300);
    /* setup time cannot be less than 2 (HW limit) */
    setup_time = ns2cp(&(slot->stepgen->setup_time_ns), 200);
    /* write it to the cache, inverted */
    slot->wr_buf[RATE_SETUP_0] = 256 - setup_time;
    /* calculate the max frequency, varies with pulse width and
       min pulse spacing */
    bd_max_freq = 10000000.0 / (pulse_width + 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 {