blocks.c

CNC 的开放码,EMC2 V2.2.8版

/********************************************************************
* Description: blocks.c
*   Assorted simple functional blocks for HAL.
*
*   See the "Users Manual" at emc2/docs/Hal_Introduction.pdf
*
*   This HAL component contains an assortment of simple blocks.
*   Each individual type of block is invoked by a parameter on
*   the insmdo command line.  Each parameter is of the form:
*   <blockname>=<number_of_blocks>
*
*   For example, wcomp=2 installs two window comparators.
*
*   List of blocks currently implemented:
*     constant = pin controlled by parameter
*     wcomp = window comparator - out is true if min < in < max
*     comp = 2 input comparator - out is true if in1 > in0
*     sum2 = 2 input summer - out = in1 * gain1 + in2 * gain2
*     mult2 = 2 input multiplier: out = in0 * in1
*     mux2 = two input analog mux - out = in1 if sel is true, else in0
*     mux4 = four input analog mux - out = in<n> based on sel1,sel0
*     integ = integrator, out = integral of in
*     ddt = differentiator, out = derivative of in
*     limit1 = first order limiter (limits output)
*     limit2 = second order limiter (limits output and 1st derivative)
*     limit3 = third order limiter (limits output, 1st & 2nd derivative)
*     estop = latch for estops or other faults, with reset
*     not = logical inverter - out is true if in is false
*     and2 = 2 input logical and - out is true if in1 and in2 are true
*     or2 = 2 input logical or - out is true if in1 or in2 is true
*     scale = gain/offset block - out = in * gain + offset
*     lowpass = lowpass filter - out = last_out * (1 - gain) + in * gain
*     match8 = 8 bit binary match detector (with input for cascading)
*     hypot = like libm hypot(): out = sqrt(in0*in0 + in1*in1 + in2*in2)
*     minmax = min/max block: min and max outputs are peak values of in
*
*********************************************************************
*
* Author: John Kasunich (jmkasunich AT att DOT net)
* License: GPL Version 2
* Created on: 2004/06/14
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
* Last change:
* $Revision: 1.26 $
* $Author: jepler $
* $Date: 2007/01/19 15:40:46 $
*
********************************************************************/

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

#include "rtapi.h"		/* RTAPI realtime OS API */
#include "rtapi_app.h"		/* RTAPI realtime module decls */
#include "hal.h"		/* HAL public API decls */

#include <linux/types.h>
#define hypot libm_hypot
#include "rtapi_math.h"
#undef hypot

/* module information */
MODULE_AUTHOR("John Kasunich");
MODULE_DESCRIPTION("Functional blocks for EMC HAL");
MODULE_LICENSE("GPL");
static int constant = 0;	/* number of constants */
RTAPI_MP_INT(constant, "constants");
static int wcomp = 0;		/* number of window comps */
RTAPI_MP_INT(wcomp, "window comparators");
static int comp = 0;		/* number of 2-input comps */
RTAPI_MP_INT(comp, "2-input comparators");
static int sum2 = 0;		/* number of 2-input summers */
RTAPI_MP_INT(sum2, "2-input summers");
static int mult2 = 0;		/* number of 2-input multipliers */
RTAPI_MP_INT(mult2, "2-input multiplier");
static int hypot = 0;		/* number of hypot calculators */
RTAPI_MP_INT(hypot, "hypot calculators");
static int mux2 = 0;		/* number of 2-input muxes */
RTAPI_MP_INT(mux2, "2-input multiplexors");
static int mux4 = 0;		/* number of 4-input muxes */
RTAPI_MP_INT(mux4, "4-input multiplexors");
static int integ = 0;		/* number of integerators */
RTAPI_MP_INT(integ, "integrators");
static int ddt = 0;		/* number of differentiators */
RTAPI_MP_INT(ddt, "differentiators");
static int limit1 = 0;		/* number of limiters */
RTAPI_MP_INT(limit1, "first order limiters");
static int limit2 = 0;		/* number of limiters */
RTAPI_MP_INT(limit2, "second order limiters");
static int limit3 = 0;		/* number of limiters */
RTAPI_MP_INT(limit3, "third order limiters");
static int n_estop = 0;		/* number of estop blocks */
RTAPI_MP_INT(n_estop, "estop latch blocks");
static int not = 0;            /* number of logical nots */
RTAPI_MP_INT(not, "logical inverters");
static int and2 = 0;            /* number of 2-input logical ands */
RTAPI_MP_INT(and2, "2-input logical ands");
static int or2 = 0;            /* number of 2-input logical ors */
RTAPI_MP_INT(or2, "2-input logical ors");
static int scale = 0;            /* number of scales */
RTAPI_MP_INT(scale, "scales");
static int lowpass = 0;            /* number of lowpass-filters */
RTAPI_MP_INT(lowpass, "lowpass-filters");
static int match8 = 0;            /* number of match detectors */
RTAPI_MP_INT(match8, "match detectors");
static int minmax = 0;            /* number of min/max blocks */
RTAPI_MP_INT(minmax, "minmax blocks");

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

