hal_motenc.c

Source code for an Numeric Cmputer

	    pDevice->boardID = j;
	    rtapi_print_msg(RTAPI_MSG_WARN, "MOTENC: WARNING, duplicate ID, remapped to %d\n", j);
	}
	driver.idPresent[pDevice->boardID] = 1;
	
	// Export pins, parameters, and functions.
	if(Device_ExportPinsParametersFunctions(pDevice, driver.componentId)){
	    hal_exit(driver.componentId);
	    return(-1);
	}
    }

    if(pCard == NULL){
	// No card present.
	rtapi_print_msg(RTAPI_MSG_WARN, "MOTENC: **** No MOTENC card detected ****\n");
	hal_exit(driver.componentId);
	return -1;
    }

    // Was 'period' specified in the insmod command?
    if (period > 0) {

	// Create a thread.
	if (hal_create_thread("motenc.thread", period, 1) < 0){
	    rtapi_print_msg(RTAPI_MSG_ERR, "MOTENC: ERROR: hal_create_thread() failed\n");
	    hal_exit(driver.componentId);
	    return(-1);
	} else {
	    rtapi_print_msg(RTAPI_MSG_INFO, "MOTENC: created %d uS thread\n", period / 1000);
	}
    }

    return(0);
}


void
rtapi_app_exit(void)
{
    int					i, j;
    Device				*pDevice;

    hal_exit(driver.componentId);

    for(i = 0; i < MAX_DEVICES; i++){
	if((pDevice = driver.deviceTable[i]) != NULL){
	    // turn off digital outputs
	    for(j = 0; j < pDevice->numFpga; j++){
		pDevice->pCard->fpga[i].digitalIo = MOTENC_DIGITAL_OUT;
	    }
	    // set DAC outputs to zero volts
	    for(i = 0; i < MOTENC_NUM_DAC_CHANNELS; i++){
		pDevice->pCard->dac[i] = MOTENC_DAC_COUNT_ZERO;
	    }
	    
	    // Unmap card.
	    iounmap((void *)(pDevice->pCard));

	    // TODO: Free device object when HAL supports free.
//	    hal_free(pDevice);
	}
    }
}


/******************************************************************************
 * DEVICE OBJECT FUNCTION DEFINITIONS
 ******************************************************************************/

/*
 * LOCAL FUNCTIONS
 */

static int
Device_Init(Device *this, MotencRegMap *pCard)
{
    int i, status;

    this->pCard = pCard;
    this->adcState = 0;
    this->watchdogControl = 0;

    // identify type of board
    status = pCard->fpga[0].boardVersion;
    if ( status == 0 ) {
	// MOTENC-100
	this->boardType = 1;
	this->typeName = "MOTENC-100";
	this->numFpga = 2;
    } else if ( status == 1 ) {
	// MOTENC-Lite
	this->boardType = 2;
	this->typeName = "MOTENC-Lite";
	this->numFpga = 1;
    } else {
	// no idea what it is
	this->boardType = 0;
	this->typeName = "unknown";
	this->numFpga = 0;
	return -1;
    }
    // extract board id from first FPGA. The user sets this via jumpers on the card.
    status = pCard->fpga[0].statusControl;
    this->boardID = (status & MOTENC_STATUS_BOARD_ID) >> MOTENC_STATUS_BOARD_ID_SHFT;
    
    // Initialize hardware.
    for(i = 0; i < this->numFpga; i++){
	pCard->fpga[i].digitalIo = MOTENC_DIGITAL_OUT;
	pCard->fpga[i].statusControl = MOTENC_CONTROL_ENCODER_RESET;
    }

    for(i = 0; i < MOTENC_NUM_DAC_CHANNELS; i++){
	pCard->dac[i] = MOTENC_DAC_COUNT_ZERO;
    }

    pCard->timerIrqDisable = 1;
    pCard->watchdogControl = this->watchdogControl;

    return(0);
}


