hal_gm.c
1 #include <rtapi_pci.h> 2 #include <rtapi_io.h> 3 4 #include "rtapi.h" // RTAPI realtime OS API 5 #include "rtapi_app.h" // RTAPI realtime module decls 6 #include "hal.h" // HAL public API decls 7 #include "gm.h" // Hardware dependent defines 8 #include "rtapi_math.h" 9 10 // Module information. 11 MODULE_AUTHOR("Bence Kovacs"); 12 MODULE_DESCRIPTION("Driver for General Mechatronics 6-Axis Motion Control Card for EMC HAL"); 13 MODULE_LICENSE("GPL"); 14 15 typedef struct { //encoder_t 16 // Pins 17 hal_bit_t *reset; 18 hal_s32_t *counts; 19 hal_float_t *position; 20 hal_float_t *velocity; 21 hal_s32_t *rawcounts; 22 hal_bit_t *index_enable; 23 24 // Parameters 25 hal_bit_t counter_mode; 26 hal_bit_t index_mode; 27 hal_bit_t index_invert; 28 hal_u32_t counts_per_rev; 29 hal_float_t position_scale; 30 hal_float_t min_speed_estimate; 31 32 // Private data 33 hal_s32_t raw_offset; 34 hal_s32_t index_offset; 35 hal_s32_t last_index_latch; 36 hal_bit_t first_index; 37 hal_bit_t module_exist; 38 } encoder_t; 39 40 typedef struct { //switches_t 41 // Pins. 42 hal_bit_t *home; 43 hal_bit_t *homeNot; 44 45 hal_bit_t *posLimSwIn; 46 hal_bit_t *posLimSwInNot; 47 hal_bit_t *negLimSwIn; 48 hal_bit_t *negLimSwInNot; 49 } switches_t; 50 51 typedef struct { //estop_t 52 // Pins. 53 hal_bit_t *in; 54 hal_bit_t *inNot; 55 } estop_t; 56 57 typedef struct { //gpio_t 58 // Pins. 59 hal_bit_t *in; 60 hal_bit_t *inNot; 61 hal_bit_t *out; 62 hal_bit_t isOut; 63 hal_bit_t invertOut; 64 } gpio_t; 65 66 typedef struct { //RS485_8output_t 67 // Pins. 68 hal_bit_t *out_0; 69 hal_bit_t *out_1; 70 hal_bit_t *out_2; 71 hal_bit_t *out_3; 72 hal_bit_t *out_4; 73 hal_bit_t *out_5; 74 hal_bit_t *out_6; 75 hal_bit_t *out_7; 76 // Parameters 77 hal_bit_t invertOut_0; 78 hal_bit_t invertOut_1; 79 hal_bit_t invertOut_2; 80 hal_bit_t invertOut_3; 81 hal_bit_t invertOut_4; 82 hal_bit_t invertOut_5; 83 hal_bit_t invertOut_6; 84 hal_bit_t invertOut_7; 85 } RS485_8output_t; 86 87 typedef struct { //RS485_8input_t 88 // Pins. 89 hal_bit_t *in_0; 90 hal_bit_t *inNot_0; 91 hal_bit_t *in_1; 92 hal_bit_t *inNot_1; 93 hal_bit_t *in_2; 94 hal_bit_t *inNot_2; 95 hal_bit_t *in_3; 96 hal_bit_t *inNot_3; 97 hal_bit_t *in_4; 98 hal_bit_t *inNot_4; 99 hal_bit_t *in_5; 100 hal_bit_t *inNot_5; 101 hal_bit_t *in_6; 102 hal_bit_t *inNot_6; 103 hal_bit_t *in_7; 104 hal_bit_t *inNot_7; 105 } RS485_8input_t; 106 107 typedef struct { //RS485_DacAdc_t 108 // Pins. 109 hal_float_t *DAC_0; 110 hal_float_t *DAC_1; 111 hal_float_t *DAC_2; 112 hal_float_t *DAC_3; 113 114 hal_bit_t *dac_0_enable; 115 hal_bit_t *dac_1_enable; 116 hal_bit_t *dac_2_enable; 117 hal_bit_t *dac_3_enable; 118 119 hal_float_t *ADC_0; 120 hal_float_t *ADC_1; 121 hal_float_t *ADC_2; 122 hal_float_t *ADC_3; 123 hal_float_t *ADC_4; 124 hal_float_t *ADC_5; 125 hal_float_t *ADC_6; 126 hal_float_t *ADC_7; 127 128 // Parameters. 129 hal_float_t DAC_0_offset; 130 hal_float_t DAC_1_offset; 131 hal_float_t DAC_2_offset; 132 hal_float_t DAC_3_offset; 133 134 hal_float_t DAC_0_min; 135 hal_float_t DAC_1_min; 136 hal_float_t DAC_2_min; 137 hal_float_t DAC_3_min; 138 139 hal_float_t DAC_0_max; 140 hal_float_t DAC_1_max; 141 hal_float_t DAC_2_max; 142 hal_float_t DAC_3_max; 143 144 hal_float_t ADC_0_offset; 145 hal_float_t ADC_1_offset; 146 hal_float_t ADC_2_offset; 147 hal_float_t ADC_3_offset; 148 hal_float_t ADC_4_offset; 149 hal_float_t ADC_5_offset; 150 hal_float_t ADC_6_offset; 151 hal_float_t ADC_7_offset; 152 153 hal_float_t ADC_0_scale; 154 hal_float_t ADC_1_scale; 155 hal_float_t ADC_2_scale; 156 hal_float_t ADC_3_scale; 157 hal_float_t ADC_4_scale; 158 hal_float_t ADC_5_scale; 159 hal_float_t ADC_6_scale; 160 hal_float_t ADC_7_scale; 161 } RS485_DacAdc_t; 162 163 typedef struct { //RS485_TeachPad_t 164 // Pins. 165 //6 ADC channel 166 hal_float_t *ADC_0; 167 hal_float_t *ADC_1; 168 hal_float_t *ADC_2; 169 hal_float_t *ADC_3; 170 hal_float_t *ADC_4; 171 hal_float_t *ADC_5; 172 //8 digital input 173 hal_bit_t *in_0; 174 hal_bit_t *inNot_0; 175 hal_bit_t *in_1; 176 hal_bit_t *inNot_1; 177 hal_bit_t *in_2; 178 hal_bit_t *inNot_2; 179 hal_bit_t *in_3; 180 hal_bit_t *inNot_3; 181 hal_bit_t *in_4; 182 hal_bit_t *inNot_4; 183 hal_bit_t *in_5; 184 hal_bit_t *inNot_5; 185 hal_bit_t *in_6; 186 hal_bit_t *inNot_6; 187 hal_bit_t *in_7; 188 hal_bit_t *inNot_7; 189 //encoder 190 hal_bit_t *enc_reset; 191 hal_s32_t *enc_counts; 192 hal_float_t *enc_position; 193 hal_s32_t *enc_rawcounts; 194 195 // Parameters 196 //6 ADC channels 197 hal_float_t ADC_0_offset; 198 hal_float_t ADC_1_offset; 199 hal_float_t ADC_2_offset; 200 hal_float_t ADC_3_offset; 201 hal_float_t ADC_4_offset; 202 hal_float_t ADC_5_offset; 203 hal_float_t ADC_0_scale; 204 hal_float_t ADC_1_scale; 205 hal_float_t ADC_2_scale; 206 hal_float_t ADC_3_scale; 207 hal_float_t ADC_4_scale; 208 hal_float_t ADC_5_scale; 209 //encoder 210 hal_float_t enc_position_scale; 211 212 // Private data 213 //encoder 214 hal_s32_t enc_raw_offset; 215 216 } RS485_TeachPad_t; 217 218 typedef struct { //RS485_mgr_t 219 hal_u32_t ID[16]; 220 hal_u32_t BYTES_TO_WRITE[16]; 221 hal_u32_t BYTES_TO_READ[16]; 222 } RS485_mgr_t; 223 224 typedef struct { //axisdac_t 225 // Pins. 226 hal_float_t *value; 227 hal_bit_t *enable; 228 229 // Parameters. 230 hal_float_t min; 231 hal_float_t max; 232 hal_float_t offset; 233 234 hal_bit_t invert_serial; 235 } axisdac_t; 236 237 typedef struct { //stepgen_t 238 // Pins. 239 hal_float_t *position_cmd; 240 hal_float_t *velocity_cmd; 241 hal_float_t *position_fb; 242 hal_s32_t *count_fb; 243 hal_bit_t *enable; 244 245 // Parameters 246 hal_u32_t step_type; //0: StepDir, 1: UpDown, 2: Quadrature 247 hal_bit_t control_type; //0: position, 1: velocity 248 hal_u32_t steplen; 249 hal_u32_t stepspace; 250 hal_u32_t dirdelay; 251 hal_float_t maxaccel; 252 hal_float_t maxvel; 253 hal_bit_t polarity_A; 254 hal_bit_t polarity_B; 255 hal_float_t position_scale; 256 257 //Saved Parameters 258 hal_u32_t curr_steplen; 259 hal_u32_t curr_stepspace; 260 hal_u32_t curr_dirdelay; 261 hal_float_t curr_maxaccel; 262 hal_float_t curr_maxvel; 263 hal_float_t curr_position_scale; 264 265 // Private data 266 hal_u32_t stepgen_fb_offset; 267 hal_float_t old_pos_cmd; 268 hal_float_t max_dv; 269 hal_float_t old_vel; 270 hal_float_t steprate_scale; 271 } stepgen_t; 272 273 typedef struct { //CardMgr_t 274 // Pins. 275 hal_bit_t *cardEnable; 276 hal_bit_t *power_enable; 277 hal_bit_t *power_fault; 278 hal_bit_t *watchdog_expired; 279 280 // Parameters 281 hal_bit_t watchdog_enable; 282 hal_u32_t watchdog_timeout_ns; 283 284 // Private data 285 hal_u32_t card_control_reg; 286 hal_bit_t disable; 287 hal_u32_t dbg_PCI_counter_last; 288 hal_u32_t cntr; 289 } cardMgr_t; 290 291 typedef struct { //CAN_GM_t 292 //Pins 293 hal_bit_t *enable; 294 hal_float_t *position_cmd; 295 hal_float_t *position_fb; 296 297 //Parameters 298 hal_float_t position_scale; 299 300 } CAN_GM_t; 301 302 typedef struct { //CANmsg_t 303 hal_bit_t Ext; //0: Standrad ID, 1: Extended ID 304 hal_u32_t ID; 305 hal_u32_t data[8]; 306 hal_u32_t DLC; 307 hal_bit_t RTR; 308 }CANmsg_t; 309 310 typedef struct { //gm_device_t 311 // Card relatad data 312 card *pCard; 313 int boardID; //Sequential nr of cards, 0 - MAX_GM_DEVICES-1 314 int cardID; //Version of the card and its modules 315 316 // Driver related data 317 switches_t switches[6]; 318 gpio_t gpio[32]; 319 estop_t estop[2]; 320 321 RS485_mgr_t RS485_mgr; 322 RS485_8input_t RS485_8input[16]; 323 RS485_8output_t RS485_8output[16]; 324 RS485_DacAdc_t RS485_DacAdc[16]; 325 RS485_TeachPad_t RS485_TeachPad[16]; 326 327 CAN_GM_t CAN_GM[6]; 328 329 stepgen_t stepgen[6]; 330 hal_u32_t stepgen_status; 331 axisdac_t axisdac[6]; 332 encoder_t encoder[6]; 333 334 cardMgr_t cardMgr; 335 336 hal_u32_t period_ns; 337 hal_float_t period_s; 338 hal_float_t rec_period_s; 339 } gm_device_t; 340 341 typedef struct { //gm_driver_t 342 int comp_id; 343 gm_device_t *device[MAX_GM_DEVICES]; 344 } gm_driver_t; 345 346 static gm_driver_t driver; 347 348 static int num_boards = 0; 349 static int failed_errno = 0; // errno of last failed registration 350 351 static struct 352 rtapi_pci_device_id gm_pci_tbl[] = { 353 // GM PCI Card 354 { 355 .vendor = PLX_VENDOR_ID, 356 .device = GM_DEVICE_ID, 357 .subvendor = PLX_VENDOR_ID, 358 .subdevice = GM_SUBDEVICE_ID_1, 359 }, 360 361 { 362 .vendor = PLX_VENDOR_ID, 363 .device = GM_DEVICE_ID, 364 .subvendor = PLX_VENDOR_ID, 365 .subdevice = GM_SUBDEVICE_ID_2, 366 }, 367 368 {0,}, 369 }; 370 371 ////////////////////////////////////////////////////////////////////////////// 372 // Function prototypes // 373 ////////////////////////////////////////////////////////////////////////////// 374 375 //Export Pins, Parameters and Functions 376 static int ExportFunctions(void *arg, int comp_id, int boardId); 377 static int ExportEncoder(void *arg, int comp_id, int version); 378 static int ExportStepgen(void *arg, int comp_id, int version); 379 static int ExportDAC(void *arg, int comp_id, int version); 380 static int ExportRS485(void *arg, int comp_id, int version); 381 static int ExportCAN(void *arg, int comp_id, int version); 382 static int ExportMixed(void *arg, int comp_id); 383 384 //PCI driver misc functions 385 static int gm_pci_probe(struct rtapi_pci_dev *dev, const struct rtapi_pci_device_id *id); 386 static void gm_pci_remove(struct rtapi_pci_dev *dev); 387 388 //Methods exported to HAL 389 static void read(void *arg, long period); 390 static void write(void *arg, long period); 391 static void RS485(void *arg, long period); 392 393 //Private methods 394 //StepGens 395 static void stepgen(void *arg, long period); 396 static void stepgenControl(void *arg, long period, unsigned int i); 397 static void stepgenCheckParameters(void *arg, long period, unsigned int channel); 398 //RS485 399 static unsigned int RS485_CheckChecksum(hal_u32_t* data, hal_u32_t length); 400 static unsigned int RS485_CalcChecksum(hal_u32_t* data, hal_u32_t length); 401 static void RS485_OrderDataRead(hal_u32_t* dataIn32, hal_u32_t* dataOut8, hal_u32_t length); 402 static void RS485_OrderDataWrite(hal_u32_t* dataIn8, hal_u32_t* dataOut32, hal_u32_t length); 403 //Encoders 404 static void encoder(void *arg, long period); 405 //CAN 406 static void GM_CAN_SERVO(void *arg); 407 static void CAN_SendDataFrame(void *arg, CANmsg_t *Msg); 408 static void CAN_ReceiveDataFrame(void *arg, CANmsg_t *Msg); 409 #ifdef CANOPEN 410 static void CAN_Reset(void *arg); 411 static void CAN_SetBaud(void *arg, hal_u32_t Baud); 412 #endif 413 static int CAN_ReadStatus(void *arg, hal_u32_t *RxCnt, hal_u32_t *TxCnt); 414 //Card management 415 static void card_mgr(void *arg, long period); 416 417 ////////////////////////////////////////////////////////////////////////////// 418 // PCI driver functions // 419 ////////////////////////////////////////////////////////////////////////////// 420 421 422 static int 423 gm_pci_probe(struct rtapi_pci_dev *dev, const struct rtapi_pci_device_id *id) 424 { 425 int error=0; 426 card *pCard = NULL; 427 gm_device_t *pDevice; 428 429 430 if (num_boards >= MAX_GM_DEVICES) { 431 rtapi_print_msg(RTAPI_MSG_ERR,"skipping AnyIO board at %s, this driver can only handle %d\n", rtapi_pci_name(dev), MAX_GM_DEVICES); 432 return -EINVAL; 433 } 434 435 // NOTE: this enables the board's BARs -- this fixes the Arty bug 436 if (rtapi_pci_enable_device(dev)) { 437 rtapi_print_msg(RTAPI_MSG_ERR,"skipping AnyIO board at %s, failed to enable PCI device\n", rtapi_pci_name(dev)); 438 return failed_errno = -ENODEV; 439 } 440 441 // Allocate memory for device object. 442 pDevice = hal_malloc(sizeof(gm_device_t)); 443 444 if (pDevice == 0) { 445 rtapi_print_msg(RTAPI_MSG_ERR, "General Mechatronics: ERROR: hal_malloc() failed.\n"); 446 hal_exit(driver.comp_id); 447 return(-ENOMEM); 448 } 449 450 // Save pointer to device object. 451 driver.device[num_boards] = pDevice; 452 453 // Map card into memory. 454 pCard = (card *)rtapi_pci_ioremap_bar(dev, 5); 455 rtapi_print_msg(RTAPI_MSG_INFO, "General Mechatronics: Card address @ %p, Len = %d.\n", pCard, (int)rtapi_pci_resource_len(dev, 5)); 456 457 // Initialize device. 458 pDevice->pCard = pCard; 459 460 // Give board id for the card, increasing from 0 461 pDevice->boardID = num_boards++; 462 463 //Check card ID 464 pDevice->cardID = pCard->cardID; 465 rtapi_print_msg(RTAPI_MSG_INFO, "General Mechatronics: Card ID: 0x%X.\n", pDevice->cardID); 466 467 if ( (pDevice->cardID & IDmask_card) != cardVersion1 ) { 468 rtapi_print_msg(RTAPI_MSG_ERR, "General Mechatronics: ERROR, unknown card detected.\nPlease, download the latest driver.\n"); 469 hal_exit(driver.comp_id); 470 return(-ENODEV); 471 } 472 473 // Export and init pins, parameters, and functions 474 rtapi_set_msg_level(RTAPI_MSG_WARN); 475 pDevice->cardMgr.disable = 0; //Enable pointers of not presented modules will be referenced to this variable 476 pDevice->period_ns = 0; 477 478 error = ExportEncoder(pDevice, driver.comp_id, pDevice->cardID & IDmask_encoder); 479 if(error == 0) error = ExportStepgen(pDevice, driver.comp_id, pDevice->cardID & IDmask_stepgen); 480 if(error == 0) error = ExportDAC(pDevice, driver.comp_id, pDevice->cardID & IDmask_dac); 481 if(error == 0) error = ExportRS485(pDevice, driver.comp_id, pDevice->cardID & IDmask_rs485); 482 if(error == 0) error = ExportCAN(pDevice, driver.comp_id, pDevice->cardID & IDmask_can); 483 if(error == 0) error = ExportMixed(pDevice, driver.comp_id); 484 if(error == 0) error = ExportFunctions(pDevice, driver.comp_id, pDevice->boardID); 485 486 pDevice->cardMgr.card_control_reg = 0; 487 488 rtapi_set_msg_level(RTAPI_MSG_ALL); 489 490 if(error){ 491 rtapi_print_msg(RTAPI_MSG_ERR, "General Mechatronics: Error exporting pins and parameters.\n"); 492 hal_exit(driver.comp_id); 493 return -EINVAL; 494 } 495 496 return 0; 497 } 498 499 static void 500 gm_pci_remove(struct rtapi_pci_dev *dev) 501 { 502 int i; 503 gm_device_t *pDevice; 504 505 for(i = 0; i < MAX_GM_DEVICES; i++){ 506 507 if((pDevice = driver.device[i]) != NULL) 508 { 509 // turn off all 510 pDevice->pCard->card_control_reg = (hal_s32_t) 0; 511 512 // Unmap card 513 rtapi_iounmap((void *)(pDevice->pCard)); 514 rtapi_pci_disable_device(dev); 515 rtapi_pci_set_drvdata(dev, NULL); 516 } 517 } 518 } 519 520 521 static struct 522 rtapi_pci_driver gm_pci_driver = { 523 .name = "hal_gm", 524 .id_table = gm_pci_tbl, 525 .probe = gm_pci_probe, 526 .remove = gm_pci_remove, 527 }; 528 529 ////////////////////////////////////////////////////////////////////////////// 530 // RTAPI main and exit functions // 531 ////////////////////////////////////////////////////////////////////////////// 532 533 int 534 rtapi_app_main(void) 535 { 536 int r = 0; 537 int msgLevel, i = 0; 538 539 msgLevel = rtapi_get_msg_level(); 540 rtapi_set_msg_level(RTAPI_MSG_ALL); 541 rtapi_print_msg(RTAPI_MSG_INFO, "General Mechatronics: Driver version 1.1.3 loading...\n"); 542 543 // Connect to the HAL. 544 driver.comp_id = hal_init("hal_gm"); 545 if (driver.comp_id < 0) { 546 rtapi_print_msg(RTAPI_MSG_ERR, "General Mechatronics: ERROR: hal_init() failed.\n"); 547 return(-EINVAL); 548 } 549 550 for(i = 0; i < MAX_GM_DEVICES; i++){ 551 driver.device[i] = NULL; 552 } 553 554 r = rtapi_pci_register_driver(&gm_pci_driver); 555 556 if (r != 0) { 557 rtapi_print_msg(RTAPI_MSG_ERR,"error registering PCI driver\n"); 558 hal_exit(driver.comp_id); 559 return r; 560 } 561 562 if(failed_errno) { 563 // at least one card registration failed 564 hal_exit(driver.comp_id); 565 rtapi_pci_unregister_driver(&gm_pci_driver); 566 return failed_errno; 567 } 568 569 if(num_boards == 0) { 570 // no cards were detected 571 hal_exit(driver.comp_id); 572 rtapi_pci_unregister_driver(&gm_pci_driver); 573 return -ENODEV; 574 } 575 576 if(num_boards == 0){ 577 // No card detected 578 rtapi_print_msg(RTAPI_MSG_WARN, "General Mechatronics: No General Mechatronics card detected :(. \n"); 579 hal_exit(driver.comp_id); 580 return -ENODEV; 581 } 582 583 hal_ready(driver.comp_id); 584 rtapi_set_msg_level(msgLevel); 585 586 return(0); 587 } 588 589 void 590 rtapi_app_exit(void) 591 { 592 rtapi_pci_unregister_driver(&gm_pci_driver); 593 hal_exit(driver.comp_id); 594 } 595 596 ////////////////////////////////////////////////////////////////////////////// 597 // Export Pins, Parameters and Functions // 598 ////////////////////////////////////////////////////////////////////////////// 599 600 static int 601 ExportEncoder(void *arg, int comp_id, int version) 602 { 603 int i, error=0, boardId; 604 gm_device_t *device = (gm_device_t *)arg; 605 card *pCard = device->pCard; 606 boardId = device->boardID; 607 608 609 //Export pins and parameters for encoder 610 switch (version) 611 { 612 case notPresented: 613 for(i=0;i<6;i++) 614 { 615 device->encoder[i].module_exist = 0; 616 } 617 rtapi_print_msg(RTAPI_MSG_WARN, "General Mechatronics: No encoder module available in this version of the Card.\n"); 618 break; 619 case encoderVersion1: 620 for(i=0;i<6;i++) 621 { 622 //Export Pins 623 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->encoder[i].reset), comp_id, "gm.%1d.encoder.%1d.reset", boardId, i); 624 if(error == 0) error = hal_pin_s32_newf(HAL_OUT, &(device->encoder[i].counts), comp_id, "gm.%1d.encoder.%1d.counts", boardId, i); 625 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->encoder[i].position), comp_id, "gm.%1d.encoder.%1d.position", boardId, i); 626 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->encoder[i].velocity), comp_id, "gm.%1d.encoder.%1d.velocity", boardId, i); 627 if(error == 0) error = hal_pin_s32_newf(HAL_OUT, &(device->encoder[i].rawcounts), comp_id, "gm.%1d.encoder.%1d.rawcounts", boardId, i); 628 if(error == 0) error = hal_pin_bit_newf(HAL_IO, &(device->encoder[i].index_enable), comp_id, "gm.%1d.encoder.%1d.index-enable", boardId, i); 629 630 //Export Parameters 631 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->encoder[i].counter_mode), comp_id, "gm.%1d.encoder.%1d.counter-mode", boardId, i); 632 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->encoder[i].index_mode), comp_id, "gm.%1d.encoder.%1d.index-mode", boardId, i); 633 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->encoder[i].index_invert), comp_id, "gm.%1d.encoder.%1d.index-invert", boardId, i); 634 if(error == 0) error = hal_param_u32_newf(HAL_RW, &(device->encoder[i].counts_per_rev), comp_id, "gm.%1d.encoder.%1d.counts-per-rev", boardId, i); 635 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->encoder[i].position_scale), comp_id, "gm.%1d.encoder.%1d.position-scale", boardId, i); 636 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->encoder[i].min_speed_estimate), comp_id, "gm.%1d.encoder.%1d.min-speed-estimate", boardId, i); 637 638 //Init parameters 639 device->encoder[i].raw_offset = pCard->ENC_counter[i]; 640 device->encoder[i].index_offset = 0; 641 device->encoder[i].last_index_latch = pCard->ENC_index_latch[i]; 642 device->encoder[i].first_index = 1; 643 device->encoder[i].module_exist = 1; 644 } 645 break; 646 default: 647 rtapi_print_msg(RTAPI_MSG_ERR, "General Mechatronics: ERROR, unknown encoder version.\nPlease, download the latest driver.\n"); 648 } 649 return error; 650 } 651 652 static int 653 ExportStepgen(void *arg, int comp_id, int version) 654 { 655 int i, error=0, boardId; 656 gm_device_t *device = (gm_device_t *)arg; 657 card *pCard = device->pCard; 658 boardId = device->boardID; 659 660 //Export pins and parameters for step generator 661 switch (version) 662 { 663 case notPresented: 664 for(i=0;i<6;i++) 665 { 666 device->stepgen[i].enable = &(device->cardMgr.disable); //Set enable pointers to a 0 value variable 667 device->stepgen[i].position_cmd = &(device->stepgen[i].old_pos_cmd); 668 } 669 rtapi_print_msg(RTAPI_MSG_WARN, "General Mechatronics: No stepgen module available in this version of the Card.\n"); 670 break; 671 case stepgenVersion1: 672 device->stepgen_status=0; 673 pCard->StepGen_status = 0; 674 //Export pins and parameters 675 for(i = 0; i < 6; i++) 676 { 677 //Export Pins 678 if(error == 0) error = hal_pin_float_newf(HAL_IN, &(device->stepgen[i].position_cmd), comp_id, "gm.%1d.stepgen.%1d.position-cmd", boardId, i); 679 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->stepgen[i].position_fb), comp_id, "gm.%1d.stepgen.%1d.position-fb", boardId, i); 680 if(error == 0) error = hal_pin_float_newf(HAL_IN, &(device->stepgen[i].velocity_cmd), comp_id, "gm.%1d.stepgen.%1d.velocity-cmd", boardId, i); 681 if(error == 0) error = hal_pin_s32_newf(HAL_OUT, &(device->stepgen[i].count_fb), comp_id, "gm.%1d.stepgen.%1d.count-fb", boardId, i); 682 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->stepgen[i].enable), comp_id, "gm.%1d.stepgen.%1d.enable", boardId, i); 683 684 //Export Parameters. 685 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->stepgen[i].control_type), comp_id, "gm.%1d.stepgen.%1d.control-type", boardId, i); //0: StepDir, 1: UpDown, 2: Quadrature 686 if(error == 0) error = hal_param_u32_newf(HAL_RW, &(device->stepgen[i].step_type), comp_id, "gm.%1d.stepgen.%1d.step-type", boardId, i); //0: position, 1: velocity 687 if(error == 0) error = hal_param_u32_newf(HAL_RW, &(device->stepgen[i].steplen), comp_id, "gm.%1d.stepgen.%1d.steplen", boardId, i); 688 if(error == 0) error = hal_param_u32_newf(HAL_RW, &(device->stepgen[i].stepspace), comp_id, "gm.%1d.stepgen.%1d.stepspace", boardId, i); 689 if(error == 0) error = hal_param_u32_newf(HAL_RW, &(device->stepgen[i].dirdelay), comp_id, "gm.%1d.stepgen.%1d.dirdelay", boardId, i); 690 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->stepgen[i].maxaccel), comp_id, "gm.%1d.stepgen.%1d.maxaccel", boardId, i); 691 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->stepgen[i].maxvel), comp_id, "gm.%1d.stepgen.%1d.maxvel", boardId, i); 692 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->stepgen[i].polarity_A), comp_id, "gm.%1d.stepgen.%1d.invert-step1", boardId, i); 693 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->stepgen[i].polarity_B), comp_id, "gm.%1d.stepgen.%1d.invert-step2", boardId, i); 694 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->stepgen[i].position_scale), comp_id, "gm.%1d.stepgen.%1d.position-scale", boardId, i); 695 if(error != 0) break; 696 697 //Init parameters 698 device->stepgen[i].curr_steplen = 0; 699 device->stepgen[i].curr_stepspace = 0; 700 device->stepgen[i].curr_dirdelay = 0; 701 device->stepgen[i].curr_maxaccel = 0.0; 702 device->stepgen[i].curr_maxvel = 0.0; 703 device->stepgen[i].curr_position_scale = 1.0; 704 device->stepgen[i].steprate_scale = 30 / 1000000000.0 * 4294967296.0; 705 device->stepgen[i].stepgen_fb_offset = pCard->StepGen_fb[i]; 706 707 //Init FPGA registers 708 pCard->StepGen_time_params[i] = 0; 709 pCard->StepGen_steprate[i] = 0; 710 } 711 break; 712 default: 713 rtapi_print_msg(RTAPI_MSG_ERR, "General Mechatronics: ERROR, unknown stepgen version.\nPlease, download the latest driver.\n"); 714 error = -1; 715 } 716 return error; 717 } 718 719 static int 720 ExportDAC(void *arg, int comp_id, int version) 721 { 722 int i, error=0, boardId; 723 gm_device_t *device = (gm_device_t *)arg; 724 card *pCard = device->pCard; 725 boardId = device->boardID; 726 727 //Export pins and parameters for DAC 728 switch (version) 729 { 730 case notPresented: 731 for(i=0;i<6;i++) 732 { 733 device->axisdac[i].enable = &(device->cardMgr.disable); //Set enable pointers to a 0 value variable 734 } 735 rtapi_print_msg(RTAPI_MSG_WARN, "General Mechatronics: No DAC module available in this version of the Card.\n"); 736 break; 737 case dacVersion1: 738 for(i=0;i<6;i++) 739 { 740 device->axisdac[i].enable = &(device->cardMgr.disable); //Set enable pointers to a 0 value variable 741 } 742 rtapi_print_msg(RTAPI_MSG_WARN, "General Mechatronics: This card supports DAC ver.1 only, which is no longer produced. No DAC pins will be exported to HAL. If you need DAC, contact to bence.kovacs@generalmechatronics.com for firmware upgrade.\n"); 743 break; 744 case dacVersion2: 745 for(i=0;i<6;i++) 746 { 747 //Export Pins 748 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->axisdac[i].enable), comp_id, "gm.%1d.dac.%1d.enable", boardId, i); 749 if(error == 0) error = hal_pin_float_newf(HAL_IN, &(device->axisdac[i].value), comp_id, "gm.%1d.dac.%1d.value", boardId, i); 750 751 //Export Parameters. 752 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->axisdac[i].min), comp_id, "gm.%1d.dac.%1d.low-limit", boardId, i); 753 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->axisdac[i].max), comp_id, "gm.%1d.dac.%1d.high-limit", boardId, i); 754 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->axisdac[i].offset), comp_id, "gm.%1d.dac.%1d.offset", boardId, i); 755 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->axisdac[i].invert_serial), comp_id, "gm.%1d.dac.%1d.invert-serial", boardId, i); 756 757 //Init Parameters 758 device->axisdac[i].max = 10; 759 device->axisdac[i].min = -10; 760 device->axisdac[i].offset = 0; 761 device->axisdac[i].invert_serial = 0; 762 763 //Init FPGA regs 764 pCard->DAC_0 = 0x1FFF1FFF; 765 pCard->DAC_1 = 0x1FFF1FFF; 766 pCard->DAC_2 = 0x1FFF1FFF; 767 } 768 break; 769 default: 770 rtapi_print_msg(RTAPI_MSG_ERR, "General Mechatronics: ERROR, unknown axis DAC version.\nPlease, download the latest driver.\n"); 771 error = -1; 772 } 773 return error; 774 } 775 776 static int 777 ExportRS485(void *arg, int comp_id, int version) 778 { 779 780 int i, error=0, boardId, temp; 781 gm_device_t *device = (gm_device_t *)arg; 782 card *pCard = device->pCard; 783 boardId = device->boardID; 784 785 //Export pins and parameters for connected RS485 modules 786 switch (version) 787 { 788 case notPresented: 789 rtapi_print_msg(RTAPI_MSG_WARN, "General Mechatronics: No RS485 module available in this version of the Card.\n"); 790 break; 791 case rs485Version1: 792 //READ IDs of connected modules 793 for(i=0; i<8; i++) 794 { 795 temp=(hal_u32_t)pCard->moduleId[i]; 796 797 if(((temp & 0xff)^0xaa) == ((temp & 0xff00)>>8)) 798 { 799 device-> RS485_mgr.ID[2*i]=(temp >> 8) & 0xff; 800 } 801 else device-> RS485_mgr.ID[2*i] = 0; 802 803 if(((temp & 0xff0000)^0xaa0000) == ((temp & 0xff000000)>>8)) 804 { 805 device-> RS485_mgr.ID[2*i+1]=(temp & 0xff000000)>>24; 806 } 807 else device-> RS485_mgr.ID[2*i+1]=0; 808 } 809 810 for(i = 0; i < 16; i++) 811 { 812 switch (device-> RS485_mgr.ID[i]) 813 { 814 case 0: 815 break; 816 case RS485MODUL_ID_8INPUT: 817 device-> RS485_mgr.BYTES_TO_WRITE[i]=0; 818 device-> RS485_mgr.BYTES_TO_READ[i]=2; //1 data byte + 1 Checksum 819 820 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].in_0), comp_id, "gm.%1d.rs485.%02d.in-0", boardId, i); 821 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].inNot_0), comp_id, "gm.%1d.rs485.%02d.in-not-0", boardId, i); 822 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].in_1), comp_id, "gm.%1d.rs485.%02d.in-1", boardId, i); 823 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].inNot_1), comp_id, "gm.%1d.rs485.%02d.in-not-1", boardId, i); 824 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].in_2), comp_id, "gm.%1d.rs485.%02d.in-2", boardId, i); 825 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].inNot_2), comp_id, "gm.%1d.rs485.%02d.in-not-2", boardId, i); 826 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].in_3), comp_id, "gm.%1d.rs485.%02d.in-3", boardId, i); 827 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].inNot_3), comp_id, "gm.%1d.rs485.%02d.in-not-3", boardId, i); 828 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].in_4), comp_id, "gm.%1d.rs485.%02d.in-4", boardId, i); 829 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].inNot_4), comp_id, "gm.%1d.rs485.%02d.in-not-4", boardId, i); 830 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].in_5), comp_id, "gm.%1d.rs485.%02d.in-5", boardId, i); 831 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].inNot_5), comp_id, "gm.%1d.rs485.%02d.in-not-5", boardId, i); 832 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].in_6), comp_id, "gm.%1d.rs485.%02d.in-6", boardId, i); 833 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].inNot_6), comp_id, "gm.%1d.rs485.%02d.in-not-6", boardId, i); 834 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].in_7), comp_id, "gm.%1d.rs485.%02d.in-7", boardId, i); 835 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_8input[i].inNot_7), comp_id, "gm.%1d.rs485.%02d.in-not-7", boardId, i); 836 break; 837 case RS485MODUL_ID_8OUTPUT: 838 device-> RS485_mgr.BYTES_TO_WRITE[i]=2; // 1 data byte + 1 Checksum 839 device-> RS485_mgr.BYTES_TO_READ[i]=0; 840 841 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->RS485_8output[i].out_0), comp_id, "gm.%1d.rs485.%02d.relay-0", boardId, i); 842 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->RS485_8output[i].out_1), comp_id, "gm.%1d.rs485.%02d.relay-1", boardId, i); 843 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->RS485_8output[i].out_2), comp_id, "gm.%1d.rs485.%02d.relay-2", boardId, i); 844 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->RS485_8output[i].out_3), comp_id, "gm.%1d.rs485.%02d.relay-3", boardId, i); 845 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->RS485_8output[i].out_4), comp_id, "gm.%1d.rs485.%02d.relay-4", boardId, i); 846 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->RS485_8output[i].out_5), comp_id, "gm.%1d.rs485.%02d.relay-5", boardId, i); 847 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->RS485_8output[i].out_6), comp_id, "gm.%1d.rs485.%02d.relay-6", boardId, i); 848 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->RS485_8output[i].out_7), comp_id, "gm.%1d.rs485.%02d.relay-7", boardId, i); 849 850 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->RS485_8output[i].invertOut_0), comp_id, "gm.%1d.rs485.%02d.invert-relay-0", boardId, i); 851 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->RS485_8output[i].invertOut_1), comp_id, "gm.%1d.rs485.%02d.invert-relay-1", boardId, i); 852 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->RS485_8output[i].invertOut_2), comp_id, "gm.%1d.rs485.%02d.invert-relay-2", boardId, i); 853 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->RS485_8output[i].invertOut_3), comp_id, "gm.%1d.rs485.%02d.invert-relay-3", boardId, i); 854 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->RS485_8output[i].invertOut_4), comp_id, "gm.%1d.rs485.%02d.invert-relay-4", boardId, i); 855 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->RS485_8output[i].invertOut_5), comp_id, "gm.%1d.rs485.%02d.invert-relay-5", boardId, i); 856 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->RS485_8output[i].invertOut_6), comp_id, "gm.%1d.rs485.%02d.invert-relay-6", boardId, i); 857 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->RS485_8output[i].invertOut_7), comp_id, "gm.%1d.rs485.%02d.invert-relay-7", boardId, i); 858 859 break; 860 case RS485MODUL_ID_DACADC: 861 device-> RS485_mgr.BYTES_TO_WRITE[i]=5; // 8 data byte + 1 Checksum 862 device-> RS485_mgr.BYTES_TO_READ[i]=9; 863 864 if(error == 0) error = hal_pin_float_newf(HAL_IN, &(device->RS485_DacAdc[i].DAC_0), comp_id, "gm.%1d.rs485.%02d.dac-0", boardId, i); 865 if(error == 0) error = hal_pin_float_newf(HAL_IN, &(device->RS485_DacAdc[i].DAC_1), comp_id, "gm.%1d.rs485.%02d.dac-1", boardId, i); 866 if(error == 0) error = hal_pin_float_newf(HAL_IN, &(device->RS485_DacAdc[i].DAC_2), comp_id, "gm.%1d.rs485.%02d.dac-2", boardId, i); 867 if(error == 0) error = hal_pin_float_newf(HAL_IN, &(device->RS485_DacAdc[i].DAC_3), comp_id, "gm.%1d.rs485.%02d.dac-3", boardId, i); 868 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->RS485_DacAdc[i].dac_0_enable), comp_id, "gm.%1d.rs485.%02d.dac-enable-0", boardId, i); 869 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->RS485_DacAdc[i].dac_1_enable), comp_id, "gm.%1d.rs485.%02d.dac-enable-1", boardId, i); 870 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->RS485_DacAdc[i].dac_2_enable), comp_id, "gm.%1d.rs485.%02d.dac-enable-2", boardId, i); 871 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->RS485_DacAdc[i].dac_3_enable), comp_id, "gm.%1d.rs485.%02d.dac-enable-3", boardId, i); 872 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_DacAdc[i].ADC_0), comp_id, "gm.%1d.rs485.%02d.adc-0", boardId, i); 873 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_DacAdc[i].ADC_1), comp_id, "gm.%1d.rs485.%02d.adc-1", boardId, i); 874 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_DacAdc[i].ADC_2), comp_id, "gm.%1d.rs485.%02d.adc-2", boardId, i); 875 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_DacAdc[i].ADC_3), comp_id, "gm.%1d.rs485.%02d.adc-3", boardId, i); 876 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_DacAdc[i].ADC_4), comp_id, "gm.%1d.rs485.%02d.adc-4", boardId, i); 877 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_DacAdc[i].ADC_5), comp_id, "gm.%1d.rs485.%02d.adc-5", boardId, i); 878 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_DacAdc[i].ADC_6), comp_id, "gm.%1d.rs485.%02d.adc-6", boardId, i); 879 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_DacAdc[i].ADC_7), comp_id, "gm.%1d.rs485.%02d.adc-7", boardId, i); 880 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].DAC_0_offset), comp_id, "gm.%1d.rs485.%02d.dac-offset-0", boardId, i); 881 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].DAC_1_offset), comp_id, "gm.%1d.rs485.%02d.dac-offset-1", boardId, i); 882 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].DAC_2_offset), comp_id, "gm.%1d.rs485.%02d.dac-offset-2", boardId, i); 883 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].DAC_3_offset), comp_id, "gm.%1d.rs485.%02d.dac-offset-3", boardId, i); 884 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].DAC_0_max), comp_id, "gm.%1d.rs485.%02d.dac-high-limit-0", boardId, i); 885 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].DAC_1_max), comp_id, "gm.%1d.rs485.%02d.dac-high-limit-1", boardId, i); 886 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].DAC_2_max), comp_id, "gm.%1d.rs485.%02d.dac-high-limit-2", boardId, i); 887 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].DAC_3_max), comp_id, "gm.%1d.rs485.%02d.dac-high-limit-3", boardId, i); 888 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].DAC_0_min), comp_id, "gm.%1d.rs485.%02d.dac-low-limit-0", boardId, i); 889 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].DAC_1_min), comp_id, "gm.%1d.rs485.%02d.dac-low-limit-1", boardId, i); 890 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].DAC_2_min), comp_id, "gm.%1d.rs485.%02d.dac-low-limit-2", boardId, i); 891 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].DAC_3_min), comp_id, "gm.%1d.rs485.%02d.dac-low-limit-3", boardId, i); 892 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_0_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-0", boardId, i); 893 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_1_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-1", boardId, i); 894 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_2_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-2", boardId, i); 895 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_3_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-3", boardId, i); 896 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_4_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-4", boardId, i); 897 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_5_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-5", boardId, i); 898 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_6_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-6", boardId, i); 899 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_7_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-7", boardId, i); 900 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_0_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-0", boardId, i); 901 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_1_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-1", boardId, i); 902 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_2_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-2", boardId, i); 903 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_3_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-3", boardId, i); 904 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_4_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-4", boardId, i); 905 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_5_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-5", boardId, i); 906 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_6_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-6", boardId, i); 907 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_DacAdc[i].ADC_7_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-7", boardId, i); 908 909 device->RS485_DacAdc[i].DAC_0_max = 10; 910 device->RS485_DacAdc[i].DAC_0_min = -10; 911 device->RS485_DacAdc[i].DAC_1_max = 10; 912 device->RS485_DacAdc[i].DAC_1_min = -10; 913 device->RS485_DacAdc[i].DAC_2_max = 10; 914 device->RS485_DacAdc[i].DAC_2_min = -10; 915 device->RS485_DacAdc[i].DAC_3_max = 10; 916 device->RS485_DacAdc[i].DAC_3_min = -10; 917 device->RS485_DacAdc[i].ADC_0_scale = 1; 918 device->RS485_DacAdc[i].ADC_1_scale = 1; 919 device->RS485_DacAdc[i].ADC_2_scale = 1; 920 device->RS485_DacAdc[i].ADC_3_scale = 1; 921 device->RS485_DacAdc[i].ADC_4_scale = 1; 922 device->RS485_DacAdc[i].ADC_5_scale = 1; 923 device->RS485_DacAdc[i].ADC_6_scale = 1; 924 device->RS485_DacAdc[i].ADC_7_scale = 1; 925 break; 926 case RS485MODUL_ID_TEACHPAD: 927 device-> RS485_mgr.BYTES_TO_WRITE[i]=0; 928 device-> RS485_mgr.BYTES_TO_READ[i]=12; //1 for 8 digit input, 6 for adc, 4 for encoder + 1 Checksum 929 930 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].in_0), comp_id, "gm.%1d.rs485.%02d.in-0", boardId, i); 931 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].inNot_0), comp_id, "gm.%1d.rs485.%02d.in-not-0", boardId, i); 932 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].in_1), comp_id, "gm.%1d.rs485.%02d.in-1", boardId, i); 933 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].inNot_1), comp_id, "gm.%1d.rs485.%02d.in-not-1", boardId, i); 934 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].in_2), comp_id, "gm.%1d.rs485.%02d.in-2", boardId, i); 935 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].inNot_2), comp_id, "gm.%1d.rs485.%02d.in-not-2", boardId, i); 936 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].in_3), comp_id, "gm.%1d.rs485.%02d.in-3", boardId, i); 937 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].inNot_3), comp_id, "gm.%1d.rs485.%02d.in-not-3", boardId, i); 938 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].in_4), comp_id, "gm.%1d.rs485.%02d.in-4", boardId, i); 939 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].inNot_4), comp_id, "gm.%1d.rs485.%02d.in-not-4", boardId, i); 940 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].in_5), comp_id, "gm.%1d.rs485.%02d.in-5", boardId, i); 941 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].inNot_5), comp_id, "gm.%1d.rs485.%02d.in-not-5", boardId, i); 942 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].in_6), comp_id, "gm.%1d.rs485.%02d.in-6", boardId, i); 943 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].inNot_6), comp_id, "gm.%1d.rs485.%02d.in-not-6", boardId, i); 944 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].in_7), comp_id, "gm.%1d.rs485.%02d.in-7", boardId, i); 945 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->RS485_TeachPad[i].inNot_7), comp_id, "gm.%1d.rs485.%02d.in-not-7", boardId, i); 946 947 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_TeachPad[i].ADC_0), comp_id, "gm.%1d.rs485.%02d.adc-0", boardId, i); 948 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_TeachPad[i].ADC_1), comp_id, "gm.%1d.rs485.%02d.adc-1", boardId, i); 949 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_TeachPad[i].ADC_2), comp_id, "gm.%1d.rs485.%02d.adc-2", boardId, i); 950 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_TeachPad[i].ADC_3), comp_id, "gm.%1d.rs485.%02d.adc-3", boardId, i); 951 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_TeachPad[i].ADC_4), comp_id, "gm.%1d.rs485.%02d.adc-4", boardId, i); 952 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_TeachPad[i].ADC_5), comp_id, "gm.%1d.rs485.%02d.adc-5", boardId, i); 953 954 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_TeachPad[i].ADC_0_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-0", boardId, i); 955 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_TeachPad[i].ADC_1_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-1", boardId, i); 956 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_TeachPad[i].ADC_2_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-2", boardId, i); 957 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_TeachPad[i].ADC_3_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-3", boardId, i); 958 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_TeachPad[i].ADC_4_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-4", boardId, i); 959 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_TeachPad[i].ADC_5_offset), comp_id, "gm.%1d.rs485.%02d.adc-offset-5", boardId, i); 960 961 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_TeachPad[i].ADC_0_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-0", boardId, i); 962 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_TeachPad[i].ADC_1_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-1", boardId, i); 963 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_TeachPad[i].ADC_2_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-2", boardId, i); 964 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_TeachPad[i].ADC_3_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-3", boardId, i); 965 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_TeachPad[i].ADC_4_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-4", boardId, i); 966 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_TeachPad[i].ADC_5_scale), comp_id, "gm.%1d.rs485.%02d.adc-scale-5", boardId, i); 967 968 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->RS485_TeachPad[i].enc_reset), comp_id, "gm.%1d.rs485.%02d.enc-reset", boardId, i); 969 if(error == 0) error = hal_pin_s32_newf(HAL_OUT, &(device->RS485_TeachPad[i].enc_counts), comp_id, "gm.%1d.rs485.%02d.enc-counts", boardId, i); 970 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->RS485_TeachPad[i].enc_position), comp_id, "gm.%1d.rs485.%02d.enc-position", boardId, i); 971 if(error == 0) error = hal_pin_s32_newf(HAL_OUT, &(device->RS485_TeachPad[i].enc_rawcounts), comp_id, "gm.%1d.rs485.%02d.enc-rawcounts", boardId, i); 972 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->RS485_TeachPad[i].enc_position_scale), comp_id, "gm.%1d.rs485.%02d.enc-position-scale", boardId, i); 973 974 device->RS485_TeachPad[i].ADC_0_scale = 1; 975 device->RS485_TeachPad[i].ADC_1_scale = 1; 976 device->RS485_TeachPad[i].ADC_2_scale = 1; 977 device->RS485_TeachPad[i].ADC_3_scale = 1; 978 device->RS485_TeachPad[i].ADC_4_scale = 1; 979 device->RS485_TeachPad[i].ADC_5_scale = 1; 980 981 device->RS485_TeachPad[i].ADC_0_offset = 0; 982 device->RS485_TeachPad[i].ADC_1_offset = 0; 983 device->RS485_TeachPad[i].ADC_2_offset = 0; 984 device->RS485_TeachPad[i].ADC_3_offset = 0; 985 device->RS485_TeachPad[i].ADC_4_offset = 0; 986 device->RS485_TeachPad[i].ADC_5_offset = 0; 987 988 device->RS485_TeachPad[i].enc_raw_offset = 0; 989 device->RS485_TeachPad[i].enc_position_scale=1; 990 break; 991 default: 992 rtapi_print_msg(RTAPI_MSG_ERR, "General Mechatronics: ERROR, unknown rs485 module type.\nPlease, download the latest driver.\n"); 993 } 994 } 995 break; 996 default: 997 rtapi_print_msg(RTAPI_MSG_ERR, "General Mechatronics: ERROR, unknown rs485 version.\nPlease, download the latest driver.\n"); 998 } 999 return error; 1000 } 1001 1002 static int 1003 ExportCAN(void *arg, int comp_id, int version) 1004 { 1005 int i, error=0, boardId; 1006 gm_device_t *device = (gm_device_t *)arg; 1007 boardId = device->boardID; 1008 1009 //Export pins and parameters for encoder 1010 switch (version) 1011 { 1012 case notPresented: 1013 for(i=0;i<6;i++) 1014 { 1015 device->CAN_GM[i].enable = &(device->cardMgr.disable); //Set enable pointers to a 0 value variable 1016 } 1017 rtapi_print_msg(RTAPI_MSG_WARN, "General Mechatronics: No CAN module available in this version of the Card.\n"); 1018 break; 1019 case canVersion1: 1020 1021 //Export Pins and Parameters for CAN GM Servo Controllers 1022 for(i=0;i<6;i++) 1023 { 1024 //Export Pins 1025 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->CAN_GM[i].enable), comp_id, "gm.%1d.can-gm.%1d.enable", boardId, i); 1026 if(error == 0) error = hal_pin_float_newf(HAL_IN, &(device->CAN_GM[i].position_cmd), comp_id, "gm.%1d.can-gm.%1d.position-cmd", boardId, i); 1027 if(error == 0) error = hal_pin_float_newf(HAL_OUT, &(device->CAN_GM[i].position_fb), comp_id, "gm.%1d.can-gm.%1d.position-fb", boardId, i); 1028 1029 //Export Parameters 1030 if(error == 0) error = hal_param_float_newf(HAL_RW, &(device->CAN_GM[i].position_scale), comp_id, "gm.%1d.can-gm.%1d.position-scale", boardId, i); 1031 } 1032 1033 //Export Pins and Parameters for CANopen Servo Controllers 1034 //In development... 1035 1036 break; 1037 default: 1038 rtapi_print_msg(RTAPI_MSG_ERR, "General Mechatronics: ERROR, unknown encoder version.\nPlease, download the latest driver.\n"); 1039 } 1040 return error; 1041 } 1042 1043 static int 1044 ExportMixed(void *arg, int comp_id) 1045 { 1046 int i, j, error=0, boardId; 1047 gm_device_t *device = (gm_device_t *)arg; 1048 boardId = device->boardID; 1049 1050 //Homing and End switches pins and parameters 1051 for(i = 0; i < 6; i++) 1052 { 1053 // Pins 1054 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->switches[i].home), comp_id, "gm.%1d.axis.%1d.home-sw-in", boardId, i); 1055 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->switches[i].homeNot), comp_id, "gm.%1d.axis.%1d.home-sw-in-not", boardId, i); 1056 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->switches[i].posLimSwIn), comp_id, "gm.%1d.axis.%1d.pos-lim-sw-in", boardId, i); 1057 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->switches[i].posLimSwInNot), comp_id, "gm.%1d.axis.%1d.pos-lim-sw-in-not", boardId, i); 1058 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->switches[i].negLimSwIn), comp_id, "gm.%1d.axis.%1d.neg-lim-sw-in", boardId, i); 1059 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->switches[i].negLimSwInNot), comp_id, "gm.%1d.axis.%1d.neg-lim-sw-in-not", boardId, i); 1060 } 1061 1062 //Power bridge Fault and Error pins 1063 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->cardMgr.power_enable), comp_id, "gm.%1d.power-enable", boardId); 1064 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->cardMgr.power_fault), comp_id, "gm.%1d.power-fault", boardId); 1065 1066 //Watchdog pins and parameters 1067 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->cardMgr.watchdog_enable), comp_id, "gm.%1d.watchdog-enable", boardId); 1068 if(error == 0) error = hal_param_u32_newf(HAL_RW, &(device->cardMgr.watchdog_timeout_ns), comp_id, "gm.%1d.watchdog-timeout-ns", boardId); 1069 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->cardMgr.watchdog_expired), comp_id, "gm.%1d.watchdog-expired", boardId); 1070 1071 //Export pins and parameters for parallel IOs 1072 for(i=0;i<4;i++) 1073 { 1074 for(j=0;j<8;j++) 1075 { 1076 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->gpio[i*8+j].in), comp_id, "gm.%1d.gpio.%1d.in-%1d", boardId, i, j); 1077 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->gpio[i*8+j].inNot), comp_id, "gm.%1d.gpio.%1d.in-not-%1d", boardId, i, j); 1078 if(error == 0) error = hal_pin_bit_newf(HAL_IN, &(device->gpio[i*8+j].out), comp_id, "gm.%1d.gpio.%1d.out-%1d", boardId, i, j); 1079 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->gpio[i*8+j].isOut), comp_id, "gm.%1d.gpio.%1d.is-out-%1d", boardId, i, j); 1080 if(error == 0) error = hal_param_bit_newf(HAL_RW, &(device->gpio[i*8+j].invertOut), comp_id, "gm.%1d.gpio.%1d.invert-out-%1d", boardId, i, j); 1081 } 1082 } 1083 1084 //Export pins and parameters for estops 1085 for(i=0;i<2;i++) 1086 { 1087 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->estop[i].in), comp_id, "gm.%1d.estop.%1d.in", boardId, i); 1088 if(error == 0) error = hal_pin_bit_newf(HAL_OUT, &(device->estop[i].inNot), comp_id, "gm.%1d.estop.%1d.in-not", boardId, i); 1089 } 1090 1091 device->cardMgr.cntr = 0; //Counter of executing card_mgr() function 1092 1093 return error; 1094 } 1095 1096 static int ExportFunctions(void *arg, int comp_id, int boardId) 1097 { 1098 int error; 1099 char str[HAL_NAME_LEN + 1]; 1100 gm_device_t *device = (gm_device_t *)arg; 1101 1102 rtapi_snprintf(str, sizeof(str), "gm.%d.write", boardId); 1103 error = hal_export_funct(str, write, device, 1, 0, comp_id); 1104 1105 if(error == 0) 1106 { 1107 rtapi_snprintf(str, sizeof(str), "gm.%d.read", boardId); 1108 error = hal_export_funct(str, read, device, 1, 0, comp_id); 1109 } 1110 1111 if(error == 0) 1112 { 1113 rtapi_snprintf(str, sizeof(str), "gm.%d.RS485", boardId); 1114 error = hal_export_funct(str, RS485, device, 1, 0, comp_id); 1115 } 1116 1117 return error; 1118 } 1119 1120 ////////////////////////////////////////////////////////////////////////////// 1121 // Read/Write HAL functions // 1122 ////////////////////////////////////////////////////////////////////////////// 1123 1124 static void 1125 read(void *arg, long period) 1126 { 1127 gm_device_t *device = (gm_device_t *)arg; 1128 card *pCard = device->pCard; 1129 int i; 1130 hal_u32_t temp; 1131 1132 //basic card functionality: watchdog, switches, estop 1133 card_mgr(arg, period); 1134 1135 //read parallel IOs 1136 temp=pCard->gpio; 1137 for(i = 0; i < 32; i++) 1138 { 1139 *(device->gpio[i].in) = (hal_bit_t)((temp & (0x0001 << i)) == 0 ? 0 : 1); 1140 *(device->gpio[i].inNot) = (hal_bit_t)((temp & (0x0001 << i)) == 0 ? 1 : 0); 1141 } 1142 1143 //Read Encoders 1144 encoder(arg, period); 1145 } 1146 1147 static void 1148 write(void *arg, long period) 1149 { 1150 gm_device_t *device = (gm_device_t *)arg; 1151 card *pCard = device->pCard; 1152 1153 int i, temp1=0, temp2=0; 1154 hal_float_t DAC[6]; 1155 hal_u32_t DAC_INTEGER[6]; 1156 1157 //Refresh DAC values 1158 for(i=0;i<6;i++) 1159 { 1160 if(*(device->axisdac[i].enable)) 1161 { 1162 if( (*(device->axisdac[i].value) + device->axisdac[i].offset) > device->axisdac[i].max) 1163 { 1164 DAC[i] = device->axisdac[i].max; 1165 } 1166 else if ( (*(device->axisdac[i].value) + device->axisdac[i].offset) < device->axisdac[i].min) 1167 { 1168 DAC[i] = device->axisdac[i].min; 1169 } 1170 else 1171 { 1172 DAC[i] = (*(device->axisdac[i].value) + device->axisdac[i].offset); 1173 } 1174 } 1175 else 1176 { 1177 DAC[i] = 0; 1178 } 1179 1180 DAC[i] = (DAC[i] * 819.15) + 8191.5; 1181 if(DAC[i] < 0) DAC[i] = 0; 1182 else if (DAC[i] > 16383) DAC[i] = 16383; 1183 1184 DAC_INTEGER[i] = (hal_u32_t)(DAC[i]); 1185 1186 if(device->axisdac[i].invert_serial) DAC_INTEGER[i] |= 0x8000; 1187 } 1188 pCard->DAC_0 = ((DAC_INTEGER[1] & 0xFFFF) << 16) | (DAC_INTEGER[0] & 0xFFFF); 1189 pCard->DAC_1 = ((DAC_INTEGER[3] & 0xFFFF) << 16) | (DAC_INTEGER[2] & 0xFFFF); 1190 pCard->DAC_2 = ((DAC_INTEGER[5] & 0xFFFF) << 16) | (DAC_INTEGER[4] & 0xFFFF); 1191 1192 //Run step generators 1193 stepgen(arg, period); 1194 1195 //Handle GM CAN Servo Controllers 1196 GM_CAN_SERVO(arg); 1197 1198 //Write parallel IOs 1199 temp1 = 0; 1200 for(i = 0; i < 32; i++) 1201 { 1202 if(device->gpio[i].isOut) temp1 |= 0x0001 << i; //write gpio mask: 1=output, 0=input 1203 if((*(device->gpio[i].out)) ^ (device->gpio[i].invertOut)) temp2 |= 0x0001 << i; //write outputs 1204 } 1205 pCard->gpioDir=temp1; 1206 pCard->gpio=temp2; 1207 1208 } 1209 1210 1211 ////////////////////////////////////////////////////////////////////////////// 1212 // CAN bus // 1213 ////////////////////////////////////////////////////////////////////////////// 1214 1215 static void 1216 GM_CAN_SERVO(void *arg) 1217 { 1218 gm_device_t *device = (gm_device_t *)arg; 1219 1220 hal_u32_t Rx_buf_cntr, Tx_buf_cntr; 1221 hal_u32_t i, temp=0; 1222 hal_s32_t posFb; 1223 CANmsg_t CAN_msg; 1224 1225 //Position references: ID 0x10 - 0x15 1226 //Position feed back: ID 0x20 - 0x25 1227 1228 //Do not run, if none of the GM CAN channels are enabled 1229 for(i=0;i<6;i++){ 1230 if(*(device->CAN_GM[i].enable) == 1) temp++; 1231 } 1232 if(temp == 0) return; 1233 1234 //Read Buffers status 1235 CAN_ReadStatus(arg, &Rx_buf_cntr, &Tx_buf_cntr); 1236 1237 //Read feedbacks 1238 for(i=0;i<Rx_buf_cntr;i++) 1239 { 1240 CAN_ReceiveDataFrame(arg, &CAN_msg); 1241 1242 if((CAN_msg.ID >= 0x20) && (CAN_msg.ID <= 0x25)) 1243 { 1244 if((device->CAN_GM[CAN_msg.ID - 0x20].position_scale<10e-6) && (device->CAN_GM[CAN_msg.ID - 0x20].position_scale>-10e-6)) 1245 { 1246 device->CAN_GM[CAN_msg.ID - 0x20].position_scale = 1; 1247 } 1248 posFb = (CAN_msg.data[3] << 24) | (CAN_msg.data[2] << 16) | (CAN_msg.data[1] << 8) | CAN_msg.data[0]; 1249 *(device->CAN_GM[CAN_msg.ID - 0x20].position_fb) = (hal_float_t)posFb / device->CAN_GM[CAN_msg.ID - 0x20].position_scale; 1250 } 1251 } 1252 1253 //Send reference 1254 for(i=0;i<6;i++) 1255 { 1256 if(*(device->CAN_GM[i].enable) == 1) 1257 { 1258 CAN_msg.RTR = 0; //Not a request frame 1259 CAN_msg.Ext = 0; //Standard ID 1260 CAN_msg.DLC = 4; //4 byte data 1261 CAN_msg.ID = 0x10 + i; 1262 1263 if((device->CAN_GM[i].position_scale<10e-6) && (device->CAN_GM[i].position_scale>-10e-6)) 1264 { 1265 device->CAN_GM[i].position_scale = 1; 1266 } 1267 temp = (hal_u32_t)(*(device->CAN_GM[i].position_cmd) * device->CAN_GM[i].position_scale); 1268 CAN_msg.data[0] = temp & 0xFF; 1269 CAN_msg.data[1] = (temp >> 8) & 0xFF; 1270 CAN_msg.data[2] = (temp >> 16) & 0xFF; 1271 CAN_msg.data[3] = (temp >> 24) & 0xFF; 1272 CAN_msg.data[4] = 0; 1273 CAN_msg.data[5] = 0; 1274 CAN_msg.data[6] = 0; 1275 CAN_msg.data[7] = 0; 1276 1277 CAN_SendDataFrame(arg, &CAN_msg); 1278 } 1279 } 1280 } 1281 static int 1282 CAN_ReadStatus(void *arg, hal_u32_t *RxCnt, hal_u32_t *TxCnt) 1283 { 1284 gm_device_t *device = (gm_device_t *)arg; 1285 card *pCard = device->pCard; 1286 1287 hal_u32_t temp; 1288 1289 temp = pCard->CAN_status_reg; 1290 *TxCnt = temp & 0x3FF; 1291 *RxCnt = (temp >> 10) & 0x3FF; 1292 1293 return (temp >> 20) & 0xFF; //return with MCP2515 IT reg 1294 } 1295 1296 static void 1297 CAN_ReceiveDataFrame(void *arg, CANmsg_t *Msg) 1298 { 1299 gm_device_t *device = (gm_device_t *)arg; 1300 card *pCard = device->pCard; 1301 1302 hal_u32_t temp; 1303 1304 temp = pCard->CAN_RX_buffer[0]; 1305 if(temp & 0x80000000) 1306 { 1307 Msg->Ext = 1; 1308 Msg->ID = temp & 0x1FFFFFFF; 1309 } 1310 else 1311 { 1312 Msg->Ext = 0; 1313 Msg->ID = temp; 1314 } 1315 1316 temp=pCard->CAN_RX_buffer[2]; 1317 Msg->data[0] = (temp>>24) & 0xFF; 1318 Msg->data[1] = (temp>>16) & 0xFF; 1319 Msg->data[2] = (temp>>8) & 0xFF; 1320 Msg->data[3] = (temp) & 0xFF; 1321 1322 temp=pCard->CAN_RX_buffer[1]; 1323 Msg->data[4] = (temp>>24) & 0xFF; 1324 Msg->data[5] = (temp>>16) & 0xFF; 1325 Msg->data[6] = (temp>>8) & 0xFF; 1326 Msg->data[7] = (temp) & 0xFF; 1327 1328 temp = pCard->CAN_RX_buffer[3]; 1329 Msg->DLC = temp & 0xF; 1330 if(temp & 0x10) Msg->RTR = 1; 1331 else Msg->RTR = 0; 1332 } 1333 1334 static void 1335 CAN_SendDataFrame(void *arg, CANmsg_t *Msg) 1336 { 1337 gm_device_t *device = (gm_device_t *)arg; 1338 card *pCard = device->pCard; 1339 1340 hal_u32_t ID; 1341 1342 ID = Msg->ID;//MsgID; 1343 if(Msg->Ext) ID |= 0x80000000; 1344 pCard->CAN_TX_buffer[0]=ID; 1345 1346 pCard->CAN_TX_buffer[2] = ((Msg->data[0] & 0xFF) << 24) |((Msg->data[1] & 0xFF) << 16) |((Msg->data[2] & 0xFF) << 8) |((Msg->data[3] & 0xFF) << 0); 1347 1348 if(Msg->DLC > 4) pCard->CAN_TX_buffer[1] = ((Msg->data[4] & 0xFF) << 24) |((Msg->data[5] & 0xFF) << 16) |((Msg->data[6] & 0xFF) << 8) |((Msg->data[7] & 0xFF) << 0); 1349 1350 pCard->CAN_TX_buffer[3] = Msg->DLC; 1351 } 1352 1353 #ifdef CANOPEN 1354 //Higher level protocols may need these functions 1355 static void 1356 CAN_Reset(void *arg) 1357 { 1358 gm_device_t *device = (gm_device_t *)arg; 1359 card *pCard = device->pCard; 1360 1361 pCard->CAN_TX_buffer[3] = 0x81; 1362 } 1363 1364 static void 1365 CAN_SetBaud(void *arg, hal_u32_t Baud) 1366 { 1367 gm_device_t *device = (gm_device_t *)arg; 1368 card *pCard = device->pCard; 1369 1370 switch(Baud) 1371 { 1372 case 125: 1373 pCard->CAN_TX_buffer[2] = 0x01880700; 1374 pCard->CAN_TX_buffer[3] = 0x82; 1375 break; 1376 1377 case 250: 1378 pCard->CAN_TX_buffer[2] = 0x01880300; 1379 pCard->CAN_TX_buffer[3] = 0x82; 1380 break; 1381 1382 case 500: 1383 pCard->CAN_TX_buffer[2] = 0x01880100; 1384 pCard->CAN_TX_buffer[3] = 0x82; 1385 break; 1386 1387 case 1000: 1388 pCard->CAN_TX_buffer[2] = 0x01880000; 1389 pCard->CAN_TX_buffer[3] = 0x82; 1390 break; 1391 1392 default: 1393 rtapi_print_msg(RTAPI_MSG_ERR, "General Mechatronics:Not valid CAN Baud Rate. Supported: 125,250,500 and 1000 kBit/s.\n"); 1394 } 1395 } 1396 #endif 1397 1398 ////////////////////////////////////////////////////////////////////////////// 1399 // Card manage functions // 1400 ////////////////////////////////////////////////////////////////////////////// 1401 static void 1402 card_mgr(void *arg, long period) 1403 { 1404 gm_device_t *device = (gm_device_t *)arg; 1405 card *pCard = device->pCard; 1406 1407 hal_u32_t temp=0, i, j; 1408 1409 //Read card status reg and process data 1410 temp = pCard->card_status_reg; //This resets watch dog timer 1411 1412 *(device->cardMgr.watchdog_expired) = (hal_bit_t)((temp & (0x0001 << 0)) == 0 ? 0 : 1); 1413 *(device->cardMgr.power_fault) = (hal_bit_t)((temp & (0x0001 << 2)) == 0 ? 0 : 1); 1414 *(device->estop[0].in) = (hal_bit_t)((temp & (0x0001 << 3)) == 0 ? 0 : 1); 1415 *(device->estop[0].inNot) = (hal_bit_t)((temp & (0x0001 << 3)) == 0 ? 1 : 0); 1416 *(device->estop[1].in) = (hal_bit_t)((temp & (0x0001 << 4)) == 0 ? 0 : 1); 1417 *(device->estop[1].inNot) = (hal_bit_t)((temp & (0x0001 << 4)) == 0 ? 1 : 0); 1418 1419 for(i=5, j=0; i<11; i++,j++) 1420 { 1421 *(device->switches[j].home) = (hal_bit_t)((temp & (0x0001 << i)) == 0 ? 0 : 1); 1422 *(device->switches[j].homeNot) = (hal_bit_t)((temp & (0x0001 << i)) == 0 ? 1 : 0); 1423 } 1424 for(j=0;i<17;i++,j++){ 1425 *(device->switches[j].posLimSwIn) = (hal_bit_t)((temp & (0x0001 << i)) == 0 ? 0 : 1); 1426 *(device->switches[j].posLimSwInNot) = (hal_bit_t)((temp & (0x0001 << i)) == 0 ? 1 : 0); 1427 } 1428 for(j=0;i<23;i++,j++){ 1429 *(device->switches[j].negLimSwIn) = (hal_bit_t)((temp & (0x0001 << i)) == 0 ? 0 : 1); 1430 *(device->switches[j].negLimSwInNot) = (hal_bit_t)((temp & (0x0001 << i)) == 0 ? 1 : 0); 1431 } 1432 1433 1434 //Chack if change happened in control reg and write control reg if well 1435 // ... Estop_1 | Estop_0 | Pwr_fault | Bus_err | Wdt_err //Card status read resets wdt 1436 temp = 1; //EMC run 1437 if(*(device->cardMgr.power_enable)) temp |= (0x0001 << 1); //power enable 1438 1439 if(device->cardMgr.watchdog_enable) //watchdog timeout in ns*256 unit. 0 if watchdog is disabled 1440 { 1441 if(device->cardMgr.watchdog_timeout_ns < 256) temp |= 0x100; 1442 else temp |= (device->cardMgr.watchdog_timeout_ns & 0xFFFFFF00); 1443 } 1444 1445 if(temp != device->cardMgr.card_control_reg) 1446 { 1447 device->cardMgr.card_control_reg = temp; 1448 pCard->card_control_reg = temp; 1449 } 1450 1451 //First 16 execution of this function measure PCI clk frequency. Result is printed to dmesg. 1452 if(device->cardMgr.cntr < 17) 1453 { 1454 if(device->cardMgr.cntr == 0) device->cardMgr.dbg_PCI_counter_last = pCard->PCI_clk_counter; 1455 else if(device->cardMgr.cntr == 16) 1456 { 1457 temp = rtapi_get_msg_level(); 1458 rtapi_set_msg_level(RTAPI_MSG_ALL); 1459 rtapi_print_msg(RTAPI_MSG_INFO, "General Mechatronics: PCI clk frequency is %d khz.\n", 1460 (int)((hal_float_t)(pCard->PCI_clk_counter - device->cardMgr.dbg_PCI_counter_last)/period*62500)); //Calculate frequency 1461 rtapi_set_msg_level(temp); 1462 } 1463 device->cardMgr.cntr++; 1464 } 1465 } 1466 1467 ////////////////////////////////////////////////////////////////////////////// 1468 // Encoder // 1469 ////////////////////////////////////////////////////////////////////////////// 1470 static void 1471 encoder(void *arg, long period) 1472 { 1473 gm_device_t *device = (gm_device_t *)arg; 1474 card *pCard = device->pCard; 1475 1476 int i; 1477 hal_s32_t temp1 = 0, temp2; 1478 hal_float_t vel; 1479 1480 //Update parameters 1481 for(i=0; i<6; i++) 1482 { 1483 if(device->encoder[i].index_invert == 1) temp1 |= (0x1 << i); 1484 if(device->encoder[i].counter_mode == 1) temp1 |= (0x1 << (i+6)); 1485 } 1486 pCard->ENC_control_reg = temp1; 1487 1488 1489 //Read encoders 1490 for(i = 0; i < 6; i++) 1491 if(device->encoder[i].module_exist) 1492 { 1493 temp1 = pCard->ENC_counter[i]; 1494 temp2 = pCard->ENC_index_latch[i]; 1495 1496 if(*(device->encoder[i].reset) == 1) //If encoder in reset state 1497 { 1498 device->encoder[i].index_offset = temp1; 1499 } 1500 else if(*(device->encoder[i].index_enable) == 1) //If not in reset and index is enabled 1501 { 1502 if (temp2 != device->encoder[i].last_index_latch) //If index puls come 1503 { 1504 if(device->encoder[i].index_mode == 0) //reset counter at index 1505 { 1506 device->encoder[i].index_offset = temp2; 1507 *(device->encoder[i].index_enable) = 0; //disable index 1508 } 1509 else //round counter at index 1510 { 1511 if(device->encoder[i].first_index == 1) //Check if not first index 1512 { 1513 device->encoder[i].first_index = 0; 1514 } 1515 else 1516 { 1517 if(temp2 > (device->encoder[i].last_index_latch + (hal_s32_t)(device->encoder[i].counts_per_rev/4))) 1518 { 1519 device->encoder[i].index_offset -= device->encoder[i].last_index_latch + device->encoder[i].counts_per_rev - temp2; 1520 } 1521 else if(temp2 < (device->encoder[i].last_index_latch - (hal_s32_t)(device->encoder[i].counts_per_rev/4))) 1522 { 1523 device->encoder[i].index_offset -= device->encoder[i].last_index_latch - device->encoder[i].counts_per_rev - temp2; 1524 } 1525 else 1526 { 1527 device->encoder[i].index_offset -= device->encoder[i].last_index_latch - temp2; 1528 } 1529 } 1530 } 1531 } 1532 } 1533 device->encoder[i].last_index_latch = temp2; 1534 1535 *(device->encoder[i].rawcounts) = temp1 - device->encoder[i].raw_offset; 1536 *(device->encoder[i].counts) = *(device->encoder[i].rawcounts) - device->encoder[i].index_offset; 1537 1538 if((device->encoder[i].position_scale < 0.000001) && (device->encoder[i].position_scale > -0.000001)) device->encoder[i].position_scale = 1; //Dont like to devide by 0 1539 *(device->encoder[i].position) = (hal_float_t) *(device->encoder[i].counts) / device->encoder[i].position_scale; 1540 1541 vel = (hal_float_t) pCard->ENC_period[i]; 1542 if(vel == 0) vel = 1; 1543 vel = 33333333 / ( vel * device->encoder[i].position_scale); //velocity in position units / s 1544 1545 if(fabs(vel) > device->encoder[i].min_speed_estimate) 1546 { 1547 *(device->encoder[i].velocity) = vel; 1548 } 1549 else 1550 { 1551 *(device->encoder[i].velocity) = 0; 1552 } 1553 } 1554 } 1555 1556 ////////////////////////////////////////////////////////////////////////////// 1557 // Stepgen // 1558 ////////////////////////////////////////////////////////////////////////////// 1559 static void 1560 stepgen(void *arg, long period) 1561 { 1562 gm_device_t *device = (gm_device_t *)arg; 1563 card *pCard = device->pCard; 1564 1565 int i; 1566 1567 //Update stepgen status with enable bits 1568 for(i=0;i<6;i++) 1569 { 1570 if(*(device->stepgen[i].enable) == 1) device->stepgen_status |= (0x1 << i); //six stepgens share one status reg, 5 bits for each. 1571 else 1572 { 1573 device->stepgen_status &= ~(0x1 << i); //LS bits of 5 bits are the enable bits 1574 pCard->StepGen_steprate[i] = 0; 1575 } 1576 } 1577 1578 //Check parameter changes, if enabled 1579 for(i=0;i<6;i++) 1580 { 1581 if(*(device->stepgen[i].enable) == 1) 1582 { 1583 stepgenCheckParameters(arg, period, i); 1584 } 1585 1586 } 1587 //Update fpga with status register (enable and parameter bits) 1588 pCard->StepGen_status = device->stepgen_status; 1589 1590 //Run steppers, if enabled 1591 for(i=0;i<6;i++) 1592 { 1593 if(*(device->stepgen[i].enable) == 1) 1594 { 1595 stepgenControl(arg, period, i); 1596 } 1597 } 1598 1599 //update old pos_cmd 1600 for(i=0;i<6;i++) 1601 { 1602 device->stepgen[i].old_pos_cmd = *(device->stepgen[i].position_cmd); 1603 } 1604 } 1605 1606 static void 1607 stepgenCheckParameters(void *arg, long period, unsigned int channel) 1608 { 1609 gm_device_t *device = (gm_device_t *)arg; 1610 card *pCard = device->pCard; 1611 1612 hal_u32_t temp1, temp2; 1613 hal_float_t min_period, max_vel; 1614 1615 //If period changed : recalc period related parameters 1616 if(device->period_ns != period) 1617 { 1618 device->period_s = period * 0.000000001; 1619 device->rec_period_s = 1.0/device->period_s; 1620 } 1621 1622 //If position scale changed : update steprate_scale 1623 if(device->stepgen[channel].curr_position_scale != device->stepgen[channel].position_scale) 1624 { 1625 //30 ns is circley time of CLK, 1/10e9 is ns -sec conversion, 42.. is 2^32 because steprate is 32 bit 1626 device->stepgen[channel].steprate_scale = device->stepgen[channel].position_scale * 30.0 / 1000000000.0 * 4294967296.0; 1627 } 1628 1629 //If steplen, stepspace, position_scale or max_vel changed : update max_vel 1630 if((device->stepgen[channel].steplen != device->stepgen[channel].curr_steplen) || (device->stepgen[channel].stepspace != device->stepgen[channel].curr_stepspace) || 1631 (device->stepgen[channel].maxvel != device->stepgen[channel].curr_maxvel) || (device->stepgen[channel].curr_position_scale != device->stepgen[channel].position_scale)) 1632 { 1633 min_period = (device->stepgen[channel].steplen + device->stepgen[channel].stepspace) * 0.000000001; 1634 max_vel = 1/((hal_float_t)min_period * fabs(device->stepgen[channel].position_scale)); 1635 1636 if(device->stepgen[channel].maxvel <= 0) 1637 { 1638 device->stepgen[channel].maxvel = max_vel; 1639 } 1640 else 1641 { 1642 if(max_vel < device->stepgen[channel].maxvel) //if stepgen given velocity is higher, then what is possible with step timing parameters 1643 { 1644 device->stepgen[channel].maxvel = max_vel; 1645 rtapi_print_msg(RTAPI_MSG_ERR, "GM: stepgen.%d.maxvel can not be reached with given 'steplen' and 'stepspace' parameters.\n", channel); 1646 } 1647 } 1648 } 1649 1650 //If steplen or dirdelay changed : update FPGA time parameter regs 1651 if((device->stepgen[channel].steplen != device->stepgen[channel].curr_steplen) || (device->stepgen[channel].dirdelay != device->stepgen[channel].curr_dirdelay)) 1652 { 1653 //Init time constants, send them to PCI 1654 temp1= (device->stepgen[channel].steplen <= 1900000) ? (device->stepgen[channel].steplen/30) : 63333; 1655 temp2= (device->stepgen[channel].dirdelay <= 1900000) ? (device->stepgen[channel].dirdelay/30) : 63333; 1656 1657 if((device->stepgen[channel].steplen > 1900000) || (device->stepgen[channel].dirdelay > 1900000)) 1658 { 1659 rtapi_print_msg(RTAPI_MSG_ERR, "GM: stepgen: 'steplen' and 'dirdelay' must be lower than 1 900 000 ns.\n"); 1660 } 1661 pCard->StepGen_time_params[channel] = (temp1 << 16) | (temp2 & 0xFFFF); 1662 } 1663 1664 //If enable, step_type or polarity bits changed : update fpga status reg 1665 if (*(device->stepgen[channel].enable) == 1) device->stepgen_status |= (0x1 << channel); //Bit 0-5 is the enable bit 1666 else device->stepgen_status &= ~(0x1 << channel); 1667 if (device->stepgen[channel].step_type == 1) device->stepgen_status |= (0x1 << (channel + 6)); //Bits 6-17 are the step_mode bits 1668 else device->stepgen_status &= ~(0x1 << (channel + 6)); 1669 if (device->stepgen[channel].step_type == 2) device->stepgen_status |= (0x1 << (channel + 12)); 1670 else device->stepgen_status &= ~(0x1 << (channel + 12)); 1671 if (device->stepgen[channel].polarity_A == 1) device->stepgen_status |= (0x1 << (channel + 18)); //18-23. bit is polarity of channel A 1672 else device->stepgen_status &= ~(0x1 << (channel + 18)); 1673 if (device->stepgen[channel].polarity_B == 1) device->stepgen_status |= (0x1 << (channel + 24)); //24-29. bit is polarity of channel B 1674 else device->stepgen_status &= ~(0x1 << (channel + 24)); 1675 1676 pCard->StepGen_status = device->stepgen_status; 1677 1678 //If max_vel, max_accel or period changed : calc max_dv 1679 if((device->stepgen[channel].maxvel != device->stepgen[channel].curr_maxvel) || (device->stepgen[channel].maxaccel != device->stepgen[channel].curr_maxaccel) || (device->period_ns != period)) 1680 { 1681 if(device->stepgen[channel].maxaccel <= 1e-20) 1682 { 1683 device->stepgen[channel].maxaccel = device->stepgen[channel].maxvel * device->rec_period_s; 1684 device->stepgen[channel].max_dv = device->stepgen[channel].maxvel; 1685 } 1686 else 1687 { 1688 device->stepgen[channel].max_dv = device->stepgen[channel].maxaccel * device->period_s; //max velocity change in position_unit/period^2 1689 } 1690 //vel = freq/pos_scale 1691 //pos=counts/pos_scale 1692 } 1693 1694 //Update current values 1695 device->period_ns = period; 1696 device->stepgen[channel].curr_position_scale = device->stepgen[channel].position_scale; 1697 device->stepgen[channel].curr_stepspace = device->stepgen[channel].stepspace; 1698 device->stepgen[channel].curr_maxvel = device->stepgen[channel].maxvel; 1699 device->stepgen[channel].curr_maxaccel = device->stepgen[channel].maxaccel; 1700 device->stepgen[channel].curr_steplen = device->stepgen[channel].steplen; 1701 device->stepgen[channel].curr_dirdelay= device->stepgen[channel].dirdelay; 1702 1703 } 1704 1705 static void 1706 stepgenControl(void *arg, long period, unsigned int channel) 1707 { 1708 gm_device_t *device = (gm_device_t *)arg; 1709 card *pCard = device->pCard; 1710 1711 hal_s32_t stepgen_fb, stepgen_fb_int, last_count_fb_LS16_bits, last_count_fb_MS16_bits, last_count_fb; 1712 hal_float_t ref_vel, match_acc, match_time, avg_v, est_out, est_cmd, est_err, dp; 1713 1714 //read and count feedbacks 1715 stepgen_fb = pCard->StepGen_fb[channel]; //pCard->StepGen_Fb[channel] is 16.16 bit fixed point feedback in [step] unit 1716 stepgen_fb -= device->stepgen[channel].stepgen_fb_offset; 1717 stepgen_fb_int= stepgen_fb >> 16; //get integer part of step feedback 1718 1719 last_count_fb = *(device->stepgen[channel].count_fb); 1720 last_count_fb_LS16_bits = last_count_fb & 0xFFFF; 1721 last_count_fb_MS16_bits = last_count_fb & 0xFFFF0000; 1722 1723 //Check for 16 bit overflow of stepgen_fb 1724 if(stepgen_fb_int > last_count_fb_LS16_bits + 32768) //16 bit step counter down overflow 1725 { 1726 *(device->stepgen[channel].count_fb) = (last_count_fb_MS16_bits + stepgen_fb_int - 65536); 1727 } 1728 else if (stepgen_fb_int + 32768 < last_count_fb_LS16_bits) //16 bit step counter up overflow 1729 { 1730 *(device->stepgen[channel].count_fb) = (last_count_fb_MS16_bits + stepgen_fb_int + 65536); 1731 } 1732 else //no overflow 1733 { 1734 *(device->stepgen[channel].count_fb) = (last_count_fb_MS16_bits + stepgen_fb_int); 1735 } 1736 1737 //save old position and get new one 1738 *(device->stepgen[channel].position_fb) = (*(device->stepgen[channel].count_fb) + ((hal_float_t)(stepgen_fb & 0xFFFF))/65536)/device->stepgen[channel].position_scale; //[pos_unit] 1739 1740 //velocity control is easy 1741 if(device->stepgen[channel].control_type == 1) 1742 { 1743 ref_vel = *(device->stepgen[channel].velocity_cmd); 1744 } 1745 1746 //Position control is more difficult 1747 /*Position control based on John Kasunich's stepgen hal component.*/ 1748 else if(device->stepgen[channel].control_type == 0) 1749 { 1750 //Reference velocity: 1751 ref_vel = (*(device->stepgen[channel].position_cmd) - device->stepgen[channel].old_pos_cmd) * device->rec_period_s; 1752 1753 if(ref_vel > device->stepgen[channel].old_vel) 1754 { 1755 match_acc = device->stepgen[channel].maxaccel; 1756 } 1757 else 1758 { 1759 match_acc = -device->stepgen[channel].maxaccel; 1760 } 1761 1762 match_time = (ref_vel - device->stepgen[channel].old_vel) / match_acc; 1763 1764 avg_v = (ref_vel + device->stepgen[channel].old_vel) * 0.5; 1765 1766 est_out = *(device->stepgen[channel].position_fb) + avg_v * match_time;; 1767 est_cmd = *(device->stepgen[channel].position_cmd) + ref_vel * (match_time - 1.5 * device->period_s); 1768 1769 est_err = est_out - est_cmd; 1770 1771 //If we can match velocity in one period 1772 if(match_time < device->period_s) 1773 { 1774 if(fabs(est_err) > 0.0001) //position correction 1775 { 1776 ref_vel -= 0.5 * est_err * device->rec_period_s; 1777 } 1778 } 1779 1780 //If we need more periods to reach, then ramp 1781 else 1782 { 1783 //position difference in case of ramping in the opposite direction 1784 dp = -2.0 * match_acc * device->period_s * match_time; //sum of velocity change 1785 1786 //decide which way to ramp 1787 if(fabs(est_err + dp*2.0) < fabs(est_err)) 1788 { 1789 match_acc = -match_acc; 1790 } 1791 ref_vel = device->stepgen[channel].old_vel + match_acc * device->period_s; 1792 } 1793 1794 } 1795 //Check max velocity, max acceleration and output baudrate 1796 1797 //Check max velocity 1798 if(ref_vel > device->stepgen[channel].maxvel) ref_vel = device->stepgen[channel].maxvel; 1799 else if(ref_vel < -device->stepgen[channel].maxvel) ref_vel = -device->stepgen[channel].maxvel; 1800 1801 //Check max acceleration 1802 if((device->stepgen[channel].old_vel-ref_vel) > device->stepgen[channel].max_dv) 1803 { 1804 ref_vel=device->stepgen[channel].old_vel-device->stepgen[channel].max_dv; 1805 } 1806 else if((device->stepgen[channel].old_vel-ref_vel) < -device->stepgen[channel].max_dv) 1807 { 1808 ref_vel=device->stepgen[channel].old_vel+device->stepgen[channel].max_dv; 1809 } 1810 //Save old velocity 1811 device->stepgen[channel].old_vel=ref_vel; 1812 //Set steprate 1813 pCard->StepGen_steprate[channel] = (hal_s32_t)(ref_vel * device->stepgen[channel].steprate_scale); 1814 1815 } 1816 1817 ////////////////////////////////////////////////////////////////////////////// 1818 // RS485 // 1819 ////////////////////////////////////////////////////////////////////////////// 1820 static void 1821 RS485(void *arg, long period) 1822 { 1823 gm_device_t *device = (gm_device_t *)arg; 1824 card *pCard = device->pCard; 1825 1826 unsigned int i, j; 1827 hal_float_t temp; 1828 hal_u32_t temp_u32; 1829 bool data_wr = 0; 1830 static hal_bit_t failed=0; 1831 1832 //for write function 1833 hal_u32_t RS485DataIn8[32], RS485DataOut32[8]; 1834 //for read function 1835 hal_u32_t RS485DataIn32[8], RS485DataOut8[32]; 1836 1837 //Check modules if any of it failed: 1838 //READ IDs of correctly connected modules and compare it with saved ID-s. 1839 for(i=0; i<8; i++) 1840 { 1841 temp_u32=(hal_u32_t)pCard->moduleId[i]; 1842 1843 if(((temp_u32 & 0xff)^0xaa) == ((temp_u32 & 0xff00)>>8)) 1844 { 1845 if((device-> RS485_mgr.ID[2*i]) != ((temp_u32 >> 8) & 0xff)) 1846 { 1847 //RS485 module falled off, error 1848 if(failed == 0) //Msg only first time, do not put 100 error msg 1849 { 1850 failed=1; 1851 rtapi_print_msg(RTAPI_MSG_ERR, "GM: ERROR: RS485 module ID:%2d failed.\n", 2*i); 1852 } 1853 *(device->cardMgr.power_fault) = 1; 1854 } 1855 } 1856 1857 if(((temp_u32 & 0xff0000)^0xaa0000) == ((temp_u32 & 0xff000000)>>8)) 1858 { 1859 if((device-> RS485_mgr.ID[2*i+1]) != ((temp_u32 & 0xff000000)>>24)) 1860 { 1861 //RS485 module falled off, error 1862 if(failed == 0) //Msg only first time, do not put 100 error msg 1863 { 1864 failed=1; 1865 rtapi_print_msg(RTAPI_MSG_ERR, "GM: ERROR: RS485 module ID:%2d failed.\n", 2*i+1); 1866 } 1867 *(device->cardMgr.power_fault) = 1; 1868 } 1869 } 1870 } 1871 1872 //read RS485-s 1873 for(i=0;i<16;i++) if((device-> RS485_mgr.ID[i] != 0) && (device-> RS485_mgr.BYTES_TO_READ[i] != 0)) //If the modul is presented and not write only 1874 { 1875 //Block ram address lookahead support 1876 if(i != 0) *(&(pCard->serialModulesDataIn[i-1][7])); 1877 else *(&(pCard->serialModulesDataOut[15][7])); 1878 //Read bytes to RS485DataIn32 array 1879 for(j=0; j<8; j++) RS485DataIn32[j]= (hal_u32_t)pCard->serialModulesDataIn[i][j]; 1880 1881 //Order data to RS485DataOut8 buffer 1882 RS485_OrderDataRead(RS485DataIn32, RS485DataOut8, device-> RS485_mgr.BYTES_TO_READ[i]); 1883 //Process data if Checksum is OK 1884 if(RS485_CheckChecksum(RS485DataOut8, device-> RS485_mgr.BYTES_TO_READ[i]) == 0) 1885 { 1886 switch (device-> RS485_mgr.ID[i]) 1887 { 1888 case RS485MODUL_ID_8INPUT: 1889 *(device->RS485_8input[i].in_0) = ((hal_bit_t)(RS485DataOut8[0] & 0x1) ? 1 : 0); 1890 *(device->RS485_8input[i].inNot_0) = (hal_bit_t)(RS485DataOut8[0] & 0x1) ? 0 : 1; 1891 *(device->RS485_8input[i].in_1) = (hal_bit_t)((RS485DataOut8[0] >> 1) & 0x1) ? 1 : 0; 1892 *(device->RS485_8input[i].inNot_1) = (hal_bit_t)((RS485DataOut8[0] >> 1) & 0x1) ? 0 : 1; 1893 *(device->RS485_8input[i].in_2) = (hal_bit_t)((RS485DataOut8[0] >> 2) & 0x1) ? 1 : 0; 1894 *(device->RS485_8input[i].inNot_2) = (hal_bit_t)((RS485DataOut8[0] >> 2) & 0x1) ? 0 : 1; 1895 *(device->RS485_8input[i].in_3) = (hal_bit_t)((RS485DataOut8[0] >> 3) & 0x1) ? 1 : 0; 1896 *(device->RS485_8input[i].inNot_3) = (hal_bit_t)((RS485DataOut8[0] >> 3) & 0x1) ? 0 : 1; 1897 *(device->RS485_8input[i].in_4) = (hal_bit_t)((RS485DataOut8[0] >> 4) & 0x1) ? 1 : 0; 1898 *(device->RS485_8input[i].inNot_4) = (hal_bit_t)((RS485DataOut8[0] >> 4) & 0x1) ? 0 : 1; 1899 *(device->RS485_8input[i].in_5) = (hal_bit_t)((RS485DataOut8[0] >> 5) & 0x1) ? 1 : 0; 1900 *(device->RS485_8input[i].inNot_5) = (hal_bit_t)((RS485DataOut8[0] >> 5) & 0x1) ? 0 : 1; 1901 *(device->RS485_8input[i].in_6) = (hal_bit_t)((RS485DataOut8[0] >> 6) & 0x1) ? 1 : 0; 1902 *(device->RS485_8input[i].inNot_6) = (hal_bit_t)((RS485DataOut8[0] >> 6) & 0x1) ? 0 : 1; 1903 *(device->RS485_8input[i].in_7) = (hal_bit_t)((RS485DataOut8[0] >> 7) & 0x1) ? 1 : 0; 1904 *(device->RS485_8input[i].inNot_7) = (hal_bit_t)((RS485DataOut8[0] >> 7) & 0x1) ? 0 : 1; 1905 break; 1906 1907 case RS485MODUL_ID_DACADC: 1908 *(device->RS485_DacAdc[i].ADC_0) = (((hal_float_t)RS485DataOut8[0])/25.5-5) * device->RS485_DacAdc[i].ADC_0_scale - device->RS485_DacAdc[i].ADC_0_offset; 1909 *(device->RS485_DacAdc[i].ADC_1) = (((hal_float_t)RS485DataOut8[1])/25.5-5) * device->RS485_DacAdc[i].ADC_1_scale - device->RS485_DacAdc[i].ADC_1_offset; 1910 *(device->RS485_DacAdc[i].ADC_2) = (((hal_float_t)RS485DataOut8[2])/25.5-5) * device->RS485_DacAdc[i].ADC_2_scale - device->RS485_DacAdc[i].ADC_2_offset; 1911 *(device->RS485_DacAdc[i].ADC_3) = (((hal_float_t)RS485DataOut8[3])/25.5-5) * device->RS485_DacAdc[i].ADC_3_scale - device->RS485_DacAdc[i].ADC_3_offset; 1912 *(device->RS485_DacAdc[i].ADC_4) = (((hal_float_t)RS485DataOut8[4])/25.5-5) * device->RS485_DacAdc[i].ADC_4_scale - device->RS485_DacAdc[i].ADC_4_offset; 1913 *(device->RS485_DacAdc[i].ADC_5) = (((hal_float_t)RS485DataOut8[5])/25.5-5) * device->RS485_DacAdc[i].ADC_5_scale - device->RS485_DacAdc[i].ADC_5_offset; 1914 *(device->RS485_DacAdc[i].ADC_6) = (((hal_float_t)RS485DataOut8[6])/25.5-5) * device->RS485_DacAdc[i].ADC_6_scale - device->RS485_DacAdc[i].ADC_6_offset; 1915 *(device->RS485_DacAdc[i].ADC_7) = (((hal_float_t)RS485DataOut8[7])/25.5-5) * device->RS485_DacAdc[i].ADC_7_scale - device->RS485_DacAdc[i].ADC_7_offset; 1916 1917 break; 1918 1919 case RS485MODUL_ID_TEACHPAD: 1920 1921 *(device->RS485_TeachPad[i].in_0) = ((hal_bit_t)(RS485DataOut8[0] & 0x1) ? 1 : 0); 1922 *(device->RS485_TeachPad[i].inNot_0) = (hal_bit_t)(RS485DataOut8[0] & 0x1) ? 0 : 1; 1923 *(device->RS485_TeachPad[i].in_1) = (hal_bit_t)((RS485DataOut8[0] >> 1) & 0x1) ? 1 : 0; 1924 *(device->RS485_TeachPad[i].inNot_1) = (hal_bit_t)((RS485DataOut8[0] >> 1) & 0x1) ? 0 : 1; 1925 *(device->RS485_TeachPad[i].in_2) = (hal_bit_t)((RS485DataOut8[0] >> 2) & 0x1) ? 1 : 0; 1926 *(device->RS485_TeachPad[i].inNot_2) = (hal_bit_t)((RS485DataOut8[0] >> 2) & 0x1) ? 0 : 1; 1927 *(device->RS485_TeachPad[i].in_3) = (hal_bit_t)((RS485DataOut8[0] >> 3) & 0x1) ? 1 : 0; 1928 *(device->RS485_TeachPad[i].inNot_3) = (hal_bit_t)((RS485DataOut8[0] >> 3) & 0x1) ? 0 : 1; 1929 *(device->RS485_TeachPad[i].in_4) = (hal_bit_t)((RS485DataOut8[0] >> 4) & 0x1) ? 1 : 0; 1930 *(device->RS485_TeachPad[i].inNot_4) = (hal_bit_t)((RS485DataOut8[0] >> 4) & 0x1) ? 0 : 1; 1931 *(device->RS485_TeachPad[i].in_5) = (hal_bit_t)((RS485DataOut8[0] >> 5) & 0x1) ? 1 : 0; 1932 *(device->RS485_TeachPad[i].inNot_5) = (hal_bit_t)((RS485DataOut8[0] >> 5) & 0x1) ? 0 : 1; 1933 *(device->RS485_TeachPad[i].in_6) = (hal_bit_t)((RS485DataOut8[0] >> 6) & 0x1) ? 1 : 0; 1934 *(device->RS485_TeachPad[i].inNot_6) = (hal_bit_t)((RS485DataOut8[0] >> 6) & 0x1) ? 0 : 1; 1935 *(device->RS485_TeachPad[i].in_7) = (hal_bit_t)((RS485DataOut8[0] >> 7) & 0x1) ? 1 : 0; 1936 *(device->RS485_TeachPad[i].inNot_7) = (hal_bit_t)((RS485DataOut8[0] >> 7) & 0x1) ? 0 : 1; 1937 1938 *(device->RS485_TeachPad[i].ADC_0) = (hal_float_t)RS485DataOut8[1]/51.2 * device->RS485_TeachPad[i].ADC_0_scale - device->RS485_TeachPad[i].ADC_0_offset; 1939 *(device->RS485_TeachPad[i].ADC_1) = (hal_float_t)RS485DataOut8[2]/51.2 * device->RS485_TeachPad[i].ADC_1_scale - device->RS485_TeachPad[i].ADC_1_offset; 1940 *(device->RS485_TeachPad[i].ADC_2) = (hal_float_t)RS485DataOut8[3]/51.2 * device->RS485_TeachPad[i].ADC_2_scale - device->RS485_TeachPad[i].ADC_2_offset; 1941 *(device->RS485_TeachPad[i].ADC_3) = (hal_float_t)RS485DataOut8[4]/51.2 * device->RS485_TeachPad[i].ADC_3_scale - device->RS485_TeachPad[i].ADC_3_offset; 1942 *(device->RS485_TeachPad[i].ADC_4) = (hal_float_t)RS485DataOut8[5]/51.2 * device->RS485_TeachPad[i].ADC_4_scale - device->RS485_TeachPad[i].ADC_4_offset; 1943 *(device->RS485_TeachPad[i].ADC_5) = (hal_float_t)RS485DataOut8[6]/51.2 * device->RS485_TeachPad[i].ADC_5_scale - device->RS485_TeachPad[i].ADC_5_offset; 1944 1945 *(device->RS485_TeachPad[i].enc_rawcounts)= (RS485DataOut8[7] & 0xff) | ((RS485DataOut8[8] & 0xff) << 8) | ((RS485DataOut8[9] & 0xff) << 16) | (RS485DataOut8[10] << 24); 1946 if(*(device->RS485_TeachPad[i].enc_reset)) 1947 { 1948 device->RS485_TeachPad[i].enc_raw_offset = *(device->RS485_TeachPad[i].enc_rawcounts); 1949 } 1950 *(device->RS485_TeachPad[i].enc_counts) = *(device->RS485_TeachPad[i].enc_rawcounts) - device->RS485_TeachPad[i].enc_raw_offset; 1951 1952 if((device->RS485_TeachPad[i].enc_position_scale < 0.000001) && (device->RS485_TeachPad[i].enc_position_scale > -0.000001)) device->RS485_TeachPad[i].enc_position_scale=1; //dont devide by 0 1953 1954 *(device->RS485_TeachPad[i].enc_position) = *(device->RS485_TeachPad[i].enc_counts) / device->RS485_TeachPad[i].enc_position_scale; 1955 1956 default: 1957 break; 1958 } 1959 1960 } 1961 } 1962 1963 //Write serial IOs 1964 for(i=0;i<16;i++) if((device-> RS485_mgr.ID[i] != 0) && (device-> RS485_mgr.BYTES_TO_WRITE[i] != 0)) //If the modul is presented and not read only 1965 { 1966 1967 switch (device-> RS485_mgr.ID[i]) 1968 { 1969 case RS485MODUL_ID_8OUTPUT: 1970 RS485DataIn8[0]=((*(device->RS485_8output[i].out_7) ^ (device->RS485_8output[i].invertOut_7)) << 7) | 1971 ((*(device->RS485_8output[i].out_6) ^ (device->RS485_8output[i].invertOut_6)) << 6) | 1972 ((*(device->RS485_8output[i].out_5) ^ (device->RS485_8output[i].invertOut_5)) << 5) | 1973 ((*(device->RS485_8output[i].out_4) ^ (device->RS485_8output[i].invertOut_4)) << 4) | 1974 ((*(device->RS485_8output[i].out_3) ^ (device->RS485_8output[i].invertOut_3)) << 3) | 1975 ((*(device->RS485_8output[i].out_2) ^ (device->RS485_8output[i].invertOut_2)) << 2) | 1976 ((*(device->RS485_8output[i].out_1) ^ (device->RS485_8output[i].invertOut_1)) << 1) | 1977 ((*(device->RS485_8output[i].out_0) ^ (device->RS485_8output[i].invertOut_0)) << 0); 1978 break; 1979 1980 case RS485MODUL_ID_DACADC: 1981 //DAC 0 1982 if(*(device->RS485_DacAdc[i].dac_0_enable)) 1983 { 1984 temp = *(device->RS485_DacAdc[i].DAC_0)+ device->RS485_DacAdc[i].DAC_0_offset; 1985 1986 if(temp > device->RS485_DacAdc[i].DAC_0_max) { temp = device->RS485_DacAdc[i].DAC_0_max; } 1987 else if(temp < device->RS485_DacAdc[i].DAC_0_min) { temp = device->RS485_DacAdc[i].DAC_0_min; } 1988 1989 temp = (temp + 10)*12.8 + 0.5; 1990 } 1991 else temp=128; 1992 1993 if(temp>255) temp=255; 1994 else if(temp <0) temp=0; 1995 RS485DataIn8[0]= (hal_u32_t)temp; 1996 //DAC 1 1997 if(*(device->RS485_DacAdc[i].dac_1_enable)) 1998 { 1999 temp = *(device->RS485_DacAdc[i].DAC_1)+ device->RS485_DacAdc[i].DAC_1_offset; 2000 2001 if(temp > device->RS485_DacAdc[i].DAC_1_max) { temp = device->RS485_DacAdc[i].DAC_1_max; } 2002 else if(temp < device->RS485_DacAdc[i].DAC_1_min) { temp = device->RS485_DacAdc[i].DAC_1_min; } 2003 2004 temp = (temp + 10)*12.8 + 0.5; 2005 } 2006 else temp=128; 2007 2008 if(temp>255) temp=255; 2009 else if(temp <0) temp=0; 2010 RS485DataIn8[1]= (hal_u32_t)temp; 2011 2012 //DAC 2 2013 if(*(device->RS485_DacAdc[i].dac_2_enable)) 2014 { 2015 temp = *(device->RS485_DacAdc[i].DAC_2)+ device->RS485_DacAdc[i].DAC_2_offset; 2016 2017 if(temp > device->RS485_DacAdc[i].DAC_2_max) { temp = device->RS485_DacAdc[i].DAC_2_max; } 2018 else if(temp < device->RS485_DacAdc[i].DAC_2_min) { temp = device->RS485_DacAdc[i].DAC_2_min; } 2019 2020 temp = (temp + 10)*12.8 + 0.5; 2021 } 2022 else temp=128; 2023 2024 if(temp>255) temp=255; 2025 else if(temp <0) temp=0; 2026 RS485DataIn8[2]= (hal_u32_t)temp; 2027 2028 //DAC 3 2029 if(*(device->RS485_DacAdc[i].dac_3_enable)) 2030 { 2031 temp = *(device->RS485_DacAdc[i].DAC_3)+ device->RS485_DacAdc[i].DAC_3_offset; 2032 2033 if(temp > device->RS485_DacAdc[i].DAC_3_max) { temp = device->RS485_DacAdc[i].DAC_3_max; } 2034 else if(temp < device->RS485_DacAdc[i].DAC_3_min) { temp = device->RS485_DacAdc[i].DAC_3_min; } 2035 2036 temp = (temp + 10)*12.8 + 0.5; 2037 } 2038 else temp=128; 2039 2040 if(temp>255) temp=255; 2041 else if(temp <0) temp=0; 2042 RS485DataIn8[3]= (hal_u32_t)temp; 2043 2044 break; 2045 2046 default: 2047 break; 2048 } 2049 2050 //Calc Checksum 2051 RS485DataIn8[device-> RS485_mgr.BYTES_TO_WRITE[i]-1]=RS485_CalcChecksum(RS485DataIn8, device-> RS485_mgr.BYTES_TO_WRITE[i] - 1); 2052 //Order 8 bit data to send to 32 bit PCI 2053 RS485_OrderDataWrite(RS485DataIn8, RS485DataOut32, device-> RS485_mgr.BYTES_TO_WRITE[i]); //order 8 bit bytes to 32 bit words 2054 //Send data 2055 for(j=7;(7-j) < ((unsigned int)(device-> RS485_mgr.BYTES_TO_WRITE[i]-1)/4) + 1;j--) 2056 { 2057 *(&(pCard->serialModulesDataOut[i][j])) = RS485DataOut32[j]; 2058 } 2059 data_wr=1; 2060 } 2061 //Ping RS485 BUS if no RS485 module with output is connected 2062 if(data_wr == 0) *(&(pCard->serialModulesDataOut[0][0])) = 0; 2063 } 2064 2065 2066 static void 2067 RS485_OrderDataRead(hal_u32_t* dataIn32, hal_u32_t* dataOut8, hal_u32_t length) 2068 { 2069 int i, j; 2070 //Order data (received to dataIn in reverse order and shifted) to dataOut[0]-dataOut[length-1] 2071 if(length > 0 && length < 32) 2072 for(i=length-1; i>=0; i--) 2073 { 2074 j = length-1-i; 2075 dataOut8[i]= (dataIn32[(unsigned int)(j/4)] >> ((j%4)*8)) & 0xff; 2076 } 2077 } 2078 2079 static void 2080 RS485_OrderDataWrite(hal_u32_t* dataIn8, hal_u32_t* dataOut32, hal_u32_t length) 2081 { 2082 int i, j; 2083 /* Byte order: 2084 RS485DataOut32[0]=0x28293031; 2085 RS485DataOut32[1]=0x24252627; 2086 RS485DataOut32[2]=0x20212223; 2087 RS485DataOut32[3]=0x16171819; 2088 2089 RS485DataOut32[4]=0x12131415; 2090 RS485DataOut32[5]=0x08091011; 2091 RS485DataOut32[6]=0x04050607; 2092 RS485DataOut32[7]=0x00010203; 2093 */ 2094 2095 for(i=0;i<length;i+=4) 2096 { 2097 j=7-(i >> 2); //To which word to write 2098 dataOut32[j]=(dataIn8[i+3] & 0xff) | ((dataIn8[i+2] & 0xff) << 8) | ((dataIn8[i+1] & 0xff) << 16) | (dataIn8[i] << 24); 2099 } 2100 } 2101 2102 static unsigned int 2103 RS485_CheckChecksum(hal_u32_t* data, hal_u32_t length) 2104 { 2105 unsigned int i=0, tempChecksum=0; 2106 2107 if(length > 0 && length < 32) 2108 for(i=0; i < length-1; i++) 2109 { 2110 tempChecksum += data[i]; 2111 } 2112 if((data[i] ^ 0xaa) == (tempChecksum & 0xff)) return 0; 2113 2114 return -1; 2115 2116 } 2117 2118 static unsigned int 2119 RS485_CalcChecksum(hal_u32_t* data, hal_u32_t length) 2120 { 2121 unsigned int i, tempChecksum=0; 2122 2123 if(length > 0 && length < 32) 2124 for(i=0; i < length; i++) 2125 { 2126 tempChecksum += data[i]; 2127 } 2128 return (tempChecksum & 0xff) ^ 0xaa; 2129 2130 } 2131