从EtherCAT中学到的高精度时间控制与测试

1
2
3
4
5
6
7
8
9
#include <stdio.h>
#include <sys/time.h>	// for struct timeval
#include <time.h>
#include <sys/mman.h>
#include <malloc.h>
#include <sched.h> /* sched_setscheduler() */

#include <stdlib.h> // for size_t
#include <stdint.h>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
//定义
struct timespec
{
  __time_t tv_sec;		/* Seconds.  */
  __syscall_slong_t tv_nsec;	/* Nanoseconds.  */
};

//初始化
struct timespec startTime, endTime, lastStartTime = {};

//timespec时间结构体的加法
#define NSEC_PER_SEC (1000000000L)  //纳秒数10^9
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY)    /*本次设置周期PERIOD_NS为1ms*/

struct timespec timespec_add(struct timespec time1, struct timespec time2)
{
    struct timespec result;

    if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
        result.tv_sec = time1.tv_sec + time2.tv_sec + 1;    // 纳秒满进1
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
    } else {
        result.tv_sec = time1.tv_sec + time2.tv_sec;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
    }

    return result;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
const struct timespec cycletime = {0, PERIOD_NS};//1ms

void cyclic_task()
{


    struct timespec wakeupTime, time;

    // 获取当前时间
    //cld_id类型为CLOCK_MONOTONIC
    clock_gettime(CLOCK_MONOTONIC, &wakeupTime);

    while(1) {

	    //printf("wakeupTime: %10u--%10un",wakeupTime.tv_sec,wakeupTime.tv_nsec);
	    //循环时间 每次增加1个cycletime 从循环开始固定
        wakeupTime = timespec_add(wakeupTime, cycletime);
        //调用线程以纳秒精度指定的时间间隔进行睡眠	（高分辨率休眠）
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &wakeupTime, NULL);
    }

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + 
       (B).tv_nsec - (A).tv_nsec)        计算时间差

void cyclic_task()
{

    struct timespec wakeupTime, time;
    //测量时间
#ifdef MEASURE_TIMING
    struct timespec startTime, endTime, lastStartTime = {};
    uint32_t period_ns = 0, exec_ns = 0, latency_ns = 0,
             latency_min_ns = 0, latency_max_ns = 0,
             period_min_ns = 0, period_max_ns = 0,
             exec_min_ns = 0, exec_max_ns = 0;
#endif

    // 获取当前时间系统
    clock_gettime(CLOCK_TO_USE, &wakeupTime);

    while(1) {

        wakeupTime = timespec_add(wakeupTime, cycletime);
        //调用线程以纳秒精度指定的时间间隔进行睡眠	（高分辨率休眠）
        clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);


#ifdef MEASURE_TIMING
        //记录工作代码开始时间
        clock_gettime(CLOCK_TO_USE, &startTime);


        //延迟时间
        latency_ns = DIFF_NS(wakeupTime, startTime);
        //周期时间
        period_ns = DIFF_NS(lastStartTime, startTime);
        //代码执行时间
        exec_ns = DIFF_NS(lastStartTime, endTime);
        //赋值    以记录完整的周期时间
        lastStartTime = startTime;


        //记录延时最大间隔
        if (latency_ns > latency_max_ns) {
            latency_max_ns = latency_ns;
        }
        if (latency_ns < latency_min_ns) {
            latency_min_ns = latency_ns;
        }
        if (period_ns > period_max_ns) {
            period_max_ns = period_ns;
        }
        if (period_ns < period_min_ns) {
            period_min_ns = period_ns;
        }
        if (exec_ns > exec_max_ns) {
            exec_max_ns = exec_ns;
        }
        if (exec_ns < exec_min_ns) {
            exec_min_ns = exec_ns;
        }
#endif

        /....../

#ifdef MEASURE_TIMING
            // 输出时间统计数据
            printf("period     %10u ... %10un",
                    period_min_ns, period_max_ns);
            printf("exec       %10u ... %10un",
                    exec_min_ns, exec_max_ns);
            printf("latency    %10u ... %10un",
                    latency_min_ns, latency_max_ns);
            period_max_ns = 0;
            period_min_ns = 0xffffffff;
            exec_max_ns = 0;
            exec_min_ns = 0xffffffff;
            latency_max_ns = 0;
            latency_min_ns = 0xffffffff;
#endif
        }

        /...主要代码.../
       
#ifdef MEASURE_TIMING
        //代码运行结束时间
        clock_gettime(CLOCK_TO_USE, &endTime);
#endif
    }
}