static int
Device_ExportPinsParametersFunctions(Device *this, int componentId)
{
    int					msgLevel, boardId, error;

    // This function exports a lot of stuff, which results in a lot of
    // logging if msg_level is at INFO or ALL. So we save the current value
    // of msg_level and restore it later.  If you actually need to log this
    // function's actions, change the second line below.
    msgLevel = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);

    boardId = this->boardID;
    
    // Export encoders.
    error = Device_ExportEncoderPinsParametersFunctions(this, componentId, boardId);

    // Export DACs.
    if(!error) error = Device_ExportDacPinsParametersFunctions(this, componentId, boardId);

    // Export ADCs.
    if(!error) error = Device_ExportAdcPinsParametersFunctions(this, componentId, boardId);

    // Export digital I/O.
    if(!error) error = Device_ExportDigitalInPinsParametersFunctions(this, componentId, boardId);
    if(!error) error = Device_ExportDigitalOutPinsParametersFunctions(this, componentId, boardId);

    // Export miscellaneous.
    if(!error) error = Device_ExportMiscPinsParametersFunctions(this, componentId, boardId);

    // Restore saved message level.
    rtapi_set_msg_level(msgLevel);

    return(error);
}


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

    // Export pins and parameters.
    halError = 0;
    for(channel = 0; channel < this->numFpga * MOTENC_FPGA_NUM_ENCODER_CHANNELS; channel++){
	// Pins.
	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.enc-%02d-count", boardId, channel);
	if((halError = hal_pin_s32_new(name, HAL_WR, &(this->encoder[channel].pCount), componentId)) != 0)
	    break;

	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.enc-%02d-position", boardId, channel);
	if((halError = hal_pin_float_new(name, HAL_WR, &(this->encoder[channel].pPosition), componentId)) != 0)
	    break;

	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.enc-%02d-index", boardId, channel);
	if((halError = hal_pin_bit_new(name, HAL_WR, &(this->encoder[channel].pIndex), componentId)) != 0)
	    break;

	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.enc-%02d-idx-latch", boardId, channel);
	if((halError = hal_pin_bit_new(name, HAL_WR, &(this->encoder[channel].pIndexLatch), componentId)) != 0)
	    break;

	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.enc-%02d-latch-index", boardId, channel);
	if((halError = hal_pin_bit_new(name, HAL_RD, &(this->encoder[channel].pLatchIndex), componentId)) != 0)
	    break;

	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.enc-%02d-reset-count", boardId, channel);
	if((halError = hal_pin_bit_new(name, HAL_RD_WR, &(this->encoder[channel].pResetCount), componentId)) != 0)
	    break;

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

	// Init encoder.
	*(this->encoder[channel].pCount) = 0;
	*(this->encoder[channel].pPosition) = 0.0;
	*(this->encoder[channel].pIndex) = 0;
	*(this->encoder[channel].pIndexLatch) = 0;
	*(this->encoder[channel].pLatchIndex) = 0;
	*(this->encoder[channel].pResetCount) = 0;
	this->encoder[channel].scale = 1.0;
    }

    // Export functions.
    if(!halError){
	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.encoder-read", boardId);
	halError = hal_export_funct(name, Device_EncoderRead, this, 1, 0, componentId);
    }

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

    return(0);
}


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

    // Export pins and parameters.
    halError = 0;
    for(channel = 0; channel < MOTENC_NUM_DAC_CHANNELS; channel++){
	// Pins.
	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.dac-%02d-value", boardId, channel);
	if((halError = hal_pin_float_new(name, HAL_RD, &(this->dac[channel].pValue), componentId)) != 0)
	    break;

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

	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.dac-%02d-gain", boardId, channel);
	if((halError = hal_param_float_new(name, HAL_WR, &(this->dac[channel].gain), componentId)) != 0)
	    break;

	// Init DAC.
	*(this->dac[channel].pValue) = 0.0;
	this->dac[channel].offset = 0.0;
	this->dac[channel].gain = 1.0;
    }

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

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

    return(0);
}


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

    // Export pins and parameters.
    halError = 0;
    for(channel = 0; channel < MOTENC_NUM_ADC_CHANNELS; channel++){
	// Pins.
	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.adc-%02d-value", boardId, channel);
	if((halError = hal_pin_float_new(name, HAL_WR, &(this->adc[channel].pValue), componentId)) != 0)
	    break;

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

	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.adc-%02d-gain", boardId, channel);
	if((halError = hal_param_float_new(name, HAL_WR, &(this->adc[channel].gain), componentId)) != 0)
	    break;

	// Init ADC.
	*(this->adc[channel].pValue) = 0.0;
	this->adc[channel].offset = 0.0;
	this->adc[channel].gain = 1.0;
    }

    // Export functions.
    if(!halError){
	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.adc-read", boardId);
	halError = hal_export_funct(name, Device_AdcRead, this, 1, 0, componentId);
    }

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

    return(0);
}


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

    // Export pins and parameters.
    halError = 0;
    for(channel = 0; channel < (this->numFpga * MOTENC_FPGA_NUM_DIGITAL_INPUTS - 4); channel++){
	// Pins.
	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.in-%02d", boardId, channel);
	if((halError = hal_pin_bit_new(name, HAL_WR, &(this->in[channel].pValue), componentId)) != 0)
	    break;

	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.in-%02d-not", boardId, channel);
	if((halError = hal_pin_bit_new(name, HAL_WR, &(this->in[channel].pValueNot), componentId)) != 0)
	    break;

	// Init pin.
	*(this->in[channel].pValue) = 0;
	*(this->in[channel].pValueNot) = 1;
    }

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

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

    return(0);
}


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

    // Export pins and parameters.
    halError = 0;
    for(channel = 0; channel < this->numFpga * MOTENC_FPGA_NUM_DIGITAL_OUTPUTS; channel++){
	// Pins.
	rtapi_snprintf(name, HAL_NAME_LEN, "motenc.%d.out-%02d", boardId, channel);
	if((halError = hal_pin_bit_new(name, HAL_RD, &(this->out[channel].pValue), componentId)) != 0)