sim_encoder.c
1 /******************************************************************** 2 * Description: sim_encoder.c 3 * A HAL component that generates A, B, and index 4 * signals as an encoder would. 5 * 6 * Author: John Kasunich 7 * License: GPL Version 2 8 * 9 * Copyright (c) 2006 All rights reserved. 10 * 11 * Last change: 12 ********************************************************************/ 13 /** This file, 'sim_encoder.c', is a HAL component that simulates 14 a quadrature encoder with an index pulse. It "rotates" at a 15 speed controlled by a HAL pin, and produces A, B, and Z outputs 16 on other HAL pins. A parameter sets the counts/revolution. 17 18 It supports up to 8 simulated encoders. The number is set by 19 an insmod command line parameter 'num_chan'. Alternatively, use the 20 names= specifier and a list of unique names separated by commas. 21 The names= and num_chan= specifiers are mutually exclusive. 22 23 The module exports two functions. 'sim-encoder.make-pulses', is 24 responsible for actually generating the A, B, and Z signals. It 25 must be executed in a fast thread to reduce pulse jitter. The 26 other function, 'sim-encoder.update-speed', is is normally called 27 from a much slower thread, and sets internal variables used by 28 'make-pulses', based on the 'speed' input pin, and the 'ppr' 29 parameter. 30 */ 31 32 /** Copyright (C) 2003 John Kasunich 33 <jmkasunich AT users DOT sourceforge DOT net> 34 */ 35 36 /** This program is free software; you can redistribute it and/or 37 modify it under the terms of version 2 of the GNU General 38 Public License as published by the Free Software Foundation. 39 This library is distributed in the hope that it will be useful, 40 but WITHOUT ANY WARRANTY; without even the implied warranty of 41 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 42 GNU General Public License for more details. 43 44 You should have received a copy of the GNU General Public 45 License along with this library; if not, write to the Free Software 46 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 47 48 THE AUTHORS OF THIS LIBRARY ACCEPT ABSOLUTELY NO LIABILITY FOR 49 ANY HARM OR LOSS RESULTING FROM ITS USE. IT IS _EXTREMELY_ UNWISE 50 TO RELY ON SOFTWARE ALONE FOR SAFETY. Any machinery capable of 51 harming persons must have provisions for completely removing power 52 from all motors, etc, before persons enter any danger area. All 53 machinery must be designed to comply with local and national safety 54 codes, and the authors of this software can not, and do not, take 55 any responsibility for such compliance. 56 57 This code was written as part of the EMC HAL project. For more 58 information, go to www.linuxcnc.org. 59 */ 60 61 #include "rtapi.h" /* RTAPI realtime OS API */ 62 #include "rtapi_app.h" /* RTAPI realtime module decls */ 63 #include "rtapi_string.h" 64 #include "hal.h" /* HAL public API decls */ 65 66 #define MAX_CHAN 8 67 68 /* module information */ 69 MODULE_AUTHOR("John Kasunich"); 70 MODULE_DESCRIPTION("Simulated Encoder for EMC HAL"); 71 MODULE_LICENSE("GPL"); 72 static int num_chan; 73 static int default_num_chan = 1; 74 RTAPI_MP_INT(num_chan, "number of 'sim_encoders'"); 75 static int howmany; 76 static char *names[MAX_CHAN] = {0,}; 77 RTAPI_MP_ARRAY_STRING(names, MAX_CHAN, "names of sim_encoder"); 78 79 /*********************************************************************** 80 * STRUCTURES AND GLOBAL VARIABLES * 81 ************************************************************************/ 82 83 /** These structures contains the runtime data for a single encoder. 84 Data is arranged in the structs in the order in which it will be 85 accessed, so fetching one item will load the next item(s) into cache. 86 */ 87 88 typedef struct { 89 signed long addval; /* frequency generator add value */ 90 unsigned long accum; /* frequency generator accumulator */ 91 signed char state; /* current quadrature state */ 92 long cycle; /* current cycle */ 93 hal_bit_t *phaseA; /* pins for output signals */ 94 hal_bit_t *phaseB; /* pins for output signals */ 95 hal_bit_t *phaseZ; /* pins for output signals */ 96 hal_u32_t *ppr; /* pin: pulses per revolution */ 97 hal_float_t *scale; /* pin: pulses per revolution */ 98 hal_float_t *speed; /* pin: speed in revs/second */ 99 hal_s32_t *rawcounts; /* pin: raw counts */ 100 double old_scale; /* internal, used to detect changes */ 101 double scale_mult; /* internal, reciprocal of scale */ 102 } sim_enc_t; 103 104 /* ptr to array of sim_enc_t structs in shared memory, 1 per channel */ 105 static sim_enc_t *sim_enc_array; 106 107 /* other globals */ 108 static int comp_id; /* component ID */ 109 static long periodns; /* makepulses function period in nanosec */ 110 static long old_periodns; /* used to detect changes in periodns */ 111 static double periodfp; /* makepulses function period in seconds */ 112 static double freqscale; /* conv. factor from Hz to addval counts */ 113 static double maxf; /* max frequency in Hz */ 114 115 /*********************************************************************** 116 * LOCAL FUNCTION DECLARATIONS * 117 ************************************************************************/ 118 119 static int export_sim_enc(sim_enc_t * addr, char *prefix); 120 static void make_pulses(void *arg, long period); 121 static void update_speed(void *arg, long period); 122 123 /*********************************************************************** 124 * INIT AND EXIT CODE * 125 ************************************************************************/ 126 127 int rtapi_app_main(void) 128 { 129 int n, retval, i; 130 131 if(num_chan && names[0]) { 132 rtapi_print_msg(RTAPI_MSG_ERR,"num_chan= and names= are mutually exclusive\n"); 133 return -EINVAL; 134 } 135 if(!num_chan && !names[0]) num_chan = default_num_chan; 136 137 if(num_chan) { 138 howmany = num_chan; 139 } else { 140 howmany = 0; 141 for (i = 0; i < MAX_CHAN; i++) { 142 if ( (names[i] == NULL) || (*names[i] == 0) ){ 143 break; 144 } 145 howmany = i + 1; 146 } 147 } 148 149 if ((howmany <= 0) || (howmany > MAX_CHAN)) { 150 rtapi_print_msg(RTAPI_MSG_ERR, 151 "SIM_ENCODER: ERROR: invalid number of channels %d\n", howmany); 152 return -1; 153 } 154 /* periodns will be set to the proper value when 'make_pulses()' 155 runs for the first time. We load a default value here to avoid 156 glitches at startup, but all these 'constants' are recomputed 157 inside 'update_speed()' using the real period. */ 158 periodns = 50000; 159 /* precompute some constants */ 160 periodfp = periodns * 0.000000001; 161 maxf = 1.0 / periodfp; 162 freqscale = ((1L << 30) * 2.0) / maxf; 163 old_periodns = periodns; 164 /* have good config info, connect to the HAL */ 165 comp_id = hal_init("sim_encoder"); 166 if (comp_id < 0) { 167 rtapi_print_msg(RTAPI_MSG_ERR, "SIM_ENCODER: ERROR: hal_init() failed\n"); 168 return -1; 169 } 170 /* allocate shared memory for encoder data */ 171 sim_enc_array = hal_malloc(howmany * sizeof(sim_enc_t)); 172 if (sim_enc_array == 0) { 173 rtapi_print_msg(RTAPI_MSG_ERR, 174 "SIM_ENCODER: ERROR: hal_malloc() failed\n"); 175 hal_exit(comp_id); 176 return -1; 177 } 178 /* export all the variables for each simulated encoder */ 179 i = 0; //for names= items 180 for (n = 0; n < howmany; n++) { 181 /* export all vars */ 182 183 if(num_chan) { 184 char buf[HAL_NAME_LEN + 1]; 185 rtapi_snprintf(buf, sizeof(buf), "sim-encoder.%d", n); 186 retval = export_sim_enc(&(sim_enc_array[n]),buf); 187 } else { 188 retval = export_sim_enc(&(sim_enc_array[n]),names[i++]); 189 } 190 191 if (retval != 0) { 192 rtapi_print_msg(RTAPI_MSG_ERR, 193 "SIM_ENCODER: ERROR: 'encoder' %d var export failed\n", n); 194 hal_exit(comp_id); 195 return -1; 196 } 197 } 198 /* export functions */ 199 retval = hal_export_funct("sim-encoder.make-pulses", make_pulses, 200 sim_enc_array, 0, 0, comp_id); 201 if (retval != 0) { 202 rtapi_print_msg(RTAPI_MSG_ERR, 203 "SIM_ENCODER: ERROR: makepulses funct export failed\n"); 204 hal_exit(comp_id); 205 return -1; 206 } 207 retval = hal_export_funct("sim-encoder.update-speed", update_speed, 208 sim_enc_array, 1, 0, comp_id); 209 if (retval != 0) { 210 rtapi_print_msg(RTAPI_MSG_ERR, 211 "SIM_ENCODER: ERROR: speed update funct export failed\n"); 212 hal_exit(comp_id); 213 return -1; 214 } 215 rtapi_print_msg(RTAPI_MSG_INFO, 216 "SIM_ENCODER: installed %d simulated encoders\n", howmany); 217 hal_ready(comp_id); 218 return 0; 219 } 220 221 void rtapi_app_exit(void) 222 { 223 hal_exit(comp_id); 224 } 225 226 /*********************************************************************** 227 * REALTIME STEP PULSE GENERATION FUNCTIONS * 228 ************************************************************************/ 229 230 /** The frequency generator works by adding a signed value proportional 231 to frequency to an accumulator. When the accumulator overflows (or 232 underflows), it is time to increment (or decrement) the state of the 233 output pins. 234 The add value is limited to +/-2^30, and overflows are detected 235 at bit 30, not bit 31. This means that with add_val at it's max 236 (or min) value, and overflow (or underflow) occurs on every cycle. 237 */ 238 239 static void make_pulses(void *arg, long period) 240 { 241 sim_enc_t *sim_enc; 242 int n, overunder, dir; 243 244 /* store period so scaling constants can be (re)calculated */ 245 periodns = period; 246 /* point to sim_enc data structures */ 247 sim_enc = arg; 248 for (n = 0; n < howmany; n++) { 249 /* get current value of bit 31 */ 250 overunder = sim_enc->accum >> 31; 251 /* update the accumulator */ 252 sim_enc->accum += sim_enc->addval; 253 /* test for overflow/underflow (change in bit 31) */ 254 overunder ^= sim_enc->accum >> 31; 255 if ( overunder ) { 256 /* time to update outputs */ 257 /* get direction bit, 1 if negative, 0 if positive */ 258 dir = sim_enc->addval >> 31; 259 if ( dir ) { 260 (*sim_enc->rawcounts) --; 261 /* negative rotation, decrement state, detect underflow */ 262 if (--(sim_enc->state) < 0) { 263 /* state underflow, roll over */ 264 sim_enc->state = 3; 265 /* decrement cycle, detect underflow */ 266 if (--(sim_enc->cycle) < 0) { 267 /* cycle underflow, roll over */ 268 sim_enc->cycle += *(sim_enc->ppr); 269 } 270 } 271 } else { 272 (*sim_enc->rawcounts) ++; 273 /* positive rotation, increment state, detect overflow */ 274 if (++(sim_enc->state) > 3) { 275 /* state overflow, roll over */ 276 sim_enc->state = 0; 277 /* increment cycle, detect overflow */ 278 if (++(sim_enc->cycle) >= *(sim_enc->ppr)) { 279 /* cycle overflow, roll over */ 280 sim_enc->cycle -= *(sim_enc->ppr); 281 } 282 } 283 } 284 } 285 /* generate outputs */ 286 switch (sim_enc->state) { 287 case 0: 288 *(sim_enc->phaseA) = 1; 289 *(sim_enc->phaseB) = 0; 290 break; 291 case 1: 292 *(sim_enc->phaseA) = 1; 293 *(sim_enc->phaseB) = 1; 294 break; 295 case 2: 296 *(sim_enc->phaseA) = 0; 297 *(sim_enc->phaseB) = 1; 298 break; 299 case 3: 300 *(sim_enc->phaseA) = 0; 301 *(sim_enc->phaseB) = 0; 302 break; 303 default: 304 /* illegal state, reset to legal one */ 305 sim_enc->state = 0; 306 } 307 if ((sim_enc->state == 0) && (sim_enc->cycle == 0)) { 308 *(sim_enc->phaseZ) = 1; 309 } else { 310 *(sim_enc->phaseZ) = 0; 311 } 312 /* move on to next 'encoder' */ 313 sim_enc++; 314 } 315 /* done */ 316 } 317 318 static void update_speed(void *arg, long period) 319 { 320 sim_enc_t *sim_enc; 321 int n; 322 double rev_sec, freq; 323 324 /* this periodns stuff is a little convoluted because we need to 325 calculate some constants here in this relatively slow thread but the 326 constants are based on the period of the much faster 'make_pulses()' 327 thread. */ 328 if (periodns != old_periodns) { 329 /* recompute various constants that depend on periodns */ 330 periodfp = periodns * 0.000000001; 331 maxf = 1.0 / periodfp; 332 freqscale = ((1L << 30) * 2.0) / maxf; 333 old_periodns = periodns; 334 } 335 /* update the 'encoders' */ 336 sim_enc = arg; 337 for (n = 0; n < howmany; n++) { 338 /* check for change in scale value */ 339 if ( *(sim_enc->scale) != sim_enc->old_scale ) { 340 /* save new scale to detect future changes */ 341 sim_enc->old_scale = *(sim_enc->scale); 342 /* scale value has changed, test and update it */ 343 if ((*(sim_enc->scale) < 1e-20) && (*(sim_enc->scale) > -1e-20)) { 344 /* value too small, divide by zero is a bad thing */ 345 *(sim_enc->scale) = 1.0; 346 } 347 /* we actually want the reciprocal */ 348 sim_enc->scale_mult = 1.0 / *(sim_enc->scale); 349 } 350 /* convert speed command (user units) to revs/sec */ 351 rev_sec = *(sim_enc->speed) * sim_enc->scale_mult; 352 /* convert speed command (revs per sec) to counts/sec */ 353 freq = rev_sec * (*(sim_enc->ppr)) * 4.0; 354 /* limit the commanded frequency */ 355 if (freq > maxf) { 356 freq = maxf; 357 } else if (freq < -maxf) { 358 freq = -maxf; 359 } 360 /* calculate new addval */ 361 sim_enc->addval = freq * freqscale; 362 sim_enc++; 363 } 364 /* done */ 365 } 366 367 /*********************************************************************** 368 * LOCAL FUNCTION DEFINITIONS * 369 ************************************************************************/ 370 371 static int export_sim_enc(sim_enc_t * addr, char *prefix) 372 { 373 int retval, msg; 374 375 /* This function exports a lot of stuff, which results in a lot of 376 logging if msg_level is at INFO or ALL. So we save the current value 377 of msg_level and restore it later. If you actually need to log this 378 function's actions, change the second line below */ 379 msg = rtapi_get_msg_level(); 380 rtapi_set_msg_level(RTAPI_MSG_WARN); 381 /* export param variable for pulses per rev */ 382 retval = hal_pin_u32_newf(HAL_IO, &(addr->ppr), comp_id, 383 "%s.ppr", prefix); 384 if (retval != 0) { 385 return retval; 386 } 387 /* export param variable for scaling */ 388 retval = hal_pin_float_newf(HAL_IO, &(addr->scale), comp_id, 389 "%s.scale", prefix); 390 if (retval != 0) { 391 return retval; 392 } 393 /* export pin for speed command */ 394 retval = hal_pin_float_newf(HAL_IN, &(addr->speed), comp_id, 395 "%s.speed", prefix); 396 if (retval != 0) { 397 return retval; 398 } 399 /* export pins for output phases */ 400 retval = hal_pin_bit_newf(HAL_OUT, &(addr->phaseA), comp_id, 401 "%s.phase-A", prefix); 402 if (retval != 0) { 403 return retval; 404 } 405 retval = hal_pin_bit_newf(HAL_OUT, &(addr->phaseB), comp_id, 406 "%s.phase-B", prefix); 407 if (retval != 0) { 408 return retval; 409 } 410 retval = hal_pin_bit_newf(HAL_OUT, &(addr->phaseZ), comp_id, 411 "%s.phase-Z", prefix); 412 if (retval != 0) { 413 return retval; 414 } 415 /* export pin for rawcounts */ 416 retval = hal_pin_s32_newf(HAL_IN, &(addr->rawcounts), comp_id, 417 "%s.rawcounts", prefix); 418 if (retval != 0) { 419 return retval; 420 } 421 /* init parameters */ 422 *(addr->ppr) = 100; 423 *(addr->scale) = 1.0; 424 /* init internal vars */ 425 addr->old_scale = 0.0; 426 addr->scale_mult = 1.0; 427 /* init the state variables */ 428 addr->accum = 0; 429 addr->addval = 0; 430 addr->state = 0; 431 addr->cycle = 0; 432 /* init the outputs */ 433 *(addr->phaseA) = 0; 434 *(addr->phaseB) = 0; 435 *(addr->phaseZ) = 0; 436 /* restore saved message level */ 437 rtapi_set_msg_level(msg); 438 return 0; 439 }