1  /********************************************************************
  2  * Description: weighted_sum.c
  3  *   Weighted summer for HAL
  4  *
  5  *   See the "Users Manual" at emc2/docs/Hal_Introduction.pdf
  6  *
  7  * This component is a "weighted summer".  It has a (user specified)
  8  * number of HAL_BIT input pins, and a HAL_S32 parameter corresponding
  9  * to each bit input.
 10  * There is one HAL_S32 output.  The output value is the sum of the
 11  * parameters for which the corresponding bit input is true.
 12  *
 13  * The default value for the parameters is 2^n, where n is the bit number.
 14  * Default behavior results in a binary -> unsigned conversion.
 15  *
 16  * There is one array parameter at module load time, the number of bits for
 17  * each weighted summer.  There is a limit of 8 weighted summers, and each
 18  * may have up to 16 input bits.
 19  *
 20  *********************************************************************
 21  *
 22  * Author: Stephen Wille Padnos (swpadnos AT sourceforge DOT net)
 23  *       Based on a work by John Kasunich
 24  * License: GPL Version 2
 25  * Created on: May 17, 2006
 26  * System: Linux
 27  *
 28  * Copyright (c) 2006 All rights reserved.
 29  *
 30  ********************************************************************/
 31  
 32  #include "rtapi.h"		/* RTAPI realtime OS API */
 33  #include "rtapi_app.h"		/* RTAPI realtime module decls */
 34  #include "hal.h"		/* HAL public API decls */
 35  
 36  #define MAX_SUMMERS	8
 37  #define MAX_SUM_BITS	16
 38  
 39  /* module information */
 40  MODULE_AUTHOR("Stephen Wille Padnos");
 41  MODULE_DESCRIPTION("Weighted Summer for EMC HAL");
 42  MODULE_LICENSE("GPL");
 43  int wsum_sizes[MAX_SUMMERS] = { -1, -1, -1, -1, -1, -1, -1, -1 };
 44  RTAPI_MP_ARRAY_INT(wsum_sizes, MAX_SUMMERS, "Sizes of up to 8 weighted summers");
 45  
 46  /***********************************************************************
 47  *                STRUCTURES AND GLOBAL VARIABLES                       *
 48  ************************************************************************/
 49  
 50  /* Data needed for each bit of a weighted summer */
 51  typedef struct {
 52      hal_bit_t *bit;		/* pin: the input bit HAL pin */
 53      hal_s32_t *weight;		/* pin: the numeric weight of this pin */
 54  } wsum_bit_t;
 55  
 56  /* Base data for a weighted summer. */
 57  typedef struct {
 58      hal_s32_t *sum;		/* output pin: the calculated sum */
 59      hal_s32_t *offset;		/* pin: offset for this summer */
 60      hal_bit_t *hold;		/* input pin: hold value if 1, update if 0 */
 61      int num_bits;		/* internal: How many bits are in this summer */
 62      wsum_bit_t *bits;		/* internal: pointer to the input bits and weights */
 63  } wsum_t;
 64  
 65  /* pointer to array of wsum structs in shmem */
 66  static wsum_t *wsum_array;
 67  static wsum_bit_t *wsum_bit_array;
 68  
 69  /* other globals */
 70  static int comp_id;		/* component ID */
 71  static int num_summers;		/* number of summers created */
 72  
 73  /***********************************************************************
 74  *                  LOCAL FUNCTION DECLARATIONS                         *
 75  ************************************************************************/
 76  
 77  static int export_wsum(int num, int num_bits, wsum_t *addr, wsum_bit_t *bitaddr);
 78  static void process_wsums(void *arg, long period);
 79  
 80  /***********************************************************************
 81  *                       INIT AND EXIT CODE                             *
 82  ************************************************************************/
 83  
 84  int rtapi_app_main(void)
 85  {
 86      int n, total_bits, retval;
 87  
 88      total_bits = 0;
 89  
 90      /* check that there's at least one valid summer requested */
 91      for (n = 0; n < MAX_SUMMERS && wsum_sizes[n] != -1 ; n++) {
 92  	if ((wsum_sizes[n] > MAX_SUM_BITS) || (wsum_sizes[n]<1)) {
 93  	    rtapi_print_msg(RTAPI_MSG_ERR,
 94  			    "WEIGHTED_SUM: ERROR: Invalid number of bits '%i' for summer %i\n",
 95  			    wsum_sizes[n], n);
 96  	    return -1;
 97  	} else {
 98  	    num_summers++;
 99  	    total_bits += wsum_sizes[n];
100  	}
101      }
102  
103      if (num_summers == 0) {
104  	rtapi_print_msg(RTAPI_MSG_ERR,
105  	    "WEIGHTED_SUM: ERROR: no summers specified\n");
106  	return -1;
107      }
108  
109      /* have good config info, connect to the HAL */
110      comp_id = hal_init("weighted_sum");
111      if (comp_id < 0) {
112  	rtapi_print_msg(RTAPI_MSG_ERR,
113  	    "WEIGHTED_SUM: ERROR: hal_init() failed\n");
114  	return -1;
115      }
116      /* allocate shared memory for summer base info */
117      wsum_array = hal_malloc(num_summers * sizeof(wsum_t));
118      if (wsum_array == 0) {
119  	rtapi_print_msg(RTAPI_MSG_ERR,
120  	    "WEIGHTED_SUM: ERROR: hal_malloc() for summer array failed\n");
121  	hal_exit(comp_id);
122  	return -1;
123      }
124      /* allocate shared memory for summer bit arrays */
125      wsum_bit_array = hal_malloc(total_bits * sizeof(wsum_bit_t));
126      if (wsum_bit_array == 0) {
127  	rtapi_print_msg(RTAPI_MSG_ERR,
128  	    "WEIGHTED_SUM: ERROR: hal_malloc() for summer bit array failed\n");
129  	hal_exit(comp_id);
130  	return -1;
131      }
132  
133      /* export pins/params for all summers */
134      total_bits = 0;
135      for (n = 0; n < num_summers; n++) {
136  	/* export all vars */
137  	retval = export_wsum(n, wsum_sizes[n], &(wsum_array[n]), &(wsum_bit_array[total_bits]));
138  	if (retval != 0) {
139  	    rtapi_print_msg(RTAPI_MSG_ERR,
140  		"WEIGHTED_SUM: ERROR: group %d export failed\n", n);
141  	    hal_exit(comp_id);
142  	    return -1;
143  	}
144  	total_bits += wsum_array[n].num_bits;
145      }
146  
147      /* export update function */
148      retval = hal_export_funct("process_wsums", process_wsums, wsum_array, 1, 0, comp_id);
149      if (retval != 0) {
150  	rtapi_print_msg(RTAPI_MSG_ERR,
151  	    "WEIGHTED_SUM: ERROR: process_wsums funct export failed\n");
152  	return -1;
153      }
154  
155      rtapi_print_msg(RTAPI_MSG_INFO,
156  	"WEIGHTED_SUM: installed %d weighted summers\n", num_summers);
157      hal_ready(comp_id);
158      return 0;
159  }
160  
161  void rtapi_app_exit(void)
162  {
163      hal_exit(comp_id);
164  }
165  
166  /***********************************************************************
167  *                     REALTIME DELAY FUNCTION                          *
168  ************************************************************************/
169  
170  /*  The summer works by checking the input bits, and adding the
171      weight to the sum if the input is true.
172  */
173  static void process_wsums(void *arg, long period)
174  {
175      wsum_t *wsums, *thissum;
176      int n, b, running_total;
177  
178      /* point to filter group */
179      wsums = (wsum_t *)arg;
180  
181      for (n=0 ; n<num_summers ; n++) {
182  	thissum = &(wsums[n]);
183  	if (*(thissum->hold)) continue;
184  	else {
185  	    running_total = *(thissum->offset);
186  	    for (b=0 ; b<thissum->num_bits ; b++) {
187  		if (*(thissum->bits[b].bit)) {
188  		    running_total += *(thissum->bits[b].weight);
189  		}
190  	    }
191  	}
192  	*(thissum->sum) = running_total;
193      }
194  }
195  
196  /***********************************************************************
197  *                   LOCAL FUNCTION DEFINITIONS                         *
198  ************************************************************************/
199  
200  static int export_wsum(int num, int num_bits, wsum_t *addr, wsum_bit_t *bitaddr)
201  {
202      int retval, i, w;
203      char buf[HAL_NAME_LEN+1], base[HAL_NAME_LEN+1];
204  
205      rtapi_snprintf(base, sizeof(base), "wsum.%d", num);
206      /* export pin for offset (input) */
207      rtapi_snprintf(buf, sizeof(buf), "%s.offset", base);
208      retval = hal_pin_s32_new(buf, HAL_IO, &(addr->offset), comp_id);
209      if (retval != 0) {
210  	rtapi_print_msg(RTAPI_MSG_ERR,
211  	    "WEIGHTED_SUM: ERROR: '%s' param export failed\n", buf);
212  	return retval;
213      }
214  
215      /* export pin for output sum */
216      rtapi_snprintf(buf, sizeof(buf), "%s.sum", base);
217      retval = hal_pin_s32_new(buf, HAL_OUT, &(addr->sum), comp_id);
218      if (retval != 0) {
219  	rtapi_print_msg(RTAPI_MSG_ERR,
220  	    "WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
221  	return retval;
222      }
223  
224      /* export pin for update hold */
225      rtapi_snprintf(buf, sizeof(buf), "%s.hold", base);
226      retval = hal_pin_bit_new(buf, HAL_IN, &(addr->hold), comp_id);
227      if (retval != 0) {
228  	rtapi_print_msg(RTAPI_MSG_ERR,
229  	    "WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
230  	return retval;
231      }
232  
233      addr->bits = bitaddr;
234      addr->num_bits = num_bits;
235      /* export the input bits and weight parameters, and set the default weights */
236      w = 1;
237      for (i=0;i<num_bits;i++) {
238  	rtapi_snprintf(buf, sizeof(buf), "%s.bit.%d.in", base, i);
239  	retval = hal_pin_bit_new(buf, HAL_IN, &(addr->bits[i].bit), comp_id);
240  	if (retval != 0) {
241  	    rtapi_print_msg(RTAPI_MSG_ERR,
242  	    "WEIGHTED_SUM: ERROR: '%s' pin export failed\n", buf);
243  	    return retval;
244  	}
245  	rtapi_snprintf(buf, sizeof(buf), "%s.bit.%d.weight", base, i);
246  	retval = hal_pin_s32_new(buf, HAL_IO, &(addr->bits[i].weight), comp_id);
247  	if (retval != 0) {
248  	    rtapi_print_msg(RTAPI_MSG_ERR,
249  	    "WEIGHTED_SUM: ERROR: '%s' param export failed\n", buf);
250  	    return retval;
251  	}
252  	*(addr->bits[i].weight) = w;
253  	w <<= 1;
254      }
255  
256      /* set initial parameter and pin values */
257      *(addr->offset) = 0;
258      *(addr->sum) = 0;
259      return 0;
260  }