sampler.c
1 /******************************************************************** 2 * Description: sampler.c 3 * A HAL component that can be used to capture data 4 * from HAL pins at a specific realtime sample rate, 5 * and allows the data to be written to stdout. 6 * 7 * Author: John Kasunich <jmkasunich at sourceforge dot net> 8 * License: GPL Version 2 9 * 10 * Copyright (c) 2006 All rights reserved. 11 * 12 ********************************************************************/ 13 /** This file, 'sampler.c', is the realtime part of a HAL component 14 that allows data from HAL pins to be sampled at a uniform realtime 15 sample rate and then be transferred to a file. When this realtime 16 module is loaded, it creates a fifo in shared memory and begins 17 capturing data from HAL pins to the fifo. Then, the user space 18 program 'halsampler' is invoked, which reads the fifo and writes 19 the data to stdout. 20 21 Loading: 22 23 loadrt sampler depth=100 cfg=uffb 24 25 26 */ 27 28 /** Copyright (C) 2006 John Kasunich 29 */ 30 31 /** This program is free software; you can redistribute it and/or 32 modify it under the terms of version 2 of the GNU General 33 Public License as published by the Free Software Foundation. 34 This library is distributed in the hope that it will be useful, 35 but WITHOUT ANY WARRANTY; without even the implied warranty of 36 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 37 GNU General Public License for more details. 38 39 You should have received a copy of the GNU General Public 40 License along with this library; if not, write to the Free Software 41 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 42 43 THE AUTHORS OF THIS LIBRARY ACCEPT ABSOLUTELY NO LIABILITY FOR 44 ANY HARM OR LOSS RESULTING FROM ITS USE. IT IS _EXTREMELY_ UNWISE 45 TO RELY ON SOFTWARE ALONE FOR SAFETY. Any machinery capable of 46 harming persons must have provisions for completely removing power 47 from all motors, etc, before persons enter any danger area. All 48 machinery must be designed to comply with local and national safety 49 codes, and the authors of this software can not, and do not, take 50 any responsibility for such compliance. 51 52 This code was written as part of the EMC HAL project. For more 53 information, go to www.linuxcnc.org. 54 */ 55 56 #include "rtapi.h" /* RTAPI realtime OS API */ 57 #include "rtapi_app.h" /* RTAPI realtime module decls */ 58 #include "hal.h" /* HAL public API decls */ 59 #include "streamer.h" /* decls and such for fifos */ 60 #include "rtapi_errno.h" 61 #include "rtapi_string.h" 62 63 /* module information */ 64 MODULE_AUTHOR("John Kasunich"); 65 MODULE_DESCRIPTION("Realtime HAL Sampler"); 66 MODULE_LICENSE("GPL"); 67 static char *cfg[MAX_SAMPLERS]; /* config string, no default */ 68 RTAPI_MP_ARRAY_STRING(cfg,MAX_SAMPLERS,"config string"); 69 static int depth[MAX_SAMPLERS]; /* depth of fifo, default 0 */ 70 RTAPI_MP_ARRAY_INT(depth,MAX_SAMPLERS,"fifo depth"); 71 72 /*********************************************************************** 73 * STRUCTURES AND GLOBAL VARIABLES * 74 ************************************************************************/ 75 76 /* this structure contains the HAL shared memory data for one sampler */ 77 78 typedef struct { 79 hal_stream_t fifo; /* pointer to user/RT fifo */ 80 hal_s32_t *curr_depth; /* pin: current fifo depth */ 81 hal_bit_t *full; /* pin: overrun flag */ 82 hal_bit_t *enable; /* pin: enable sampling */ 83 hal_s32_t *overruns; /* pin: number of overruns */ 84 hal_s32_t *sample_num; /* pin: sample ID / timestamp */ 85 int num_pins; 86 pin_data_t pins[HAL_STREAM_MAX_PINS]; 87 } sampler_t; 88 89 /* other globals */ 90 static int comp_id; /* component ID */ 91 static int nsamplers; 92 static sampler_t *samplers; 93 94 /*********************************************************************** 95 * LOCAL FUNCTION DECLARATIONS * 96 ************************************************************************/ 97 98 static int init_sampler(int num, sampler_t *tmp_fifo); 99 static void sample(void *arg, long period); 100 101 /*********************************************************************** 102 * INIT AND EXIT CODE * 103 ************************************************************************/ 104 105 int rtapi_app_main(void) 106 { 107 int n, retval; 108 109 comp_id = hal_init("sampler"); 110 if (comp_id < 0) { 111 rtapi_print_msg(RTAPI_MSG_ERR, "SAMPLER: ERROR: hal_init() failed\n"); 112 return -EINVAL; 113 } 114 115 samplers = hal_malloc(MAX_SAMPLERS * sizeof(sampler_t)); 116 /* validate config info */ 117 for ( n = 0 ; n < MAX_SAMPLERS ; n++ ) { 118 if (( cfg[n] == NULL ) || ( *cfg == '\0' ) || ( depth[n] <= 0 )) { 119 break; 120 } 121 retval = hal_stream_create(&samplers[n].fifo, comp_id, SAMPLER_SHMEM_KEY+n, depth[n], cfg[n]); 122 if(retval < 0) { 123 goto fail; 124 } 125 nsamplers++; 126 retval = init_sampler(n, &samplers[n]); 127 } 128 if ( n == 0 ) { 129 rtapi_print_msg(RTAPI_MSG_ERR, 130 "SAMPLER: ERROR: no channels specified\n"); 131 return -EINVAL; 132 } 133 134 hal_ready(comp_id); 135 return 0; 136 fail: 137 for(n=0; n<nsamplers; n++) hal_stream_detach(&samplers[n].fifo); 138 hal_exit(comp_id); 139 return retval; 140 } 141 142 void rtapi_app_exit(void) 143 { 144 int i; 145 for(i=0; i<nsamplers; i++) hal_stream_detach(&samplers[i].fifo); 146 hal_exit(comp_id); 147 } 148 149 /*********************************************************************** 150 * REALTIME COUNTER COUNTING AND UPDATE FUNCTIONS * 151 ************************************************************************/ 152 153 static void sample(void *arg, long period) 154 { 155 sampler_t *samp; 156 pin_data_t *pptr; 157 int n; 158 159 /* point at sampler struct in HAL shmem */ 160 samp = arg; 161 /* are we enabled? */ 162 if ( ! *(samp->enable) ) { 163 *(samp->curr_depth) = hal_stream_depth(&samp->fifo); 164 *(samp->full) = !hal_stream_writable(&samp->fifo); 165 return; 166 } 167 /* point at pins in hal shmem */ 168 pptr = samp->pins; 169 union hal_stream_data data[HAL_STREAM_MAX_PINS], *dptr=data; 170 /* copy data from HAL pins to fifo */ 171 int num_pins = hal_stream_element_count(&samp->fifo); 172 for ( n = 0 ; n < num_pins ; n++ ) { 173 switch ( hal_stream_element_type(&samp->fifo, n) ) { 174 case HAL_FLOAT: 175 dptr->f = *(pptr->hfloat); 176 break; 177 case HAL_BIT: 178 if ( *(pptr->hbit) ) { 179 dptr->b = 1; 180 } else { 181 dptr->b = 0; 182 } 183 break; 184 case HAL_U32: 185 dptr->u = *(pptr->hu32); 186 break; 187 case HAL_S32: 188 dptr->s = *(pptr->hs32); 189 break; 190 default: 191 break; 192 } 193 dptr++; 194 pptr++; 195 } 196 if ( hal_stream_write(&samp->fifo, data) < 0) { 197 /* fifo is full, data is lost */ 198 /* log the overrun */ 199 (*samp->overruns)++; 200 *(samp->full) = 1; 201 *(samp->curr_depth) = hal_stream_maxdepth(&samp->fifo); 202 } else { 203 *(samp->full) = 0; 204 *(samp->curr_depth) = hal_stream_depth(&samp->fifo); 205 } 206 } 207 208 /*********************************************************************** 209 * LOCAL FUNCTION DEFINITIONS * 210 ************************************************************************/ 211 212 static int init_sampler(int num, sampler_t *str) 213 { 214 int retval, usefp, n; 215 pin_data_t *pptr; 216 char buf[HAL_NAME_LEN + 1]; 217 218 /* export "standard" pins and params */ 219 retval = hal_pin_bit_newf(HAL_OUT, &(str->full), comp_id, 220 "sampler.%d.full", num); 221 if (retval != 0 ) { 222 rtapi_print_msg(RTAPI_MSG_ERR, 223 "SAMPLER: ERROR: 'full' pin export failed\n"); 224 return -EIO; 225 } 226 retval = hal_pin_bit_newf(HAL_IN, &(str->enable), comp_id, 227 "sampler.%d.enable", num); 228 if (retval != 0 ) { 229 rtapi_print_msg(RTAPI_MSG_ERR, 230 "SAMPLER: ERROR: 'enable' pin export failed\n"); 231 return -EIO; 232 } 233 retval = hal_pin_s32_newf(HAL_OUT, &(str->curr_depth), comp_id, 234 "sampler.%d.curr-depth", num); 235 if (retval != 0 ) { 236 rtapi_print_msg(RTAPI_MSG_ERR, 237 "SAMPLEr: ERROR: 'curr_depth' pin export failed\n"); 238 return -EIO; 239 } 240 retval = hal_pin_s32_newf(HAL_IO, &(str->overruns), comp_id, 241 "sampler.%d.overruns", num); 242 if (retval != 0 ) { 243 rtapi_print_msg(RTAPI_MSG_ERR, 244 "SAMPLER: ERROR: 'overruns' parameter export failed\n"); 245 return -EIO; 246 } 247 retval = hal_pin_s32_newf(HAL_IO, &(str->sample_num), comp_id, 248 "sampler.%d.sample-num", num); 249 if (retval != 0 ) { 250 rtapi_print_msg(RTAPI_MSG_ERR, 251 "SAMPLER: ERROR: 'sample-num' parameter export failed\n"); 252 return -EIO; 253 } 254 /* init the standard pins and params */ 255 *(str->full) = 0; 256 *(str->enable) = 1; 257 *(str->curr_depth) = 0; 258 *(str->overruns) = 0; 259 *(str->sample_num) = 0; 260 pptr = str->pins; 261 usefp = 0; 262 /* export user specified pins (the ones that sample data) */ 263 for ( n = 0 ; n < hal_stream_element_count(&str->fifo) ; n++ ) { 264 rtapi_snprintf(buf, sizeof(buf), "sampler.%d.pin.%d", num, n); 265 retval = hal_pin_new(buf, hal_stream_element_type(&str->fifo, n), HAL_IN, (void **)pptr, comp_id ); 266 if (retval != 0 ) { 267 rtapi_print_msg(RTAPI_MSG_ERR, 268 "SAMPLER: ERROR: pin '%s' export failed\n", buf); 269 return -EIO; 270 } 271 /* init the pin value */ 272 switch ( hal_stream_element_type(&str->fifo, n) ) { 273 case HAL_FLOAT: 274 *(pptr->hfloat) = 0.0; 275 usefp = 1; 276 break; 277 case HAL_BIT: 278 *(pptr->hbit) = 0; 279 break; 280 case HAL_U32: 281 *(pptr->hu32) = 0; 282 break; 283 case HAL_S32: 284 *(pptr->hs32) = 0; 285 break; 286 default: 287 break; 288 } 289 pptr++; 290 } 291 /* export update function */ 292 rtapi_snprintf(buf, sizeof(buf), "sampler.%d", num); 293 retval = hal_export_funct(buf, sample, str, usefp, 0, comp_id); 294 if (retval != 0) { 295 rtapi_print_msg(RTAPI_MSG_ERR, 296 "SAMPLER: ERROR: function export failed\n"); 297 return retval; 298 } 299 300 return 0; 301 } 302