kai
/
pounder_test
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Merge branch 'master' into feature/dma-updates

This commit is contained in:
Ryan Summers 2020-11-24 16:49:11 +01:00
parent b7c6b6d203 769cfdfb7f
commit 2b443f9334
9 changed files with 261 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Cargo.lock generated
View File

@ -185,6 +185,13 @@ dependencies = [
"cortex-m",
]
[[package]]
name = "dsp"
version = "0.1.0"
dependencies = [
"serde",
]
[[package]]
name = "embedded-dma"
version = "0.1.2"
@ -466,6 +473,7 @@ dependencies = [
"cortex-m-log",
"cortex-m-rt",
"cortex-m-rtic",
"dsp",
"embedded-hal",
"enum-iterator",
"heapless",

1
Cargo.toml
View File

@ -41,6 +41,7 @@ nb = "1.0.0"
asm-delay = "0.9.0"
enum-iterator = "0.6.0"
paste = "1"
dsp = { path = "dsp" }
[dependencies.mcp23017]
git = "https://github.com/mrd0ll4r/mcp23017.git"

2
README.md
View File

@ -1,4 +1,4 @@
![Continuous Integration](https://github.com/quartiq/stabilizer/workflows/Continuous%20Integration/badge.svg)
[![QUARTIQ Matrix Chat](https://img.shields.io/matrix/quartiq:matrix.org)](https://matrix.to/#/#quartiq:matrix.org)
# Stabilizer Firmware

63
dsp/Cargo.lock generated Normal file
View File

@ -0,0 +1,63 @@
# This file is automatically @generated by Cargo.
# It is not intended for manual editing.
[[package]]
name = "dsp"
version = "0.1.0"
dependencies = [
"serde",
]
[[package]]
name = "proc-macro2"
version = "1.0.24"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "1e0704ee1a7e00d7bb417d0770ea303c1bccbabf0ef1667dae92b5967f5f8a71"
dependencies = [
"unicode-xid",
]
[[package]]
name = "quote"
version = "1.0.7"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "aa563d17ecb180e500da1cfd2b028310ac758de548efdd203e18f283af693f37"
dependencies = [
"proc-macro2",
]
[[package]]
name = "serde"
version = "1.0.117"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "b88fa983de7720629c9387e9f517353ed404164b1e482c970a90c1a4aaf7dc1a"
dependencies = [
"serde_derive",
]
[[package]]
name = "serde_derive"
version = "1.0.117"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "cbd1ae72adb44aab48f325a02444a5fc079349a8d804c1fc922aed3f7454c74e"
dependencies = [
"proc-macro2",
"quote",
"syn",
]
[[package]]
name = "syn"
version = "1.0.50"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "443b4178719c5a851e1bde36ce12da21d74a0e60b4d982ec3385a933c812f0f6"
dependencies = [
"proc-macro2",
"quote",
"unicode-xid",
]
[[package]]
name = "unicode-xid"
version = "0.2.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "f7fe0bb3479651439c9112f72b6c505038574c9fbb575ed1bf3b797fa39dd564"

8
dsp/Cargo.toml Normal file
View File

@ -0,0 +1,8 @@
[package]
name = "dsp"
version = "0.1.0"
authors = ["Robert Jördens <rj@quartiq.de>"]
edition = "2018"
[dependencies]
serde = { version = "1.0", features = ["derive"], default-features = false }

176
dsp/src/iir.rs Normal file
View File

@ -0,0 +1,176 @@
use core::ops::{Add, Mul};
use serde::{Deserialize, Serialize};
use core::f32;
// These are implemented here because core::f32 doesn't have them (yet).
// They are naive and don't handle inf/nan.
// `compiler-intrinsics`/llvm should have better (robust, universal, and
// faster) implementations.
fn abs(x: f32) -> f32 {
if x >= 0. {
x
} else {
-x
}
}
fn copysign(x: f32, y: f32) -> f32 {
if (x >= 0. && y >= 0.) || (x <= 0. && y <= 0.) {
x
} else {
-x
}
}
fn max(x: f32, y: f32) -> f32 {
if x > y {
x
} else {
y
}
}
fn min(x: f32, y: f32) -> f32 {
if x < y {
x
} else {
y
}
}
// Multiply-accumulate vectors `x` and `a`.
//
// A.k.a. dot product.
// Rust/LLVM optimize this nicely.
fn macc<T>(y0: T, x: &[T], a: &[T]) -> T
where
T: Add<Output = T> + Mul<Output = T> + Copy,
{
x.iter()
.zip(a)
.map(|(&x, &a)| x * a)
.fold(y0, |y, xa| y + xa)
}
/// IIR state and coefficients type.
///
/// To represent the IIR state (input and output memory) during the filter update
/// this contains the three inputs (x0, x1, x2) and the two outputs (y1, y2)
/// concatenated.
/// To represent the IIR coefficients, this contains the feed-forward
/// coefficients (b0, b1, b2) followd by the feed-back coefficients (a1, a2),
/// all normalized such that a0 = 1.
pub type IIRState = [f32; 5];
/// IIR configuration.
///
/// Contains the coeeficients `ba`, the output offset `y_offset`, and the
/// output limits `y_min` and `y_max`.
///
/// This implementation achieves several important properties:
///
/// * Its transfer function is universal in the sense that any biquadratic
/// transfer function can be implemented (high-passes, gain limits, second
/// order integrators with inherent anti-windup, notches etc) without code
/// changes preserving all features.
/// * It inherits a universal implementation of "integrator anti-windup", also
/// and especially in the presence of set-point changes and in the presence
/// of proportional or derivative gain without any back-off that would reduce
/// steady-state output range.
/// * It has universal derivative-kick (undesired, unlimited, and un-physical
/// amplification of set-point changes by the derivative term) avoidance.
/// * An offset at the input of an IIR filter (a.k.a. "set-point") is
/// equivalent to an offset at the output. They are related by the
/// overall (DC feed-forward) gain of the filter.
/// * It stores only previous outputs and inputs. These have direct and
/// invariant interpretation (independent of gains and offsets).
/// Therefore it can trivially implement bump-less transfer.
/// * Cascading multiple IIR filters allows stable and robust
/// implementation of transfer functions beyond bequadratic terms.
#[derive(Copy, Clone, Deserialize, Serialize)]
pub struct IIR {
pub ba: IIRState,
pub y_offset: f32,
pub y_min: f32,
pub y_max: f32,
}
impl IIR {
/// Configures IIR filter coefficients for proportional-integral behavior
/// with gain limit.
///
/// # Arguments
///
/// * `kp` - Proportional gain. Also defines gain sign.
/// * `ki` - Integral gain at Nyquist. Sign taken from `kp`.
/// * `g` - Gain limit.
pub fn set_pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &str> {
let ki = copysign(ki, kp);
let g = copysign(g, kp);
let (a1, b0, b1) = if abs(ki) < f32::EPSILON {
(0., kp, 0.)
} else {
let c = if abs(g) < f32::EPSILON {
1.
} else {
1. / (1. + ki / g)
};
let a1 = 2. * c - 1.;
let b0 = ki * c + kp;
let b1 = ki * c - a1 * kp;
if abs(b0 + b1) < f32::EPSILON {
return Err("low integrator gain and/or gain limit");
}
(a1, b0, b1)
};
self.ba.copy_from_slice(&[b0, b1, 0., a1, 0.]);
Ok(())
}
/// Compute the overall (DC feed-forward) gain.
pub fn get_k(&self) -> f32 {
self.ba[..3].iter().sum()
}
/// Compute input-referred (`x`) offset from output (`y`) offset.
pub fn get_x_offset(&self) -> Result<f32, &str> {
let k = self.get_k();
if abs(k) < f32::EPSILON {
Err("k is zero")
} else {
Ok(self.y_offset / k)
}
}
/// Convert input (`x`) offset to equivalent output (`y`) offset and apply.
///
/// # Arguments
/// * `xo`: Input (`x`) offset.
pub fn set_x_offset(&mut self, xo: f32) {
self.y_offset = xo * self.get_k();
}
/// Feed a new input value into the filter, update the filter state, and
/// return the new output. Only the state `xy` is modified.
///
/// # Arguments
/// * `xy` - Current filter state.
/// * `x0` - New input.
pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 {
// `xy` contains x0 x1 y0 y1 y2
// Increment time x1 x2 y1 y2 y3
// Rotate y3 x1 x2 y1 y2
xy.rotate_right(1);
// Store x0 x0 x1 x2 y1 y2
xy[0] = x0;
// Compute y0 by multiply-accumulate
let y0 = macc(self.y_offset, xy, &self.ba);
// Limit y0
let y0 = max(self.y_min, min(self.y_max, y0));
// Store y0 x0 x1 y0 y1 y2
xy[xy.len() / 2] = y0;
y0
}
}

3
dsp/src/lib.rs Normal file
View File

@ -0,0 +1,3 @@
#![no_std]
pub mod iir;

108
src/iir.rs
View File

@ -1,108 +0,0 @@
use core::ops::{Add, Mul};
use serde::{Deserialize, Serialize};
use core::f32;
pub type IIRState = [f32; 5];
#[derive(Copy, Clone, Deserialize, Serialize)]
pub struct IIR {
pub ba: IIRState,
pub y_offset: f32,
pub y_min: f32,
pub y_max: f32,
}
fn abs(x: f32) -> f32 {
if x >= 0. {
x
} else {
-x
}
}
fn copysign(x: f32, y: f32) -> f32 {
if (x >= 0. && y >= 0.) || (x <= 0. && y <= 0.) {
x
} else {
-x
}
}
fn max(x: f32, y: f32) -> f32 {
if x > y {
x
} else {
y
}
}
fn min(x: f32, y: f32) -> f32 {
if x < y {
x
} else {
y
}
}
fn macc<T>(y0: T, x: &[T], a: &[T]) -> T
where
T: Add<Output = T> + Mul<Output = T> + Copy,
{
x.iter()
.zip(a.iter())
.map(|(&i, &j)| i * j)
.fold(y0, |y, xa| y + xa)
}
impl IIR {
pub fn set_pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &str> {
let ki = copysign(ki, kp);
let g = copysign(g, kp);
let (a1, b0, b1) = if abs(ki) < f32::EPSILON {
(0., kp, 0.)
} else {
let c = if abs(g) < f32::EPSILON {
1.
} else {
1. / (1. + ki / g)
};
let a1 = 2. * c - 1.;
let b0 = ki * c + kp;
let b1 = ki * c - a1 * kp;
if abs(b0 + b1) < f32::EPSILON {
return Err("low integrator gain and/or gain limit");
}
(a1, b0, b1)
};
self.ba[0] = b0;
self.ba[1] = b1;
self.ba[2] = 0.;
self.ba[3] = a1;
self.ba[4] = 0.;
Ok(())
}
pub fn get_x_offset(&self) -> Result<f32, &str> {
let b: f32 = self.ba[..3].iter().sum();
if abs(b) < f32::EPSILON {
Err("b is zero")
} else {
Ok(self.y_offset / b)
}
}
pub fn set_x_offset(&mut self, xo: f32) {
let b: f32 = self.ba[..3].iter().sum();
self.y_offset = xo * b;
}
pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 {
xy.rotate_right(1);
xy[0] = x0;
let y0 = macc(self.y_offset, xy, &self.ba);
let y0 = max(self.y_min, min(self.y_max, y0));
xy[xy.len() / 2] = y0;
y0
}
}

2
src/main.rs
View File

@ -64,13 +64,13 @@ mod adc;
mod afe;
mod dac;
mod eeprom;
mod iir;
mod pounder;
mod sampling_timer;
mod server;
use adc::{Adc0Input, Adc1Input, AdcInputs};
use dac::{Dac0Output, Dac1Output, DacOutputs};
use dsp::iir;
#[cfg(not(feature = "semihosting"))]
fn init_log() {}