Switched to a fixed window for averaging
This commit is contained in:
parent
6ff7b039f5
commit
87f84d5254
100
src/main.rs
100
src/main.rs
|
@ -79,7 +79,6 @@ mod app {
|
|||
|
||||
defmt::info!("PWM Setup done");
|
||||
|
||||
|
||||
let tim2 = cx.device.TIM2;
|
||||
unsafe {
|
||||
let rcc = &*RCC::ptr();
|
||||
|
@ -162,6 +161,8 @@ mod app {
|
|||
)
|
||||
}
|
||||
|
||||
const WINDOW_LEN : usize = 60;
|
||||
|
||||
#[task(local=[tim2, tim3, pwm, board_led])]
|
||||
async fn update_pwm(cx: update_pwm::Context) {
|
||||
defmt::info!("Update Task started");
|
||||
|
@ -171,10 +172,9 @@ mod app {
|
|||
let pwm = cx.local.pwm;
|
||||
let board_led = cx.local.board_led;
|
||||
|
||||
let mut last_ic = 0u32;
|
||||
let mut avg = 10f64;
|
||||
let max_pwm = pwm.get_max_duty() as u32;
|
||||
let mut cur_pwm = 3000u32; //max_pwm / 2;
|
||||
let max_pwm = pwm.get_max_duty() as i32;
|
||||
let mut cur_pwm = max_pwm / 2;
|
||||
|
||||
// Inialize last_ic
|
||||
while !tim2.sr.read().cc1if().bit_is_set() || !tim3.sr.read().cc1if().bit_is_set() {
|
||||
|
@ -183,55 +183,71 @@ mod app {
|
|||
let ic1 = tim2.ccr1().read().bits();
|
||||
let ic2 = tim3.ccr1().read().bits();
|
||||
|
||||
last_ic = ic2 << 16 | ic1;
|
||||
let mut last_ic = ic2 << 16 | ic1;
|
||||
|
||||
loop {
|
||||
while !tim3.sr.read().cc1if().bit_is_set() || !tim3.sr.read().cc1if().bit_is_set() {
|
||||
Systick::delay(10.millis()).await;
|
||||
loop {
|
||||
let mut short_avg = 0.0f64;
|
||||
|
||||
let mut count = 0;
|
||||
while count < WINDOW_LEN {
|
||||
while !tim3.sr.read().cc1if().bit_is_set() || !tim3.sr.read().cc1if().bit_is_set() {
|
||||
Systick::delay(10.millis()).await;
|
||||
}
|
||||
let ic1 = tim2.ccr1().read().bits();
|
||||
let ic2 = tim3.ccr1().read().bits();
|
||||
|
||||
let sum_ic = ic2 << 16 | ic1;
|
||||
|
||||
let diff_ic = if sum_ic > last_ic {
|
||||
sum_ic - last_ic
|
||||
} else {
|
||||
u32::MAX - last_ic + sum_ic
|
||||
};
|
||||
|
||||
defmt::info!("ic1:\t{}", ic1);
|
||||
defmt::info!("ic2:\t{}", ic2);
|
||||
defmt::info!("sum_ic:\t{}", sum_ic);
|
||||
defmt::info!("diff_ic:\t{}", diff_ic);
|
||||
|
||||
last_ic = sum_ic;
|
||||
|
||||
let freq = (diff_ic as f64) / 1_000_000f64;
|
||||
defmt::info!("freq:\t{} MHz", freq);
|
||||
|
||||
let diff = freq - avg;
|
||||
if diff > 0.000_100 || diff < -0.000_100 {
|
||||
defmt::info!("Out of range, dropping sample.");
|
||||
continue;
|
||||
}
|
||||
|
||||
short_avg += freq / WINDOW_LEN as f64;
|
||||
avg = avg * 0.999 + freq * 0.001;
|
||||
|
||||
|
||||
count += 1;
|
||||
board_led.toggle();
|
||||
}
|
||||
|
||||
let ic1 = tim2.ccr1().read().bits();
|
||||
let ic2 = tim3.ccr1().read().bits();
|
||||
defmt::info!("short_avg:\t{} MHz", short_avg);
|
||||
|
||||
let sum_ic = ic2 << 16 | ic1;
|
||||
|
||||
let diff_ic = if sum_ic > last_ic {
|
||||
sum_ic - last_ic
|
||||
} else {
|
||||
u32::MAX - last_ic + sum_ic
|
||||
};
|
||||
|
||||
last_ic = sum_ic;
|
||||
|
||||
let freq = (diff_ic as f64) / 1_000_000f64;
|
||||
let diff = freq - avg;
|
||||
|
||||
board_led.toggle();
|
||||
|
||||
if diff > 0.000_100 || diff < -0.000_100 {
|
||||
continue;
|
||||
let diff = (10.0 - short_avg) * 1_000_000.0;
|
||||
if diff > 1.0 || diff < -1.0 {
|
||||
cur_pwm += diff as i32 * 10;
|
||||
} else if short_avg > 10.0 {
|
||||
cur_pwm -= 1;
|
||||
} else if short_avg < 10.0 {
|
||||
cur_pwm += 1;
|
||||
}
|
||||
|
||||
avg = avg * 0.999 + freq * 0.001;
|
||||
|
||||
cur_pwm = if 10_000_000 >= diff_ic {
|
||||
cur_pwm + (10_000_000 - diff_ic)
|
||||
} else {
|
||||
cur_pwm - (diff_ic - 10_000_000)
|
||||
};
|
||||
cur_pwm = if cur_pwm < 0 { 0 } else { cur_pwm };
|
||||
cur_pwm = if cur_pwm > max_pwm { max_pwm } else { cur_pwm };
|
||||
|
||||
pwm.set_duty(Channel::C1, cur_pwm as u16);
|
||||
|
||||
defmt::info!("ic1:\t{}", ic1);
|
||||
defmt::info!("ic2:\t{}", ic2);
|
||||
defmt::info!("sum_ic:\t{}", sum_ic);
|
||||
defmt::info!("diff_ic:\t{}", diff_ic);
|
||||
defmt::info!("freq:\t{} MHz", freq);
|
||||
defmt::info!("avg:\t{} MHz", avg);
|
||||
defmt::info!("pwm:\t{}", cur_pwm);
|
||||
|
||||
Systick::delay(500.millis()).await;
|
||||
}
|
||||
Systick::delay(500.millis()).await;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue