hal_stg.c

Source code for an Numeric Cmputer

/********************************************************************
* Description:  hal_stg.c
*               This is the driver for Servo-To-Go Model I & II board.
*
* Author: Alex Joni
* License: GPL Version 2
*    
* Copyright (c) 2004 All rights reserved.
* see below for aditional notes
*
* Last change: 
* $Revision: 1.18 $
* $Author: alex_joni $
* $Date: 2006/03/21 00:17:13 $
********************************************************************/

/** This is the driver for Servo-To-Go Model I & II board.
    The board includes 8 channels of quadrature encoder input,
    8 channels of analog input and output, 32 bits digital I/O,
    and an interval timer with interrupt.
        
    Installation of the driver only realtime:
    
	insmod hal_stg num_chan=8 dio="IIOO"
	    - autodetects the address
	or
    
	insmod hal_stg base=0x200 num_chan=8 dio="IIOO"
    
    Check your Hardware manual for your base address.

    The digital inputs/outputs configuration is determined by a 
    config string passed to insmod when loading the module.  
    The format consists by a four character string that sets the
    direction of each group of pins. Each character of the direction
    string is either "I" or "O".  The first character sets the
    direction of port A (Port A - DIO.0-7), the next sets 
    port B (Port B - DIO.8-15), the next sets port C (Port C - DIO.16-23), 
    and the fourth sets port D (Port D - DIO.24-31).
    
    The following items are exported to the HAL.
   
    Encoders:
      Parameters:
	float	stg.<channel>.position-scale  (counts per unit)
   
      Pins:
	s32	stg.<channel>.counts
	float	stg.<channel>.position

/todo   bit	stg.<channel>.enc-index
/todo  	bit	stg.<channel>.enc-idx-latch
/todo  	bit	stg.<channel>.enc-latch-index
/todo  	bit	stg.<channel>.enc-reset-count
   
      Functions:
        void    stg.<channel>.capture_position
   
   
    DACs:
      Parameters:
	float	stg.<channel>.dac-offset
	float	stg.<channel>.dac-gain
   
      Pins:
	float	stg.<channel>.dac-value
   
      Functions:
	void    stg.<channel>.dac-write
   
   
    ADC:
      Parameters:
/totest	float	stg.<channel>.adc-offset
/totest	float	stg.<channel>.adc-gain
   
      Pins:
/totest	float	stg.<channel>.adc-value
   
      Functions:
/totest	void    stg.<channel>.adc-read
   
   
    Digital In:
      Pins:
	bit	stg.in-<pinnum>
	bit	stg.in-<pinnum>-not
   
      Functions:
	void    stg.digital-in-read
   
   
    Digital Out:
      Parameters:
	bit	stg.out-<pinnum>-invert
   
      Pins:
	bit	stg.out-<pinnum>
   
      Functions:
	void    stg.digital-out-write

*/

/** Copyright (C) 2004 Alex Joni
                       <alex DOT joni AT robcon DOT ro>
*/

/** Copyright (C) 2003 John Kasunich
                       <jmkasunich AT users DOT sourceforge DOT net>
*/

/* Based on STGMEMBS.CPP from the Servo To Go Windows drivers 
    - Copyright (c) 1996 Servo To Go Inc and released under GPL Version 2 */
/* Also relates to the EMC1 code (very similar to STGMEMBS.CPP)
    work done by Fred Proctor, Will Shackleford */

/** This program is free software; you can redistribute it and/or
    modify it under the terms of version 2.1 of the GNU General
    Public License as published by the Free Software Foundation.
    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111 USA

    THE AUTHORS OF THIS LIBRARY ACCEPT ABSOLUTELY NO LIABILITY FOR
    ANY HARM OR LOSS RESULTING FROM ITS USE.  IT IS _EXTREMELY_ UNWISE
    TO RELY ON SOFTWARE ALONE FOR SAFETY.  Any machinery capable of
    harming persons must have provisions for completely removing power
    from all motors, etc, before persons enter any danger area.  All
    machinery must be designed to comply with local and national safety
    codes, and the authors of this software can not, and do not, take
    any responsibility for such compliance.

    This code was written as part of the EMC HAL project.  For more
    information, go to www.linuxcnc.org.
*/


#ifndef RTAPI
#error This is a realtime component only!
#endif

#include <asm/io.h>
#include "rtapi.h"		/* RTAPI realtime OS API */
#include "rtapi_app.h"		/* RTAPI realtime module decls */
#include "hal.h"		/* HAL public API decls */
#include "hal_stg.h"		/* STG related defines */

