motion.c

Source code for an Numeric Cmputer

/********************************************************************
* Description: motion.c
*   Main module initialisation and cleanup routines.
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
* Last change:
* $Revision: 1.51 $
* $Author: cradek $
* $Date: 2006/03/21 02:42:28 $
********************************************************************/

#ifndef RTAPI
#error This is a realtime component only!
#endif
#include <stdarg.h>
#include "rtapi.h"		/* RTAPI realtime OS API */
#include "rtapi_app.h"		/* RTAPI realtime module decls */
#include "hal.h"		/* decls for HAL implementation */
#include "emcmotglb.h"
#include "motion.h"
#include "mot_priv.h"

/***********************************************************************
*                    KERNEL MODULE PARAMETERS                          *
************************************************************************/

#ifdef MODULE
/* module information */
/* register symbols to be modified by insmod
   see "Linux Device Drivers", Alessandro Rubini, p. 385
   (p.42-44 in 2nd edition) */
MODULE_AUTHOR("Matt Shaver/John Kasunich");
MODULE_DESCRIPTION("Motion Controller for EMC");
#ifdef MODULE_LICENSE
MODULE_LICENSE("GPL");
#endif /* MODULE_LICENSE */

/*! \todo FIXME - find a better way to do this */
int DEBUG_MOTION = 0;
MODULE_PARM(DEBUG_MOTION, "i");

/* RTAPI shmem key - for comms with higher level user space stuff */
static int key = 100;		/* the shared memory key, default value */
MODULE_PARM(key, "i");
MODULE_PARM_DESC(key, "shared memory key");

/*! \todo Another #if 0 */
#if 0
/*! \todo FIXME - currently HAL has a fixed stacksize of 16384...
   the upcoming HAL rewrite may make it a paramater of the
   create_thread call, in which case this will be restored */
static int EMCMOT_TASK_STACKSIZE = 8192;	/* default stacksize */
MODULE_PARM(EMCMOT_TASK_STACKSIZE, "i");
MODULE_PARM_DESC(EMCMOT_TASK_STACKSIZE, "motion stack size");
#endif

static long base_period_nsec = 0;	/* fastest thread period */
MODULE_PARM(base_period_nsec, "l");
MODULE_PARM_DESC(base_period_nsec, "fastest thread period (nsecs)");
static long servo_period_nsec = 0;	/* servo thread period */
MODULE_PARM(servo_period_nsec, "l");
MODULE_PARM_DESC(servo_period_nsec, "servo thread period (nsecs)");
static long traj_period_nsec = 0;	/* trajectory planner period */
MODULE_PARM(traj_period_nsec, "l");
MODULE_PARM_DESC(traj_period_nsec, "trajectory planner period (nsecs)");

#endif /* MODULE */

/***********************************************************************
*                  GLOBAL VARIABLE DEFINITIONS                         *
************************************************************************/

/* pointer to emcmot_hal_data_t struct in HAL shmem, with all HAL data */
emcmot_hal_data_t *emcmot_hal_data = 0;

/* pointer to joint data */
emcmot_joint_t *joints = 0;

#ifndef STRUCTS_IN_SHMEM
/* allocate array for joint data */
emcmot_joint_t joint_array[EMCMOT_MAX_AXIS];
#endif

int mot_comp_id;		/* component ID for motion module */

int kinType = 0;

/*
  Principles of communication:

  Data is copied in or out from the various types of comm mechanisms:
  mbuff mapped memory for Linux/RT-Linux, or OS shared memory for Unixes.

  emcmotStruct is ptr to this memory.

  emcmotCommand points to emcmotStruct->command,
  emcmotStatus points to emcmotStruct->status,
  emcmotError points to emcmotStruct->error, and
 */
emcmot_struct_t *emcmotStruct = 0;
/* ptrs to either buffered copies or direct memory for
   command and status */
