calculate pi coefficients

This commit is contained in:
Robert Jördens 2019-03-28 09:02:32 +00:00
parent ed663b536e
commit 9350f17ca3
2 changed files with 59 additions and 15 deletions

View File

@ -1,23 +1,62 @@
use core::f32;
pub type IIRState = [f32; 5]; pub type IIRState = [f32; 5];
#[derive(Default,Copy,Clone,Debug)]
pub struct IIR { pub struct IIR {
pub y0: f32, pub x_offset: f32,
pub y_offset: f32,
pub ba: IIRState, pub ba: IIRState,
pub scale: f32, pub scale: f32,
} }
impl IIR { impl IIR {
pub fn pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &str> {
if ki < 0. {
return Err("ki < 0");
}
let (a1, b1, b0) = match () {
_ if ki < 0. => return Err("ki < 0"),
_ if ki < f32::EPSILON => (0., 0., kp),
_ => {
let (c, a1) = match () {
_ if g < 0. => return Err("g < 0"),
_ if g < f32::EPSILON => (1., 1.),
_ => {
let c = 1./(1. + ki/g);
(c, 2.*c - 1.)
}
};
let b0 = kp + ki*c;
let b1 = b0 - 2.*kp*c;
if b0 + b1 > -f32::EPSILON && b0 + b1 < f32::EPSILON {
return Err("low integrator gain and/or gain limit");
}
(a1, b1, b0)
}
};
if b0 > 1. || b0 < -1. || b1 > 1. || b1 < -1. {
return Err("high gains");
}
self.ba[0] = b0;
self.ba[1] = b1;
self.ba[2] = 0.;
self.ba[3] = a1;
self.ba[4] = 0.;
Ok(())
}
pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 { pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 {
xy.rotate_right(1); xy.rotate_right(1);
xy[0] = x0; xy[0] = x0 - self.x_offset;
let y0 = macc(self.y0, xy, &self.ba, self.scale); let y0 = macc(self.y_offset, xy, &self.ba)
.min(self.scale).max(-self.scale);
xy[xy.len()/2] = y0; xy[xy.len()/2] = y0;
y0 y0
} }
} }
fn macc(y0: f32, x: &[f32], a: &[f32], scale: f32) -> f32 { fn macc(y0: f32, x: &[f32], a: &[f32]) -> f32 {
y0 + x.iter().zip(a.iter()) y0 + x.iter().zip(a.iter())
.map(|(&i, &j)| i * j).sum::<f32>() .map(|(&i, &j)| i * j).sum::<f32>()
.min(scale).max(-scale)
} }

View File

@ -426,6 +426,8 @@ fn main() -> ! {
rcc.apb1lenr.modify(|_, w| w.tim2en().set_bit()); rcc.apb1lenr.modify(|_, w| w.tim2en().set_bit());
tim2_setup(&dp.TIM2); tim2_setup(&dp.TIM2);
unsafe { IIR_CH[0].pi(-0.01, 10.*2e-6/2., 0.).expect("bad coefficients"); }
cortex_m::interrupt::free(|cs| { cortex_m::interrupt::free(|cs| {
cp.NVIC.enable(stm32::Interrupt::SPI1); cp.NVIC.enable(stm32::Interrupt::SPI1);
cp.NVIC.enable(stm32::Interrupt::TIM2); // FIXME cp.NVIC.enable(stm32::Interrupt::TIM2); // FIXME
@ -434,22 +436,24 @@ fn main() -> ! {
}); });
loop { loop {
cortex_m::asm::wfi(); for _ in 0..1000000 { cortex_m::asm::wfi(); }
let (x0, y0) = unsafe { (IIR_STATE[0][0], IIR_STATE[0][2]) };
info!("x0={} y0={}", x0, y0);
} }
} }
#[interrupt] #[interrupt]
fn TIM2() { // FIXME fn TIM2() { // FIXME
let dp = unsafe { Peripherals::steal() }; let dp = unsafe { Peripherals::steal() };
dp.TIM2.sr.modify(|_, w| w.uif().clear_bit()); dp.TIM2.sr.write(|w| w.uif().clear_bit()); // rc_w0
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; const SCALE: f32 = ((1 << 15) - 1) as f32;
static mut IIR_STATE: [IIRState; 2] = [[0.; 5]; 2]; static mut IIR_STATE: [IIRState; 2] = [[0.; 5]; 2];
static mut IIR_CH: [IIR; 2] = [ static mut IIR_CH: [IIR; 2] = [
IIR{ y0: 0., ba: [1., 0., 0., 0., 0.], scale: SCALE }, IIR{ y_offset: 0., x_offset: 0., ba: [0., 0., 0., 0., 0.], scale: SCALE },
IIR{ y0: 0., ba: [1., 0., 0., 0., 0.], scale: SCALE }]; IIR{ y_offset: 0., x_offset: 0., ba: [0., 0., 0., 0., 0.], scale: SCALE }];
#[interrupt] #[interrupt]
fn SPI1() { fn SPI1() {
@ -458,8 +462,6 @@ fn SPI1() {
cortex_m::interrupt::free(|cs| { cortex_m::interrupt::free(|cs| {
let spi1p = SPI1P.borrow(cs).borrow(); let spi1p = SPI1P.borrow(cs).borrow();
let spi1 = spi1p.as_ref().unwrap(); let spi1 = spi1p.as_ref().unwrap();
let spi2p = SPI2P.borrow(cs).borrow();
let spi2 = spi2p.as_ref().unwrap();
let sr = spi1.sr.read(); let sr = spi1.sr.read();
if sr.eot().bit_is_set() { if sr.eot().bit_is_set() {
spi1.ifcr.write(|w| w.eotc().set_bit()); spi1.ifcr.write(|w| w.eotc().set_bit());
@ -468,12 +470,15 @@ fn SPI1() {
// needs to be a half word read // needs to be a half word read
let rxdr1 = &spi1.rxdr as *const _ as *const u16; let rxdr1 = &spi1.rxdr as *const _ as *const u16;
let a = unsafe { ptr::read_volatile(rxdr1) }; let a = unsafe { ptr::read_volatile(rxdr1) };
let x0 = a as i16 as f32;
let y0 = unsafe { IIR_CH[0].update(&mut IIR_STATE[0], x0) };
let d = y0 as i16 as u16 ^ 0x8000;
let spi2p = SPI2P.borrow(cs).borrow();
let spi2 = spi2p.as_ref().unwrap();
// 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;
let x0 = a as f32; unsafe { ptr::write_volatile(txdr2, d) };
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")]