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

use realtime_core::*;
use std::thread;
use std::time::{Duration, Instant};
use thread_priority::*;

pub struct RtThread {
    last_time: Instant,
    period: Duration,
}
impl RtThread {
    pub fn new(prio: u8) -> Result<Self> {
        if prio > 0 {
            let thread_id = thread_native_id();
            #[cfg(not(windows))]
            let policy = ThreadSchedulePolicy::Realtime(RealtimeThreadSchedulePolicy::RoundRobin);
            let rt = if prio > 99 { 99 } else { prio };
            let priority = ThreadPriority::Crossplatform(rt.try_into().unwrap());
            #[cfg(not(windows))]
            if !nix::unistd::Uid::effective().is_root() {
                return Err(ErrorKind::NotRoot);
            }
            #[cfg(not(windows))]
            let result = set_thread_priority_and_policy(thread_id, priority, policy);
            #[cfg(windows)]
            let result = set_thread_priority(thread_id, priority);
            if let Err(err) = result {
                log::warn!("failed to set_thread_priority: {:?}", err);
            }
        }

        if !core_affinity::set_for_current(core_affinity::CoreId { id: 0 }) {
            log::warn!("failed to set_cpu_affinity");
        }
        Ok(Self {
            last_time: Instant::now(),
            period: Duration::ZERO,
        })
    }
}
impl RealTime for RtThread {
    fn start(&mut self, period: Duration) -> Result<()> {
        self.last_time = Instant::now();
        self.period = period;
        Ok(())
    }
    fn stop(&mut self) -> Result<()> {
        self.period = Duration::ZERO;
        Ok(())
    }
    fn wait_period(&mut self) -> Result<()> {
        if self.period == Duration::ZERO {
            return Err(ErrorKind::NotStart);
        };
        let next_time = self.last_time + self.period;
        let now = Instant::now();
        if now < next_time {
            let sleep = next_time - now;
            thread::sleep(sleep);
        }
        self.last_time = next_time;
        Ok(())
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn tolerance() {
        let mut rtai = RtThread::new(90).unwrap();
        rtai.start(Duration::from_millis(1)).unwrap();
        rtai.wait_period().unwrap();
        let mut last_time = clock_source::now();
        for _ in 0..1_000 {
            rtai.wait_period().unwrap();
            let now = clock_source::now();
            if now - last_time > 1_600_000 {
                println!("{}", now - last_time - 1_000_000);
            }
            assert!(now - last_time < 1_600_000);
            last_time = now;
        }
    }
}