#ifndef MODULE
#define MODULE
#endif

#ifdef MODULE
/* module information */
MODULE_AUTHOR("Alex Joni");
MODULE_DESCRIPTION("Driver for Servo-to-Go Model I  & II for EMC HAL");
#ifdef MODULE_LICENSE
MODULE_LICENSE("GPL");
#endif /* MODULE_LICENSE */
static int base = 0x00;		/* board base address, 0 means autodetect */
MODULE_PARM(base, "i");
MODULE_PARM_DESC(base, "board base address, don't use for autodetect");
static int model = 0;		/* board model, leave empty and autodetect */
MODULE_PARM(model, "i");
MODULE_PARM_DESC(model, "board model, use with caution. it overrides the detected model");
static int num_chan = MAX_CHANS;	/* number of channels - default = 8 */
MODULE_PARM(num_chan, "i");
MODULE_PARM_DESC(num_chan, "number of channels");
static long period = 0;			/* thread period - default = no thread */
MODULE_PARM(period, "l");
MODULE_PARM_DESC(period, "thread period (nsecs)");
static char *dio = "IIOO";		/* dio config - default = port A&B inputs, port C&D outputs */
MODULE_PARM(dio, "s");
MODULE_PARM_DESC(dio, "dio config string - expects something like IIOO");
#endif /* MODULE */

#define	EPSILON		1e-20

/***********************************************************************
*                STRUCTURES AND GLOBAL VARIABLES                       *
************************************************************************/

typedef struct {
    hal_bit_t *data;		/* basic pin for input or output */
    union {
	hal_bit_t *not;		/* pin for inverted data (input only) */
	hal_bit_t invert;	/* param for inversion (output only) */
	} io;
} io_pin;

typedef struct {
/* counter data */
    hal_s32_t *count[MAX_CHANS];		/* captured binary count value */
    hal_float_t *pos[MAX_CHANS];		/* scaled position (floating point) */
    hal_float_t pos_scale[MAX_CHANS];		/* parameter: scaling factor for pos */

/* dac data */
    hal_float_t *dac_value[MAX_CHANS];		/* value to be written to dac */
    hal_float_t dac_offset[MAX_CHANS];		/* offset value for DAC */
    hal_float_t dac_gain[MAX_CHANS];		/* gain to be applied */

/* adc data */
    hal_float_t *adc_value[MAX_CHANS];		/* value to be read from adc */
    hal_float_t adc_offset[MAX_CHANS];		/* offset value for ADC */
    hal_float_t adc_gain[MAX_CHANS];		/* gain to be applied */
    int adc_current_chan;			/* holds the currently converting channel */

/* dio data */
    io_pin port[4][8];				/* holds 4 ports each 8 pins, either input or output */
    unsigned char dir_bits;			/* remembers config (which port is input which is output) */
    
    unsigned char model;

} stg_struct;

static stg_struct *stg_driver;

/* other globals */
static int comp_id;		/* component ID */
static int outpinnum=0, inputpinnum=0;

#define DATA(x) (base + (2 * x) - (x % 2))	/* Address of Data register 0 */
#define CTRL(x) (base + (2 * (x+1)) - (x % 2))	/* Address of Control register 0 */

/***********************************************************************
*                  LOCAL FUNCTION DECLARATIONS                         *
************************************************************************/
/* helper functions, to export HAL pins & co. */
static int export_counter(int num, stg_struct * addr);
static int export_dac(int num, stg_struct * addr);
static int export_adc(int num, stg_struct * addr);
static int export_pins(int num, int dir, stg_struct * addr);
static int export_input_pin(int pinnum, io_pin * pin);
static int export_output_pin(int pinnum, io_pin * pin);

/* Board specific functions */

/* initializes the STG, takes care of autodetection, all initialisations */
static int stg_init_card(void);
/* sets up interrupt to be used */
static int stg_set_interrupt(short interrupt);
/* scans possible addresses for STG cards */
static unsigned short stg_autodetect(void);

/* counter related functions */
static int stg_counter_init(int ch);
static long stg_counter_read(int i);

/* dac related functions */
static int stg_dac_init(int ch);
static int stg_dac_write(int ch, short value);

/* adc related functions */
static int stg_adc_init(int ch);
static int stg_adc_start(void *arg, unsigned short wAxis);
static short stg_adc_read(void *arg, int ch);

/* dio related functions */
static int stg_dio_init(void);