/** These structures contain the runtime data for a single block. */

typedef struct {
    hal_float_t *out;		/* pin: output */
    hal_float_t value;		/* parameter: value of constant */
} constant_t;

typedef struct {
    hal_float_t *in;		/* pin: input */
    hal_bit_t *out;		/* pin: output */
    hal_float_t min;		/* parameter: low threashold */
    hal_float_t max;		/* parameter: high threashold */
} wcomp_t;

typedef struct {
    hal_float_t *in0;		/* pin: input0 (inverting) */
    hal_float_t *in1;		/* pin: input1 (non-inverting) */
    hal_bit_t *out;		/* pin: output */
    hal_float_t hyst;		/* parameter: hysteresis */
} comp_t;

typedef struct {
    hal_float_t *in0;		/* pin: input for X axis */
    hal_float_t *in1;		/* pin: input for Y axis */
    hal_float_t *in2;		/* pin: input for Z axis */
    hal_float_t *out;		/* pin: output */
} hypot_t;

typedef struct {
    hal_float_t *in0;		/* pin: input used when sel = 0 */
    hal_float_t *in1;		/* pin: input used when sel != 0 */
    hal_float_t *out;		/* pin: output */
    hal_bit_t *sel;		/* pin: select input */
} mux2_t;

typedef struct {
    hal_float_t *in0;		/* pin: input when sel1,0 = 0,0 */
    hal_float_t *in1;		/* pin: input when sel1,0 = 0,1 */
    hal_float_t *in2;		/* pin: input when sel1,0 = 1,0 */
    hal_float_t *in3;		/* pin: input when sel1,0 = 1,1 */
    hal_float_t *out;		/* pin: output */
    hal_bit_t *sel0;		/* pin: select input */
    hal_bit_t *sel1;		/* pin: select input */
} mux4_t;

typedef struct {
    hal_float_t *in0;		/* pin: input 0 */
    hal_float_t *in1;		/* pin: input 1 */
    hal_float_t *out;		/* pin: output */
    hal_float_t gain0;		/* param: gain for input 0 */
    hal_float_t gain1;		/* param: gain for input 1 */
} sum2_t;

typedef struct {
    hal_float_t *in0;		/* pin: input 0 */
    hal_float_t *in1;		/* pin: input 1 */
    hal_float_t *out;		/* pin: output */
} mult2_t;

typedef struct {
    hal_float_t *in;		/* pin: input */
    hal_float_t *out;		/* pin: output */
} integ_t;

typedef struct {
    hal_float_t *in;		/* pin: input */
    hal_float_t *out;		/* pin: output */
    float old;			/* internal state */
} ddt_t;

typedef struct {
    hal_float_t *in;		/* pin: input */
    hal_float_t *out;		/* pin: output */
    hal_float_t max;		/* param: maximum value */
    hal_float_t min;		/* param: minimum value */
} limit1_t;

typedef struct {
    hal_float_t *in;		/* pin: input */
    hal_float_t *out;		/* pin: output */
    hal_float_t max;		/* param: maximum value */
    hal_float_t min;		/* param: minimum value */
    hal_float_t maxv;		/* param: 1st derivative limit */
    double old_out;		/* previous output */
} limit2_t;

typedef struct {
    hal_float_t *in;		/* pin: input */
    hal_float_t *out;		/* pin: output */
    hal_float_t min;		/* param: minimum value */
    hal_float_t max;		/* param: maximum value */
    hal_float_t maxv;		/* param: 1st derivative limit */
    hal_float_t maxa;		/* param: 2nd derivative limit */
    double old_in;		/* previous input */
    double old_out;		/* previous output */
    double old_v;		/* previous 1st derivative */
} limit3_t;

typedef struct {
    hal_bit_t *ok_in;		/* pin: input, FALSE will trip */
    hal_bit_t *fault_in;	/* pin: input, TRUE will trip */
    hal_bit_t *ok_out;		/* pin: output, TRUE when OK */
    hal_bit_t *fault_out;	/* pin: output, TRUE when tripped */
    hal_bit_t *reset;		/* pin: latch reset (rising edge) */
    hal_bit_t *wd;		/* pin: watchdog/charge pump drive */
    hal_bit_t old_reset;	/* internal, for rising edge detect */
} estop_t;

typedef struct {
    hal_bit_t *in;		/* pin: input 0 */
    hal_bit_t *out;		/* pin: output  */
} not_t;

