sebastian
/
cheapsdo2.0
1
0
Fork 0
Code Issues Pull Requests Actions Releases Wiki Activity

Switched to a fixed window for averaging

This commit is contained in:
Sebastian 2023-12-18 11:23:26 +01:00
parent 6ff7b039f5
commit 87f84d5254
1 changed files with 58 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
src/main.rs
View File

@ -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;
}
}
}
}