GitHub
commit
659a6879f7
|
|
@ -29,6 +29,11 @@ jobs:
|
|||
- uses: actions-rs/clippy-check@v1
|
||||
with:
|
||||
token: ${{ secrets.GITHUB_TOKEN }}
|
||||
- name: cargo check
|
||||
uses: actions-rs/cargo@v1
|
||||
with:
|
||||
command: check
|
||||
args: --verbose
|
||||
|
||||
compile:
|
||||
runs-on: ubuntu-latest
|
||||
|
|
@ -37,6 +42,12 @@ jobs:
|
|||
toolchain:
|
||||
- stable
|
||||
- beta
|
||||
bin:
|
||||
- dual-iir
|
||||
- lockin
|
||||
features:
|
||||
- ''
|
||||
- pounder_v1_1
|
||||
steps:
|
||||
- uses: actions/checkout@v2
|
||||
- name: Install Rust ${{ matrix.toolchain }}
|
||||
|
|
@ -46,20 +57,11 @@ jobs:
|
|||
target: thumbv7em-none-eabihf
|
||||
override: true
|
||||
components: llvm-tools-preview
|
||||
- name: cargo check
|
||||
uses: actions-rs/cargo@v1
|
||||
with:
|
||||
command: check
|
||||
args: --verbose
|
||||
- name: cargo build
|
||||
uses: actions-rs/cargo@v1
|
||||
with:
|
||||
command: build
|
||||
- name: cargo build release
|
||||
uses: actions-rs/cargo@v1
|
||||
with:
|
||||
command: build
|
||||
args: --release
|
||||
args: --release --features "${{ matrix.features }}" --bin ${{ matrix.bin }}
|
||||
- name: cargo-binutils
|
||||
uses: actions-rs/cargo@v1
|
||||
with:
|
||||
|
|
@ -69,25 +71,19 @@ jobs:
|
|||
uses: actions-rs/cargo@v1
|
||||
with:
|
||||
command: size
|
||||
args: --release
|
||||
args: --release --features "${{ matrix.features }}" --bin ${{ matrix.bin }}
|
||||
- name: cargo objcopy
|
||||
uses: actions-rs/cargo@v1
|
||||
with:
|
||||
command: objcopy
|
||||
args: --release --verbose -- -O binary stabilizer-release.bin
|
||||
args: --release --features "${{ matrix.features }}" --bin ${{ matrix.bin }} --verbose -- -O binary ${{ matrix.bin }}-release.bin
|
||||
- uses: actions/upload-artifact@v2
|
||||
if: ${{ matrix.toolchain == 'stable' }}
|
||||
if: ${{ matrix.toolchain == 'stable' && matrix.features == '' }}
|
||||
with:
|
||||
name: stabilizer_${{ github.sha }}
|
||||
name: stabilizer_${{ matrix.bin }}
|
||||
path: |
|
||||
target/*/release/stabilizer
|
||||
stabilizer-release.bin
|
||||
|
||||
- name: Build (Pounder v1.1)
|
||||
uses: actions-rs/cargo@v1
|
||||
with:
|
||||
command: build
|
||||
args: --features pounder_v1_1
|
||||
target/*/release/${{ matrix.bin }}
|
||||
${{ matrix.bin }}-release.bin
|
||||
|
||||
test:
|
||||
runs-on: ubuntu-latest
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
use core::f32::consts::PI;
|
||||
use criterion::{black_box, criterion_group, criterion_main, Criterion};
|
||||
use dsp::trig::{atan2, cossin};
|
||||
use dsp::{atan2, cossin};
|
||||
|
||||
fn atan2_bench(c: &mut Criterion) {
|
||||
let xi = (10 << 16) as i32;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,135 @@
|
|||
/// 2-argument arctangent function.
|
||||
///
|
||||
/// This implementation uses all integer arithmetic for fast
|
||||
/// computation. It is designed to have high accuracy near the axes
|
||||
/// and lower away from the axes. It is additionally designed so that
|
||||
/// the error changes slowly with respect to the angle.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `y` - Y-axis component.
|
||||
/// * `x` - X-axis component.
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// The angle between the x-axis and the ray to the point (x,y). The
|
||||
/// result range is from i32::MIN to i32::MAX, where i32::MIN
|
||||
/// represents -pi and, equivalently, +pi. i32::MAX represents one
|
||||
/// count less than +pi.
|
||||
pub fn atan2(y: i32, x: i32) -> i32 {
|
||||
let sign = (x < 0, y < 0);
|
||||
|
||||
let mut y = y.wrapping_abs() as u32;
|
||||
let mut x = x.wrapping_abs() as u32;
|
||||
|
||||
let y_greater = y > x;
|
||||
if y_greater {
|
||||
core::mem::swap(&mut y, &mut x);
|
||||
}
|
||||
|
||||
let z = (16 - y.leading_zeros() as i32).max(0);
|
||||
|
||||
x >>= z;
|
||||
if x == 0 {
|
||||
return 0;
|
||||
}
|
||||
y >>= z;
|
||||
let r = (y << 16) / x;
|
||||
debug_assert!(r <= 1 << 16);
|
||||
|
||||
// Uses the general procedure described in the following
|
||||
// Mathematics stack exchange answer:
|
||||
//
|
||||
// https://math.stackexchange.com/a/1105038/583981
|
||||
//
|
||||
// The atan approximation method has been modified to be cheaper
|
||||
// to compute and to be more compatible with integer
|
||||
// arithmetic. The approximation technique used here is
|
||||
//
|
||||
// pi / 4 * r + C * r * (1 - abs(r))
|
||||
//
|
||||
// which is taken from Rajan 2006: Efficient Approximations for
|
||||
// the Arctangent Function.
|
||||
//
|
||||
// The least mean squared error solution is C = 0.279 (no the 0.285 that
|
||||
// Rajan uses). K = C*4/pi.
|
||||
// Q5 for K provides sufficient correction accuracy while preserving
|
||||
// as much smoothness of the quadratic correction as possible.
|
||||
const FP_K: usize = 5;
|
||||
const K: u32 = (0.35489 * (1 << FP_K) as f64) as u32;
|
||||
// debug_assert!(K == 11);
|
||||
|
||||
// `r` is unsigned Q16.16 and <= 1
|
||||
// `angle` is signed Q1.31 with 1 << 31 == +- pi
|
||||
// Since K < 0.5 and r*(1 - r) <= 0.25 the correction product can use
|
||||
// 4 bits for K, and 15 bits for r and 1-r to remain within the u32 range.
|
||||
let mut angle = ((r << 13)
|
||||
+ ((K * (r >> 1) * ((1 << 15) - (r >> 1))) >> (FP_K + 1)))
|
||||
as i32;
|
||||
|
||||
if y_greater {
|
||||
angle = (1 << 30) - angle;
|
||||
}
|
||||
|
||||
if sign.0 {
|
||||
angle = i32::MAX - angle;
|
||||
}
|
||||
|
||||
if sign.1 {
|
||||
angle = angle.wrapping_neg();
|
||||
}
|
||||
|
||||
angle
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use core::f64::consts::PI;
|
||||
|
||||
fn angle_to_axis(angle: f64) -> f64 {
|
||||
let angle = angle % (PI / 2.);
|
||||
(PI / 2. - angle).min(angle)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn atan2_absolute_error() {
|
||||
const N: usize = 321;
|
||||
let mut test_vals = [0i32; N + 4];
|
||||
let scale = (1i64 << 31) as f64;
|
||||
for i in 0..N {
|
||||
test_vals[i] = (scale * (-1. + 2. * i as f64 / N as f64)) as i32;
|
||||
}
|
||||
|
||||
assert!(test_vals.contains(&i32::MIN));
|
||||
test_vals[N] = i32::MAX;
|
||||
test_vals[N + 1] = 0;
|
||||
test_vals[N + 2] = -1;
|
||||
test_vals[N + 3] = 1;
|
||||
|
||||
let mut rms_err = 0f64;
|
||||
let mut abs_err = 0f64;
|
||||
let mut rel_err = 0f64;
|
||||
|
||||
for &x in test_vals.iter() {
|
||||
for &y in test_vals.iter() {
|
||||
let want = (y as f64 / scale).atan2(x as f64 / scale);
|
||||
let have = atan2(y, x) as f64 * PI / scale;
|
||||
|
||||
let err = (have - want).abs();
|
||||
abs_err = abs_err.max(err);
|
||||
rms_err += err * err;
|
||||
if err > 3e-5 {
|
||||
rel_err = rel_err.max(err / angle_to_axis(want));
|
||||
}
|
||||
}
|
||||
}
|
||||
rms_err = rms_err.sqrt() / test_vals.len() as f64;
|
||||
println!("max abs err: {:.2e}", abs_err);
|
||||
println!("rms abs err: {:.2e}", rms_err);
|
||||
println!("max rel err: {:.2e}", rel_err);
|
||||
assert!(abs_err < 5e-3);
|
||||
assert!(rms_err < 3e-3);
|
||||
assert!(rel_err < 0.6);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
use super::atan2;
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
|
||||
pub struct Complex<T>(pub T, pub T);
|
||||
|
||||
impl Complex<i32> {
|
||||
pub fn power(&self) -> i32 {
|
||||
(((self.0 as i64) * (self.0 as i64)
|
||||
+ (self.1 as i64) * (self.1 as i64))
|
||||
>> 32) as i32
|
||||
}
|
||||
|
||||
pub fn phase(&self) -> i32 {
|
||||
atan2(self.1, self.0)
|
||||
}
|
||||
}
|
||||
|
|
@ -3,90 +3,6 @@ use core::f64::consts::PI;
|
|||
|
||||
include!(concat!(env!("OUT_DIR"), "/cossin_table.rs"));
|
||||
|
||||
/// 2-argument arctangent function.
|
||||
///
|
||||
/// This implementation uses all integer arithmetic for fast
|
||||
/// computation. It is designed to have high accuracy near the axes
|
||||
/// and lower away from the axes. It is additionally designed so that
|
||||
/// the error changes slowly with respect to the angle.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `y` - Y-axis component.
|
||||
/// * `x` - X-axis component.
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// The angle between the x-axis and the ray to the point (x,y). The
|
||||
/// result range is from i32::MIN to i32::MAX, where i32::MIN
|
||||
/// represents -pi and, equivalently, +pi. i32::MAX represents one
|
||||
/// count less than +pi.
|
||||
pub fn atan2(y: i32, x: i32) -> i32 {
|
||||
let sign = (x < 0, y < 0);
|
||||
|
||||
let mut y = y.wrapping_abs() as u32;
|
||||
let mut x = x.wrapping_abs() as u32;
|
||||
|
||||
let y_greater = y > x;
|
||||
if y_greater {
|
||||
core::mem::swap(&mut y, &mut x);
|
||||
}
|
||||
|
||||
let z = (16 - y.leading_zeros() as i32).max(0);
|
||||
|
||||
x >>= z;
|
||||
if x == 0 {
|
||||
return 0;
|
||||
}
|
||||
y >>= z;
|
||||
let r = (y << 16) / x;
|
||||
debug_assert!(r <= 1 << 16);
|
||||
|
||||
// Uses the general procedure described in the following
|
||||
// Mathematics stack exchange answer:
|
||||
//
|
||||
// https://math.stackexchange.com/a/1105038/583981
|
||||
//
|
||||
// The atan approximation method has been modified to be cheaper
|
||||
// to compute and to be more compatible with integer
|
||||
// arithmetic. The approximation technique used here is
|
||||
//
|
||||
// pi / 4 * r + C * r * (1 - abs(r))
|
||||
//
|
||||
// which is taken from Rajan 2006: Efficient Approximations for
|
||||
// the Arctangent Function.
|
||||
//
|
||||
// The least mean squared error solution is C = 0.279 (no the 0.285 that
|
||||
// Rajan uses). K = C*4/pi.
|
||||
// Q5 for K provides sufficient correction accuracy while preserving
|
||||
// as much smoothness of the quadratic correction as possible.
|
||||
const FP_K: usize = 5;
|
||||
const K: u32 = (0.35489 * (1 << FP_K) as f64) as u32;
|
||||
// debug_assert!(K == 11);
|
||||
|
||||
// `r` is unsigned Q16.16 and <= 1
|
||||
// `angle` is signed Q1.31 with 1 << 31 == +- pi
|
||||
// Since K < 0.5 and r*(1 - r) <= 0.25 the correction product can use
|
||||
// 4 bits for K, and 15 bits for r and 1-r to remain within the u32 range.
|
||||
let mut angle = ((r << 13)
|
||||
+ ((K * (r >> 1) * ((1 << 15) - (r >> 1))) >> (FP_K + 1)))
|
||||
as i32;
|
||||
|
||||
if y_greater {
|
||||
angle = (1 << 30) - angle;
|
||||
}
|
||||
|
||||
if sign.0 {
|
||||
angle = i32::MAX - angle;
|
||||
}
|
||||
|
||||
if sign.1 {
|
||||
angle = angle.wrapping_neg();
|
||||
}
|
||||
|
||||
angle
|
||||
}
|
||||
|
||||
/// Compute the cosine and sine of an angle.
|
||||
/// This is ported from the MiSoC cossin core.
|
||||
/// (https://github.com/m-labs/misoc/blob/master/misoc/cores/cossin.py)
|
||||
|
|
@ -153,7 +69,7 @@ pub fn cossin(phase: i32) -> Complex<i32> {
|
|||
sin *= -1;
|
||||
}
|
||||
|
||||
(cos, sin)
|
||||
Complex(cos, sin)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
|
|
@ -161,66 +77,21 @@ mod tests {
|
|||
use super::*;
|
||||
use core::f64::consts::PI;
|
||||
|
||||
fn angle_to_axis(angle: f64) -> f64 {
|
||||
let angle = angle % (PI / 2.);
|
||||
(PI / 2. - angle).min(angle)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn atan2_absolute_error() {
|
||||
const N: usize = 321;
|
||||
let mut test_vals = [0i32; N + 4];
|
||||
let scale = (1i64 << 31) as f64;
|
||||
for i in 0..N {
|
||||
test_vals[i] = (scale * (-1. + 2. * i as f64 / N as f64)) as i32;
|
||||
}
|
||||
|
||||
assert!(test_vals.contains(&i32::MIN));
|
||||
test_vals[N] = i32::MAX;
|
||||
test_vals[N + 1] = 0;
|
||||
test_vals[N + 2] = -1;
|
||||
test_vals[N + 3] = 1;
|
||||
|
||||
let mut rms_err = 0f64;
|
||||
let mut abs_err = 0f64;
|
||||
let mut rel_err = 0f64;
|
||||
|
||||
for &x in test_vals.iter() {
|
||||
for &y in test_vals.iter() {
|
||||
let want = (y as f64 / scale).atan2(x as f64 / scale);
|
||||
let have = atan2(y, x) as f64 * PI / scale;
|
||||
|
||||
let err = (have - want).abs();
|
||||
abs_err = abs_err.max(err);
|
||||
rms_err += err * err;
|
||||
if err > 3e-5 {
|
||||
rel_err = rel_err.max(err / angle_to_axis(want));
|
||||
}
|
||||
}
|
||||
}
|
||||
rms_err = rms_err.sqrt() / test_vals.len() as f64;
|
||||
println!("max abs err: {:.2e}", abs_err);
|
||||
println!("rms abs err: {:.2e}", rms_err);
|
||||
println!("max rel err: {:.2e}", rel_err);
|
||||
assert!(abs_err < 5e-3);
|
||||
assert!(rms_err < 3e-3);
|
||||
assert!(rel_err < 0.6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn cossin_error_max_rms_all_phase() {
|
||||
// Constant amplitude error due to LUT data range.
|
||||
const AMPLITUDE: f64 = ((1i64 << 31) - (1i64 << 15)) as f64;
|
||||
const MAX_PHASE: f64 = (1i64 << 32) as f64;
|
||||
let mut rms_err: Complex<f64> = (0., 0.);
|
||||
let mut sum_err: Complex<f64> = (0., 0.);
|
||||
let mut max_err: Complex<f64> = (0., 0.);
|
||||
let mut sum: Complex<f64> = (0., 0.);
|
||||
let mut demod: Complex<f64> = (0., 0.);
|
||||
const AMPLITUDE: f64 = ((1i64 << 31) - (1i64 << 15)) as _;
|
||||
const MAX_PHASE: f64 = (1i64 << 32) as _;
|
||||
let mut rms_err = Complex(0f64, 0f64);
|
||||
let mut sum_err = Complex(0f64, 0f64);
|
||||
let mut max_err = Complex(0f64, 0f64);
|
||||
let mut sum = Complex(0f64, 0f64);
|
||||
let mut demod = Complex(0f64, 0f64);
|
||||
|
||||
// use std::{fs::File, io::{BufWriter, prelude::*}, path::Path};
|
||||
// let mut file = BufWriter::new(File::create(Path::new("data.bin")).unwrap());
|
||||
|
||||
// log2 of the number of phase values to check
|
||||
const PHASE_DEPTH: usize = 20;
|
||||
|
||||
for phase in 0..(1 << PHASE_DEPTH) {
|
||||
|
|
@ -1,6 +1,7 @@
|
|||
use serde::{Deserialize, Serialize};
|
||||
|
||||
pub type IIRState = [i32; 5];
|
||||
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
|
||||
pub struct IIRState(pub [i32; 5]);
|
||||
|
||||
fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
|
||||
// Rounding bias, half up
|
||||
|
|
@ -18,7 +19,7 @@ fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
|
|||
/// See `dsp::iir::IIR` for general implementation details.
|
||||
/// Offset and limiting disabled to suit lowpass applications.
|
||||
/// Coefficient scaling fixed and optimized.
|
||||
#[derive(Copy, Clone, Deserialize, Serialize)]
|
||||
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
|
||||
pub struct IIR {
|
||||
pub ba: IIRState,
|
||||
// pub y_offset: i32,
|
||||
|
|
@ -27,9 +28,9 @@ pub struct IIR {
|
|||
}
|
||||
|
||||
impl IIR {
|
||||
/// Coefficient fixed point: signed Q2.30.
|
||||
/// Tailored to low-passes PI, II etc.
|
||||
const SHIFT: u32 = 30;
|
||||
/// Coefficient fixed point format: signed Q2.30.
|
||||
/// Tailored to low-passes, PI, II etc.
|
||||
pub const SHIFT: u32 = 30;
|
||||
|
||||
/// Feed a new input value into the filter, update the filter state, and
|
||||
/// return the new output. Only the state `xy` is modified.
|
||||
|
|
@ -38,21 +39,21 @@ impl IIR {
|
|||
/// * `xy` - Current filter state.
|
||||
/// * `x0` - New input.
|
||||
pub fn update(&self, xy: &mut IIRState, x0: i32) -> i32 {
|
||||
let n = self.ba.len();
|
||||
debug_assert!(xy.len() == n);
|
||||
let n = self.ba.0.len();
|
||||
debug_assert!(xy.0.len() == n);
|
||||
// `xy` contains x0 x1 y0 y1 y2
|
||||
// Increment time x1 x2 y1 y2 y3
|
||||
// Shift x1 x1 x2 y1 y2
|
||||
// This unrolls better than xy.rotate_right(1)
|
||||
xy.copy_within(0..n - 1, 1);
|
||||
xy.0.copy_within(0..n - 1, 1);
|
||||
// Store x0 x0 x1 x2 y1 y2
|
||||
xy[0] = x0;
|
||||
xy.0[0] = x0;
|
||||
// Compute y0 by multiply-accumulate
|
||||
let y0 = macc(0, xy, &self.ba, IIR::SHIFT);
|
||||
let y0 = macc(0, &xy.0, &self.ba.0, IIR::SHIFT);
|
||||
// Limit y0
|
||||
// let y0 = y0.max(self.y_min).min(self.y_max);
|
||||
// Store y0 x0 x1 y0 y1 y2
|
||||
xy[n / 2] = y0;
|
||||
xy.0[n / 2] = y0;
|
||||
y0
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -3,8 +3,6 @@
|
|||
|
||||
use core::ops::{Add, Mul, Neg};
|
||||
|
||||
pub type Complex<T> = (T, T);
|
||||
|
||||
/// Bit shift, round up half.
|
||||
///
|
||||
/// # Arguments
|
||||
|
|
@ -114,12 +112,19 @@ where
|
|||
.fold(y0, |y, xa| y + xa)
|
||||
}
|
||||
|
||||
mod atan2;
|
||||
mod complex;
|
||||
mod cossin;
|
||||
pub mod iir;
|
||||
pub mod iir_int;
|
||||
pub mod lockin;
|
||||
pub mod pll;
|
||||
pub mod reciprocal_pll;
|
||||
pub mod trig;
|
||||
pub mod unwrap;
|
||||
|
||||
pub use atan2::atan2;
|
||||
pub use complex::Complex;
|
||||
pub use cossin::cossin;
|
||||
|
||||
#[cfg(test)]
|
||||
mod testing;
|
||||
pub mod testing;
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -62,11 +62,18 @@ impl TimestampHandler {
|
|||
self.reference_frequency = frequency as u32 as i64;
|
||||
}
|
||||
|
||||
let demodulation_frequency = divide_round(
|
||||
let demodulation_frequency: u32;
|
||||
let demodulation_initial_phase: u32;
|
||||
|
||||
if self.reference_frequency == 0 {
|
||||
demodulation_frequency = u32::MAX;
|
||||
demodulation_initial_phase = u32::MAX;
|
||||
} else {
|
||||
demodulation_frequency = divide_round(
|
||||
1 << (32 + self.adc_sample_ticks_log2),
|
||||
self.reference_frequency,
|
||||
) as u32;
|
||||
let demodulation_initial_phase = divide_round(
|
||||
demodulation_initial_phase = divide_round(
|
||||
(((self.batch_index as i64)
|
||||
<< (self.adc_sample_ticks_log2
|
||||
+ self.sample_buffer_size_log2))
|
||||
|
|
@ -74,6 +81,7 @@ impl TimestampHandler {
|
|||
<< 32,
|
||||
self.reference_frequency,
|
||||
) as u32;
|
||||
}
|
||||
|
||||
if self.batch_index
|
||||
< (1 << (32
|
||||
|
|
|
|||
|
|
@ -1,6 +1,22 @@
|
|||
#![allow(dead_code)]
|
||||
use super::Complex;
|
||||
|
||||
/// Maximum acceptable error between a computed and actual value given fixed and relative
|
||||
/// tolerances.
|
||||
///
|
||||
/// # Args
|
||||
/// * `a` - First input.
|
||||
/// * `b` - Second input. The relative tolerance is computed with respect to the maximum of the
|
||||
/// absolute values of the first and second inputs.
|
||||
/// * `rtol` - Relative tolerance.
|
||||
/// * `atol` - Fixed tolerance.
|
||||
///
|
||||
/// # Returns
|
||||
/// Maximum acceptable error.
|
||||
pub fn max_error(a: f64, b: f64, rtol: f64, atol: f64) -> f64 {
|
||||
rtol * a.abs().max(b.abs()) + atol
|
||||
}
|
||||
|
||||
pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool {
|
||||
(a - b).abs() <= a.abs().max(b.abs()) * rtol + atol
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,353 @@
|
|||
#![deny(warnings)]
|
||||
#![no_std]
|
||||
#![no_main]
|
||||
#![cfg_attr(feature = "nightly", feature(core_intrinsics))]
|
||||
|
||||
use stm32h7xx_hal as hal;
|
||||
|
||||
#[macro_use]
|
||||
extern crate log;
|
||||
|
||||
use rtic::cyccnt::{Instant, U32Ext};
|
||||
|
||||
use heapless::{consts::*, String};
|
||||
|
||||
use stabilizer::{
|
||||
hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2,
|
||||
};
|
||||
|
||||
use dsp::{iir, iir_int, lockin::Lockin, reciprocal_pll::TimestampHandler};
|
||||
use hardware::{
|
||||
Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
|
||||
};
|
||||
|
||||
const SCALE: f32 = ((1 << 15) - 1) as f32;
|
||||
|
||||
const TCP_RX_BUFFER_SIZE: usize = 8192;
|
||||
const TCP_TX_BUFFER_SIZE: usize = 8192;
|
||||
|
||||
// The number of cascaded IIR biquads per channel. Select 1 or 2!
|
||||
const IIR_CASCADE_LENGTH: usize = 1;
|
||||
|
||||
#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
|
||||
const APP: () = {
|
||||
struct Resources {
|
||||
afes: (AFE0, AFE1),
|
||||
adcs: (Adc0Input, Adc1Input),
|
||||
dacs: (Dac0Output, Dac1Output),
|
||||
net_interface: hardware::Ethernet,
|
||||
|
||||
// Format: iir_state[ch][cascade-no][coeff]
|
||||
#[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])]
|
||||
iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2],
|
||||
#[init([[iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
|
||||
iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
|
||||
|
||||
timestamper: InputStamper,
|
||||
pll: TimestampHandler,
|
||||
lockin: Lockin,
|
||||
}
|
||||
|
||||
#[init]
|
||||
fn init(c: init::Context) -> init::LateResources {
|
||||
// Configure the microcontroller
|
||||
let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
|
||||
|
||||
let pll = TimestampHandler::new(
|
||||
4, // relative PLL frequency bandwidth: 2**-4, TODO: expose
|
||||
3, // relative PLL phase bandwidth: 2**-3, TODO: expose
|
||||
ADC_SAMPLE_TICKS_LOG2 as usize,
|
||||
SAMPLE_BUFFER_SIZE_LOG2,
|
||||
);
|
||||
|
||||
let lockin = Lockin::new(
|
||||
&iir_int::IIRState::default(), // TODO: lowpass, expose
|
||||
);
|
||||
|
||||
// Enable ADC/DAC events
|
||||
stabilizer.adcs.0.start();
|
||||
stabilizer.adcs.1.start();
|
||||
stabilizer.dacs.0.start();
|
||||
stabilizer.dacs.1.start();
|
||||
|
||||
// Start recording digital input timestamps.
|
||||
stabilizer.timestamp_timer.start();
|
||||
|
||||
// Start sampling ADCs.
|
||||
stabilizer.adc_dac_timer.start();
|
||||
|
||||
init::LateResources {
|
||||
afes: stabilizer.afes,
|
||||
adcs: stabilizer.adcs,
|
||||
dacs: stabilizer.dacs,
|
||||
net_interface: stabilizer.net.interface,
|
||||
timestamper: stabilizer.timestamper,
|
||||
|
||||
pll,
|
||||
lockin,
|
||||
}
|
||||
}
|
||||
|
||||
/// Main DSP processing routine for Stabilizer.
|
||||
///
|
||||
/// # Note
|
||||
/// Processing time for the DSP application code is bounded by the following constraints:
|
||||
///
|
||||
/// DSP application code starts after the ADC has generated a batch of samples and must be
|
||||
/// completed by the time the next batch of ADC samples has been acquired (plus the FIFO buffer
|
||||
/// time). If this constraint is not met, firmware will panic due to an ADC input overrun.
|
||||
///
|
||||
/// The DSP application code must also fill out the next DAC output buffer in time such that the
|
||||
/// DAC can switch to it when it has completed the current buffer. If this constraint is not met
|
||||
/// it's possible that old DAC codes will be generated on the output and the output samples will
|
||||
/// be delayed by 1 batch.
|
||||
///
|
||||
/// Because the ADC and DAC operate at the same rate, these two constraints actually implement
|
||||
/// the same time bounds, meeting one also means the other is also met.
|
||||
///
|
||||
/// TODO: document lockin
|
||||
#[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch, lockin, timestamper, pll], priority=2)]
|
||||
fn process(c: process::Context) {
|
||||
let adc_samples = [
|
||||
c.resources.adcs.0.acquire_buffer(),
|
||||
c.resources.adcs.1.acquire_buffer(),
|
||||
];
|
||||
|
||||
let dac_samples = [
|
||||
c.resources.dacs.0.acquire_buffer(),
|
||||
c.resources.dacs.1.acquire_buffer(),
|
||||
];
|
||||
|
||||
let iir_ch = c.resources.iir_ch;
|
||||
let iir_state = c.resources.iir_state;
|
||||
let lockin = c.resources.lockin;
|
||||
|
||||
let (pll_phase, pll_frequency) = c
|
||||
.resources
|
||||
.pll
|
||||
.update(c.resources.timestamper.latest_timestamp());
|
||||
|
||||
// Harmonic index of the LO: -1 to _de_modulate the fundamental
|
||||
let harmonic: i32 = -1;
|
||||
// Demodulation LO phase offset
|
||||
let phase_offset: i32 = 0;
|
||||
let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
|
||||
let mut sample_phase = phase_offset
|
||||
.wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
|
||||
|
||||
for i in 0..adc_samples[0].len() {
|
||||
// Convert to signed, MSB align the ADC sample.
|
||||
let input = (adc_samples[0][i] as i16 as i32) << 16;
|
||||
// Obtain demodulated, filtered IQ sample.
|
||||
let output = lockin.update(input, sample_phase);
|
||||
// Advance the sample phase.
|
||||
sample_phase = sample_phase.wrapping_add(sample_frequency);
|
||||
|
||||
// Convert from IQ to power and phase.
|
||||
let mut power = output.power() as _;
|
||||
let mut phase = output.phase() as _;
|
||||
|
||||
// Filter power and phase through IIR filters.
|
||||
// Note: Normalization to be done in filters. Phase will wrap happily.
|
||||
for j in 0..iir_state[0].len() {
|
||||
power = iir_ch[0][j].update(&mut iir_state[0][j], power);
|
||||
phase = iir_ch[1][j].update(&mut iir_state[1][j], phase);
|
||||
}
|
||||
|
||||
// Note(unsafe): range clipping to i16 is ensured by IIR filters above.
|
||||
// Convert to DAC data.
|
||||
unsafe {
|
||||
dac_samples[0][i] =
|
||||
power.to_int_unchecked::<i16>() as u16 ^ 0x8000;
|
||||
dac_samples[1][i] =
|
||||
phase.to_int_unchecked::<i16>() as u16 ^ 0x8000;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[idle(resources=[net_interface, iir_state, iir_ch, afes])]
|
||||
fn idle(mut c: idle::Context) -> ! {
|
||||
let mut socket_set_entries: [_; 8] = Default::default();
|
||||
let mut sockets =
|
||||
smoltcp::socket::SocketSet::new(&mut socket_set_entries[..]);
|
||||
|
||||
let mut rx_storage = [0; TCP_RX_BUFFER_SIZE];
|
||||
let mut tx_storage = [0; TCP_TX_BUFFER_SIZE];
|
||||
let tcp_handle = {
|
||||
let tcp_rx_buffer =
|
||||
smoltcp::socket::TcpSocketBuffer::new(&mut rx_storage[..]);
|
||||
let tcp_tx_buffer =
|
||||
smoltcp::socket::TcpSocketBuffer::new(&mut tx_storage[..]);
|
||||
let tcp_socket =
|
||||
smoltcp::socket::TcpSocket::new(tcp_rx_buffer, tcp_tx_buffer);
|
||||
sockets.add(tcp_socket)
|
||||
};
|
||||
|
||||
let mut server = server::Server::new();
|
||||
|
||||
let mut time = 0u32;
|
||||
let mut next_ms = Instant::now();
|
||||
|
||||
// TODO: Replace with reference to CPU clock from CCDR.
|
||||
next_ms += 400_000.cycles();
|
||||
|
||||
loop {
|
||||
let tick = Instant::now() > next_ms;
|
||||
|
||||
if tick {
|
||||
next_ms += 400_000.cycles();
|
||||
time += 1;
|
||||
}
|
||||
|
||||
{
|
||||
let socket =
|
||||
&mut *sockets.get::<smoltcp::socket::TcpSocket>(tcp_handle);
|
||||
if socket.state() == smoltcp::socket::TcpState::CloseWait {
|
||||
socket.close();
|
||||
} else if !(socket.is_open() || socket.is_listening()) {
|
||||
socket
|
||||
.listen(1235)
|
||||
.unwrap_or_else(|e| warn!("TCP listen error: {:?}", e));
|
||||
} else {
|
||||
server.poll(socket, |req| {
|
||||
info!("Got request: {:?}", req);
|
||||
stabilizer::route_request!(req,
|
||||
readable_attributes: [
|
||||
"stabilizer/iir/state": (|| {
|
||||
let state = c.resources.iir_state.lock(|iir_state|
|
||||
server::Status {
|
||||
t: time,
|
||||
x0: iir_state[0][0][0],
|
||||
y0: iir_state[0][0][2],
|
||||
x1: iir_state[1][0][0],
|
||||
y1: iir_state[1][0][2],
|
||||
});
|
||||
|
||||
Ok::<server::Status, ()>(state)
|
||||
}),
|
||||
// "_b" means cascades 2nd IIR
|
||||
"stabilizer/iir_b/state": (|| { let state = c.resources.iir_state.lock(|iir_state|
|
||||
server::Status {
|
||||
t: time,
|
||||
x0: iir_state[0][IIR_CASCADE_LENGTH-1][0],
|
||||
y0: iir_state[0][IIR_CASCADE_LENGTH-1][2],
|
||||
x1: iir_state[1][IIR_CASCADE_LENGTH-1][0],
|
||||
y1: iir_state[1][IIR_CASCADE_LENGTH-1][2],
|
||||
});
|
||||
|
||||
Ok::<server::Status, ()>(state)
|
||||
}),
|
||||
"stabilizer/afe0/gain": (|| c.resources.afes.0.get_gain()),
|
||||
"stabilizer/afe1/gain": (|| c.resources.afes.1.get_gain())
|
||||
],
|
||||
|
||||
modifiable_attributes: [
|
||||
"stabilizer/iir0/state": server::IirRequest, (|req: server::IirRequest| {
|
||||
c.resources.iir_ch.lock(|iir_ch| {
|
||||
if req.channel > 1 {
|
||||
return Err(());
|
||||
}
|
||||
|
||||
iir_ch[req.channel as usize][0] = req.iir;
|
||||
|
||||
Ok::<server::IirRequest, ()>(req)
|
||||
})
|
||||
}),
|
||||
"stabilizer/iir1/state": server::IirRequest, (|req: server::IirRequest| {
|
||||
c.resources.iir_ch.lock(|iir_ch| {
|
||||
if req.channel > 1 {
|
||||
return Err(());
|
||||
}
|
||||
|
||||
iir_ch[req.channel as usize][0] = req.iir;
|
||||
|
||||
Ok::<server::IirRequest, ()>(req)
|
||||
})
|
||||
}),
|
||||
"stabilizer/iir_b0/state": server::IirRequest, (|req: server::IirRequest| {
|
||||
c.resources.iir_ch.lock(|iir_ch| {
|
||||
if req.channel > 1 {
|
||||
return Err(());
|
||||
}
|
||||
|
||||
iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir;
|
||||
|
||||
Ok::<server::IirRequest, ()>(req)
|
||||
})
|
||||
}),
|
||||
"stabilizer/iir_b1/state": server::IirRequest,(|req: server::IirRequest| {
|
||||
c.resources.iir_ch.lock(|iir_ch| {
|
||||
if req.channel > 1 {
|
||||
return Err(());
|
||||
}
|
||||
|
||||
iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir;
|
||||
|
||||
Ok::<server::IirRequest, ()>(req)
|
||||
})
|
||||
}),
|
||||
"stabilizer/afe0/gain": hardware::AfeGain, (|gain| {
|
||||
c.resources.afes.0.set_gain(gain);
|
||||
Ok::<(), ()>(())
|
||||
}),
|
||||
"stabilizer/afe1/gain": hardware::AfeGain, (|gain| {
|
||||
c.resources.afes.1.set_gain(gain);
|
||||
Ok::<(), ()>(())
|
||||
})
|
||||
]
|
||||
)
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
let sleep = match c.resources.net_interface.poll(
|
||||
&mut sockets,
|
||||
smoltcp::time::Instant::from_millis(time as i64),
|
||||
) {
|
||||
Ok(changed) => !changed,
|
||||
Err(smoltcp::Error::Unrecognized) => true,
|
||||
Err(e) => {
|
||||
info!("iface poll error: {:?}", e);
|
||||
true
|
||||
}
|
||||
};
|
||||
|
||||
if sleep {
|
||||
cortex_m::asm::wfi();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = ETH, priority = 1)]
|
||||
fn eth(_: eth::Context) {
|
||||
unsafe { hal::ethernet::interrupt_handler() }
|
||||
}
|
||||
|
||||
#[task(binds = SPI2, priority = 3)]
|
||||
fn spi2(_: spi2::Context) {
|
||||
panic!("ADC0 input overrun");
|
||||
}
|
||||
|
||||
#[task(binds = SPI3, priority = 3)]
|
||||
fn spi3(_: spi3::Context) {
|
||||
panic!("ADC0 input overrun");
|
||||
}
|
||||
|
||||
#[task(binds = SPI4, priority = 3)]
|
||||
fn spi4(_: spi4::Context) {
|
||||
panic!("DAC0 output error");
|
||||
}
|
||||
|
||||
#[task(binds = SPI5, priority = 3)]
|
||||
fn spi5(_: spi5::Context) {
|
||||
panic!("DAC1 output error");
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
// hw interrupt handlers for RTIC to use for scheduling tasks
|
||||
// one per priority
|
||||
fn DCMI();
|
||||
fn JPEG();
|
||||
fn SDMMC();
|
||||
}
|
||||
};
|
||||
|
|
@ -20,6 +20,7 @@ mod timers;
|
|||
pub use adc::{Adc0Input, Adc1Input};
|
||||
pub use afe::Gain as AfeGain;
|
||||
pub use dac::{Dac0Output, Dac1Output};
|
||||
pub use digital_input_stamper::InputStamper;
|
||||
pub use pounder::DdsOutput;
|
||||
|
||||
// Type alias for the analog front-end (AFE) for ADC0.
|
||||
|
|
|
|||
|
|
@ -9,7 +9,9 @@ pub mod server;
|
|||
// The number of ticks in the ADC sampling timer. The timer runs at 100MHz, so the step size is
|
||||
// equal to 10ns per tick.
|
||||
// Currently, the sample rate is equal to: Fsample = 100/256 MHz = 390.625 KHz
|
||||
const ADC_SAMPLE_TICKS: u16 = 256;
|
||||
pub const ADC_SAMPLE_TICKS_LOG2: u16 = 8;
|
||||
pub const ADC_SAMPLE_TICKS: u16 = 1 << ADC_SAMPLE_TICKS_LOG2;
|
||||
|
||||
// The desired ADC sample processing buffer size.
|
||||
const SAMPLE_BUFFER_SIZE: usize = 8;
|
||||
pub const SAMPLE_BUFFER_SIZE_LOG2: usize = 3;
|
||||
pub const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2;
|
||||
|
|
|
|||
Loading…
Reference in New Issue