typedef struct {
    hal_bit_t *in0;		/* pin: input 0 */
    hal_bit_t *in1;		/* pin: input 1 */
    hal_bit_t *out;		/* pin: output  */
} and2_t;

typedef struct {
    hal_bit_t *in0;		/* pin: input 0 */
    hal_bit_t *in1;		/* pin: input 1 */
    hal_bit_t *out;		/* pin: output  */
} or2_t;

typedef struct {
    hal_float_t *in;		/* pin: input  */
    hal_float_t *out;		/* pin: output */
    hal_float_t gain;		/* param: gain */
    hal_float_t offset;		/* param: offset */
} scale_t;

typedef struct {
    hal_float_t *in;		/* pin: input  */
    hal_float_t *out;		/* pin: output */
    hal_float_t gain;		/* param: gain */
    double last_out;		/* prevoius output */
} lowpass_t;

typedef struct {
    hal_bit_t *a[8];		/* pins: input bits to compare */
    hal_bit_t *b[8];		/* pins: input bits to compare */
    hal_bit_t *in;		/* pin: cascade or enable input */
    hal_bit_t *out;		/* pin: output, TRUE if A matches B */
} match8_t;

typedef struct {
    hal_float_t *in;		/* pin: input */
    hal_float_t *min;		/* pin: minimum value */
    hal_float_t *max;		/* pin: maximum value */
    hal_bit_t *reset;		/* pin: reset input */
} minmax_t;


/* other globals */
static int comp_id;		/* component ID */

/***********************************************************************
*                  LOCAL FUNCTION DECLARATIONS                         *
************************************************************************/

static int export_constant(int num);
static int export_wcomp(int num);
static int export_comp(int num);
static int export_mux2(int num);
static int export_hypot(int num);
static int export_mux4(int num);
static int export_sum2(int num);
static int export_mult2(int num);
static int export_integ(int num);
static int export_ddt(int num);
static int export_limit1(int num);
static int export_limit2(int num);
static int export_limit3(int num);
static int export_estop(int num);
static int export_not(int num);
static int export_and2(int num);
static int export_or2(int num);
static int export_scale(int num);
static int export_lowpass(int num);
static int export_match8(int num);
static int export_minmax(int num);

static void constant_funct(void *arg, long period);
static void wcomp_funct(void *arg, long period);
static void comp_funct(void *arg, long period);
static void mux2_funct(void *arg, long period);
static void hypot_funct(void *arg, long period);
static void mux4_funct(void *arg, long period);
static void sum2_funct(void *arg, long period);
static void mult2_funct(void *arg, long period);
static void integ_funct(void *arg, long period);
static void ddt_funct(void *arg, long period);
static void limit1_funct(void *arg, long period);
static void limit2_funct(void *arg, long period);
static void limit3_funct(void *arg, long period);
static void estop_funct(void *arg, long period);
static void not_funct(void *arg, long period);
static void and2_funct(void *arg, long period);
static void or2_funct(void *arg, long period);
static void scale_funct(void *arg, long period);
static void lowpass_funct(void *arg, long period);
static void match8_funct(void *arg, long period);
static void minmax_funct(void *arg, long period);


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

int rtapi_app_main(void)
{
    int n;

    /* connect to the HAL */
    comp_id = hal_init("blocks");
    if (comp_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR, "BLOCKS: ERROR: hal_init() failed\n");
	return -1;
    }
    /* allocate and export constants */
    if (constant > 0) {
	for (n = 0; n < constant; n++) {
	    if (export_constant(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_constant(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d constants\n", constant);
    }
    /* allocate and export window comparators */
    if (wcomp > 0) {
	for (n = 0; n < wcomp; n++) {
	    if (export_wcomp(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_wcomp(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d window comparators\n", wcomp);
    }
    /* allocate and export 2-input comparators */
    if (comp > 0) {
	for (n = 0; n < comp; n++) {
	    if (export_comp(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_comp(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d 2-input comparators\n", comp);
    }
    /* allocate and export 2 input multiplexors */
    if (mux2 > 0) {
	for (n = 0; n < mux2; n++) {
	    if (export_mux2(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_mux2(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d 2-input muxes\n", mux2);
    }
    /* allocate and export hypot() calculators */
    if (hypot > 0) {
	for (n = 0; n < hypot; n++) {
	    if (export_hypot(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_hypot(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d hypotenuse calculators\n", hypot);
    }
    /* allocate and export 4 input multiplexors */
    if (mux4 > 0) {
	for (n = 0; n < mux4; n++) {
	    if (export_mux4(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_mux4(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d 4-input muxes\n", mux4);
    }
    /* allocate and export 2 input summers */
    if (sum2 > 0) {
	for (n = 0; n < sum2; n++) {
	    if (export_sum2(n) != 0) {