emcmot_command_t *emcmotCommand = 0;
emcmot_status_t *emcmotStatus = 0;
emcmot_config_t *emcmotConfig = 0;
emcmot_debug_t *emcmotDebug = 0;
emcmot_internal_t *emcmotInternal = 0;
emcmot_error_t *emcmotError = 0;	/* unused for RT_FIFO */

/***********************************************************************
*                  LOCAL VARIABLE DECLARATIONS                         *
************************************************************************/

/* RTAPI shmem ID - for comms with higher level user space stuff */
static int emc_shmem_id;	/* the shared memory ID */

/***********************************************************************
*                   LOCAL FUNCTION PROTOTYPES                          *
************************************************************************/

/* init_hal_io() exports HAL pins and parameters making data from
   the realtime control module visible and usable by the world
*/
static int init_hal_io(void);

/* functions called by init_hal_io() */
static int export_axis(int num, axis_hal_t * addr);

/* init_comm_buffers() allocates and initializes the command,
   status, and error buffers used to communicate witht the user
   space parts of emc.
*/
static int init_comm_buffers(void);

/* functions called by init_comm_buffers() */

/* init_threads() creates realtime threads, exports functions to
   do the realtime control, and adds the functions to the threads.
*/
static int init_threads(void);

/* functions called by init_threads() */
static int setTrajCycleTime(double secs);
static int setServoCycleTime(double secs);

/***********************************************************************
*                     PUBLIC FUNCTION CODE                             *
************************************************************************/

void emcmot_config_change(void)
{
    if (emcmotConfig->head == emcmotConfig->tail) {
	emcmotConfig->config_num++;
	emcmotStatus->config_num = emcmotConfig->config_num;
	emcmotConfig->head++;
    }
}

void reportError(const char *fmt, ...)
{
    va_list args;
    char error[EMCMOT_ERROR_LEN + 2];

    va_start(args, fmt);
    /* Don't use the rtapi_snprintf... */
    vsnprintf(error, EMCMOT_ERROR_LEN, fmt, args);
    va_end(args);
/*! \todo FIXME - eventually should print _only_ to the RCS buffer, I think */
/* print to the kernel buffer... */
    rtapi_print("%d: ERROR: %s\n", emcmotStatus->heartbeat, error);
/* print to the RCS buffer... */
    emcmotErrorPut(emcmotError, error);
}

int init_module(void)
{
    int retval;

    /*! \todo FIXME - debug only */
//    rtapi_set_msg_level(RTAPI_MSG_ALL);

    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_module() starting...\n");

    /* connect to the HAL and RTAPI */
    mot_comp_id = hal_init("motmod");
    if (mot_comp_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR, "MOTION: hal_init() failed\n");
	return -1;
    }

    /* initialize/export HAL pins and parameters */
    retval = init_hal_io();
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR, "MOTION: init_hal_io() failed\n");
	hal_exit(mot_comp_id);
	return -1;
    }

    /* allocate/initialize user space comm buffers (cmd/status/err) */
    retval = init_comm_buffers();
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "MOTION: init_comm_buffers() failed\n");
	hal_exit(mot_comp_id);
	return -1;
    }

    /* set up for realtime execution of code */
    retval = init_threads();
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR, "MOTION: init_threads() failed\n");
	hal_exit(mot_comp_id);
	return -1;
    }

    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_module() complete\n");

    return 0;
}

void cleanup_module(void)
{
//    int axis;
    int retval;

    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: cleanup_module() started.\n");

    retval = hal_stop_threads();
    if (retval != HAL_SUCCESS) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "MOTION: hal_stop_threads() failed, returned %d\n", retval);
    }
/*! \todo Another #if 0 */
#if 0
    /* WPS these were moved from above to avoid a possible mutex problem. */
    /* There is no point in clearing the trajectory queue since the planner
       should be dead by now anyway. */
    if (emcmotStruct != 0 && emcmotDebug != 0 && emcmotConfig != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR, "motion: disabling amps\n");
	for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
	    extAmpEnable(axis, !GET_AXIS_ENABLE_POLARITY(axis));
	}
    }
