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
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
use crate::core::scalar::ControlScalar;
/// Heartbeat monitor for periodic signal health checking.
///
/// Expects a signal to "beat" at least once every `period` time units.
/// If `max_missed` consecutive beats are missed, the monitor trips.
#[derive(Debug, Clone, Copy)]
pub struct Heartbeat<S: ControlScalar> {
/// Expected beat period (s).
pub period: S,
/// Maximum consecutive missed beats before tripping.
pub max_missed: u32,
/// Elapsed time since last beat (s).
elapsed: S,
/// Number of consecutive missed beats.
missed_count: u32,
/// Whether the monitor has tripped.
tripped: bool,
}
impl<S: ControlScalar> Heartbeat<S> {
pub fn new(period: S, max_missed: u32) -> Self {
Self {
period,
max_missed,
elapsed: S::ZERO,
missed_count: 0,
tripped: false,
}
}
/// Register a heartbeat signal. Resets the elapsed timer.
pub fn beat(&mut self) {
self.elapsed = S::ZERO;
self.missed_count = 0;
}
/// Advance time by `dt`. Returns `true` if heartbeat is healthy.
///
/// Should be called every control cycle with `dt` = cycle time.
pub fn tick(&mut self, dt: S) -> bool {
if self.tripped {
return false;
}
self.elapsed += dt;
if self.elapsed >= self.period {
self.elapsed = S::ZERO;
self.missed_count += 1;
if self.missed_count > self.max_missed {
self.tripped = true;
return false;
}
}
true
}
/// Whether the monitor has detected a missed-beat fault.
pub fn is_tripped(&self) -> bool {
self.tripped
}
/// Number of consecutive missed beats.
pub fn missed_count(&self) -> u32 {
self.missed_count
}
pub fn reset(&mut self) {
self.elapsed = S::ZERO;
self.missed_count = 0;
self.tripped = false;
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn healthy_heartbeat_does_not_trip() {
let mut hb = Heartbeat::new(0.1_f64, 2);
// Beat regularly every 50ms
for _ in 0..10 {
for _ in 0..5 {
assert!(hb.tick(0.01));
}
hb.beat();
}
assert!(!hb.is_tripped());
}
#[test]
fn missed_beats_trip_monitor() {
let mut hb = Heartbeat::new(0.1_f64, 1); // allow 1 missed beat
// Advance 300ms without beating (3 periods → 3 missed)
for _ in 0..300 {
hb.tick(0.001);
}
assert!(hb.is_tripped());
}
#[test]
fn single_miss_allowed_with_max_2() {
let mut hb = Heartbeat::new(0.1_f64, 2);
// One full period passes without beat (1 miss, allowed)
for _ in 0..100 {
hb.tick(0.001);
}
assert!(!hb.is_tripped(), "one miss should be tolerated");
assert_eq!(hb.missed_count(), 1);
// Beat arrives
hb.beat();
assert!(!hb.is_tripped());
}
#[test]
fn reset_clears_trip() {
let mut hb = Heartbeat::new(0.1_f64, 0);
for _ in 0..200 {
hb.tick(0.001);
}
assert!(hb.is_tripped());
hb.reset();
assert!(!hb.is_tripped());
assert!(hb.tick(0.001));
}
}