37 lines
799 B
Rust
37 lines
799 B
Rust
|
use rtic::cyccnt::{Duration, Instant, U32Ext};
|
||
|
|
|
||
|
|
use stm32h7xx_hal::time::Hertz;
|
||
|
|
|
||
|
|
pub struct CycleCounter {
|
||
|
|
next_tick: Instant,
|
||
|
|
ticks: u32,
|
||
|
|
increment: Duration,
|
||
|
|
}
|
||
|
|
|
||
|
|
impl CycleCounter {
|
||
|
|
pub fn new(
|
||
|
|
mut dwt: cortex_m::peripheral::DWT,
|
||
|
|
cpu_frequency: impl Into<Hertz>,
|
||
|
|
) -> Self {
|
||
|
|
dwt.enable_cycle_counter();
|
||
|
|
let increment =
|
||
|
|
((cpu_frequency.into().0 as f32 / 1000.0) as u32).cycles();
|
||
|
|
|
||
|
|
Self {
|
||
|
|
increment,
|
||
|
|
ticks: 0,
|
||
|
|
next_tick: Instant::now() + increment,
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
pub fn current_ms(&mut self) -> u32 {
|
||
|
|
let now = Instant::now();
|
||
|
|
while now > self.next_tick {
|
||
|
|
self.next_tick += self.increment;
|
||
|
|
self.ticks += 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
self.ticks
|
||
|
|
}
|
||
|
|
}
|