hal_motenc.c

Source code for an Numeric Cmputer

	    break;

	// Parameters.
	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.out-%02d-invert", boardId, channel);
	if((halError = hal_param_bit_new(name, HAL_WR, &(this->out[channel].invert), componentId)) != 0)
	    break;

	// Init pin.
	*(this->out[channel].pValue) = 0;
	this->out[channel].invert = 0;
    }

    // Export functions.
    if(!halError){
	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.digital-out-write", boardId);
	halError = hal_export_funct(name, Device_DigitalOutWrite, this, 0, 0, componentId);
    }

    if(halError){
	rtapi_print_msg(RTAPI_MSG_ERR, "MOTENC: ERROR: export digital out failed\n");
	return(-1);
    }

    return(0);
}


static int
Device_ExportMiscPinsParametersFunctions(Device *this, int componentId, int boardId)
{
    int					halError;
    char				name[HAL_NAME_LEN + 2];

    // Export Pins.
    rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.estop-in", boardId);
    halError = hal_pin_bit_new(name, HAL_WR, &(this->misc.pEstopIn), componentId);

    if(!halError){
	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.estop-in-not", boardId);
	halError = hal_pin_bit_new(name, HAL_WR, &(this->misc.pEstopInNot), componentId);
    }

    if(!halError){
	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.watchdog-reset", boardId);
	halError = hal_pin_bit_new(name, HAL_RD_WR, &(this->misc.pWatchdogReset), componentId);
    }

    // Export Parameters.
    if(!halError){
	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.watchdog-control", boardId);
	halError = hal_param_u32_new(name, HAL_WR, &(this->misc.watchdogControl), componentId);
    }

    // Init pins.
    if(!halError){
	*(this->misc.pEstopIn) = 0;
	*(this->misc.pEstopInNot) = 1;
	*(this->misc.pWatchdogReset) = 0;
	this->misc.watchdogControl = this->watchdogControl;
    }

    // Export functions.
    if(!halError){
	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.misc-update", boardId);
	halError = hal_export_funct(name, Device_MiscUpdate, this, 0, 0, componentId);
    }

    if(halError){
	rtapi_print_msg(RTAPI_MSG_ERR, "MOTENC: ERROR: export miscellaneous failed\n");
	return(-1);
    }

    return(0);
}


/*
 * HAL EXPORTED FUNCTIONS
 */

static void
Device_EncoderRead(void *arg, long period)
{
    Device				*this = (Device *)arg;
    MotencRegMap			*pCard = this->pCard;
    EncoderPinsParams			*pEncoder;
    int					i, j;
    hal_u32_t				status;

    pEncoder = &this->encoder[0];

    // For each FPGA.
    for(i = 0; i < this->numFpga; i++){

	// For each encoder.
	for(j = 0; j < MOTENC_FPGA_NUM_ENCODER_CHANNELS; j++, pEncoder++){

	    // Check reset pin.
	    if(*(pEncoder->pResetCount)){
		// Clear pin.
		*(pEncoder->pResetCount) = 0;

		// Reset encoder.
		pCard->fpga[i].statusControl = 1 << (MOTENC_CONTROL_ENCODER_RESET_SHFT + j);
	    }

	    // Write index latch with pin value.
	    pCard->fpga[i].encoderCount[j] = *(pEncoder->pLatchIndex);

	    // Update index and index latched pins.
	    status = pCard->fpga[i].statusControl;
	    *(pEncoder->pIndex) = (status >> (MOTENC_STATUS_INDEX_SHFT + j)) & 1;
	    *(pEncoder->pIndexLatch) = (status >> (MOTENC_STATUS_INDEX_LATCH_SHFT + j)) & 1;

	    // Read encoder counts.
	    *(pEncoder->pCount) = pCard->fpga[i].encoderCount[j];
	    /* check for change in scale value */
	    if ( pEncoder->scale != pEncoder->old_scale ) {
		/* get ready to detect future scale changes */
		pEncoder->old_scale = pEncoder->scale;
		/* validate the new scale value */
		if ((pEncoder->scale < 1e-20) && (pEncoder->scale > -1e-20)) {
		    /* value too small, divide by zero is a bad thing */
		    pEncoder->scale = 1.0;
		}
		/* we will need the reciprocal */
		pEncoder->scale_recip = 1.0 / pEncoder->scale;
	    }
	    // Scale count to make floating point position.
	    *(pEncoder->pPosition) = *(pEncoder->pCount) * pEncoder->scale_recip;
	}
    }
}


static void
Device_DacWrite(void *arg, long period)
{
    Device				*this = (Device *)arg;
    MotencRegMap			*pCard = this->pCard;
    DacPinsParams			*pDac;
    int					i;
    hal_float_t				volts;
    hal_u32_t				dacCount;

    pDac = &this->dac[0];

    // For each DAC.
    for(i = 0; i < MOTENC_NUM_DAC_CHANNELS; i++, pDac++){

	// Calculate hardware register value.
	volts = (*(pDac->pValue) - pDac->offset) * pDac->gain;

	// Truncate volts to DAC limits.
	if(volts > MOTENC_DAC_VOLTS_MAX){
	    volts = MOTENC_DAC_VOLTS_MAX;
	}else if(volts < MOTENC_DAC_VOLTS_MIN){
	    volts = MOTENC_DAC_VOLTS_MIN;
	}

	// Transform volts to counts.
	dacCount = (hal_u32_t)(volts * MOTENC_DAC_SCALE_MULTIPLY /
				MOTENC_DAC_SCALE_DIVIDE + MOTENC_DAC_COUNT_ZERO);

	// Write DAC.
	pCard->dac[i] = dacCount;
    }
}


