main_profile_velocity_2.c 20 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659
  1. /**
  2. * manual compile : gcc main_profile_velocity_2.c -o ectest_PV -I/opt/etherlab/include -L/opt/etherlab/lib -lethercat -Wl,--rpath -Wl,/opt/etherlab/lib
  3. */
  4. #include <stdio.h>
  5. #include <stdint.h>
  6. #include <unistd.h>
  7. #include <stdbool.h>
  8. #include <string.h>
  9. #include <signal.h>
  10. #include <errno.h>
  11. #include <sys/mman.h>
  12. #include <sys/types.h>
  13. #include <sys/resource.h>
  14. #include <time.h>
  15. #include <sched.h>
  16. #include "ecrt.h"
  17. // **********************************************************
  18. // Application parameters.
  19. #define FREQUENCY 1000
  20. #define CLOCK_TO_USE CLOCK_REALTIME
  21. #define MEASURE_TIMING
  22. #define NSEC_PER_SEC (1000000000L)
  23. #define PERIOD_NS (NSEC_PER_SEC / FREQUENCY)
  24. #define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + (B).tv_nsec - (A).tv_nsec)
  25. #define TIMESPEC2NS(T) ((uint64_t)(T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)
  26. #ifdef MEASURE_TIMING
  27. // Log tht save data.
  28. char logname[50];
  29. FILE *logfile = NULL;
  30. int log_item_id = 1;
  31. int num_of_timeouts = 0;
  32. #endif
  33. // quit flag.
  34. volatile bool is_quit = false;
  35. // Period.
  36. const struct timespec cycletime = {0, PERIOD_NS};
  37. // **********************************************************
  38. // Operation model.
  39. #define MOTOR_MODEL_PROFILE_VELOCITY 0x03 // Profile Velocity Mode.
  40. // Control word.
  41. #define MOTOR_MODEL_CONTROL_WORD_HALT 0x1 << 8 // Set motor halt.
  42. // Profile_Acceleration.
  43. #define MOTOR_PROFILE_ACCELERATION (10 * 1000) // 10s, unit: millisecond from 0 rpm to 3000 rpm.
  44. // Profile_Deceleration.
  45. #define MOTOR_PROFILE_DECELERATION (10 * 1000) // 10s, unit: millisecond from 0 rpm to 3000 rpm.
  46. // Target velocity.
  47. #define MOTOR_TARGET_VELOCITY_FORWARD 15000 // forwaed, 1500 rpm, uint: 0.1 rpm, maximum 50000.
  48. #define MOTOR_TARGET_VELOCITY_REVERSE -15000 // reverse, 1500 rpm, uint: 0.1 rpm, maximum 50000.
  49. // Slave num.
  50. #define SLAVE_NUM 1
  51. // PDO information of device, get from `sudo ethercat cstruct` in cmd line
  52. /* Master 0, Slave 0
  53. * Vendor ID: 0x000001dd
  54. * Product code: 0x10305070
  55. * Revision number: 0x02040608
  56. */
  57. // Slave alias, position, vendor ID and product ID.
  58. #define SLAVE_0_ALIAS 0
  59. #define SLAVE_0_POSITION 0
  60. #define SLAVE_0_VID_PID 0x000001dd, 0x10305070
  61. // Offsets for PDO entries.
  62. static struct _SlaveOffset
  63. {
  64. unsigned int Operation_Mode;
  65. unsigned int Ctrl_Word;
  66. unsigned int Target_Velocity;
  67. unsigned int Profile_Acceleration;
  68. unsigned int Profile_Deceleration;
  69. unsigned int Status_Word;
  70. unsigned int Current_Velocity;
  71. } slave_offset[SLAVE_NUM];
  72. struct _SlaveInfo
  73. {
  74. uint32_t Alias;
  75. uint32_t Position;
  76. uint32_t Vendor_ID;
  77. uint32_t Product_Code;
  78. };
  79. struct _SlaveInfo slave_info[] = {{SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID}};
  80. struct _SlaveConfig
  81. {
  82. // Slave configuration.
  83. ec_slave_config_t *sc;
  84. ec_slave_config_state_t sc_state;
  85. struct _SlaveOffset offset;
  86. int current_velocity;
  87. };
  88. struct _Domain
  89. {
  90. ec_domain_t *domain;
  91. ec_domain_state_t domain_state;
  92. // Domain process data.
  93. uint8_t *domain_pd;
  94. };
  95. // Configure the PDOs entries register
  96. const static ec_pdo_entry_reg_t domain_regs[] = {
  97. // Slave 0.
  98. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x6040, 0, &slave_offset[0].Ctrl_Word},
  99. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x6060, 0, &slave_offset[0].Operation_Mode},
  100. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x60FF, 0, &slave_offset[0].Target_Velocity},
  101. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x6083, 0, &slave_offset[0].Profile_Acceleration},
  102. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x6084, 0, &slave_offset[0].Profile_Deceleration},
  103. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x6041, 0, &slave_offset[0].Status_Word},
  104. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x606B, 0, &slave_offset[0].Current_Velocity},
  105. {}};
  106. ec_pdo_entry_info_t slave_pdo_entries[] = {
  107. {0x6040, 0x00, 16}, // Control word.
  108. {0x6060, 0x00, 8}, // Operation mode.
  109. {0x60FF, 0x00, 32}, // Target velocity.
  110. {0x6083, 0x00, 32}, // Profile acceleration.
  111. {0x6084, 0x00, 32}, // Profile deceleeration.
  112. {0x6041, 0x00, 16}, // Status word.
  113. {0x606B, 0x00, 32}, // Current velocity.
  114. };
  115. ec_pdo_info_t slave_pdos[] = {
  116. {0x1600, 5, slave_pdo_entries + 0},
  117. {0x1a00, 2, slave_pdo_entries + 5},
  118. };
  119. ec_sync_info_t slave_syncs[] = {
  120. {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
  121. {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
  122. {2, EC_DIR_OUTPUT, 1, slave_pdos + 0, EC_WD_DISABLE},
  123. {3, EC_DIR_INPUT, 1, slave_pdos + 1, EC_WD_DISABLE},
  124. {0xff}};
  125. // Define EtherCAT master and corresponding states.
  126. static ec_master_t *master = NULL;
  127. static ec_master_state_t master_state = {};
  128. // Define process data of domain
  129. struct _Domain domain1;
  130. // Check process data state.
  131. void check_domain_state(struct _Domain *domain)
  132. {
  133. ec_domain_state_t ds;
  134. ecrt_domain_state(domain->domain, &ds);
  135. if (ds.working_counter != domain->domain_state.working_counter)
  136. {
  137. /* Periodically check the read/write status of the slave memory, and return 'working_counter + i' if it can be read and write.
  138. * send read-only command(xRD), if slave memory readable, 'working_counter + 1';
  139. * send write-only command(xWR), if slave memory writable, 'working_counter + 1';
  140. * send read-write command(xRW), if slave memory readable, 'working_counter + 1', if slave memory writable, 'working_counter + 2', that means 'working_counter + 3' ;
  141. */
  142. printf("Domain: WC %u.\n", ds.working_counter);
  143. }
  144. if (ds.wc_state != domain->domain_state.wc_state)
  145. {
  146. /*
  147. * 0: No registered process data were exchanged;
  148. * 1: Some of the registered process data were exchanged.
  149. * 2: All registered process data were exchanged.
  150. */
  151. printf("Domain: State %u.\n", ds.wc_state);
  152. }
  153. domain->domain_state = ds;
  154. }
  155. // Check for master state.
  156. void check_master_state()
  157. {
  158. ec_master_state_t ms;
  159. ecrt_master_state(master, &ms);
  160. if (ms.slaves_responding != master_state.slaves_responding)
  161. {
  162. // Sum of responding slaves on all Ethernet devices.
  163. printf("%u slave(s).\n", ms.slaves_responding);
  164. }
  165. if (ms.al_states != master_state.al_states)
  166. {
  167. // Application-layer states of all slaves. 1:init; 2:preop; 4:safeop; 8:op;
  168. printf("AL states: 0x%02X.\n", ms.al_states);
  169. }
  170. if (ms.link_up != master_state.link_up)
  171. {
  172. printf("Link is %s.\n", ms.link_up ? "up" : "down");
  173. if (!ms.link_up)
  174. {
  175. is_quit = true;
  176. }
  177. }
  178. master_state = ms;
  179. }
  180. // Check for slave configuration state.
  181. void check_slave_config_state(struct _SlaveConfig *slave_config)
  182. {
  183. ec_slave_config_state_t s;
  184. int i;
  185. for (i = 0; i < SLAVE_NUM; i++)
  186. {
  187. memset(&s, 0, sizeof(s));
  188. ecrt_slave_config_state(slave_config[i].sc, &s);
  189. if (s.al_state != slave_config[i].sc_state.al_state)
  190. {
  191. // The application-layer state of the slave. 1:init; 2:preop; 4:safeop; 8:op;
  192. printf("slave[%d]: State 0x%02X.\n", i, s.al_state);
  193. }
  194. if (s.online != slave_config[i].sc_state.online)
  195. {
  196. printf("slave[%d]: %s.\n", i, s.online ? "online" : "offline");
  197. }
  198. if (!s.online)
  199. {
  200. is_quit = true;
  201. }
  202. if (s.operational != slave_config[i].sc_state.operational)
  203. {
  204. // The slave was brought into \a OP state using the specified configuration.
  205. printf("slave[%d]: %soperational.\n", i, s.operational ? "" : "Not ");
  206. }
  207. slave_config[i].sc_state = s;
  208. }
  209. }
  210. struct timespec timespec_add(struct timespec time1, struct timespec time2)
  211. {
  212. struct timespec result;
  213. if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC)
  214. {
  215. result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
  216. result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
  217. }
  218. else
  219. {
  220. result.tv_sec = time1.tv_sec + time2.tv_sec;
  221. result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
  222. }
  223. return result;
  224. }
  225. // ************************************************************************
  226. void cyclic_task(struct _SlaveConfig *slave_config, struct _Domain *domain)
  227. {
  228. // Used to determine the value of the status word.
  229. uint16_t proof_value = 0x004F;
  230. uint16_t ctrl_word[] = {0x0006, 0x0007, 0x000f};
  231. uint16_t fault_reset = 0x008f;
  232. uint16_t status;
  233. uint32_t count = 0;
  234. static int direction = 0;
  235. struct timespec wakeupTime;
  236. #ifdef MEASURE_TIMING
  237. struct timespec startTime, endTime, lastStartTime = {};
  238. uint32_t period_ns = 0, exec_ns = 0, latency_ns = 0, latency_min_ns = 0, latency_max_ns = 0,
  239. period_min_ns = 0, period_max_ns = 0, exec_min_ns = 0, exec_max_ns = 0, period_max_print = 0;
  240. #endif
  241. int i;
  242. // Get current time.
  243. clock_gettime(CLOCK_TO_USE, &wakeupTime);
  244. while (!is_quit)
  245. {
  246. // Period time: 1ms.
  247. wakeupTime = timespec_add(wakeupTime, cycletime);
  248. clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
  249. #ifdef MEASURE_TIMING
  250. clock_gettime(CLOCK_TO_USE, &startTime);
  251. // The time interval of receiving and sending EtherCAT data.
  252. latency_ns = DIFF_NS(wakeupTime, startTime);
  253. // EtherCAT communication cycle.
  254. period_ns = DIFF_NS(lastStartTime, startTime);
  255. // Wait time for wake up.
  256. exec_ns = DIFF_NS(lastStartTime, endTime);
  257. lastStartTime = startTime;
  258. #endif
  259. ecrt_master_receive(master);
  260. ecrt_domain_process(domain->domain);
  261. check_domain_state(domain);
  262. check_master_state();
  263. check_slave_config_state(slave_config);
  264. #ifdef MEASURE_TIMING
  265. latency_max_ns = (latency_ns > latency_max_ns) ? latency_ns : latency_max_ns;
  266. latency_min_ns = (latency_ns < latency_min_ns) ? latency_ns : latency_min_ns;
  267. period_max_ns = (period_ns > period_max_ns) ? period_ns : period_max_ns;
  268. period_min_ns = (period_ns < period_min_ns) ? period_ns : period_min_ns;
  269. exec_max_ns = (exec_ns > exec_max_ns) ? exec_ns : exec_max_ns;
  270. exec_min_ns = (exec_ns < exec_min_ns) ? exec_ns : exec_min_ns;
  271. // Do not print frequently, or it will affect real-time performance.
  272. count++;
  273. if (count == FREQUENCY)
  274. {
  275. // Count the number of times the period exceeds 1100000ns and avoid the impact within the first second.
  276. if (log_item_id > 1 && period_max_ns >= 1100000)
  277. num_of_timeouts++;
  278. // output timing stats
  279. printf("period\tmin / max = %u / %u(ns)\n", period_min_ns, period_max_ns);
  280. printf("exec\tmin / max = %u / %u(ns)\n", exec_min_ns, exec_max_ns);
  281. printf("latency\tmin / max = %u / %u(ns)\n\n", latency_min_ns, latency_max_ns);
  282. if ((log_item_id++) > 1)
  283. {
  284. period_max_print = (period_max_print > period_max_ns) ? period_max_print : period_max_ns;
  285. printf("number of timeouts=%d, maximum period=%u\n\n", num_of_timeouts, period_max_print);
  286. }
  287. fprintf(logfile, "%d,%u,%u,%u,%u,%u,%u\r\n", log_item_id, period_min_ns, period_max_ns, exec_min_ns, exec_max_ns, latency_min_ns, latency_max_ns);
  288. period_max_ns = 0;
  289. period_min_ns = 0xffffffff;
  290. exec_max_ns = 0;
  291. exec_min_ns = 0xffffffff;
  292. latency_max_ns = 0;
  293. latency_min_ns = 0xffffffff;
  294. count = 0;
  295. }
  296. #endif
  297. for (i = SLAVE_NUM - 1; i >= 0; i--)
  298. {
  299. // Read state word.
  300. status = EC_READ_U16(domain->domain_pd + slave_offset[i].Status_Word);
  301. switch (status & proof_value)
  302. {
  303. // Control phase 1.
  304. case 0x0040:
  305. // Set operation mode to velocity Mode.
  306. EC_WRITE_U8(domain->domain_pd + slave_offset[i].Operation_Mode, MOTOR_MODEL_PROFILE_VELOCITY);
  307. // Set Profile_Acceleration and Profile_Deceleration.
  308. EC_WRITE_U32(domain->domain_pd + slave_offset[i].Profile_Acceleration, MOTOR_PROFILE_ACCELERATION);
  309. EC_WRITE_U32(domain->domain_pd + slave_offset[i].Profile_Deceleration, MOTOR_PROFILE_DECELERATION);
  310. EC_WRITE_U16(domain->domain_pd + slave_offset[i].Ctrl_Word, ctrl_word[0]);
  311. proof_value = 0x006F;
  312. break;
  313. // Control phase 2.
  314. case 0x0021:
  315. EC_WRITE_U16(domain->domain_pd + slave_offset[i].Ctrl_Word, ctrl_word[1]);
  316. proof_value = 0x006F;
  317. break;
  318. // Control phase 3.
  319. case 0x0023:
  320. EC_WRITE_U16(domain->domain_pd + slave_offset[i].Ctrl_Word, ctrl_word[2]);
  321. proof_value = 0x006F;
  322. break;
  323. // Control phase 3.
  324. case 0x0027:
  325. slave_config[i].current_velocity = EC_READ_U32(domain->domain_pd + slave_offset[i].Current_Velocity);
  326. // Determine whether to turn forward or reverse.
  327. if (direction == 0)
  328. {
  329. // forward.
  330. if (slave_config[i].current_velocity == 0)
  331. {
  332. EC_WRITE_S32(domain->domain_pd + slave_offset[i].Target_Velocity, MOTOR_TARGET_VELOCITY_FORWARD);
  333. }
  334. else if (slave_config[i].current_velocity == MOTOR_TARGET_VELOCITY_FORWARD)
  335. {
  336. EC_WRITE_S32(domain->domain_pd + slave_offset[i].Target_Velocity, 0);
  337. direction = 1;
  338. }
  339. }
  340. else if (direction == 1)
  341. {
  342. // reverse.
  343. if (slave_config[i].current_velocity == 0)
  344. {
  345. EC_WRITE_S32(domain->domain_pd + slave_offset[i].Target_Velocity, MOTOR_TARGET_VELOCITY_REVERSE);
  346. }
  347. else if (slave_config[i].current_velocity == MOTOR_TARGET_VELOCITY_REVERSE)
  348. {
  349. EC_WRITE_S32(domain->domain_pd + slave_offset[i].Target_Velocity, 0);
  350. direction = 0;
  351. }
  352. }
  353. break;
  354. default:
  355. // Fault reset.
  356. EC_WRITE_U16(domain->domain_pd + slave_offset[i].Ctrl_Word, 0x008f);
  357. break;
  358. }
  359. }
  360. // Send process data.
  361. ecrt_domain_queue(domain->domain);
  362. ecrt_master_send(master);
  363. #ifdef MEASURE_TIMING
  364. clock_gettime(CLOCK_TO_USE, &endTime);
  365. #endif
  366. }
  367. printf("Exit cyclic task.\n");
  368. }
  369. // Function stop servo.
  370. void stop_servo()
  371. {
  372. int i;
  373. // Receive process data.
  374. ecrt_master_receive(master);
  375. ecrt_domain_process(domain1.domain);
  376. for (i = 0; i < SLAVE_NUM; i++)
  377. {
  378. EC_WRITE_U16(domain1.domain_pd + slave_offset[i].Ctrl_Word, MOTOR_MODEL_CONTROL_WORD_HALT);
  379. }
  380. // Send process data, queues all domain datagrams in the master's datagram queue.
  381. ecrt_domain_queue(domain1.domain);
  382. // Sends all datagrams in the queue.
  383. ecrt_master_send(master);
  384. }
  385. // Function exit program.
  386. static void sig_handle(int signal)
  387. {
  388. is_quit = true;
  389. stop_servo();
  390. fclose(logfile);
  391. printf("The log file is saved as %s\n", logname);
  392. printf("Manually exit the program!\n");
  393. }
  394. // Function initial the ethercat.
  395. int init_ethercat(struct _SlaveConfig *slave_config, int *ret, int *status)
  396. {
  397. int i;
  398. // Request EtherCAT master.
  399. master = ecrt_request_master(0);
  400. if (!master)
  401. {
  402. printf("Request master failed.\n");
  403. return -1;
  404. }
  405. printf("Request master success.\n");
  406. memset(&domain1, 0, sizeof(domain1));
  407. // Create domain.
  408. domain1.domain = ecrt_master_create_domain(master);
  409. if (!domain1.domain)
  410. {
  411. *status = -1;
  412. printf("Create domain failed.\n");
  413. goto err_leave;
  414. }
  415. printf("Create domain success.\n");
  416. // Get slave configuration.
  417. for (i = 0; i < SLAVE_NUM; i++)
  418. {
  419. slave_config[i].sc = ecrt_master_slave_config(master, slave_info[i].Alias,
  420. slave_info[i].Position, slave_info[i].Vendor_ID,
  421. slave_info[i].Product_Code);
  422. if (!slave_config[i].sc)
  423. {
  424. *status = -1;
  425. printf("Get slave configuration failed.\n");
  426. goto err_leave;
  427. }
  428. }
  429. printf("Get slave configuration success.\n");
  430. // Configuring PDO.
  431. for (i = 0; i < SLAVE_NUM; i++)
  432. {
  433. *ret = ecrt_slave_config_pdos(slave_config[i].sc, EC_END, slave_syncs);
  434. if (*ret != 0)
  435. {
  436. *status = -1;
  437. printf("Configuration PDO failed.\n");
  438. goto err_leave;
  439. }
  440. }
  441. printf("Configuration PDO success.\n");
  442. // Registers a bunch of PDO entries for a domain.
  443. *ret = ecrt_domain_reg_pdo_entry_list(domain1.domain, domain_regs);
  444. if (*ret != 0)
  445. {
  446. *status = -1;
  447. printf("Failed to register bunch of PDO entries for domain.\n");
  448. goto err_leave;
  449. }
  450. printf("Success to register bunch of PDO entries for domain.\n");
  451. // Activate EtherCAT master.
  452. *ret = ecrt_master_activate(master);
  453. if (*ret < 0)
  454. {
  455. *status = -1;
  456. printf("Activate master failed.\n");
  457. goto err_leave;
  458. }
  459. printf("Activate master success.\n");
  460. // Get Pointer to the process data memory.
  461. domain1.domain_pd = ecrt_domain_data(domain1.domain);
  462. if (!domain1.domain_pd)
  463. {
  464. *status = -1;
  465. printf("Get pointer to the process data memory failed.\n");
  466. goto err_leave;
  467. }
  468. printf("Get pointer to the process data memory success.\n");
  469. return *status;
  470. // Function error leaving program.
  471. err_leave:
  472. // Releases EtherCAT master.
  473. ecrt_release_master(master);
  474. printf("Release master.\n");
  475. #ifdef MEASURE_TIMING
  476. fclose(logfile);
  477. #endif
  478. return *status;
  479. }
  480. int main(int argc, char **argv)
  481. {
  482. int status = 0, ret = -1;
  483. struct _SlaveConfig slave_config[SLAVE_NUM];
  484. if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
  485. {
  486. perror("mlockall failed");
  487. return -1;
  488. }
  489. // Ctrl+c handler.
  490. signal(SIGINT, sig_handle);
  491. memset(&slave_config, 0, sizeof(slave_config));
  492. #ifdef MEASURE_TIMING
  493. // Create a .csv file named by time.
  494. time_t current_time;
  495. struct tm *time_info;
  496. char time_buffer[20];
  497. time(&current_time);
  498. time_info = localtime(&current_time);
  499. strftime(time_buffer, sizeof(time_buffer), "%Y%m%d%H%M%S", time_info);
  500. snprintf(logname, sizeof(logname), "%s.csv", time_buffer);
  501. logfile = fopen(logname, "w");
  502. if (logfile == NULL)
  503. {
  504. perror("Error opening file");
  505. return 1;
  506. }
  507. // Defining List headers.
  508. fwrite("ID, period_min,period_max,exec_min,exec_max,latency_min,latency_max,\r\n", 1, strlen("ID, period_min,period_max,exec_min,exec_max,latency_min,latency_max,\r\n"), logfile);
  509. #endif
  510. // init ethercat
  511. if (init_ethercat(slave_config, &ret, &status) == -1)
  512. {
  513. perror("init ethercat fail.");
  514. exit(EXIT_FAILURE);
  515. }
  516. // Set highest priority.
  517. pid_t pid = getpid();
  518. struct sched_param param;
  519. int max_priority = sched_get_priority_max(SCHED_FIFO);
  520. int min_priority = sched_get_priority_min(SCHED_FIFO);
  521. param.sched_priority = max_priority;
  522. if (sched_setscheduler(pid, SCHED_FIFO, &param) == -1)
  523. {
  524. perror("sched_setscheduler");
  525. exit(EXIT_FAILURE);
  526. }
  527. printf("Max priority: %d\n", max_priority);
  528. printf("Min priority: %d\n", min_priority);
  529. printf("Current priority: %d\n", sched_get_priority_max(SCHED_FIFO));
  530. // Start cyclic function.
  531. printf("Run cycle task...\n");
  532. cyclic_task(slave_config, &domain1);
  533. return status;
  534. }