add prelim iir
This commit is contained in:
parent
eff2dc6c4e
commit
ed663b536e
23
src/iir.rs
Normal file
23
src/iir.rs
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
pub type IIRState = [f32; 5];
|
||||||
|
|
||||||
|
pub struct IIR {
|
||||||
|
pub y0: f32,
|
||||||
|
pub ba: IIRState,
|
||||||
|
pub scale: f32,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl IIR {
|
||||||
|
pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 {
|
||||||
|
xy.rotate_right(1);
|
||||||
|
xy[0] = x0;
|
||||||
|
let y0 = macc(self.y0, xy, &self.ba, self.scale);
|
||||||
|
xy[xy.len()/2] = y0;
|
||||||
|
y0
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
fn macc(y0: f32, x: &[f32], a: &[f32], scale: f32) -> f32 {
|
||||||
|
y0 + x.iter().zip(a.iter())
|
||||||
|
.map(|(&i, &j)| i * j).sum::<f32>()
|
||||||
|
.min(scale).max(-scale)
|
||||||
|
}
|
14
src/main.rs
14
src/main.rs
@ -23,6 +23,9 @@ use cortex_m_rt::{entry, exception};
|
|||||||
use stm32h7::stm32h7x3::{self as stm32, Peripherals, CorePeripherals, interrupt};
|
use stm32h7::stm32h7x3::{self as stm32, Peripherals, CorePeripherals, interrupt};
|
||||||
use cortex_m::interrupt::Mutex;
|
use cortex_m::interrupt::Mutex;
|
||||||
|
|
||||||
|
mod iir;
|
||||||
|
use iir::*;
|
||||||
|
|
||||||
#[cfg(not(feature = "semihosting"))]
|
#[cfg(not(feature = "semihosting"))]
|
||||||
fn init_log() {}
|
fn init_log() {}
|
||||||
|
|
||||||
@ -442,6 +445,11 @@ fn TIM2() { // FIXME
|
|||||||
dp.SPI1.cr1.write(|w| unsafe { w.bits(0x201) });
|
dp.SPI1.cr1.write(|w| unsafe { w.bits(0x201) });
|
||||||
}
|
}
|
||||||
|
|
||||||
|
const SCALE: f32 = ((1 << 15) - 1) as f32;
|
||||||
|
static mut IIR_STATE: [IIRState; 2] = [[0.; 5]; 2];
|
||||||
|
static mut IIR_CH: [IIR; 2] = [
|
||||||
|
IIR{ y0: 0., ba: [1., 0., 0., 0., 0.], scale: SCALE },
|
||||||
|
IIR{ y0: 0., ba: [1., 0., 0., 0., 0.], scale: SCALE }];
|
||||||
|
|
||||||
#[interrupt]
|
#[interrupt]
|
||||||
fn SPI1() {
|
fn SPI1() {
|
||||||
@ -462,8 +470,10 @@ fn SPI1() {
|
|||||||
let a = unsafe { ptr::read_volatile(rxdr1) };
|
let a = unsafe { ptr::read_volatile(rxdr1) };
|
||||||
// needs to be a half word write
|
// needs to be a half word write
|
||||||
let txdr2 = &spi2.txdr as *const _ as *mut u16;
|
let txdr2 = &spi2.txdr as *const _ as *mut u16;
|
||||||
unsafe { ptr::write_volatile(txdr2, a ^ 0x8000) };
|
let x0 = a as f32;
|
||||||
info!("adc: {:#x}", a);
|
let y0 = unsafe { IIR_CH[0].update(&mut IIR_STATE[0], x0) };
|
||||||
|
unsafe { ptr::write_volatile(txdr2, y0 as i16 as u16 ^ 0x8000) };
|
||||||
|
info!("adc={:.1} dac={:.1}", x0, y0);
|
||||||
}
|
}
|
||||||
});
|
});
|
||||||
#[cfg(feature = "bkpt")]
|
#[cfg(feature = "bkpt")]
|
||||||
|
Loading…
Reference in New Issue
Block a user