weighted_sum.c

来自「CNC 的开放码,EMC2 V2.2.8版」· C语言 代码 · 共 265 行

/********************************************************************
* Description: weighted_sum.c
*   Weighted summer for HAL
*
*   See the "Users Manual" at emc2/docs/Hal_Introduction.pdf
*
* This component is a "weighted summer".  It has a (user specified)
* number of HAL_BIT input pins, and a HAL_S32 parameter corresponding
* to each bit input.
* There is one HAL_S32 output.  The output value is the sum of the
* parameters for which the corresponding bit input is true.
*
* The default value for the parameters is 2^n, where n is the bit number.
* Default behavior results in a binary -> unsigned conversion.
*
* There is one array parameter at module load time, the number of bits for
* each weighted summer.  There is a limit of 8 weighted summers, and each
* may have up to 16 input bits.
*
*********************************************************************
*
* Author: Stephen Wille Padnos (swpadnos AT sourceforge DOT net)
*       Based on a work by John Kasunich
* License: GPL Version 2
* Created on: May 17, 2006
* System: Linux
*
* Copyright (c) 2006 All rights reserved.
*
********************************************************************/

#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 */

#define MAX_SUMMERS	8
#define MAX_SUM_BITS	16

/* module information */
MODULE_AUTHOR("Stephen Wille Padnos");
MODULE_DESCRIPTION("Weighted Summer for EMC HAL");
MODULE_LICENSE("GPL");
int wsum_sizes[MAX_SUMMERS] = { -1, -1, -1, -1, -1, -1, -1, -1 };
RTAPI_MP_ARRAY_INT(wsum_sizes, MAX_SUMMERS, "Sizes of up to 8 weighted summers");

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

/* Data needed for each bit of a weighted summer */
typedef struct {
    hal_bit_t *bit;		/* pin: the input bit HAL pin */
    hal_s32_t weight;		/* parameter: the numeric weight of this pin */
} wsum_bit_t;

/* Base data for a weighted summer. */
typedef struct {
    hal_s32_t *sum;		/* output pin: the calculated sum */
    hal_s32_t offset;		/* parameter: offset for this summer */
    hal_bit_t *hold;		/* input pin: hold value if 1, update if 0 */
    int num_bits;		/* internal: How many bits are in this summer */
    wsum_bit_t *bits;		/* internal: pointer to the input bits and weights */
} wsum_t;

/* pointer to array of wsum structs in shmem */
static wsum_t *wsum_array;
static wsum_bit_t *wsum_bit_array;

/* other globals */
static int comp_id;		/* component ID */
static int num_summers;		/* number of summers created */

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

static int export_wsum(int num, int num_bits, wsum_t *addr, wsum_bit_t *bitaddr);
static void process_wsums(void *arg, long period);

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

int rtapi_app_main(void)
{
    int n, total_bits, retval;

    total_bits = 0;

    /* check that there's at least one valid summer requested */
    for (n = 0; n < MAX_SUMMERS && wsum_sizes[n] != -1 ; n++) {
	if ((wsum_sizes[n] > MAX_SUM_BITS) || (wsum_sizes[n]<1)) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
			    "WEIGHTED_SUM: ERROR: Invalid number of bits '%i' for summer %i\n",
			    wsum_sizes[n], n);
	    return -1;
	} else {
	    num_summers++;
	    total_bits += wsum_sizes[n];
	}
    }

    if (num_summers == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: no summers specified\n");
	return -1;
    }

    /* have good config info, connect to the HAL */
    comp_id = hal_init("weighted_sum");
    if (comp_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: hal_init() failed\n");
	return -1;
    }
    /* allocate shared memory for summer base info */
    wsum_array = hal_malloc(num_summers * sizeof(wsum_t));
    if (wsum_array == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: hal_malloc() for summer array failed\n");
	hal_exit(comp_id);
	return -1;
    }
    /* allocate shared memory for summer bit arrays */
    wsum_bit_array = hal_malloc(total_bits * sizeof(wsum_bit_t));
    if (wsum_bit_array == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: hal_malloc() for summer bit array failed\n");
	hal_exit(comp_id);
	return -1;
    }

    /* export pins/params for all summers */
    total_bits = 0;
    for (n = 0; n < num_summers; n++) {
	/* export all vars */
	retval = export_wsum(n, wsum_sizes[n], &(wsum_array[n]), &(wsum_bit_array[total_bits]));
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		"WEIGHTED_SUM: ERROR: group %d export failed\n", n);
	    hal_exit(comp_id);
	    return -1;
	}
	total_bits += wsum_array[n].num_bits;
    }

    /* export update function */
    retval = hal_export_funct("process_wsums", process_wsums, wsum_array, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: process_wsums funct export failed\n");
	return -1;
    }

    rtapi_print_msg(RTAPI_MSG_INFO,
	"WEIGHTED_SUM: installed %d weighted summers\n", num_summers);
    hal_ready(comp_id);
    return 0;
}

