100 #define DigInSlave01_Pos 0, 3 |
101 #define DigInSlave01_Pos 0, 3 |
101 #define AnaOutSlave01_Pos 0, 4 |
102 #define AnaOutSlave01_Pos 0, 4 |
102 #define AnaInSlave01_Pos 0, 5 |
103 #define AnaInSlave01_Pos 0, 5 |
103 #define BusCoupler02_Pos 0, 6 |
104 #define BusCoupler02_Pos 0, 6 |
104 #define AnaInSlave02_Pos 0, 7 |
105 #define AnaInSlave02_Pos 0, 7 |
105 |
106 #define DPSlave01_Pos 0, 8 |
106 |
107 |
107 |
108 |
108 #define Beckhoff_EK1100 0x00000002, 0x044c2c52 |
109 #define Beckhoff_EK1100 0x00000002, 0x044c2c52 |
109 #define Beckhoff_EL1014 0x00000002, 0x03f63052 |
110 #define Beckhoff_EL1014 0x00000002, 0x03f63052 |
110 #define Beckhoff_EL2004 0x00000002, 0x07d43052 |
111 #define Beckhoff_EL2004 0x00000002, 0x07d43052 |
427 |
428 |
428 |
429 |
429 void rt_check_domain_state(void) |
430 void rt_check_domain_state(void) |
430 { |
431 { |
431 ec_domain_state_t ds; |
432 ec_domain_state_t ds; |
432 unsigned int printed=0; |
|
433 |
433 |
434 if (rt_fd>=0) |
434 if (rt_fd>=0) |
435 { |
435 { |
436 ecrt_rtdm_domain_state(rt_fd,&ds); |
436 ecrt_rtdm_domain_state(rt_fd,&ds); |
437 } |
437 } |
438 |
438 |
439 /* if (ds.working_counter != domain1_state.working_counter) |
439 if (ds.working_counter != domain1_state.working_counter) |
440 { |
440 { |
441 printf("Domain1: WC %u.\n", ds.working_counter); |
441 rt_printf("Domain1: WC %u.\n", ds.working_counter); |
442 printed=1; |
|
443 |
|
444 } |
442 } |
445 if (ds.wc_state != domain1_state.wc_state) |
443 if (ds.wc_state != domain1_state.wc_state) |
446 { |
444 { |
447 printf("Domain1: State %u.\n", ds.wc_state); |
445 rt_printf("Domain1: State %u.\n", ds.wc_state); |
448 printed=1; |
|
449 |
|
450 } |
446 } |
451 if (printed) |
447 |
452 { |
|
453 int ret; |
|
454 |
|
455 // return to realtime mode after print |
|
456 |
|
457 ret = rt_task_set_mode(0, T_PRIMARY, NULL); |
|
458 if (ret) |
|
459 { |
|
460 printf("error while rt_task_set_mode, code %d\n",ret); |
|
461 } |
|
462 } |
|
463 */ |
|
464 domain1_state = ds; |
448 domain1_state = ds; |
465 } |
449 } |
466 |
450 |
467 void rt_check_master_state(void) |
451 void rt_check_master_state(void) |
468 { |
452 { |
469 ec_master_state_t ms; |
453 ec_master_state_t ms; |
470 unsigned int printed=0; |
|
471 |
454 |
472 if (rt_fd>=0) |
455 if (rt_fd>=0) |
473 { |
456 { |
474 ecrt_rtdm_master_state(rt_fd,&ms); |
457 ecrt_rtdm_master_state(rt_fd,&ms); |
475 } |
458 } |
476 |
459 |
477 /* |
|
478 if (ms.slaves_responding != master_state.slaves_responding) |
460 if (ms.slaves_responding != master_state.slaves_responding) |
479 { |
461 { |
480 printf("%u slave(s).\n", ms.slaves_responding); |
462 rt_printf("%u slave(s).\n", ms.slaves_responding); |
481 printed=1; |
|
482 |
|
483 } |
463 } |
484 if (ms.al_states != master_state.al_states) |
464 if (ms.al_states != master_state.al_states) |
485 { |
465 { |
486 printf("AL states: 0x%02X.\n", ms.al_states); |
466 rt_printf("AL states: 0x%02X.\n", ms.al_states); |
487 printed=1; |
|
488 |
|
489 } |
467 } |
490 if (ms.link_up != master_state.link_up) |
468 if (ms.link_up != master_state.link_up) |
491 { |
469 { |
492 printf("Link is %s.\n", ms.link_up ? "up" : "down"); |
470 rt_printf("Link is %s.\n", ms.link_up ? "up" : "down"); |
493 printed=1; |
471 } |
494 |
|
495 } |
|
496 if (printed) |
|
497 { |
|
498 int ret; |
|
499 |
|
500 // return to realtime mode after print |
|
501 |
|
502 ret = rt_task_set_mode(0, T_PRIMARY, NULL); |
|
503 if (ret) |
|
504 { |
|
505 printf("error while rt_task_set_mode, code %d\n",ret); |
|
506 } |
|
507 }*/ |
|
508 master_state = ms; |
472 master_state = ms; |
509 } |
473 } |
510 |
474 |
511 void rt_receive() |
475 |
512 { |
476 |
513 |
|
514 if (rt_fd>=0) |
|
515 { |
|
516 ecrt_rtdm_master_recieve(rt_fd); |
|
517 } |
|
518 } |
|
519 |
|
520 void rt_send() |
|
521 { |
|
522 |
|
523 if (rt_fd>=0) |
|
524 { |
|
525 ecrt_rtdm_master_send(rt_fd); |
|
526 } |
|
527 } |
|
528 |
477 |
529 void rt_sync() |
478 void rt_sync() |
530 { |
479 { |
531 RTIME now; |
480 RTIME now; |
532 int ret; |
|
533 now = rt_timer_read(); |
481 now = rt_timer_read(); |
534 //now -= 946684800ULL * 1000000000ULL; |
482 |
535 |
|
536 printf("Write Sync Time %i\n",now); |
|
537 ret = rt_task_set_mode(0, T_PRIMARY, NULL); |
|
538 if (ret) |
|
539 { |
|
540 printf("error while rt_task_set_mode, code %d\n",ret); |
|
541 } |
|
542 |
483 |
543 if (rt_fd>=0) |
484 if (rt_fd>=0) |
544 { |
485 { |
545 ecrt_rtdm_master_application_time(rt_fd, &now); |
486 ecrt_rtdm_master_application_time(rt_fd, &now); |
546 } |
487 } |
563 /*****************************************************************************/ |
504 /*****************************************************************************/ |
564 |
505 |
565 #if SDO_ACCESS |
506 #if SDO_ACCESS |
566 void read_sdo(void) |
507 void read_sdo(void) |
567 { |
508 { |
568 switch (ecrt_sdo_request_state(sdo)) |
509 switch (ecrt_sdo_request_state(sdo)) |
569 { |
510 { |
570 case EC_REQUEST_UNUSED: // request was not used yet |
511 case EC_REQUEST_UNUSED: // request was not used yet |
571 ecrt_sdo_request_read(sdo); // trigger first read |
512 ecrt_sdo_request_read(sdo); // trigger first read |
572 break; |
513 break; |
573 case EC_REQUEST_BUSY: |
514 case EC_REQUEST_BUSY: |
574 fprintf(stderr, "Still busy...\n"); |
515 fprintf(stderr, "Still busy...\n"); |
575 break; |
516 break; |
576 case EC_REQUEST_SUCCESS: |
517 case EC_REQUEST_SUCCESS: |
577 fprintf(stderr, "SDO value: 0x%04X\n", |
518 fprintf(stderr, "SDO value: 0x%04X\n", |
578 EC_READ_U16(ecrt_sdo_request_data(sdo))); |
519 EC_READ_U16(ecrt_sdo_request_data(sdo))); |
579 ecrt_sdo_request_read(sdo); // trigger next read |
520 ecrt_sdo_request_read(sdo); // trigger next read |
580 break; |
521 break; |
581 case EC_REQUEST_ERROR: |
522 case EC_REQUEST_ERROR: |
582 fprintf(stderr, "Failed to read SDO!\n"); |
523 fprintf(stderr, "Failed to read SDO!\n"); |
583 ecrt_sdo_request_read(sdo); // retry reading |
524 ecrt_sdo_request_read(sdo); // retry reading |
584 break; |
525 break; |
585 } |
526 } |
586 } |
527 } |
587 |
528 |
588 void PrintSDOState(void) |
529 void PrintSDOState(void) |
589 { |
530 { |
590 switch (ecrt_sdo_request_state(sdo)) |
531 switch (ecrt_sdo_request_state(sdo)) |
591 { |
532 { |
592 case EC_REQUEST_UNUSED: // request was not used yet |
533 case EC_REQUEST_UNUSED: // request was not used yet |
593 fprintf(stderr, "SDO State: EC_REQUEST_UNUSED\n"); // trigger first read |
534 fprintf(stderr, "SDO State: EC_REQUEST_UNUSED\n"); // trigger first read |
594 break; |
535 break; |
595 case EC_REQUEST_BUSY: |
536 case EC_REQUEST_BUSY: |
596 fprintf(stderr, "SDO State: EC_REQUEST_BUSY\n"); |
537 fprintf(stderr, "SDO State: EC_REQUEST_BUSY\n"); |
597 break; |
538 break; |
598 case EC_REQUEST_SUCCESS: |
539 case EC_REQUEST_SUCCESS: |
599 fprintf(stderr, "SDO State: EC_REQUEST_SUCCESS\n"); |
540 fprintf(stderr, "SDO State: EC_REQUEST_SUCCESS\n"); |
600 break; |
541 break; |
601 case EC_REQUEST_ERROR: |
542 case EC_REQUEST_ERROR: |
602 fprintf(stderr, "SDO State: EC_REQUEST_ERROR\n"); |
543 fprintf(stderr, "SDO State: EC_REQUEST_ERROR\n"); |
603 break; |
544 break; |
604 default: |
545 default: |
605 fprintf(stderr, "SDO State: undefined\n"); |
546 fprintf(stderr, "SDO State: undefined\n"); |
606 break; |
547 break; |
607 } |
548 } |
608 } |
549 } |
609 #endif |
550 #endif |
610 |
551 |
611 |
552 |
849 if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { |
796 if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { |
850 fprintf(stderr, "PDO entry registration failed!\n"); |
797 fprintf(stderr, "PDO entry registration failed!\n"); |
851 return -1; |
798 return -1; |
852 } |
799 } |
853 #endif |
800 #endif |
|
801 |
|
802 |
|
803 printf("Get Configuring EL6731...\n"); |
|
804 sc_dpslv_01 = ecrt_master_slave_config(master, DPSlave01_Pos, Beckhoff_EL6731); |
|
805 if (!sc_dpslv_01) { |
|
806 fprintf(stderr, "Failed to get slave configuration.\n"); |
|
807 return -1; |
|
808 } |
|
809 |
|
810 printf("Configuring EL6731...\n"); |
|
811 if (ecrt_slave_config_pdos(sc_dpslv_01, EC_END, slave_7_syncs)) |
|
812 { |
|
813 fprintf(stderr, "Failed to configure PDOs.\n"); |
|
814 return -1; |
|
815 } |
|
816 |
|
817 #if SDO_ACCESS |
|
818 |
|
819 |
|
820 // DP Slave Parameter Set |
|
821 fprintf(stderr, "Creating SDO requests...\n"); |
|
822 if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8000, 0, 1))) { |
|
823 fprintf(stderr, "Failed to create SDO request.\n"); |
|
824 return -1; |
|
825 } |
|
826 ecrt_sdo_request_timeout(sdo, 500); // ms |
|
827 EC_WRITE_U8(ecrt_sdo_request_data(sdo), 0); |
|
828 PrintSDOState(); |
|
829 ecrt_sdo_request_write(sdo); |
|
830 PrintSDOState(); |
|
831 |
|
832 // Station Address |
|
833 if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8000, 1, 2))) { |
|
834 fprintf(stderr, "Failed to create SDO request.\n"); |
|
835 return -1; |
|
836 } |
|
837 ecrt_sdo_request_timeout(sdo, 500); // ms |
|
838 EC_WRITE_U16(ecrt_sdo_request_data(sdo), 5); |
|
839 //EC_WRITE_U8(ecrt_sdo_request_data(sdo), 00); |
|
840 //EC_WRITE_U8(ecrt_sdo_request_data(sdo)+1, 10); |
|
841 PrintSDOState(); |
|
842 ecrt_sdo_request_write(sdo); |
|
843 PrintSDOState(); |
|
844 |
|
845 // Device Type (DP Ident Number) |
|
846 if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8000, 4, 4))) { |
|
847 fprintf(stderr, "Failed to create SDO request.\n"); |
|
848 return -1; |
|
849 } |
|
850 ecrt_sdo_request_timeout(sdo, 500); // ms |
|
851 sdo_adr = ecrt_sdo_request_data(sdo); |
|
852 EC_WRITE_U32(sdo_adr, 0x095F); |
|
853 //EC_WRITE_U8(sdo_ad, 0x00); // Device Type |
|
854 //EC_WRITE_U8(sdo_adr+1, 0x00); |
|
855 //EC_WRITE_U8(sdo_adr+2, 0x09); |
|
856 //EC_WRITE_U8(sdo_adr+3, 0x5F); |
|
857 PrintSDOState(); |
|
858 ecrt_sdo_request_write(sdo); |
|
859 PrintSDOState(); |
|
860 |
|
861 // DP CfgData Slave |
|
862 if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8002, 0, 244))) { |
|
863 fprintf(stderr, "Failed to create SDO request.\n"); |
|
864 return -1; |
|
865 } |
|
866 ecrt_sdo_request_timeout(sdo, 500); // ms |
|
867 sdo_adr = ecrt_sdo_request_data(sdo); |
|
868 EC_WRITE_U8(sdo_adr, 0x10); // Device Type |
|
869 EC_WRITE_U8(sdo_adr+1, 0x20); |
|
870 PrintSDOState(); |
|
871 ecrt_sdo_request_write(sdo); |
|
872 PrintSDOState(); |
|
873 |
|
874 // DP Slave Parameter Set |
|
875 if (!(sdo = ecrt_slave_config_create_sdo_request(sc_dpslv_01, 0x8000, 0, 1))) { |
|
876 fprintf(stderr, "Failed to create SDO request.\n"); |
|
877 return -1; |
|
878 } |
|
879 |
|
880 ecrt_sdo_request_timeout(sdo, 500); // ms |
|
881 |
|
882 EC_WRITE_U8(ecrt_sdo_request_data(sdo), 0x33); // DP Slave Parameter Set |
|
883 PrintSDOState(); |
|
884 ecrt_sdo_request_write(sdo); |
|
885 PrintSDOState(); |
|
886 #endif |
854 |
887 |
855 |
888 |
856 |
889 |
857 sprintf(&rt_dev_file[0],"%s%u",EC_RTDM_DEV_FILE_NAME,0); |
890 sprintf(&rt_dev_file[0],"%s%u",EC_RTDM_DEV_FILE_NAME,0); |
858 |
891 |