diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 1314418..6fa0e99 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -41,13 +41,19 @@ jobs: matrix: toolchain: - stable - - beta bin: - dual-iir - - lockin + - lockin-internal + - lockin-external features: - '' - - pounder_v1_1 + include: + - toolchain: beta + bin: dual-iir + features: '' + - toolchain: stable + bin: lockin-internal + features: pounder_v1_1 steps: - uses: actions/checkout@v2 - name: Install Rust ${{ matrix.toolchain }} diff --git a/.gitignore b/.gitignore index c024ef6..ea8c4bf 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1 @@ /target -openocd.gdb diff --git a/Cargo.lock b/Cargo.lock index f4c420b..7dc698b 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -36,7 +36,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b9a69a963b70ddacfcd382524f72a4576f359af9334b3bf48a79566590bb8bfa" dependencies = [ "bitrate", - "cortex-m", + "cortex-m 0.7.1", "embedded-hal", ] @@ -129,12 +129,6 @@ dependencies = [ "rustc_version", ] -[[package]] -name = "cfg-if" -version = "0.1.10" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4785bdd1c96b2a846b2bd7cc02e86b6b3dbf14e7e53446c4f54c92a361040822" - [[package]] name = "cfg-if" version = "1.0.0" @@ -160,23 +154,36 @@ checksum = "cd51eab21ab4fd6a3bf889e2d0958c0a6e3a61ad04260325e919e652a2a62826" [[package]] name = "cortex-m" -version = "0.6.4" +version = "0.6.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "88cdafeafba636c00c467ded7f1587210725a1adfab0c24028a7844b87738263" +checksum = "9075300b07c6a56263b9b582c214d0ff037b00d45ec9fde1cc711490c56f1bb9" dependencies = [ "aligned", "bare-metal 0.2.5", "bitfield", + "cortex-m 0.7.1", + "volatile-register", +] + +[[package]] +name = "cortex-m" +version = "0.7.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a0b756a8bffc56025de45218a48ff9b801180440c0ee49a722b32d49dcebc771" +dependencies = [ + "bare-metal 0.2.5", + "bitfield", + "embedded-hal", "volatile-register", ] [[package]] name = "cortex-m-log" -version = "0.6.2" +version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1d63959cb1e003dd97233fee6762351540253237eadf06fcdcb98cbfa3f9be4a" +checksum = "0e202d2eac4e34adf7524a563e36623bae6f69cc0a73ef9bd22a4c93a5a806fa" dependencies = [ - "cortex-m", + "cortex-m 0.7.1", "cortex-m-semihosting", "log", ] @@ -208,7 +215,7 @@ version = "0.5.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b30efcb6b7920d9016182c485687f0012487032a14c415d2fce6e9862ef8260e" dependencies = [ - "cortex-m", + "cortex-m 0.6.7", "cortex-m-rt", "cortex-m-rtic-macros", "heapless", @@ -234,21 +241,21 @@ version = "0.3.7" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6bffa6c1454368a6aa4811ae60964c38e6996d397ff8095a8b9211b1c1f749bc" dependencies = [ - "cortex-m", + "cortex-m 0.7.1", ] [[package]] name = "criterion" -version = "0.3.3" +version = "0.3.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "70daa7ceec6cf143990669a04c7df13391d55fb27bd4079d252fca774ba244d8" +checksum = "ab327ed7354547cc2ef43cbe20ef68b988e70b4b593cbd66a2a61733123a3d23" dependencies = [ "atty", "cast", "clap", "criterion-plot", "csv", - "itertools", + "itertools 0.10.0", "lazy_static", "num-traits", "oorandom", @@ -270,7 +277,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e022feadec601fba1649cfa83586381a4ad31c6bf3a9ab7d408118b05dd9889d" dependencies = [ "cast", - "itertools", + "itertools 0.9.0", ] [[package]] @@ -279,7 +286,7 @@ version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "dca26ee1f8d361640700bde38b2c37d8c22b3ce2d360e1fc1c74ea4b0aa7d775" dependencies = [ - "cfg-if 1.0.0", + "cfg-if", "crossbeam-utils", ] @@ -289,7 +296,7 @@ version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "94af6efb46fef72616855b036a624cf27ba656ffc9be1b9a3c931cfc7749a9a9" dependencies = [ - "cfg-if 1.0.0", + "cfg-if", "crossbeam-epoch", "crossbeam-utils", ] @@ -300,7 +307,7 @@ version = "0.9.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a1aaa739f95311c2c7887a76863f500026092fb1dce0161dab577e559ef3569d" dependencies = [ - "cfg-if 1.0.0", + "cfg-if", "const_fn", "crossbeam-utils", "lazy_static", @@ -315,7 +322,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "02d96d1e189ef58269ebe5b97953da3274d83a93af647c2ddd6f9dab28cedb8d" dependencies = [ "autocfg", - "cfg-if 1.0.0", + "cfg-if", "lazy_static", ] @@ -358,6 +365,8 @@ dependencies = [ "criterion", "libm", "miniconf", + "ndarray", + "rand", "serde", "serde-json-core 0.1.0", ] @@ -446,6 +455,17 @@ dependencies = [ "version_check", ] +[[package]] +name = "getrandom" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c9495705279e7140bf035dde1f6e750c162df8b625267cd52cc44e0b156732c8" +dependencies = [ + "cfg-if", + "libc", + "wasi", +] + [[package]] name = "half" version = "1.6.0" @@ -508,6 +528,15 @@ dependencies = [ "either", ] +[[package]] +name = "itertools" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "37d572918e350e82412fe766d24b15e6682fb2ed2bbe018280caa810397cb319" +dependencies = [ + "either", +] + [[package]] name = "itoa" version = "0.4.6" @@ -543,11 +572,11 @@ checksum = "c7d73b3f436185384286bd8098d17ec07c9a7d2388a6599f824d8502b529702a" [[package]] name = "log" -version = "0.4.13" +version = "0.4.14" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fcf3805d4480bb5b86070dcfeb9e2cb2ebc148adb753c5cca5f884d1d65a42b2" +checksum = "51b9bbe6c47d51fc3e1a9b945965946b4c44142ab8792c50835a980d362c2710" dependencies = [ - "cfg-if 0.1.10", + "cfg-if", ] [[package]] @@ -556,6 +585,15 @@ version = "0.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c75de51135344a4f8ed3cfe2720dc27736f7711989703a0b43aadf3753c55577" +[[package]] +name = "matrixmultiply" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "916806ba0031cd542105d916a97c8572e1fa6dd79c9c51e7eb43a09ec2dd84c1" +dependencies = [ + "rawpointer", +] + [[package]] name = "mcp23017" version = "0.1.1" @@ -594,7 +632,7 @@ dependencies = [ [[package]] name = "minimq" version = "0.1.0" -source = "git+https://github.com/quartiq/minimq.git#dc459f5a3978fd90410adc916ce12b56ca449c47" +source = "git+https://github.com/quartiq/minimq.git#83e946544cebd09c9dd07ff1271be639138ec1ce" dependencies = [ "bit_field", "embedded-nal", @@ -618,12 +656,44 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "546c37ac5d9e56f55e73b677106873d9d9f5190605e41a856503623648488cae" +[[package]] +name = "ndarray" +version = "0.14.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6c0d5c9540a691d153064dc47a4db2504587a75eae07bf1d73f7a596ebc73c04" +dependencies = [ + "matrixmultiply", + "num-complex", + "num-integer", + "num-traits", + "rawpointer", +] + [[package]] name = "no-std-net" version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2178127478ae4ee9be7180bc9c3bffb6354dd7238400db567102f98c413a9f35" +[[package]] +name = "num-complex" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "747d632c0c558b87dbabbe6a82f3b4ae03720d0646ac5b7b4dae89394be5f2c5" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-integer" +version = "0.1.44" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d2cc698a63b549a70bc047073d2949cce27cd1c7b0a4a862d08a8031bc2801db" +dependencies = [ + "autocfg", + "num-traits", +] + [[package]] name = "num-traits" version = "0.2.14" @@ -661,7 +731,7 @@ version = "0.5.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c3d55dedd501dfd02514646e0af4d7016ce36bc12ae177ef52056989966a1eec" dependencies = [ - "cortex-m", + "cortex-m 0.7.1", "cortex-m-semihosting", ] @@ -673,16 +743,38 @@ checksum = "c5d65c4d95931acda4498f675e332fcbdc9a06705cd07086c510e9b6009cd1c1" [[package]] name = "plotters" -version = "0.2.15" +version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0d1685fbe7beba33de0330629da9d955ac75bd54f33d7b79f9a895590124f6bb" +checksum = "45ca0ae5f169d0917a7c7f5a9c1a3d3d9598f18f529dd2b8373ed988efea307a" dependencies = [ - "js-sys", "num-traits", + "plotters-backend", + "plotters-svg", "wasm-bindgen", "web-sys", ] +[[package]] +name = "plotters-backend" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b07fffcddc1cb3a1de753caa4e4df03b79922ba43cf882acc1bdd7e8df9f4590" + +[[package]] +name = "plotters-svg" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b38a02e23bd9604b842a812063aec4ef702b57989c37b655254bb61c471ad211" +dependencies = [ + "plotters-backend", +] + +[[package]] +name = "ppv-lite86" +version = "0.2.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac74c624d6b2d21f425f752262f42188365d7b8ff1aff74c82e45136510a4857" + [[package]] name = "proc-macro2" version = "1.0.24" @@ -707,6 +799,52 @@ version = "0.2.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e2a38df5b15c8d5c7e8654189744d8e396bddc18ad48041a500ce52d6948941f" +[[package]] +name = "rand" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ef9e7e66b4468674bfcb0c81af8b7fa0bb154fa9f28eb840da5c447baeb8d7e" +dependencies = [ + "libc", + "rand_chacha", + "rand_core", + "rand_hc", +] + +[[package]] +name = "rand_chacha" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e12735cf05c9e10bf21534da50a147b924d555dc7a547c42e6bb2d5b6017ae0d" +dependencies = [ + "ppv-lite86", + "rand_core", +] + +[[package]] +name = "rand_core" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c026d7df8b298d90ccbbc5190bd04d85e159eaf5576caeacf8741da93ccbd2e5" +dependencies = [ + "getrandom", +] + +[[package]] +name = "rand_hc" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3190ef7066a446f2e7f42e239d161e905420ccab01eb967c9eb27d21b2322a73" +dependencies = [ + "rand_core", +] + +[[package]] +name = "rawpointer" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" + [[package]] name = "rayon" version = "1.5.0" @@ -820,9 +958,9 @@ checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" [[package]] name = "serde" -version = "1.0.120" +version = "1.0.123" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "166b2349061381baf54a58e4b13c89369feb0ef2eaa57198899e2312aac30aab" +checksum = "92d5161132722baa40d802cc70b15262b98258453e85e5d1d365c757c73869ae" dependencies = [ "serde_derive", ] @@ -859,9 +997,9 @@ dependencies = [ [[package]] name = "serde_derive" -version = "1.0.120" +version = "1.0.123" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0ca2a8cb5805ce9e3b95435e3765b7b553cecc762d938d409434338386cb5775" +checksum = "9391c295d64fc0abb2c556bad848f33cb8296276b1ad2677d1ae1ace4f258f31" dependencies = [ "proc-macro2", "quote", @@ -890,23 +1028,13 @@ dependencies = [ "managed", ] -[[package]] -name = "smoltcp-nal" -version = "0.1.0" -source = "git+https://github.com/vertigo-designs/smoltcp-nal.git?branch=main#6a656dd78c5f7543475e95c0eaf81def95fc5a10" -dependencies = [ - "embedded-nal", - "heapless", - "smoltcp", -] - [[package]] name = "stabilizer" version = "0.4.1" dependencies = [ "ad9959", "asm-delay", - "cortex-m", + "cortex-m 0.6.7", "cortex-m-log", "cortex-m-rt", "cortex-m-rtic", @@ -922,7 +1050,7 @@ dependencies = [ "panic-semihosting", "paste", "serde", - "smoltcp-nal", + "smoltcp", "stm32h7xx-hal", ] @@ -939,7 +1067,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7571f17d1ed7d67957d0004de6c52bd1ef5e736ed5ddc2bcecf001512269f77c" dependencies = [ "bare-metal 0.2.5", - "cortex-m", + "cortex-m 0.6.7", "cortex-m-rt", "vcell", ] @@ -951,7 +1079,7 @@ source = "git+https://github.com/quartiq/stm32h7xx-hal?branch=rs/smoltcp-update# dependencies = [ "bare-metal 1.0.0", "cast", - "cortex-m", + "cortex-m 0.6.7", "cortex-m-rt", "embedded-dma", "embedded-hal", @@ -964,9 +1092,9 @@ dependencies = [ [[package]] name = "syn" -version = "1.0.58" +version = "1.0.60" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cc60a3d73ea6594cd712d830cc1f0390fd71542d8c8cd24e70cc54cdfd5e05d5" +checksum = "c700597eca8a5a762beb35753ef6b94df201c81cca676604f547495a0d7f0081" dependencies = [ "proc-macro2", "quote", @@ -1048,13 +1176,19 @@ dependencies = [ "winapi-util", ] +[[package]] +name = "wasi" +version = "0.10.2+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fd6fbd9a79829dd1ad0cc20627bf1ed606756a7f77edff7b66b7064f9cb327c6" + [[package]] name = "wasm-bindgen" version = "0.2.69" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3cd364751395ca0f68cafb17666eee36b63077fb5ecd972bbcd74c90c4bf736e" dependencies = [ - "cfg-if 1.0.0", + "cfg-if", "wasm-bindgen-macro", ] diff --git a/Cargo.toml b/Cargo.toml index 182d62b..26016e1 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -30,7 +30,7 @@ members = ["ad9959", "dsp"] [dependencies] cortex-m = { version = "0.6", features = ["const-fn"] } cortex-m-rt = { version = "0.6", features = ["device"] } -cortex-m-log = { version = "0.6", features = ["log-integration"] } +cortex-m-log = { version = "0.7", features = ["log-integration"] } log = "0.4" panic-semihosting = { version = "0.5", optional = true } panic-halt = "0.2" @@ -52,9 +52,10 @@ branch = "feature/mqtt-interface" [dependencies.mcp23017] git = "https://github.com/mrd0ll4r/mcp23017.git" -[dependencies.smoltcp-nal] -git = "https://github.com/vertigo-designs/smoltcp-nal.git" -branch = "main" +[dependencies.smoltcp] +version = "0.7" +features = ["ethernet", "proto-ipv4", "socket-tcp", "proto-ipv6"] +default-features = false [dependencies.stm32h7xx-hal] features = ["stm32h743v", "rt", "unproven", "ethernet", "quadspi"] diff --git a/Embed.toml b/Embed.toml new file mode 100644 index 0000000..8a34951 --- /dev/null +++ b/Embed.toml @@ -0,0 +1,3 @@ +[default.general] +chip = "STM32H743ZITx" +connect_under_reset = true diff --git a/README.md b/README.md index 8ee69f2..7f0967b 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ ## Limitations/TODOs * Fixed AFE gains -* The IP and MAC address are [hardcoded](src/main.rs) +* The IP and MAC address are [hardcoded](src/hardware/configuration.rs) * Expose configurable limits * 100Base-T only * Digital IO, GPIO header, AFE header, EEM header are not handled @@ -40,6 +40,11 @@ See https://github.com/sinara-hw/Stabilizer * `cargo build --release` * Do not try the debug (default) mode. It is guaranteed to panic. +### Using Cargo-embed + +* Install `cargo-embed`: `cargo install cargo-embed` +* Program the device: `cargo embed --bin dual-iir --release` + ### Using GDB/OpenOCD * Get a recent openocd, a JTAG adapter ("st-link" or some clone) and @@ -64,14 +69,14 @@ See https://github.com/sinara-hw/Stabilizer * Install the DFU USB tool (`dfu-util`) * Connect to the Micro USB connector below the RJ45 * Short JC2/BOOT -* `cargo objcopy --release --bin stabilizer -- -O binary stabilizer.bin` or `arm-none-eabi-objcopy -O binary target/thumbv7em-none-eabihf/release/stabilizer stabilizer.bin` -* `dfu-util -a 0 -s 0x08000000:leave -D stabilizer.bin` +* `cargo objcopy --release --bin dual-iir -- -O binary dual-iir.bin` or `arm-none-eabi-objcopy -O binary target/thumbv7em-none-eabihf/release/dual-iir dual-iir.bin` +* `dfu-util -a 0 -s 0x08000000:leave -D dual-iir.bin` ### Using ST-Link virtual mass storage -* `cargo objcopy --release --bin stabilizer -- -O binary stabilizer.bin` or `arm-none-eabi-objcopy -O binary target/thumbv7em-none-eabihf/release/stabilizer stabilizer.bin` +* `cargo objcopy --release --bin dual-iir -- -O binary dual-iir.bin` or `arm-none-eabi-objcopy -O binary target/thumbv7em-none-eabihf/release/dual-iir dual-iir.bin` * Connect the ST-Link debugger -* copy `stabilizer.bin` to the `NODE_H743ZI` USB disk +* copy `dual-iir.bin` to the `NODE_H743ZI` USB disk ## Protocol diff --git a/cargosha256-dual-iir.nix b/cargosha256-dual-iir.nix new file mode 100644 index 0000000..8775167 --- /dev/null +++ b/cargosha256-dual-iir.nix @@ -0,0 +1 @@ +"0ysy8fg6kbblhmjyavq6pg77n21fcygwc0hvidmg2yywkhgdi348" diff --git a/cargosha256.nix b/cargosha256.nix deleted file mode 100644 index 92745cf..0000000 --- a/cargosha256.nix +++ /dev/null @@ -1 +0,0 @@ -"0mrnm74wd5c1cl3av8iqndg6xrm07vs862882m59pjnrgy3z2zqj" diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml index 44cc6fb..25d395a 100644 --- a/dsp/Cargo.toml +++ b/dsp/Cargo.toml @@ -15,9 +15,11 @@ branch = "feature/mqtt-interface" [dev-dependencies] criterion = "0.3" +rand = "0.8" +ndarray = "0.14" [[bench]] -name = "trig" +name = "micro" harness = false [features] diff --git a/dsp/benches/micro.rs b/dsp/benches/micro.rs new file mode 100644 index 0000000..56b9149 --- /dev/null +++ b/dsp/benches/micro.rs @@ -0,0 +1,70 @@ +use core::f32::consts::PI; +use criterion::{black_box, criterion_group, criterion_main, Criterion}; +use dsp::{atan2, cossin}; +use dsp::{iir, iir_int}; +use dsp::{pll::PLL, rpll::RPLL}; + +fn atan2_bench(c: &mut Criterion) { + let xi = (10 << 16) as i32; + let xf = xi as f32 / i32::MAX as f32; + + let yi = (-26_328 << 16) as i32; + let yf = yi as f32 / i32::MAX as f32; + + c.bench_function("atan2(y, x)", |b| { + b.iter(|| atan2(black_box(yi), black_box(xi))) + }); + c.bench_function("y.atan2(x)", |b| { + b.iter(|| black_box(yf).atan2(black_box(xf))) + }); +} + +fn cossin_bench(c: &mut Criterion) { + let zi = -0x7304_2531_i32; + let zf = zi as f32 / i32::MAX as f32 * PI; + c.bench_function("cossin(zi)", |b| b.iter(|| cossin(black_box(zi)))); + c.bench_function("zf.sin_cos()", |b| b.iter(|| black_box(zf).sin_cos())); +} + +fn rpll_bench(c: &mut Criterion) { + let mut dut = RPLL::new(8); + c.bench_function("RPLL::update(Some(t), 21, 20)", |b| { + b.iter(|| dut.update(black_box(Some(0x241)), 21, 20)) + }); + c.bench_function("RPLL::update(Some(t), sf, sp)", |b| { + b.iter(|| { + dut.update(black_box(Some(0x241)), black_box(21), black_box(20)) + }) + }); +} + +fn pll_bench(c: &mut Criterion) { + let mut dut = PLL::default(); + c.bench_function("PLL::update(t, 12, 11)", |b| { + b.iter(|| dut.update(black_box(0x1234), 12, 1)) + }); + c.bench_function("PLL::update(t, sf, sp)", |b| { + b.iter(|| dut.update(black_box(0x241), black_box(21), black_box(20))) + }); +} + +fn iir_int_bench(c: &mut Criterion) { + let dut = iir_int::IIR::default(); + let mut xy = iir_int::Vec5::default(); + c.bench_function("int_iir::IIR::update(s, x)", |b| { + b.iter(|| dut.update(&mut xy, black_box(0x2832))) + }); +} + +fn iir_bench(c: &mut Criterion) { + let dut = iir::IIR::default(); + let mut xy = iir::Vec5::default(); + c.bench_function("int::IIR::update(s, x)", |b| { + b.iter(|| dut.update(&mut xy, black_box(0.32241))) + }); +} + +criterion_group!(trig, atan2_bench, cossin_bench); +criterion_group!(pll, rpll_bench, pll_bench); +criterion_group!(iir, iir_int_bench, iir_bench); +criterion_main!(trig, pll, iir); diff --git a/dsp/benches/trig.rs b/dsp/benches/trig.rs deleted file mode 100644 index df17a65..0000000 --- a/dsp/benches/trig.rs +++ /dev/null @@ -1,28 +0,0 @@ -use core::f32::consts::PI; -use criterion::{black_box, criterion_group, criterion_main, Criterion}; -use dsp::{atan2, cossin}; - -fn atan2_bench(c: &mut Criterion) { - let xi = (10 << 16) as i32; - let xf = xi as f32 / i32::MAX as f32; - - let yi = (-26_328 << 16) as i32; - let yf = yi as f32 / i32::MAX as f32; - - c.bench_function("atan2(y, x)", |b| { - b.iter(|| atan2(black_box(yi), black_box(xi))) - }); - c.bench_function("y.atan2(x)", |b| { - b.iter(|| black_box(yf).atan2(black_box(xf))) - }); -} - -fn cossin_bench(c: &mut Criterion) { - let zi = -0x7304_2531_i32; - let zf = zi as f32 / i32::MAX as f32 * PI; - c.bench_function("cossin(zi)", |b| b.iter(|| cossin(black_box(zi)))); - c.bench_function("zf.sin_cos()", |b| b.iter(|| black_box(zf).sin_cos())); -} - -criterion_group!(benches, atan2_bench, cossin_bench); -criterion_main!(benches); diff --git a/dsp/src/accu.rs b/dsp/src/accu.rs new file mode 100644 index 0000000..954c35f --- /dev/null +++ b/dsp/src/accu.rs @@ -0,0 +1,20 @@ +#[derive(Copy, Clone, Default, PartialEq, Debug)] +pub struct Accu { + state: i32, + step: i32, +} + +impl Accu { + pub fn new(state: i32, step: i32) -> Self { + Self { state, step } + } +} + +impl Iterator for Accu { + type Item = i32; + fn next(&mut self) -> Option { + let s = self.state; + self.state = self.state.wrapping_add(self.step); + Some(s) + } +} diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs index 4366b43..256cde2 100644 --- a/dsp/src/complex.rs +++ b/dsp/src/complex.rs @@ -1,17 +1,57 @@ -use super::atan2; -use serde::{Deserialize, Serialize}; +use super::{atan2, cossin}; -#[derive(Copy, Clone, Default, Deserialize, Serialize)] +#[derive(Copy, Clone, Default, PartialEq, Debug)] pub struct Complex(pub T, pub T); impl Complex { - pub fn power(&self) -> i32 { - (((self.0 as i64) * (self.0 as i64) - + (self.1 as i64) * (self.1 as i64)) - >> 32) as i32 + /// Return a Complex on the unit circle given an angle. + /// + /// Example: + /// + /// ``` + /// use dsp::Complex; + /// Complex::::from_angle(0); + /// Complex::::from_angle(1 << 30); // pi/2 + /// Complex::::from_angle(-1 << 30); // -pi/2 + /// ``` + pub fn from_angle(angle: i32) -> Self { + let (c, s) = cossin(angle); + Self(c, s) } - pub fn phase(&self) -> i32 { + /// Return the absolute square (the squared magnitude). + /// + /// Note: Normalization is `1 << 31`, i.e. Q0.31. + /// + /// Example: + /// + /// ``` + /// use dsp::Complex; + /// assert_eq!(Complex(i32::MAX, 0).abs_sqr(), i32::MAX - 1); + /// assert_eq!(Complex(i32::MIN + 1, 0).abs_sqr(), i32::MAX - 1); + /// ``` + pub fn abs_sqr(&self) -> i32 { + (((self.0 as i64) * (self.0 as i64) + + (self.1 as i64) * (self.1 as i64)) + >> 31) as i32 + } + + /// Return the angle. + /// + /// Note: Normalization is `1 << 31 == pi`. + /// + /// Example: + /// + /// ``` + /// use dsp::Complex; + /// assert_eq!(Complex(i32::MAX, 0).arg(), 0); + /// assert_eq!(Complex(-i32::MAX, 1).arg(), i32::MAX); + /// assert_eq!(Complex(-i32::MAX, -1).arg(), -i32::MAX); + /// assert_eq!(Complex(0, -i32::MAX).arg(), -i32::MAX >> 1); + /// assert_eq!(Complex(0, i32::MAX).arg(), (i32::MAX >> 1) + 1); + /// assert_eq!(Complex(i32::MAX, i32::MAX).arg(), (i32::MAX >> 2) + 1); + /// ``` + pub fn arg(&self) -> i32 { atan2(self.1, self.0) } } diff --git a/dsp/src/cossin.rs b/dsp/src/cossin.rs index f9cc42e..e5746a3 100644 --- a/dsp/src/cossin.rs +++ b/dsp/src/cossin.rs @@ -1,4 +1,3 @@ -use super::Complex; use core::f64::consts::PI; include!(concat!(env!("OUT_DIR"), "/cossin_table.rs")); @@ -11,10 +10,10 @@ include!(concat!(env!("OUT_DIR"), "/cossin_table.rs")); /// * `phase` - 32-bit phase. /// /// # Returns -/// The cos and sin values of the provided phase as a `Complex` -/// value. With a 7-bit deep LUT there is 1e-5 max and 6e-8 RMS error +/// The cos and sin values of the provided phase as a `(i32, i32)` +/// tuple. With a 7-bit deep LUT there is 1e-5 max and 6e-8 RMS error /// in each quadrature over 20 bit phase. -pub fn cossin(phase: i32) -> Complex { +pub fn cossin(phase: i32) -> (i32, i32) { // Phase bits excluding the three highes MSB const OCTANT_BITS: usize = 32 - 3; @@ -69,12 +68,13 @@ pub fn cossin(phase: i32) -> Complex { sin *= -1; } - Complex(cos, sin) + (cos, sin) } #[cfg(test)] mod tests { use super::*; + use crate::Complex; use core::f64::consts::PI; #[test] diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index 398a900..4206c2b 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -3,8 +3,6 @@ use serde::{Deserialize, Serialize}; use super::{abs, copysign, macc, max, min}; use core::f32; -use miniconf::StringSet; - /// IIR state and coefficients type. /// /// To represent the IIR state (input and output memory) during the filter update @@ -13,7 +11,8 @@ use miniconf::StringSet; /// To represent the IIR coefficients, this contains the feed-forward /// coefficients (b0, b1, b2) followd by the negated feed-back coefficients /// (-a1, -a2), all five normalized such that a0 = 1. -pub type IIRState = [f32; 5]; +#[derive(Copy, Clone, Default, Deserialize, Serialize)] +pub struct Vec5(pub [f32; 5]); /// IIR configuration. /// @@ -40,15 +39,24 @@ pub type IIRState = [f32; 5]; /// 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, Debug, Default, Deserialize, Serialize, StringSet)] +#[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct IIR { - pub ba: IIRState, + pub ba: Vec5, pub y_offset: f32, pub y_min: f32, pub y_max: f32, } impl IIR { + pub const fn new(gain: f32, y_min: f32, y_max: f32) -> Self { + Self { + ba: Vec5([gain, 0., 0., 0., 0.]), + y_offset: 0., + y_min, + y_max, + } + } + /// Configures IIR filter coefficients for proportional-integral behavior /// with gain limit. /// @@ -76,13 +84,13 @@ impl IIR { } (a1, b0, b1) }; - self.ba.copy_from_slice(&[b0, b1, 0., a1, 0.]); + self.ba.0.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() + self.ba.0[..3].iter().sum() } /// Compute input-referred (`x`) offset from output (`y`) offset. @@ -109,22 +117,22 @@ impl IIR { /// # Arguments /// * `xy` - Current filter state. /// * `x0` - New input. - pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 { - let n = self.ba.len(); - debug_assert!(xy.len() == n); + pub fn update(&self, xy: &mut Vec5, x0: f32) -> f32 { + 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(self.y_offset, xy, &self.ba); + let y0 = macc(self.y_offset, &xy.0, &self.ba.0); // Limit y0 let y0 = max(self.y_min, min(self.y_max, y0)); // Store y0 x0 x1 y0 y1 y2 - xy[n / 2] = y0; + xy.0[n / 2] = y0; y0 } } diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs index 64ab175..daff43c 100644 --- a/dsp/src/iir_int.rs +++ b/dsp/src/iir_int.rs @@ -5,9 +5,9 @@ use serde::{Deserialize, Serialize}; /// This struct is used to hold the x/y input/output data vector or the b/a coefficient /// vector. #[derive(Copy, Clone, Default, Deserialize, Serialize)] -pub struct IIRState(pub [i32; 5]); +pub struct Vec5(pub [i32; 5]); -impl IIRState { +impl Vec5 { /// Lowpass biquad filter using cutoff and sampling frequencies. Taken from: /// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html /// @@ -19,11 +19,12 @@ impl IIRState { /// /// # Returns /// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1. - pub fn lowpass(f: f32, q: f32, k: f32) -> IIRState { + pub fn lowpass(f: f32, q: f32, k: f32) -> Self { // 3rd order Taylor approximation of sin and cos. let f = f * 2. * PI; - let fsin = f - f * f * f / 6.; - let fcos = 1. - f * f / 2.; + let f2 = f * f * 0.5; + let fcos = 1. - f2; + let fsin = f * (1. - f2 / 3.); let alpha = fsin / (2. * q); // IIR uses Q2.30 fixed point let a0 = (1. + alpha) / (1 << IIR::SHIFT) as f32; @@ -31,7 +32,7 @@ impl IIRState { let a1 = (2. * fcos / a0) as _; let a2 = ((alpha - 1.) / a0) as _; - IIRState([b0, 2 * b0, b0, a1, a2]) + Self([b0, 2 * b0, b0, a1, a2]) } } @@ -53,7 +54,7 @@ fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 { /// Coefficient scaling fixed and optimized. #[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct IIR { - pub ba: IIRState, + pub ba: Vec5, // pub y_offset: i32, // pub y_min: i32, // pub y_max: i32, @@ -70,7 +71,7 @@ impl IIR { /// # Arguments /// * `xy` - Current filter state. /// * `x0` - New input. - pub fn update(&self, xy: &mut IIRState, x0: i32) -> i32 { + pub fn update(&self, xy: &mut Vec5, x0: i32) -> i32 { let n = self.ba.0.len(); debug_assert!(xy.0.len() == n); // `xy` contains x0 x1 y0 y1 y2 @@ -92,11 +93,11 @@ impl IIR { #[cfg(test)] mod test { - use super::IIRState; + use super::Vec5; #[test] fn lowpass_gen() { - let ba = IIRState::lowpass(1e-3, 1. / 2f32.sqrt(), 2.); + let ba = Vec5::lowpass(1e-3, 1. / 2f32.sqrt(), 2.); println!("{:?}", ba.0); } } diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 191054a..f42e7c7 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -3,38 +3,6 @@ use core::ops::{Add, Mul, Neg}; -/// Bit shift, round up half. -/// -/// # Arguments -/// -/// `x` - Value to shift and round. -/// `shift` - Number of bits to right shift `x`. -/// -/// # Returns -/// -/// Shifted and rounded value. -#[inline(always)] -pub fn shift_round(x: i32, shift: usize) -> i32 { - (x + (1 << (shift - 1))) >> shift -} - -/// Integer division, round up half. -/// -/// # Arguments -/// -/// `dividend` - Value to divide. -/// `divisor` - Value that divides the -/// dividend. `dividend`+`divisor`-1 must be inside [i64::MIN, -/// i64::MAX]. -/// -/// # Returns -/// -/// Divided and rounded value. -#[inline(always)] -pub fn divide_round(dividend: i64, divisor: i64) -> i64 { - (dividend + (divisor - 1)) / divisor -} - fn abs(x: T) -> T where T: PartialOrd + Default + Neg, @@ -112,6 +80,7 @@ where .fold(y0, |y, xa| y + xa) } +pub mod accu; mod atan2; mod complex; mod cossin; @@ -122,6 +91,7 @@ pub mod pll; pub mod rpll; pub mod unwrap; +pub use accu::Accu; pub use atan2::atan2; pub use complex::Complex; pub use cossin::cossin; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index aa397bb..ea80836 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -1,160 +1,41 @@ -use super::{cossin, iir_int, Complex}; +use super::{ + iir_int::{Vec5, IIR}, + Complex, +}; use serde::{Deserialize, Serialize}; #[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct Lockin { - iir: iir_int::IIR, - iir_state: [iir_int::IIRState; 2], + iir: IIR, + state: [Vec5; 2], } impl Lockin { - pub fn new(ba: &iir_int::IIRState) -> Self { - let mut iir = iir_int::IIR::default(); - iir.ba.0.copy_from_slice(&ba.0); - Lockin { - iir, - iir_state: [iir_int::IIRState::default(); 2], + /// Create a new Lockin with given IIR coefficients. + pub fn new(ba: Vec5) -> Self { + Self { + iir: IIR { ba }, + state: [Vec5::default(); 2], } } - pub fn update(&mut self, signal: i32, phase: i32) -> Complex { + /// Update the lockin with a sample taken at a given phase. + pub fn update(&mut self, sample: i32, phase: i32) -> Complex { // Get the LO signal for demodulation. - let m = cossin(phase); + let lo = Complex::from_angle(phase); // Mix with the LO signal, filter with the IIR lowpass, // return IQ (in-phase and quadrature) data. // Note: 32x32 -> 64 bit multiplications are pretty much free. Complex( self.iir.update( - &mut self.iir_state[0], - ((signal as i64 * m.0 as i64) >> 32) as _, + &mut self.state[0], + ((sample as i64 * lo.0 as i64) >> 32) as _, ), self.iir.update( - &mut self.iir_state[1], - ((signal as i64 * m.1 as i64) >> 32) as _, + &mut self.state[1], + ((sample as i64 * lo.1 as i64) >> 32) as _, ), ) } } - -#[cfg(test)] -mod test { - use crate::{ - atan2, - iir_int::IIRState, - lockin::Lockin, - rpll::RPLL, - testing::{isclose, max_error}, - Complex, - }; - - use std::f64::consts::PI; - use std::vec::Vec; - - /// ADC full scale in machine units (16 bit signed). - const ADC_SCALE: f64 = ((1 << 15) - 1) as _; - - struct PllLockin { - harmonic: i32, - phase: i32, - lockin: Lockin, - } - - impl PllLockin { - pub fn new(harmonic: i32, phase: i32, iir: &IIRState) -> Self { - PllLockin { - harmonic, - phase, - lockin: Lockin::new(iir), - } - } - - pub fn update( - &mut self, - input: Vec, - phase: i32, - frequency: i32, - ) -> Complex { - let sample_frequency = frequency.wrapping_mul(self.harmonic); - let mut sample_phase = - self.phase.wrapping_add(phase.wrapping_mul(self.harmonic)); - input - .iter() - .map(|&s| { - let input = (s as i32) << 16; - let signal = - self.lockin.update(input, sample_phase.wrapping_neg()); - sample_phase = sample_phase.wrapping_add(sample_frequency); - signal - }) - .last() - .unwrap_or(Complex::default()) - } - } - - /// Single-frequency sinusoid. - #[derive(Copy, Clone)] - struct Tone { - // Frequency (in Hz). - frequency: f64, - // Phase offset (in radians). - phase: f64, - // Amplitude in dBFS (decibels relative to full-scale). - // A 16-bit ADC has a minimum dBFS for each sample of -90. - amplitude_dbfs: f64, - } - - /// Convert dBFS to a linear ratio. - fn linear(dbfs: f64) -> f64 { - 10f64.powf(dbfs / 20.) - } - - impl Tone { - fn eval(&self, time: f64) -> f64 { - linear(self.amplitude_dbfs) - * (self.phase + self.frequency * time).cos() - } - } - - /// Generate a full batch of samples with size `sample_buffer_size` starting at `time_offset`. - fn sample_tones( - tones: &Vec, - time_offset: f64, - sample_buffer_size: u32, - ) -> Vec { - (0..sample_buffer_size) - .map(|i| { - let time = 2. * PI * (time_offset + i as f64); - let x: f64 = tones.iter().map(|t| t.eval(time)).sum(); - assert!(-1. < x && x < 1.); - (x * ADC_SCALE) as i16 - }) - .collect() - } - - /// Total maximum noise amplitude of the input signal after 2nd order lowpass filter. - /// Constructive interference is assumed. - /// - /// # Args - /// * `tones` - Noise sources at the ADC input. - /// * `frequency` - Frequency of the signal of interest. - /// * `corner` - Low-pass filter 3dB corner cutoff frequency. - /// - /// # Returns - /// Upper bound of the total amplitude of all noise sources in linear units full scale. - fn sampled_noise_amplitude( - tones: &Vec, - frequency: f64, - corner: f64, - ) -> f64 { - tones - .iter() - .map(|t| { - let df = (t.frequency - frequency) / corner; - // Assuming a 2nd order lowpass filter: 40dB/decade. - linear(t.amplitude_dbfs - 40. * df.abs().max(1.).log10()) - }) - .sum::() - .max(1. / ADC_SCALE / 2.) // 1/2 LSB from quantization - } -} diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index 2bfd29c..d4bd86f 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -3,13 +3,14 @@ /// Consumes noisy, quantized timestamps of a reference signal and reconstructs /// the phase and frequency of the update() invocations with respect to (and in units of /// 1 << 32 of) that reference. +/// In other words, `update()` rate ralative to reference frequency, +/// `u32::MAX` corresponding to both being equal. #[derive(Copy, Clone, Default)] pub struct RPLL { dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio - t: i32, // current counter time x: i32, // previous timestamp - ff: i32, // current frequency estimate from frequency loop - f: i32, // current frequency estimate from both frequency and phase loop + ff: u32, // current frequency estimate from frequency loop + f: u32, // current frequency estimate from both frequency and phase loop y: i32, // current phase estimate } @@ -18,14 +19,12 @@ impl RPLL { /// /// Args: /// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio. - /// * t: Counter time. Counter value at the first update() call. Typically 0. /// /// Returns: /// Initialized RPLL instance. - pub fn new(dt2: u8, t: i32) -> RPLL { - RPLL { + pub fn new(dt2: u8) -> Self { + Self { dt2, - t, ..Default::default() } } @@ -43,42 +42,230 @@ impl RPLL { /// /// Returns: /// A tuple containing the current phase (wrapping at the i32 boundary, pi) and - /// frequency (wrapping at the i32 boundary, Nyquist) estimate. + /// frequency. pub fn update( &mut self, input: Option, shift_frequency: u8, shift_phase: u8, - ) -> (i32, i32) { - debug_assert!(shift_frequency > self.dt2); - debug_assert!(shift_phase > self.dt2); + ) -> (i32, u32) { + debug_assert!(shift_frequency >= self.dt2); + debug_assert!(shift_phase >= self.dt2); // Advance phase - self.y = self.y.wrapping_add(self.f); + self.y = self.y.wrapping_add(self.f as i32); if let Some(x) = input { // Reference period in counter cycles let dx = x.wrapping_sub(self.x); // Store timestamp for next time. self.x = x; // Phase using the current frequency estimate - let p_sig_long = (self.ff as i64).wrapping_mul(dx as i64); + let p_sig_64 = self.ff as u64 * dx as u64; // Add half-up rounding bias and apply gain/attenuation - let p_sig = (p_sig_long.wrapping_add(1i64 << (shift_frequency - 1)) - >> shift_frequency) as i32; + let p_sig = ((p_sig_64 + (1u32 << (shift_frequency - 1)) as u64) + >> shift_frequency) as u32; // Reference phase (1 << dt2 full turns) with gain/attenuation applied - let p_ref = 1i32 << (32 + self.dt2 - shift_frequency); + let p_ref = 1u32 << (32 + self.dt2 - shift_frequency); // Update frequency lock self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig)); // Time in counter cycles between timestamp and "now" - let dt = self.t.wrapping_sub(x); + let dt = (x.wrapping_neg() & ((1 << self.dt2) - 1)) as u32; // Reference phase estimate "now" - let y_ref = (self.f >> self.dt2).wrapping_mul(dt); - // Phase error - let dy = y_ref.wrapping_sub(self.y); + let y_ref = (self.f >> self.dt2).wrapping_mul(dt) as i32; + // Phase error with gain + let dy = y_ref.wrapping_sub(self.y) >> (shift_phase - self.dt2); // Current frequency estimate from frequency lock and phase error - self.f = self.ff.wrapping_add(dy >> (shift_phase - self.dt2)); + self.f = self.ff.wrapping_add(dy as u32); } - // Advance time - self.t = self.t.wrapping_add(1 << self.dt2); (self.y, self.f) } } + +#[cfg(test)] +mod test { + use super::RPLL; + use ndarray::prelude::*; + use rand::{prelude::*, rngs::StdRng}; + use std::vec::Vec; + + #[test] + fn make() { + let _ = RPLL::new(8); + } + + struct Harness { + rpll: RPLL, + dt2: u8, + shift_frequency: u8, + shift_phase: u8, + noise: i32, + period: i32, + next: i32, + next_noisy: i32, + time: i32, + rng: StdRng, + } + + impl Harness { + fn default() -> Self { + Self { + rpll: RPLL::new(8), + dt2: 8, + shift_frequency: 9, + shift_phase: 8, + noise: 0, + period: 333, + next: 111, + next_noisy: 111, + time: 0, + rng: StdRng::seed_from_u64(42), + } + } + + fn run(&mut self, n: usize) -> (Vec, Vec) { + assert!(self.period >= 1 << self.dt2); + assert!(self.period < 1 << self.shift_frequency); + assert!(self.period < 1 << self.shift_phase + 1); + + let mut y = Vec::::new(); + let mut f = Vec::::new(); + for _ in 0..n { + let timestamp = if self.time - self.next_noisy >= 0 { + assert!(self.time - self.next_noisy < 1 << self.dt2); + self.next = self.next.wrapping_add(self.period); + let timestamp = self.next_noisy; + let p_noise = self.rng.gen_range(-self.noise..=self.noise); + self.next_noisy = self.next.wrapping_add(p_noise); + Some(timestamp) + } else { + None + }; + let (yi, fi) = self.rpll.update( + timestamp, + self.shift_frequency, + self.shift_phase, + ); + + let y_ref = (self.time.wrapping_sub(self.next) as i64 + * (1i64 << 32) + / self.period as i64) as i32; + // phase error + y.push(yi.wrapping_sub(y_ref) as f32 / 2f32.powi(32)); + + let p_ref = 1 << 32 + self.dt2; + let p_sig = fi as u64 * self.period as u64; + // relative frequency error + f.push( + p_sig.wrapping_sub(p_ref) as i64 as f32 + / 2f32.powi(32 + self.dt2 as i32), + ); + + // advance time + self.time = self.time.wrapping_add(1 << self.dt2); + } + (y, f) + } + + fn measure(&mut self, n: usize, limits: [f32; 4]) { + let t_settle = (1 << self.shift_frequency - self.dt2 + 4) + + (1 << self.shift_phase - self.dt2 + 4); + self.run(t_settle); + + let (y, f) = self.run(n); + let y = Array::from(y); + let f = Array::from(f); + // println!("{:?} {:?}", f, y); + + let fm = f.mean().unwrap(); + let fs = f.std_axis(Axis(0), 0.).into_scalar(); + let ym = y.mean().unwrap(); + let ys = y.std_axis(Axis(0), 0.).into_scalar(); + + println!("f: {:.2e}±{:.2e}; y: {:.2e}±{:.2e}", fm, fs, ym, ys); + + let m = [fm, fs, ym, ys]; + + print!("relative: "); + for i in 0..m.len() { + let rel = m[i].abs() / limits[i].abs(); + print!("{:.2e} ", rel); + assert!( + rel <= 1., + "idx {}, have |{:.2e}| > limit {:.2e}", + i, + m[i], + limits[i] + ); + } + println!(); + } + } + + #[test] + fn default() { + let mut h = Harness::default(); + + h.measure(1 << 16, [1e-11, 4e-8, 2e-8, 2e-8]); + } + + #[test] + fn noisy() { + let mut h = Harness::default(); + h.noise = 10; + h.shift_frequency = 23; + h.shift_phase = 22; + + h.measure(1 << 16, [3e-9, 3e-6, 4e-4, 2e-4]); + } + + #[test] + fn narrow_fast() { + let mut h = Harness::default(); + h.period = 990; + h.next = 351; + h.next_noisy = h.next; + h.noise = 5; + h.shift_frequency = 23; + h.shift_phase = 22; + + h.measure(1 << 16, [2e-9, 2e-6, 1e-3, 1e-4]); + } + + #[test] + fn narrow_slow() { + let mut h = Harness::default(); + h.period = 1818181; + h.next = 35281; + h.next_noisy = h.next; + h.noise = 1000; + h.shift_frequency = 23; + h.shift_phase = 22; + + h.measure(1 << 16, [2e-5, 6e-4, 2e-4, 2e-4]); + } + + #[test] + fn wide_fast() { + let mut h = Harness::default(); + h.period = 990; + h.next = 351; + h.next_noisy = h.next; + h.noise = 5; + h.shift_frequency = 10; + h.shift_phase = 9; + + h.measure(1 << 16, [5e-7, 3e-2, 2e-5, 2e-2]); + } + + #[test] + fn wide_slow() { + let mut h = Harness::default(); + h.period = 1818181; + h.next = 35281; + h.next_noisy = h.next; + h.noise = 1000; + h.shift_frequency = 21; + h.shift_phase = 20; + + h.measure(1 << 16, [2e-4, 6e-3, 2e-4, 2e-3]); + } +} diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs index 940a02c..e76020a 100644 --- a/src/bin/dual-iir.rs +++ b/src/bin/dual-iir.rs @@ -20,7 +20,7 @@ use hardware::{ Adc0Input, Adc1Input, Dac0Output, Dac1Output, NetworkStack, AFE0, AFE1, }; -const SCALE: f32 = ((1 << 15) - 1) as f32; +const SCALE: f32 = i16::MAX as _; // The number of cascaded IIR biquads per channel. Select 1 or 2! const IIR_CASCADE_LENGTH: usize = 1; @@ -47,9 +47,9 @@ const APP: () = { mqtt_interface: MqttInterface, // 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])] + #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])] + iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2], + #[init([[iir::IIR::new(1., -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2])] iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], } diff --git a/src/bin/lockin.rs b/src/bin/lockin-external.rs similarity index 65% rename from src/bin/lockin.rs rename to src/bin/lockin-external.rs index 1f0b6a1..bbe09a3 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin-external.rs @@ -15,12 +15,12 @@ use miniconf::{ }; use serde::Deserialize; -use dsp::{iir, iir_int, lockin::Lockin, rpll::RPLL}; +use dsp::{iir, iir_int, lockin::Lockin, rpll::RPLL, Accu}; use hardware::{ Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, }; -const SCALE: f32 = ((1 << 15) - 1) as f32; +const SCALE: f32 = i16::MAX as _; // The number of cascaded IIR biquads per channel. Select 1 or 2! const IIR_CASCADE_LENGTH: usize = 1; @@ -47,9 +47,9 @@ const APP: () = { mqtt_interface: MqttInterface, // 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])] + #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])] + iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2], + #[init([[iir::IIR::new(1./(1 << 16) as f32, -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2])] iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], timestamper: InputStamper, @@ -71,10 +71,10 @@ const APP: () = { ) .unwrap(); - let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2, 0); + let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2); let lockin = Lockin::new( - &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose + iir_int::Vec5::lowpass(1e-3, 0.707, 2.), // TODO: expose ); // Enable ADC/DAC events @@ -101,24 +101,13 @@ const APP: () = { } } - /// Main DSP processing routine for Stabilizer. + /// Main DSP processing routine. /// - /// # Note - /// Processing time for the DSP application code is bounded by the following constraints: + /// See `dual-iir` for general notes on processing time and timing. /// - /// 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 + /// This is an implementation of a externally (DI0) referenced PLL lockin on the ADC0 signal. + /// It outputs either I/Q or power/phase on DAC0/DAC1. Data is normalized to full scale. + /// PLL bandwidth, filter bandwidth, slope, and x/y or power/phase post-filters are available. #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch, lockin, timestamper, pll], priority=2)] fn process(c: process::Context) { let adc_samples = [ @@ -135,47 +124,67 @@ const APP: () = { let iir_state = c.resources.iir_state; let lockin = c.resources.lockin; + let timestamp = c + .resources + .timestamper + .latest_timestamp() + .unwrap_or_else(|t| t) // Ignore timer capture overflows. + .map(|t| t as i32); let (pll_phase, pll_frequency) = c.resources.pll.update( - c.resources.timestamper.latest_timestamp().map(|t| t as i32), - 22, // relative PLL frequency bandwidth: 2**-22, TODO: expose - 22, // relative PLL phase bandwidth: 2**-22, TODO: expose + timestamp, + 22, // frequency settling time (log2 counter cycles), TODO: expose + 22, // phase settling time, TODO: expose ); - // 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 >> SAMPLE_BUFFER_SIZE_LOG2).wrapping_mul(harmonic); - let mut sample_phase = + // Harmonic index of the LO: -1 to _de_modulate the fundamental (complex conjugate) + let harmonic: i32 = -1; // TODO: expose + // Demodulation LO phase offset + let phase_offset: i32 = 0; // TODO: expose + + let sample_frequency = ((pll_frequency + // .wrapping_add(1 << SAMPLE_BUFFER_SIZE_LOG2 - 1) // half-up rounding bias + >> SAMPLE_BUFFER_SIZE_LOG2) as i32) + .wrapping_mul(harmonic); + let sample_phase = phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic)); - for i in 0..adc_samples[0].len() { + let output = adc_samples[0] + .iter() + .zip(Accu::new(sample_phase, sample_frequency)) // 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); + .map(|(&sample, phase)| { + lockin.update((sample as i16 as i32) << 16, phase) + }) + .last() + .unwrap(); + // convert i/q to power/phase, + let power_phase = true; // TODO: expose + + let mut output = if power_phase { // Convert from IQ to power and phase. - let mut power = output.power() as _; - let mut phase = output.phase() as _; + [output.abs_sqr() as _, output.arg() as _] + } else { + [output.0 as _, output.1 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); + // 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() { + for k in 0..output.len() { + output[k] = + iir_ch[k][j].update(&mut iir_state[k][j], output[k]); } + } - // Note(unsafe): range clipping to i16 is ensured by IIR filters above. - // Convert to DAC data. + // Note(unsafe): range clipping to i16 is ensured by IIR filters above. + // Convert to DAC data. + for i in 0..dac_samples[0].len() { unsafe { dac_samples[0][i] = - power.to_int_unchecked::() as u16 ^ 0x8000; + output[0].to_int_unchecked::() as u16 ^ 0x8000; dac_samples[1][i] = - phase.to_int_unchecked::() as u16 ^ 0x8000; + output[1].to_int_unchecked::() as u16 ^ 0x8000; } } } diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs new file mode 100644 index 0000000..e14a4da --- /dev/null +++ b/src/bin/lockin-internal.rs @@ -0,0 +1,165 @@ +#![deny(warnings)] +#![no_std] +#![no_main] +#![cfg_attr(feature = "nightly", feature(core_intrinsics))] + +use dsp::{iir_int, lockin::Lockin, Accu}; +use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1}; +use stabilizer::{hardware, SAMPLE_BUFFER_SIZE, SAMPLE_BUFFER_SIZE_LOG2}; + +// A constant sinusoid to send on the DAC output. +// Full-scale gives a +/- 10V amplitude waveform. Scale it down to give +/- 1V. +const ONE: i16 = (0.1 * u16::MAX as f32) as _; +const SQRT2: i16 = (ONE as f32 * 0.707) as _; +const DAC_SEQUENCE: [i16; SAMPLE_BUFFER_SIZE] = + [ONE, SQRT2, 0, -SQRT2, -ONE, -SQRT2, 0, SQRT2]; + +#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)] +const APP: () = { + struct Resources { + afes: (AFE0, AFE1), + adc1: Adc1Input, + dacs: (Dac0Output, Dac1Output), + + lockin: Lockin, + } + + #[init] + fn init(c: init::Context) -> init::LateResources { + // Configure the microcontroller + let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); + + let lockin = Lockin::new( + iir_int::Vec5::lowpass(1e-3, 0.707, 2.), // TODO: expose + ); + + // Enable ADC/DAC events + stabilizer.adcs.1.start(); + stabilizer.dacs.0.start(); + stabilizer.dacs.1.start(); + + // Start sampling ADCs. + stabilizer.adc_dac_timer.start(); + + init::LateResources { + lockin, + afes: stabilizer.afes, + adc1: stabilizer.adcs.1, + dacs: stabilizer.dacs, + } + } + + /// 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 + #[task(binds=DMA1_STR4, resources=[adc1, dacs, lockin], priority=2)] + fn process(c: process::Context) { + let lockin = c.resources.lockin; + let adc_samples = c.resources.adc1.acquire_buffer(); + let dac_samples = [ + c.resources.dacs.0.acquire_buffer(), + c.resources.dacs.1.acquire_buffer(), + ]; + + // DAC0 always generates a fixed sinusoidal output. + dac_samples[0] + .iter_mut() + .zip(DAC_SEQUENCE.iter()) + .for_each(|(d, s)| *d = *s as u16 ^ 0x8000); + + // Reference phase and frequency are known. + let pll_phase = 0; + let pll_frequency = 1i32 << (32 - SAMPLE_BUFFER_SIZE_LOG2); + + // Harmonic index of the LO: -1 to _de_modulate the fundamental + let harmonic: i32 = -1; + + // Demodulation LO phase offset + let phase_offset: i32 = (0.25 * i32::MAX as f32) as i32; + let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic); + let sample_phase = phase_offset + .wrapping_add((pll_phase as i32).wrapping_mul(harmonic)); + + let output = adc_samples + .iter() + // Zip in the LO phase. + .zip(Accu::new(sample_phase, sample_frequency)) + // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter) + .map(|(&sample, phase)| { + lockin.update((sample as i16 as i32) << 16, phase) + }) + // Decimate + .last() + .unwrap(); + + // convert i/q to power/phase, + let power_phase = true; // TODO: expose + + let output = if power_phase { + // Convert from IQ to power and phase. + [output.abs_sqr(), output.arg()] + } else { + [output.0, output.1] + }; + + for value in dac_samples[1].iter_mut() { + *value = (output[1] >> 16) as u16 ^ 0x8000; + } + } + + #[idle(resources=[afes])] + fn idle(_: idle::Context) -> ! { + loop { + // TODO: Implement network interface. + cortex_m::asm::wfi(); + } + } + + #[task(binds = ETH, priority = 1)] + fn eth(_: eth::Context) { + unsafe { stm32h7xx_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!("ADC1 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(); + } +}; diff --git a/src/hardware/adc.rs b/src/hardware/adc.rs index 188e436..1cb6c17 100644 --- a/src/hardware/adc.rs +++ b/src/hardware/adc.rs @@ -180,18 +180,21 @@ macro_rules! adc_input { hal::spi::Spi, PeripheralToMemory, &'static mut [u16; SAMPLE_BUFFER_SIZE], + hal::dma::DBTransfer, >, trigger_transfer: Transfer< hal::dma::dma::$trigger_stream, [< $spi CR >], MemoryToPeripheral, &'static mut [u32; 1], + hal::dma::DBTransfer, >, clear_transfer: Transfer< hal::dma::dma::$clear_stream, [< $spi IFCR >], MemoryToPeripheral, &'static mut [u32; 1], + hal::dma::DBTransfer, >, } @@ -239,6 +242,7 @@ macro_rules! adc_input { _, MemoryToPeripheral, _, + _, > = Transfer::init( clear_stream, [< $spi IFCR >]::new(clear_channel), @@ -276,6 +280,7 @@ macro_rules! adc_input { _, MemoryToPeripheral, _, + _, > = Transfer::init( trigger_stream, [< $spi CR >]::new(trigger_channel), @@ -306,7 +311,7 @@ macro_rules! adc_input { // The data transfer is always a transfer of data from the peripheral to a RAM // buffer. - let data_transfer: Transfer<_, _, PeripheralToMemory, _> = + let data_transfer: Transfer<_, _, PeripheralToMemory, _, _> = Transfer::init( data_stream, spi, diff --git a/src/hardware/configuration.rs b/src/hardware/configuration.rs index c2c8333..d7def67 100644 --- a/src/hardware/configuration.rs +++ b/src/hardware/configuration.rs @@ -161,7 +161,6 @@ pub fn setup( // Configure the timer to count at the designed tick rate. We will manually set the // period below. timer2.pause(); - timer2.reset_counter(); timer2.set_tick_freq(design_parameters::TIMER_FREQUENCY); let mut sampling_timer = timers::SamplingTimer::new(timer2); @@ -220,13 +219,15 @@ pub fn setup( timer5.pause(); timer5.set_tick_freq(design_parameters::TIMER_FREQUENCY); - // The timestamp timer must run at exactly a multiple of the sample timer based on the - // batch size. To accomodate this, we manually set the prescaler identical to the sample - // timer, but use a period that is longer. + // The timestamp timer runs at the counter cycle period as the sampling timers. + // To accomodate this, we manually set the prescaler identical to the sample + // timer, but use maximum overflow period. let mut timer = timers::TimestampTimer::new(timer5); - let period = digital_input_stamper::calculate_timestamp_timer_period(); - timer.set_period_ticks(period); + // TODO: Check hardware synchronization of timestamping and the sampling timers + // for phase shift determinism. + + timer.set_period_ticks(u32::MAX); timer }; @@ -517,11 +518,11 @@ pub fn setup( let store = unsafe { &mut NET_STORE }; store.ip_addrs[0] = smoltcp::wire::IpCidr::new( - smoltcp::wire::IpAddress::v4(10, 0, 16, 99), + smoltcp::wire::IpAddress::v4(10, 34, 16, 103), 24, ); - let default_v4_gw = smoltcp::wire::Ipv4Address::new(10, 0, 16, 1); + let default_v4_gw = smoltcp::wire::Ipv4Address::new(10, 34, 16, 1); let mut routes = smoltcp::iface::Routes::new(&mut store.routes_cache[..]); routes.add_default_ipv4_route(default_v4_gw).unwrap(); diff --git a/src/hardware/dac.rs b/src/hardware/dac.rs index 09d0497..5ca65fb 100644 --- a/src/hardware/dac.rs +++ b/src/hardware/dac.rs @@ -122,6 +122,7 @@ macro_rules! dac_output { $spi, MemoryToPeripheral, &'static mut [u16; SAMPLE_BUFFER_SIZE], + hal::dma::DBTransfer, >, } @@ -164,7 +165,7 @@ macro_rules! dac_output { } // Construct the trigger stream to write from memory to the peripheral. - let transfer: Transfer<_, _, MemoryToPeripheral, _> = + let transfer: Transfer<_, _, MemoryToPeripheral, _, _> = Transfer::init( stream, $spi::new(trigger_channel, spi), diff --git a/src/hardware/digital_input_stamper.rs b/src/hardware/digital_input_stamper.rs index 4262800..6bd8629 100644 --- a/src/hardware/digital_input_stamper.rs +++ b/src/hardware/digital_input_stamper.rs @@ -25,42 +25,6 @@ ///! This module only supports DI0 for timestamping due to trigger constraints on the DIx pins. If ///! timestamping is desired in DI1, a separate timer + capture channel will be necessary. use super::{hal, timers}; -use crate::{ADC_SAMPLE_TICKS, SAMPLE_BUFFER_SIZE}; - -/// Calculate the period of the digital input timestamp timer. -/// -/// # Note -/// The period returned will be 1 less than the required period in timer ticks. The value returned -/// can be immediately programmed into a hardware timer period register. -/// -/// The period is calculated to be some power-of-two multiple of the batch size, such that N batches -/// will occur between each timestamp timer overflow. -/// -/// # Returns -/// A 32-bit value that can be programmed into a hardware timer period register. -pub fn calculate_timestamp_timer_period() -> u32 { - // Calculate how long a single batch requires in timer ticks. - let batch_duration_ticks: u64 = - SAMPLE_BUFFER_SIZE as u64 * ADC_SAMPLE_TICKS as u64; - - // Calculate the largest power-of-two that is less than or equal to - // `batches_per_overflow`. This is completed by eliminating the least significant - // bits of the value until only the msb remains, which is always a power of two. - let batches_per_overflow: u64 = - (1u64 + u32::MAX as u64) / batch_duration_ticks; - let mut j = batches_per_overflow; - while (j & (j - 1)) != 0 { - j = j & (j - 1); - } - - // Once the number of batches per timestamp overflow is calculated, we can figure out the final - // period of the timestamp timer. The period is always 1 larger than the value configured in the - // register. - let period: u64 = batch_duration_ticks * j - 1u64; - assert!(period <= u32::MAX as u64); - - period as u32 -} /// The timestamper for DI0 reference clock inputs. pub struct InputStamper { @@ -98,15 +62,12 @@ impl InputStamper { /// Get the latest timestamp that has occurred. /// /// # Note - /// This function must be called sufficiently often. If an over-capture event occurs, this - /// function will panic, as this indicates a timestamp was inadvertently dropped. - /// - /// To prevent timestamp loss, the batch size and sampling rate must be adjusted such that at - /// most one timestamp will occur in each data processing cycle. + /// This function must be called at least as often as timestamps arrive. + /// If an over-capture event occurs, this function will clear the overflow, + /// and return a new timestamp of unknown recency an `Err()`. + /// Note that this indicates at least one timestamp was inadvertently dropped. #[allow(dead_code)] - pub fn latest_timestamp(&mut self) -> Option { - self.capture_channel - .latest_capture() - .expect("DI0 timestamp overrun") + pub fn latest_timestamp(&mut self) -> Result, Option> { + self.capture_channel.latest_capture() } } diff --git a/src/hardware/pounder/timestamp.rs b/src/hardware/pounder/timestamp.rs index 5ccf235..e97bc2c 100644 --- a/src/hardware/pounder/timestamp.rs +++ b/src/hardware/pounder/timestamp.rs @@ -43,6 +43,7 @@ pub struct Timestamper { timers::tim8::Channel1InputCapture, PeripheralToMemory, &'static mut [u16; SAMPLE_BUFFER_SIZE], + hal::dma::DBTransfer, >, } @@ -89,7 +90,7 @@ impl Timestamper { input_capture.listen_dma(); // The data transfer is always a transfer of data from the peripheral to a RAM buffer. - let data_transfer: Transfer<_, _, PeripheralToMemory, _> = + let data_transfer: Transfer<_, _, PeripheralToMemory, _, _> = Transfer::init( stream, input_capture, diff --git a/src/hardware/timers.rs b/src/hardware/timers.rs index 7199730..b686220 100644 --- a/src/hardware/timers.rs +++ b/src/hardware/timers.rs @@ -281,28 +281,26 @@ macro_rules! timer_channels { impl [< Channel $index InputCapture >] { /// Get the latest capture from the channel. #[allow(dead_code)] - pub fn latest_capture(&mut self) -> Result, ()> { + pub fn latest_capture(&mut self) -> Result, Option<$size>> { // Note(unsafe): This channel owns all access to the specific timer channel. // Only atomic operations on completed on the timer registers. let regs = unsafe { &*<$TY>::ptr() }; - let sr = regs.sr.read(); - let result = if sr.[< cc $index if >]().bit_is_set() { + let result = if regs.sr.read().[< cc $index if >]().bit_is_set() { // Read the capture value. Reading the captured value clears the flag in the // status register automatically. - let ccx = regs.[< ccr $index >].read(); - Some(ccx.ccr().bits()) + Some(regs.[< ccr $index >].read().ccr().bits()) } else { None }; // Read SR again to check for a potential over-capture. If there is an // overcapture, return an error. - if regs.sr.read().[< cc $index of >]().bit_is_clear() { - Ok(result) - } else { + if regs.sr.read().[< cc $index of >]().bit_is_set() { regs.sr.modify(|_, w| w.[< cc $index of >]().clear_bit()); - Err(()) + Err(result) + } else { + Ok(result) } }