void rtapi_app_exit(void)
{
    hal_exit(comp_id);
}

/***********************************************************************
*                     REALTIME DELAY FUNCTION                          *
************************************************************************/

/*  The summer works by checking the input bits, and adding the
    weight to the sum if the input is true.
*/
static void process_wsums(void *arg, long period)
{
    wsum_t *wsums, *thissum;
    int n, b, running_total;

    /* point to filter group */
    wsums = (wsum_t *)arg;

    for (n=0 ; n<num_summers ; n++) {
	thissum = &(wsums[n]);
	if (*(thissum->hold)) continue;
	else {
	    running_total = thissum->offset;
	    for (b=0 ; b<thissum->num_bits ; b++) {
		if (*(thissum->bits[b].bit)) {
		    running_total += thissum->bits[b].weight;
		}
	    }
	}
	*(thissum->sum) = running_total;
    }
}

/***********************************************************************
*                   LOCAL FUNCTION DEFINITIONS                         *
************************************************************************/

static int export_wsum(int num, int num_bits, wsum_t *addr, wsum_bit_t *bitaddr)
{
    int retval, i, w;
    char buf[HAL_NAME_LEN + 2], base[HAL_NAME_LEN];

    rtapi_snprintf(base, HAL_NAME_LEN, "wsum.%d", num);
    /* export pin for offset (input) */
    rtapi_snprintf(buf, HAL_NAME_LEN, "%s.offset", base);
    retval = hal_param_s32_new(buf, HAL_RW, &(addr->offset), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: '%s' param export failed\n", buf);
	return retval;
    }

    /* export pin for output sum */
    rtapi_snprintf(buf, HAL_NAME_LEN, "%s.sum", base);
    retval = hal_pin_s32_new(buf, HAL_OUT, &(addr->sum), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }

    /* export pin for update hold */
    rtapi_snprintf(buf, HAL_NAME_LEN, "%s.hold", base);
    retval = hal_pin_bit_new(buf, HAL_IN, &(addr->hold), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }

    addr->bits = bitaddr;
    addr->num_bits = num_bits;
    /* export the input bits and weight parameters, and set the default weights */
    w = 1;
    for (i=0;i<num_bits;i++) {
	rtapi_snprintf(buf, HAL_NAME_LEN, "%s.bit.%d.in", base, i);
	retval = hal_pin_bit_new(buf, HAL_IN, &(addr->bits[i].bit), comp_id);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
	    return retval;
	}
	rtapi_snprintf(buf, HAL_NAME_LEN, "%s.bit.%d.weight", base, i);
	retval = hal_param_s32_new(buf, HAL_RW, &(addr->bits[i].weight), comp_id);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
	    "WEIGHTED_SUM: ERROR: '%s' param export failed\n", buf);
	    return retval;
	}
	addr->bits[i].weight = w;
	w <<= 1;
    }

    /* set initial parameter and pin values */
    addr->offset = 0;
    *(addr->sum) = 0;
    return 0;
}