static void
Device_AdcRead(void *arg, long period)
{
    Device				*this = (Device *)arg;
    MotencRegMap			*pCard = this->pCard;

    switch(this->adcState){
    // Start conversion on first 4 channels.
    case 0:
	this->adcState++;
	pCard->adcDataCommand = MOTENC_ADC_COMMAND_CHN_0_1_2_3;
	pCard->adcStartConversion = 1;
	break;
    
    // Wait for first conversion, start conversion on second 4 channels.
    case 1:
	if(Device_AdcRead4(this, 0)){
	    this->adcState++;

	    // Start next conversion.
	    pCard->adcDataCommand = MOTENC_ADC_COMMAND_CHN_4_5_6_7;
	    pCard->adcStartConversion = 1;
	}
	break;

    // Wait for second conversion, start conversion on fisrt 4 channels.
    case 2:
	if(Device_AdcRead4(this, 4)){
	    this->adcState = 1;

	    // Start next conversion.
	    pCard->adcDataCommand = MOTENC_ADC_COMMAND_CHN_0_1_2_3;
	    pCard->adcStartConversion = 1;
	}
	break;

    default:
	this->adcState = 0;
    }
}


static int
Device_AdcRead4(Device *this, int startChannel)
{
    MotencRegMap			*pCard = this->pCard;
    AdcPinsParams			*pAdc;
    int					i;
    hal_s32_t				adcCount;
    hal_float_t				volts;

    if(pCard->fpga[0].statusControl & MOTENC_STATUS_ADC_DONE)
	return(0);

    pAdc = &this->adc[startChannel];

    // Get conversion results.
    for(i = 0; i < 4; i++, pAdc++){
	adcCount = pCard->adcDataCommand;

	// Sign extend result.
	if(adcCount & MOTENC_ADC_SIGN_BIT){
	    adcCount |= MOTENC_ADC_SIGN_EXTEND;
	}

	// Transform count to volts.
	volts = adcCount * MOTENC_ADC_SCALE_MULTIPLY / MOTENC_ADC_SCALE_DIVIDE;

	// Scale and offset volts.
	volts = volts * pAdc->gain - pAdc->offset;

	// Update pin.
	*(pAdc->pValue) = volts;
    }

    return(1);
}


static void
Device_DigitalInRead(void *arg, long period)
{
    Device				*this = (Device *)arg;
    MotencRegMap			*pCard = this->pCard;
    DigitalInPinsParams			*pDigitalIn;
    int					i, j, n;
    hal_u32_t				pins;

    pDigitalIn = &this->in[0];

    // For each FPGA.
    for(i = 0; i < this->numFpga; i++){

	// Read digital I/O register.
	pins = ~pCard->fpga[i].digitalIo >> MOTENC_DIGITAL_IN_SHFT;

	// For each pin.
	for(j = 0; j < 16; j++, pDigitalIn++){

	    // Update pins.
	    *(pDigitalIn->pValue) = pins & 1;
	    *(pDigitalIn->pValueNot) = !*(pDigitalIn->pValue);

	    pins >>= 1;
	}

	// Read status register.
	pins = ~pCard->fpga[i].statusControl >> MOTENC_STATUS_DIGITAL_IN_SHFT;

	// First FPGA only has 16 inputs in the status register. The other 4 are
	// used for special purpose inputs like board id, ADC done, and E-STOP.
	n = (i)? 20: 16;

	// For each pin.
	for(j = 0; j < n; j++, pDigitalIn++){

	    // Update pins.
	    *(pDigitalIn->pValue) = pins & 1;
	    *(pDigitalIn->pValueNot) = !*(pDigitalIn->pValue);

	    pins >>= 1;
	}
    }
}


static void
Device_DigitalOutWrite(void *arg, long period)
{
    Device				*this = (Device *)arg;
    MotencRegMap			*pCard = this->pCard;
    DigitalOutPinsParams		*pDigitalOut;
    int					i, j;
    hal_u32_t				pins, mask;

    pDigitalOut = &this->out[0];

    // For each FPGA.
    for(i = 0; i < this->numFpga; i++){

	pins = 0;
	mask = 1;

	// For each pin.
	for(j = 0; j < MOTENC_FPGA_NUM_DIGITAL_OUTPUTS; j++, pDigitalOut++){

	    // Build hardware register value.
	    if(*(pDigitalOut->pValue) != pDigitalOut->invert){
		pins |= mask;
	    }

	    mask <<=1;
	}

	// Write digital I/O register.
	// invert for active low OPTO-22 modules
	pCard->fpga[i].digitalIo = ~pins << MOTENC_DIGITAL_OUT_SHFT;
    }
}


static void
Device_MiscUpdate(void *arg, long period)
{
    Device				*this = (Device *)arg;
    MotencRegMap			*pCard = this->pCard;

    // Check watchdog control parameter.
    if(this->watchdogControl != this->misc.watchdogControl){
	// Update shadow register.
	this->watchdogControl = this->misc.watchdogControl;

	// Write hardware.
	pCard->watchdogControl = this->watchdogControl;
    }

    // Check watchdog reset pin.
    if(*(this->misc.pWatchdogReset)){
	// Clear pin.
	*(this->misc.pWatchdogReset) = 0;

	// Reset the watchdog timer.
	pCard->watchdogReset = MOTENC_WATCHDOG_RESET_VALUE;
    }

    // Update E-STOP pin.
    *(this->misc.pEstopIn) = (pCard->fpga[0].statusControl & MOTENC_STATUS_ESTOP)? 1: 0;
    *(this->misc.pEstopInNot) = !*(this->misc.pEstopIn);
}