counter.c
1 /******************************************************************** 2 * Description: counter.c 3 * This file, 'counter.c', is a HAL component that 4 * provides software-based counting of pulse streams 5 * with an optional reset input. 6 * 7 * Author: Chris Radek <chris@timeguy.com> 8 * License: GPL Version 2 9 * 10 * Copyright (c) 2006 All rights reserved. 11 * 12 ********************************************************************/ 13 /** This file, 'counter.c', is a HAL component that provides software- 14 based counting that is useful for spindle position sensing and 15 maybe other things. Instead of using a real encoder that outputs 16 quadrature, some lathes have a sensor that generates a simple pulse 17 stream as the spindle turns and an index pulse once per revolution. 18 This component simply counts up when a "count" pulse (phase-A) 19 is received, and if reset is enabled, resets when the "index" 20 (phase-Z) pulse is received. 21 22 This is of course only useful for a unidirectional spindle, as it 23 is not possible to sense the direction of rotation. 24 25 The maximum count rate will depend on the speed of the PC, but is 26 expected to exceed 2kHz for even the slowest computers, and may 27 well be over 25kHz on fast ones. It is a realtime component. 28 29 It supports up to eight counters, with optional index pulses. 30 The number of counters is set by the module parameter 'num_chan' 31 when the component is insmod'ed. 32 33 The driver exports variables for each counter's inputs and outputs. 34 It also exports two functions: "counter.update-counters" must be 35 called in a high speed thread, at least twice the maximum desired 36 count rate. "counter.capture-position" can be called at a much 37 slower rate, and updates the output variables. 38 */ 39 40 /** Copyright (C) 2006 Chris Radek <chris@timeguy.com> 41 * 42 * Based heavily on the "encoder" hal module by John Kasunich 43 */ 44 45 #include "rtapi.h" /* RTAPI realtime OS API */ 46 #include "rtapi_app.h" /* RTAPI realtime module decls */ 47 #include "rtapi_errno.h" /* EINVAL etc */ 48 #include "hal.h" /* HAL public API decls */ 49 50 /* module information */ 51 MODULE_AUTHOR("Chris Radek"); 52 MODULE_DESCRIPTION("Pulse Counter for EMC HAL"); 53 MODULE_LICENSE("GPL"); 54 static int num_chan = 1; /* number of channels - default = 1 */ 55 RTAPI_MP_INT(num_chan, "number of channels"); 56 57 /*********************************************************************** 58 * STRUCTURES AND GLOBAL VARIABLES * 59 ************************************************************************/ 60 61 /* this structure contains the runtime data for a single counter */ 62 63 typedef struct { 64 unsigned char oldZ; /* previous value of phase Z */ 65 unsigned char oldA; /* previous value of phase A */ 66 unsigned char reset_on_index; 67 unsigned char pad; /* padding for alignment */ 68 hal_s32_t *raw_count; /* pin: raw binary count value */ 69 hal_bit_t *phaseA; /* quadrature input */ 70 hal_bit_t *phaseZ; /* index pulse input */ 71 hal_bit_t *index_ena; /* index enable input */ 72 hal_bit_t *reset; /* counter reset input */ 73 hal_s32_t *count; /* captured binary count value */ 74 hal_float_t *pos; /* scaled position (floating point) */ 75 hal_float_t *vel; /* scaled velocity (floating point) */ 76 hal_float_t *pos_scale; /* pin: scaling factor for pos */ 77 double old_scale; /* stored scale value */ 78 double scale; /* reciprocal value used for scaling */ 79 hal_s32_t last_count; 80 hal_s32_t last_index_count; 81 } counter_t; 82 83 /* pointer to array of counter_t structs in shmem, 1 per counter */ 84 static counter_t *counter_array; 85 86 /* other globals */ 87 static int comp_id; /* component ID */ 88 89 /*********************************************************************** 90 * LOCAL FUNCTION DECLARATIONS * 91 ************************************************************************/ 92 93 static int export_counter(int num, counter_t * addr); 94 static void update(void *arg, long period); 95 static void capture(void *arg, long period); 96 97 /*********************************************************************** 98 * INIT AND EXIT CODE * 99 ************************************************************************/ 100 101 #define MAX_CHAN 8 102 103 int rtapi_app_main(void) 104 { 105 int n, retval; 106 107 /* test for number of channels */ 108 if ((num_chan <= 0) || (num_chan > MAX_CHAN)) { 109 rtapi_print_msg(RTAPI_MSG_ERR, 110 "COUNTER: ERROR: invalid num_chan: %d\n", num_chan); 111 return -EINVAL; 112 } 113 /* have good config info, connect to the HAL */ 114 comp_id = hal_init("counter"); 115 if (comp_id < 0) { 116 rtapi_print_msg(RTAPI_MSG_ERR, "COUNTER: ERROR: hal_init() failed\n"); 117 return -EINVAL; 118 } 119 /* allocate shared memory for counter data */ 120 counter_array = hal_malloc(num_chan * sizeof(counter_t)); 121 if (!counter_array) { 122 rtapi_print_msg(RTAPI_MSG_ERR, 123 "COUNTER: ERROR: hal_malloc() failed\n"); 124 hal_exit(comp_id); 125 return -ENOMEM; 126 } 127 /* export all the variables for each counter */ 128 for (n = 0; n < num_chan; n++) { 129 /* export all vars */ 130 retval = export_counter(n, &(counter_array[n])); 131 if (retval != 0) { 132 rtapi_print_msg(RTAPI_MSG_ERR, 133 "COUNTER: ERROR: counter %d var export failed\n", n); 134 hal_exit(comp_id); 135 return -EIO; 136 } 137 /* init counter */ 138 counter_array[n].oldZ = 0; 139 counter_array[n].oldA = 0; 140 counter_array[n].reset_on_index = 0; 141 *(counter_array[n].raw_count) = 0; 142 *(counter_array[n].count) = 0; 143 *(counter_array[n].pos) = 0.0; 144 *(counter_array[n].pos_scale) = 1.0; 145 counter_array[n].old_scale = 1.0; 146 counter_array[n].scale = 1.0; 147 } 148 /* export functions */ 149 retval = hal_export_funct("counter.update-counters", update, 150 counter_array, 0, 0, comp_id); 151 if (retval != 0) { 152 rtapi_print_msg(RTAPI_MSG_ERR, 153 "COUNTER: ERROR: count funct export failed\n"); 154 hal_exit(comp_id); 155 return -EIO; 156 } 157 retval = hal_export_funct("counter.capture-position", capture, 158 counter_array, 1, 0, comp_id); 159 if (retval != 0) { 160 rtapi_print_msg(RTAPI_MSG_ERR, 161 "COUNTER: ERROR: capture funct export failed\n"); 162 hal_exit(comp_id); 163 return -EIO; 164 } 165 rtapi_print_msg(RTAPI_MSG_INFO, 166 "COUNTER: installed %d counter counters\n", num_chan); 167 hal_ready(comp_id); 168 return 0; 169 } 170 171 void rtapi_app_exit(void) 172 { 173 hal_exit(comp_id); 174 } 175 176 /*********************************************************************** 177 * REALTIME COUNTER COUNTING AND UPDATE FUNCTIONS * 178 ************************************************************************/ 179 180 static void update(void *arg, long period) 181 { 182 counter_t *cntr; 183 int n; 184 185 for (cntr = arg, n = 0; n < num_chan; cntr++, n++) { 186 // count on rising edge 187 if(!cntr->oldA && *cntr->phaseA) 188 (*cntr->raw_count)++; 189 cntr->oldA = *cntr->phaseA; 190 191 // reset on rising edge 192 if(cntr->reset_on_index && !cntr->oldZ && *cntr->phaseZ) { 193 cntr->last_index_count = *(cntr->raw_count); 194 *(cntr->index_ena) = 0; 195 } 196 cntr->oldZ = *cntr->phaseZ; 197 } 198 } 199 200 static void capture(void *arg, long period) 201 { 202 counter_t *cntr; 203 int n; 204 205 for (cntr = arg, n = 0; n < num_chan; cntr++, n++) { 206 /* check reset input */ 207 int raw_count; 208 int counts; 209 if (*(cntr->reset)) { 210 /* reset is active, reset the counter */ 211 *(cntr->raw_count) = 0; 212 cntr->last_index_count = 0; 213 cntr->last_count = 0; 214 } 215 /* capture raw counts to latches */ 216 raw_count = *(cntr->raw_count); 217 *(cntr->count) = raw_count - cntr->last_index_count; 218 counts = (raw_count - cntr->last_count); 219 cntr->last_count = raw_count; 220 221 /* check for change in scale value */ 222 if ( *(cntr->pos_scale) != cntr->old_scale ) { 223 /* save new scale to detect future changes */ 224 cntr->old_scale = *(cntr->pos_scale); 225 /* scale value has changed, test and update it */ 226 if ((*(cntr->pos_scale) < 1e-20) && (*(cntr->pos_scale) > -1e-20)) { 227 /* value too small, divide by zero is a bad thing */ 228 *(cntr->pos_scale) = 1.0; 229 } 230 /* we actually want the reciprocal */ 231 cntr->scale = 1.0 / *(cntr->pos_scale); 232 } 233 /* scale count to make floating point position */ 234 *(cntr->pos) = *(cntr->count) * cntr->scale; 235 /* scale counts to make floating point velocity */ 236 *(cntr->vel) = counts * cntr->scale * 1e9 / period; 237 238 /* update reset_on_index based on index_ena */ 239 cntr->reset_on_index = *(cntr->index_ena); 240 } 241 } 242 243 /*********************************************************************** 244 * LOCAL FUNCTION DEFINITIONS * 245 ************************************************************************/ 246 247 static int export_counter(int num, counter_t * addr) 248 { 249 int retval, msg; 250 251 /* This function exports a lot of stuff, which results in a lot of 252 logging if msg_level is at INFO or ALL. So we save the current value 253 of msg_level and restore it later. If you actually need to log this 254 function's actions, change the second line below */ 255 msg = rtapi_get_msg_level(); 256 rtapi_set_msg_level(RTAPI_MSG_WARN); 257 258 /* export pins for the quadrature inputs */ 259 retval = hal_pin_bit_newf(HAL_IN, &(addr->phaseA), comp_id, "counter.%d.phase-A", num); 260 if (retval != 0) { 261 return retval; 262 } 263 /* export pin for the index input */ 264 retval = hal_pin_bit_newf(HAL_IN, &(addr->phaseZ), comp_id, "counter.%d.phase-Z", num); 265 if (retval != 0) { 266 return retval; 267 } 268 /* export pin for the index enable input */ 269 retval = hal_pin_bit_newf(HAL_IO, &(addr->index_ena), comp_id, "counter.%d.index-enable", num); 270 if (retval != 0) { 271 return retval; 272 } 273 /* export pin for the reset input */ 274 retval = hal_pin_bit_newf(HAL_IN, &(addr->reset), comp_id, "counter.%d.reset", num); 275 if (retval != 0) { 276 return retval; 277 } 278 /* export parameter for raw counts */ 279 retval = hal_pin_s32_newf(HAL_OUT, &(addr->raw_count), comp_id, "counter.%d.rawcounts", num); 280 if (retval != 0) { 281 return retval; 282 } 283 /* export pin for counts captured by capture() */ 284 retval = hal_pin_s32_newf(HAL_OUT, &(addr->count), comp_id, "counter.%d.counts", num); 285 if (retval != 0) { 286 return retval; 287 } 288 /* export pin for scaled position captured by capture() */ 289 retval = hal_pin_float_newf(HAL_OUT, &(addr->pos), comp_id, "counter.%d.position", num); 290 if (retval != 0) { 291 return retval; 292 } 293 /* export pin for scaled velocity captured by capture() */ 294 retval = hal_pin_float_newf(HAL_OUT, &(addr->vel), comp_id, "counter.%d.velocity", num); 295 if (retval != 0) { 296 return retval; 297 } 298 /* export parameter for scaling */ 299 retval = hal_pin_float_newf(HAL_IO, &(addr->pos_scale), comp_id, "counter.%d.position-scale", num); 300 if (retval != 0) { 301 return retval; 302 } 303 /* restore saved message level */ 304 rtapi_set_msg_level(msg); 305 return 0; 306 }