/* periodic functions registered to HAL */
static void stg_adcs_read(void *arg, long period); //reads adc data from the board, check long description at the beginning of the function
static void stg_dacs_write(void *arg, long period); //writes dac's to the STG
static void stg_counter_capture(void *arg, long period); //captures encoder counters
static void stg_di_read(void *arg, long period); //reads digital inputs from the STG
static void stg_do_write(void *arg, long period); //writes digital outputs to the STG

/***********************************************************************
*                       INIT AND EXIT CODE                             *
************************************************************************/

#define MAX_CHAN 8

int rtapi_app_main(void)
{
    int n, retval, mask, m;
    unsigned char dir_bits;

    /* test for number of channels */
    if ((num_chan <= 0) || (num_chan > MAX_CHAN)) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "STG: ERROR: invalid num_chan: %d\n", num_chan);
	return -1;
    }

    /* test for config string */
    if ((dio == 0) || (dio[0] == '\0')) {
	rtapi_print_msg(RTAPI_MSG_ERR, "STG: ERROR: no dio config string\n");
	return -1;
    }

    /* have good config info, connect to the HAL */
    comp_id = hal_init("hal_stg");
    if (comp_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR, "STG: ERROR: hal_init() failed\n");
	return -1;
    }

    /* allocate shared memory for stg data */
    stg_driver = hal_malloc(num_chan * sizeof(stg_struct));
    if (stg_driver == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR, "STG: ERROR: hal_malloc() failed\n");
	hal_exit(comp_id);
	return -1;
    }

    /* takes care of all initialisations, also autodetection and model if necessary */
    if ((retval=stg_init_card()) != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "STG: ERROR: stg_init_card() failed\n");
	hal_exit(comp_id);
	return retval;
    }

    /* dio should be a string of 4 'I" or "O" characters */
    dir_bits = 0;
    mask = 0x01;
    for ( m = 0 ; m < 4 ; m++ ) {
	/* test character and set/clear bit */
	if ((dio[m] == 'i') || (dio[m] == 'I')) {
	    /* input, set mask bit to zero */
	    dir_bits &= ~mask;
	} else if ((dio[m] == 'o') || (dio[m] == 'O')) {
	    /* output, set mask bit to one */
	    dir_bits |= mask;
	} else {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		"STG: ERROR: bad config info for port %d\n", m);
	    return -1;
	}
	/* shift mask for next bit */
	mask <<= 1;
    }

    /* we now should have directions figured out, next is exporting the pins based on that */
    mask = 0x01;
    for ( m = 0 ; m < 4 ; m++ ) {
    
	/*          port, direction, driver */
	export_pins(m, (dir_bits & mask), stg_driver);

	/* shift mask for next bit */
	mask <<= 1;
    }
    stg_driver->dir_bits = dir_bits; /* remember direction of each port, will be used in the write / read functions */

    stg_dio_init();
    
    /* export all the variables for each counter, dac */
    for (n = 0; n < num_chan; n++) {
	/* export all vars */
	retval = export_counter(n, stg_driver);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		"STG: ERROR: counter %d var export failed\n", n + 1);
	    hal_exit(comp_id);
	    return -1;
	}
	/* init counter */
	*(stg_driver->count[n]) = 0;
	*(stg_driver->pos[n]) = 0.0;
	stg_driver->pos_scale[n] = 1.0;

	/* init counter chip */
	stg_counter_init(n);
	
	retval = export_dac(n, stg_driver);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		"STG: ERROR: dac %d var export failed\n", n + 1);
	    hal_exit(comp_id);
	    return -1;
	}
	/* init counter */
	*(stg_driver->dac_value[n]) = 0;
	stg_driver->dac_offset[n] = 0.0;
	stg_driver->dac_gain[n] = 1.0;

	/* init dac chip */
	stg_dac_init(n);

	retval = export_adc(n, stg_driver);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		"STG: ERROR: adc %d var export failed\n", n + 1);
	    hal_exit(comp_id);
	    return -1;
	}
	/* init counter */
	*(stg_driver->adc_value[n]) = 0;
	stg_driver->adc_offset[n] = 0.0;
	stg_driver->adc_gain[n] = 1.0;
	
	stg_driver->adc_current_chan = -1; /* notify that no conversion has been started yet */

	/* init adc chip */
	stg_adc_init(n);
    }
    
    /* export functions */
    retval = hal_export_funct("stg.capture-position", stg_counter_capture,
	stg_driver, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "STG: ERROR: stg.counter-capture funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    rtapi_print_msg(RTAPI_MSG_INFO,
	"STG: installed %d encoder counters\n", num_chan);

    retval = hal_export_funct("stg.write-dacs", stg_dacs_write,
	stg_driver, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,