#endif

    /* free shared memory */
    retval = rtapi_shmem_delete(emc_shmem_id, mot_comp_id);
    if (retval != RTAPI_SUCCESS) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "MOTION: rtapi_shmem_delete() failed, returned %d\n", retval);
    }
    /* disconnect from HAL and RTAPI */
    retval = hal_exit(mot_comp_id);
    if (retval != HAL_SUCCESS) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "MOTION: hal_exit() failed, returned %d\n", retval);
    }
    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: cleanup_module() finished.\n");
}

/***********************************************************************
*                         LOCAL FUNCTION CODE                          *
************************************************************************/

/* init_hal_io() exports HAL pins and parameters making data from
   the realtime control module visible and usable by the world
*/
static int init_hal_io(void)
{
    int n, retval;
    axis_hal_t *axis_data;
    char buf[HAL_NAME_LEN + 2];

    rtapi_print_msg(RTAPI_MSG_ERR, "MOTION: init_hal_io() starting...\n");

    /* allocate shared memory for machine data */
    emcmot_hal_data = hal_malloc(sizeof(emcmot_hal_data_t));
    if (emcmot_hal_data == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "MOTION: emcmot_hal_data malloc failed\n");
	return -1;
    }

    /* export machine wide hal pins */
    rtapi_snprintf(buf, HAL_NAME_LEN, "motion.probe-input");
    retval =
	hal_pin_bit_new(buf, HAL_RD, &(emcmot_hal_data->probe_input),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }

    rtapi_snprintf(buf, HAL_NAME_LEN, "motion.spindle-sync");
    retval =
	hal_pin_bit_new(buf, HAL_WR, &(emcmot_hal_data->spindle_sync),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }

    rtapi_snprintf(buf, HAL_NAME_LEN, "motion.spindle-revs");
    retval =
	hal_pin_float_new(buf, HAL_RD, &(emcmot_hal_data->spindle_revs),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }

    rtapi_snprintf(buf, HAL_NAME_LEN, "motion.enable");
    retval =
	hal_pin_bit_new(buf, HAL_RD, &(emcmot_hal_data->enable),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }

    /* export motion-synched digital output pins */
    for (n = 0; n < EMCMOT_MAX_DIO; n++) {
	rtapi_snprintf(buf, HAL_NAME_LEN, "motion.digital-out-%02d",n);
	retval =
	    hal_pin_bit_new(buf, HAL_WR, &(emcmot_hal_data->synch_do[n]),
	    mot_comp_id);
	if (retval != 0) {
	    return retval;
	}
    }

    /* export machine wide hal parameters */
    rtapi_snprintf(buf, HAL_NAME_LEN, "motion.motion-enabled");
    retval =
	hal_param_bit_new(buf, HAL_RD, &(emcmot_hal_data->motion_enabled),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "motion.in-position");
    retval =
	hal_param_bit_new(buf, HAL_RD, &(emcmot_hal_data->in_position),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "motion.coord-mode");
    retval =
	hal_param_bit_new(buf, HAL_RD, &(emcmot_hal_data->coord_mode),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "motion.teleop-mode");
    retval =
	hal_param_bit_new(buf, HAL_RD, &(emcmot_hal_data->teleop_mode),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "motion.coord-error");
    retval =
	hal_param_bit_new(buf, HAL_RD, &(emcmot_hal_data->coord_error),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export debug parameters */
    /* these can be used to view any internal variable, simply change a line
       in control.c:output_to_hal() and recompile */
    rtapi_snprintf(buf, HAL_NAME_LEN, "motion.debug-bit-0");
    retval =
	hal_param_bit_new(buf, HAL_RD, &(emcmot_hal_data->debug_bit_0),
	mot_comp_id);