diff --git a/Cargo.lock b/Cargo.lock index 34f7f66..4330a9f 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -16,9 +16,9 @@ dependencies = [ [[package]] name = "aligned" -version = "0.4.2" +version = "0.4.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "377e4c0ba83e4431b10df45c1d4666f178ea9c552cac93e60c3a88bf32785923" +checksum = "ee4508988c62edf04abd8d92897fca0c2995d907ce1dfeaf369dac3716a40685" dependencies = [ "as-slice", ] @@ -61,21 +61,21 @@ name = "bioz-firmware-rs" version = "0.1.0" dependencies = [ "bioz-icd-rs", - "bitflags 2.9.1", + "bitflags 2.10.0", "cortex-m", "cortex-m-rt", "defmt 1.0.1", "defmt-rtt", - "embassy-embedded-hal 0.4.0", + "embassy-embedded-hal", "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync 0.6.2", - "embassy-time", + "embassy-sync", + "embassy-time 0.4.0", "embassy-usb", "embedded-hal 1.0.0", "heapless 0.9.1", - "libm", + "num", "panic-probe", "postcard-rpc", "static_cell", @@ -117,9 +117,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] name = "bitflags" -version = "2.9.1" +version = "2.10.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1b8e56985ec62d17e9c1001dc89c88ecd7dc08e47eba5ec7c29c7b5eeecde967" +checksum = "812e12b5285cc515a9c72a5c1d3b6d46a19dac5acfef5265968c166106e31dd3" [[package]] name = "block-device-driver" @@ -268,9 +268,9 @@ dependencies = [ [[package]] name = "defmt-rtt" -version = "1.0.0" +version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b2cac3b8a5644a9e02b75085ebad3b6deafdbdbdec04bb25086523828aa4dfd1" +checksum = "93d5a25c99d89c40f5676bec8cefe0614f17f0f40e916f98e345dae941807f9e" dependencies = [ "critical-section", "defmt 1.0.1", @@ -287,33 +287,15 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" -version = "0.3.1" +version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8578db196d74db92efdd5ebc546736dac1685499ee245b22eff92fa5e4b57945" +checksum = "554e3e840696f54b4c9afcf28a0f24da431c927f4151040020416e7393d6d0d8" dependencies = [ "defmt 1.0.1", "embassy-futures", - "embassy-hal-internal 0.3.0", - "embassy-sync 0.7.0", - "embassy-time", - "embedded-hal 0.2.7", - "embedded-hal 1.0.0", - "embedded-hal-async", - "embedded-storage", - "embedded-storage-async", - "nb 1.1.0", -] - -[[package]] -name = "embassy-embedded-hal" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d1611b7a7ab5d1fbed84c338df26d56fd9bded58006ebb029075112ed2c5e039" -dependencies = [ - "defmt 1.0.1", - "embassy-futures", - "embassy-hal-internal 0.3.0", - "embassy-sync 0.7.0", + "embassy-hal-internal", + "embassy-sync", + "embassy-time 0.5.0", "embedded-hal 0.2.7", "embedded-hal 1.0.0", "embedded-hal-async", @@ -324,22 +306,23 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.7.0" +version = "0.9.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90327bcc66333a507f89ecc4e2d911b265c45f5c9bc241f98eee076752d35ac6" +checksum = "06070468370195e0e86f241c8e5004356d696590a678d47d6676795b2e439c6b" dependencies = [ "cortex-m", "critical-section", - "defmt 0.3.100", + "defmt 1.0.1", "document-features", "embassy-executor-macros", + "embassy-executor-timer-queue", ] [[package]] name = "embassy-executor-macros" -version = "0.6.2" +version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3577b1e9446f61381179a330fc5324b01d511624c55f25e3c66c9e3c626dbecf" +checksum = "dfdddc3a04226828316bf31393b6903ee162238576b1584ee2669af215d55472" dependencies = [ "darling", "proc-macro2", @@ -348,22 +331,16 @@ dependencies = [ ] [[package]] -name = "embassy-futures" -version = "0.1.1" +name = "embassy-executor-timer-queue" +version = "0.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1f878075b9794c1e4ac788c95b728f26aa6366d32eeb10c7051389f898f7d067" +checksum = "2fc328bf943af66b80b98755db9106bf7e7471b0cf47dc8559cd9a6be504cc9c" [[package]] -name = "embassy-hal-internal" -version = "0.2.0" +name = "embassy-futures" +version = "0.1.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0ef3bac31ec146321248a169e9c7b5799f1e0b3829c7a9b324cb4600a7438f59" -dependencies = [ - "cortex-m", - "critical-section", - "defmt 0.3.100", - "num-traits", -] +checksum = "dc2d050bdc5c21e0862a89256ed8029ae6c290a93aecefc73084b3002cdebb01" [[package]] name = "embassy-hal-internal" @@ -371,6 +348,9 @@ version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "95285007a91b619dc9f26ea8f55452aa6c60f7115a4edc05085cd2bd3127cd7a" dependencies = [ + "cortex-m", + "critical-section", + "defmt 1.0.1", "num-traits", ] @@ -385,37 +365,37 @@ dependencies = [ [[package]] name = "embassy-net-driver-channel" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "25a567ab50319d866ad5e6c583ed665ba9b07865389644d3d82e45bf1497c934" +checksum = "b7b2739fbcf6cd206ae08779c7d709087b16577d255f2ea4a45bc4bbbf305b3f" dependencies = [ "embassy-futures", "embassy-net-driver", - "embassy-sync 0.7.0", + "embassy-sync", ] [[package]] name = "embassy-stm32" -version = "0.2.0" +version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e1e0bb733acdddbc7097765a47ce80bde2385647cf1d8427331931e06cff9a87" +checksum = "0d972eab325cc96afee98f80a91ca6b00249b6356dc0fdbff68b70c200df9fae" dependencies = [ "aligned", "bit_field", - "bitflags 2.9.1", + "bitflags 2.10.0", "block-device-driver", "cfg-if", "cortex-m", "cortex-m-rt", "critical-section", - "defmt 0.3.100", + "defmt 1.0.1", "document-features", - "embassy-embedded-hal 0.3.1", + "embassy-embedded-hal", "embassy-futures", - "embassy-hal-internal 0.2.0", + "embassy-hal-internal", "embassy-net-driver", - "embassy-sync 0.6.2", - "embassy-time", + "embassy-sync", + "embassy-time 0.5.0", "embassy-time-driver", "embassy-time-queue-utils", "embassy-usb-driver", @@ -433,7 +413,8 @@ dependencies = [ "nb 1.1.0", "proc-macro2", "quote", - "rand_core", + "rand_core 0.6.4", + "rand_core 0.9.5", "sdio-host", "static_assertions", "stm32-fmc", @@ -444,30 +425,16 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.6.2" +version = "0.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8d2c8cdff05a7a51ba0087489ea44b0b1d97a296ca6b1d6d1a33ea7423d34049" +checksum = "73974a3edbd0bd286759b3d483540f0ebef705919a5f56f4fc7709066f71689b" dependencies = [ "cfg-if", "critical-section", - "defmt 0.3.100", + "defmt 1.0.1", "embedded-io-async", + "futures-core", "futures-sink", - "futures-util", - "heapless 0.8.0", -] - -[[package]] -name = "embassy-sync" -version = "0.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cef1a8a1ea892f9b656de0295532ac5d8067e9830d49ec75076291fd6066b136" -dependencies = [ - "cfg-if", - "critical-section", - "embedded-io-async", - "futures-sink", - "futures-util", "heapless 0.8.0", ] @@ -489,35 +456,53 @@ dependencies = [ ] [[package]] -name = "embassy-time-driver" -version = "0.2.0" +name = "embassy-time" +version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8d45f5d833b6d98bd2aab0c2de70b18bfaa10faf661a1578fd8e5dfb15eb7eba" +checksum = "f4fa65b9284d974dad7a23bb72835c4ec85c0b540d86af7fc4098c88cff51d65" +dependencies = [ + "cfg-if", + "critical-section", + "defmt 1.0.1", + "document-features", + "embassy-time-driver", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "futures-core", +] + +[[package]] +name = "embassy-time-driver" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a0a244c7dc22c8d0289379c8d8830cae06bb93d8f990194d0de5efb3b5ae7ba6" dependencies = [ "document-features", ] [[package]] name = "embassy-time-queue-utils" -version = "0.1.0" +version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc55c748d16908a65b166d09ce976575fb8852cf60ccd06174092b41064d8f83" +checksum = "80e2ee86063bd028a420a5fb5898c18c87a8898026da1d4c852af2c443d0a454" dependencies = [ - "embassy-executor", + "embassy-executor-timer-queue", "heapless 0.8.0", ] [[package]] name = "embassy-usb" -version = "0.4.0" +version = "0.5.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6e651b9b7b47b514e6e6d1940a6e2e300891a2c33641917130643602a0cb6386" +checksum = "dc4462e48b19a4f401a11901bdd981aab80c6a826608016a0bdc73cbbab31954" dependencies = [ - "defmt 0.3.100", + "defmt 1.0.1", "embassy-futures", "embassy-net-driver-channel", - "embassy-sync 0.6.2", + "embassy-sync", "embassy-usb-driver", + "embedded-io-async", "heapless 0.8.0", "ssmarshal", "usbd-hid", @@ -525,22 +510,23 @@ dependencies = [ [[package]] name = "embassy-usb-driver" -version = "0.1.0" +version = "0.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4fc247028eae04174b6635104a35b1ed336aabef4654f5e87a8f32327d231970" +checksum = "17119855ccc2d1f7470a39756b12068454ae27a3eabb037d940b5c03d9c77b7a" dependencies = [ - "defmt 0.3.100", + "defmt 1.0.1", + "embedded-io-async", ] [[package]] name = "embassy-usb-synopsys-otg" -version = "0.2.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "08e753b23799329780c7ac434264026d0422044d6649ed70a73441b14a6436d7" +checksum = "288751f8eaa44a5cf2613f13cee0ca8e06e6638cb96e897e6834702c79084b23" dependencies = [ "critical-section", - "defmt 0.3.100", - "embassy-sync 0.6.2", + "defmt 1.0.1", + "embassy-sync", "embassy-usb-driver", ] @@ -775,6 +761,58 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" +[[package]] +name = "num" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "35bd024e8b2ff75562e5f34e7f4905839deb4b22955ef5e73d2fea1b9813cb23" +dependencies = [ + "num-complex", + "num-integer", + "num-iter", + "num-rational", + "num-traits", +] + +[[package]] +name = "num-complex" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-integer" +version = "0.1.46" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-iter" +version = "0.1.45" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1429034a0490724d0075ebb2bc9e875d6503c3cf69e235a8941aa757d83ef5bf" +dependencies = [ + "autocfg", + "num-integer", + "num-traits", +] + +[[package]] +name = "num-rational" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" +dependencies = [ + "num-integer", + "num-traits", +] + [[package]] name = "num-traits" version = "0.2.19" @@ -782,6 +820,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" dependencies = [ "autocfg", + "libm", ] [[package]] @@ -842,18 +881,18 @@ dependencies = [ [[package]] name = "postcard-rpc" -version = "0.11.15" +version = "0.12.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c7e1944dfb9859e440511700c442edce3eacd5862f90f5a9997d004bd3553f3b" +checksum = "d1b0f9cc8bd8f38f5893fb7f07626b1c8722b3166ead66cfb14b5c5d5352822c" dependencies = [ - "defmt 0.3.100", + "defmt 1.0.1", "embassy-executor", "embassy-futures", - "embassy-sync 0.6.2", - "embassy-time", + "embassy-sync", + "embassy-time 0.5.0", "embassy-usb", "embassy-usb-driver", - "heapless 0.8.0", + "heapless 0.9.1", "portable-atomic", "postcard", "postcard-schema", @@ -919,6 +958,12 @@ version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +[[package]] +name = "rand_core" +version = "0.9.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "76afc826de14238e6e8c374ddcc1fa19e374fd8dd986b0d2af0d02377261d83c" + [[package]] name = "rustc_version" version = "0.2.3" @@ -945,9 +990,9 @@ checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" [[package]] name = "sdio-host" -version = "0.5.0" +version = "0.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" +checksum = "b328e2cb950eeccd55b7f55c3a963691455dcd044cfb5354f0c5e68d2c2d6ee2" [[package]] name = "semver" @@ -1051,12 +1096,13 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "16.0.0" +version = "18.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc520f60f6653a32479a95b9180b33908f0cbbdf106609465ee7dea98f4f5b37" +checksum = "6fd8ec3a292a0d9fc4798416a61b21da5ae50341b2e7b8d12e662bf305366097" dependencies = [ "cortex-m", "cortex-m-rt", + "defmt 0.3.100", ] [[package]] diff --git a/Cargo.toml b/Cargo.toml index 8a1d981..57f4e7e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -5,24 +5,26 @@ edition = "2021" [dependencies] # Change stm32h563zi to your chip name, if necessary. -embassy-stm32 = { version = "0.2.0", features = ["defmt", "stm32h533re", "memory-x", "time-driver-any", "exti", "unstable-pac"] } -embassy-sync = { version = "0.6.0", features = ["defmt"] } -embassy-executor = { version = "0.7.0", features = ["arch-cortex-m", "executor-thread", "task-arena-size-32768", "defmt"] } +embassy-stm32 = { version = "0.4.0", features = ["defmt", "stm32h533re", "memory-x", "time-driver-any", "exti", "unstable-pac"] } +embassy-sync = { version = "0.7.2", features = ["defmt"] } +embassy-executor = { version = "0.9.1", features = ["arch-cortex-m", "executor-thread", "defmt"] } embassy-time = { version = "0.4.0", features = ["defmt", "defmt-timestamp-uptime", "tick-hz-32_768"] } -embassy-embedded-hal = { version = "0.4.0", features = ["defmt"]} +embassy-embedded-hal = { version = "0.5.0", features = ["defmt"]} # embassy-net = { version = "0.7.0", features = ["defmt", "tcp", "dhcpv4", "medium-ethernet", "proto-ipv6"] } -embassy-usb = { version = "0.4.0", features = ["defmt"] } +embassy-usb = { version = "0.5.1", features = ["defmt"] } embassy-futures = { version = "0.1.0"} defmt = "1.0.1" -defmt-rtt = "1.0.0" +defmt-rtt = "1.1.0" -bitflags = "2.9.1" +bitflags = "2.10.0" -postcard-rpc = {version = "0.11.13", features = ["embassy-usb-0_4-server", "defmt"]} +postcard-rpc = {version = "0.12.1", features = ["embassy-usb-0_5-server", "defmt"]} bioz-icd-rs = {path = "../bioz-icd-rs"} -libm = { version = "0.2.15" } +# libm = { version = "0.2.15" } + +num = {version = "0.4.3", default-features = false, features = ["libm"]} heapless = { version = "0.9.1" } diff --git a/src/ad5940.rs b/src/ad5940.rs index 78a746d..8ba8a06 100644 --- a/src/ad5940.rs +++ b/src/ad5940.rs @@ -68,9 +68,59 @@ impl SwitchConfig { } } +#[allow(dead_code)] +#[derive(Default)] +pub struct LpConfig { + data_reset: Option, + power_enable: Option, + data_6bit: Option, + common_mode_switch: Option, + tia_switches_enabled: Option, + tia_enable: Option, + filter_resistor: Option, +} + +impl LpConfig { + pub fn data_reset(&mut self, rsten: bool) -> &mut Self { + self.data_reset = Some(rsten); + self + } + + pub fn power_enable(&mut self, pwden: bool) -> &mut Self { + self.power_enable = Some(pwden); + self + } + + pub fn data_6bit(&mut self, data: u8) -> &mut Self { + self.data_6bit = Some(data & 0x3F); + self + } + + pub fn common_mode_enable(&mut self, enable: bool) -> &mut Self { + self.common_mode_switch = Some(enable); + self + } + + pub fn tia_switches_enabled(&mut self, enable: u16) -> &mut Self { + self.tia_switches_enabled = Some(enable); + self + } + + pub fn tia_enable(&mut self, enable: bool) -> &mut Self { + self.tia_enable = Some(enable); + self + } + + pub fn filter_resistor(&mut self, tiarf: TIARF) -> &mut Self { + self.filter_resistor = Some(tiarf); + self + } +} + #[allow(dead_code)] #[derive(Default)] pub struct DspConfig { + pub gnpgain: Option, muxseln: Option, muxselp: Option, ctiacon: Option, @@ -87,6 +137,11 @@ pub struct DspConfig { } impl DspConfig { + pub fn gnpgain(&mut self, gnpgain: GNPGAIN) -> &mut Self { + self.gnpgain = Some(gnpgain); + self + } + pub fn adc_mux_n(&mut self, muxseln: MUXSELN) -> &mut Self { self.muxseln = Some(muxseln); self @@ -588,10 +643,66 @@ impl AD5940 { Ok(()) } + pub async fn apply_lp_config(&mut self, config: &LpConfig) -> Result<(), Error> { + // LPDACCON0 + let mut current = self.read_reg(Register::LPDACCON0).await?; + + if let Some(data_reset) = config.data_reset { + current = RSTEN::apply(current, data_reset as u32); + } + + if let Some(power_enable) = config.power_enable { + current = PWDEN::apply(current, !power_enable as u32); + } + + self.write_reg(Register::LPDACCON0, current).await?; + + // LPDACDAT0 + let mut current = self.read_reg(Register::LPDACDAT0).await?; + + if let Some(data) = config.data_6bit { + current = DACIN6::apply(current, data as u32); + } + + self.write_reg(Register::LPDACDAT0, current).await?; + + // SWMUX + let mut current = self.read_reg(Register::SWMUX).await?; + + if let Some(common_mode_switch) = config.common_mode_switch { + current = SWMUX::apply(current, common_mode_switch as u32); + } + + self.write_reg(Register::SWMUX, current).await?; + + // LPTIASW0 + if let Some(tia_switches_enabled) = config.tia_switches_enabled { + self.write_reg(Register::LPTIASW0, tia_switches_enabled as u32).await?; + } + + // LPTIACON0 + let mut current = self.read_reg(Register::LPTIACON0).await?; + + if let Some(tia_enable) = config.tia_enable { + current = TIAPDEN::apply(current, !tia_enable as u32); + } + + if let Some(tiarf) = config.filter_resistor { + current = TIARF::apply(current, tiarf as u32); + } + + self.write_reg(Register::LPTIACON0, current).await?; + + Ok(()) + } + pub async fn apply_dsp_config(&mut self, config: &DspConfig) -> Result<(), Error> { // ADCCON let mut current = self.read_reg(Register::ADCCON).await?; + if let Some(gnpgain) = config.gnpgain { + current = GNPGAIN::apply(current, gnpgain as u32); + } if let Some(muxseln) = config.muxseln { current = MUXSELN::apply(current, muxseln as u32); } @@ -840,6 +951,10 @@ pub enum Register { CMDFIFOWRITE = 0x0000_2070, // Command FIFO Write Register TEMPSENSDAT = 0x0000_2084, // Temperature Sensor Result Register ADCDAT = 0x0000_2074, // ADC Raw Result Register + LPTIASW0 = 0x0000_20E4, // Low power TIA switch configuration + LPTIACON0 = 0x0000_20EC, // Low power TIA control bits, Channel 0 + LPDACDAT0 = 0x0000_2120, // Low power DAC data output Register + LPDACCON0 = 0x0000_2128, // Low power DAC configuration register TEMPSENS = 0x0000_2174, // Temperature Sensor Configuration Register ADCCON = 0x0000_21A8, // ADC Configuration Register SEQ0INFO = 0x0000_21CC, // Sequence 0 Information Register @@ -850,11 +965,14 @@ pub enum Register { AFEGENINTSTA = 0x0000_209C, // Analog Generation Interrupt Register CMDFIFOWADDR = 0x0000_21D4, // Command FIFO Write Address Register PMBW = 0x0000_22F0, // Power Mode Configuration Register + SWMUX = 0x0000_235C, // Common-mode switch mux select register INTCFLAG0 = 0x0000_3010, // Interrupt Control Flag 0 Register INTCFLAG1 = 0x0000_3014, // Interrupt Control Flag 1 Register OSCCON = 0x0000_0A10, // Oscillator Control Register SEQTIMEOUT = 0x0000_2068, // Sequencer Timeout Counter Register SEQCRC = 0x0000_2060, // Sequencer CRC Value Register + DFTREAL = 0x0000_2078, // DFT Result, Real Device Register + DFTIMAG = 0x0000_207C, // DFT Result, Imaginary Device Register DATAFIFOTHRES = 0x0000_21E0, // Data FIFO Threshold Register SYNCEXTDEVICE = 0x0000_2054, // Sync External Device Register GP0CON = 0x0000_0000, // GPIO Port 0 Configuration Register diff --git a/src/ad5940_registers.rs b/src/ad5940_registers.rs index 49ce3c2..4aaac62 100644 --- a/src/ad5940_registers.rs +++ b/src/ad5940_registers.rs @@ -130,6 +130,126 @@ impl RegisterField for DMUXCON { const MASK: u32 = 0b1111; } +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum RSTEN { + Disabled = 0b0, + Enabled = 0b1, +} + +impl RegisterField for RSTEN { + fn reset() -> Self { + RSTEN::Disabled + } + const BIT_OFFSET: u32 = 0; + const MASK: u32 = 0b1; +} + +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum PWDEN { + DACPoweredOn = 0b0, + DACPoweredOff = 0b1, +} + +impl RegisterField for PWDEN { + fn reset() -> Self { + PWDEN::DACPoweredOff + } + const BIT_OFFSET: u32 = 1; + const MASK: u32 = 0b1; +} + +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum DACIN6 { + Default = 0b0, +} + +impl RegisterField for DACIN6 { + fn reset() -> Self { + DACIN6::Default + } + const BIT_OFFSET: u32 = 12; + const MASK: u32 = 0b11_1111; +} + +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum SWMUX { + CommonModeOff = 0b0, + COmmonModeOn = 0b1, +} + +impl RegisterField for SWMUX { + fn reset() -> Self { + SWMUX::CommonModeOff + } + const BIT_OFFSET: u32 = 3; + const MASK: u32 = 0b1; +} + +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum TIAPDEN { + PowerUp = 0b0, + PowerDown = 0b1, +} + +impl RegisterField for TIAPDEN { + fn reset() -> Self { + TIAPDEN::PowerDown + } + const BIT_OFFSET: u32 = 0; + const MASK: u32 = 0b1; +} + +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum TIARF { + Disconnected = 0b0, + R0 = 0b1, + R20k = 0b10, + R100k = 0b11, + R200k = 0b100, + R400k = 0b101, + R600k = 0b110, + R1M = 0b111, +} + +impl RegisterField for TIARF { + fn reset() -> Self { + TIARF::Disconnected + } + const BIT_OFFSET: u32 = 13; + const MASK: u32 = 0b111; +} + +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum GNPGAIN { + Gain1 = 0b0, + Gain1_5 = 0b1, + Gain2 = 0b10, + Gain4 = 0b11, + Gain9 = 0b100, +} + +impl RegisterField for GNPGAIN { + fn reset() -> Self { + GNPGAIN::Gain1 + } + const BIT_OFFSET: u32 = 16; + const MASK: u32 = 0b111; +} + #[allow(dead_code)] #[repr(u32)] #[derive(Copy, Clone)] @@ -138,7 +258,11 @@ pub enum MUXSELN Floating = 0b00000, HsTiaNeg = 0b00001, LpTiaNeg = 0b00010, - AIN1 = 0b00101, + AIN1 = 0b00101, + AIN2 = 0b00110, + AIN3 = 0b00111, + VBIAS_CAP= 0b01000, + ExciNNode= 0b10100, } impl RegisterField for MUXSELN { @@ -155,7 +279,11 @@ impl RegisterField for MUXSELN { pub enum MUXSELP { Floating = 0b00000, HsTiaPos = 0b00001, - AIN1 = 0b00101, + AIN1 = 0b00101, + AIN2 = 0b00110, + AIN3 = 0b00111, + CE0 = 0b011001, + ExciPNode= 0b100100, } impl RegisterField for MUXSELP { @@ -163,7 +291,7 @@ impl RegisterField for MUXSELP { MUXSELP::Floating } const BIT_OFFSET: u32 = 0; - const MASK: u32 = 0b11111; + const MASK: u32 = 0b111111; } #[allow(dead_code)] diff --git a/src/adg2128.rs b/src/adg2128.rs index e8d098d..bb219fc 100644 --- a/src/adg2128.rs +++ b/src/adg2128.rs @@ -1,5 +1,5 @@ use embassy_embedded_hal::shared_bus::blocking::i2c::I2cDevice; -use embassy_stm32::{i2c::I2c, mode::Blocking}; +use embassy_stm32::{i2c::I2c, mode::Blocking, i2c::Master}; use embassy_sync::blocking_mutex::raw::NoopRawMutex; use embedded_hal_1::i2c::I2c as _; @@ -99,13 +99,13 @@ enum LDSW { } pub struct ADG2128 { - i2c: I2cDevice<'static, NoopRawMutex, I2c<'static, Blocking>>, + i2c: I2cDevice<'static, NoopRawMutex, I2c<'static, Blocking, Master>>, address: u8, states_xy: [u8; 12] } impl ADG2128 { - pub fn new(i2c: I2cDevice<'static, NoopRawMutex, I2c<'static, Blocking>>, address: u8) -> Self { + pub fn new(i2c: I2cDevice<'static, NoopRawMutex, I2c<'static, Blocking, Master>>, address: u8) -> Self { ADG2128 { i2c, address, states_xy: [0u8; 12]} } diff --git a/src/communication.rs b/src/communication.rs index 01c171b..4a36559 100644 --- a/src/communication.rs +++ b/src/communication.rs @@ -12,17 +12,17 @@ use postcard_rpc::{ define_dispatch, header::VarHeader, server::{ - impls::embassy_usb_v0_4::{ + impls::embassy_usb_v0_5::{ dispatch_impl::{spawn_fn, WireRxBuf, WireRxImpl, WireSpawnImpl, WireStorage, WireTxImpl}, - PacketBuffers, + PacketBuffers, USB_FS_MAX_PACKET_SIZE, }, Dispatch, Server, Sender, SpawnContext, }, }; -use bioz_icd_rs::{GetUniqueIdEndpoint, MeasurementPointSet, MultiImpedanceOutputTopic, MultiImpedanceResult, MultiImpedanceStartRequest, PingEndpoint, SetGreenLedEndpoint, SingleImpedanceOutputTopic, SingleImpedanceStartRequest, StartMultiImpedanceEndpoint, StartSingleImpedanceEndpoint, StopMultiImpedanceEndpoint, StopSingleImpedanceEndpoint, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST}; +use bioz_icd_rs::{GetUniqueIdEndpoint, MeasurementPointSet, SweepImpedanceOutputTopic, SweepImpedanceResult, SweepImpedanceStartRequest, PingEndpoint, SetGreenLedEndpoint, SingleImpedanceOutputTopic, SingleImpedanceStartRequest, StartSweepImpedanceEndpoint, StartSingleImpedanceEndpoint, StopImpedanceEndpoint, BioImpedanceLeadMode, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST}; -use crate::impedance::{ImpedanceSetupType, RunningMode, IMPEDANCE_CHANNEL_MULTI, IMPEDANCE_CHANNEL_SINGLE}; +use crate::impedance::{ImpedanceSetupType, RunningMode, IMPEDANCE_CHANNEL_SWEEP, IMPEDANCE_CHANNEL_SINGLE}; // Postcard RPC types type AppDriver = usb::Driver<'static, peripherals::USB>; @@ -70,9 +70,8 @@ define_dispatch! { | GetUniqueIdEndpoint | blocking | get_unique_id_handler | | SetGreenLedEndpoint | async | set_green_led_handler | | StartSingleImpedanceEndpoint | spawn | start_single_impedance_handler | - | StopSingleImpedanceEndpoint | async | stop_single_impedance_handler | - | StartMultiImpedanceEndpoint | spawn | start_multi_impedance_handler | - | StopMultiImpedanceEndpoint | async | stop_multi_impedance_handler | + | StartSweepImpedanceEndpoint | spawn | start_sweep_impedance_handler | + | StopImpedanceEndpoint | async | stop_impedance_handler | }; topics_in: { list: TOPICS_IN_LIST; @@ -128,7 +127,7 @@ pub fn init_communication(usb_driver: Driver<'static, peripherals::USB>, impedan impedance_setup: impedance_setup, }; - let (device, tx_impl, rx_impl) = STORAGE.init(usb_driver, config, pbufs.tx_buf.as_mut_slice()); + let (device, tx_impl, rx_impl) = STORAGE.init(usb_driver, config, pbufs.tx_buf.as_mut_slice(), USB_FS_MAX_PACKET_SIZE); let dispatcher = MyApp::new(context, spawner.into()); let vkk = dispatcher.min_key_len(); let server: AppServer = Server::new( @@ -169,10 +168,16 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader info!("Start impedance measurement at {:?} Hz.", rqst.sinus_frequency); // Mark the impedance setup as running - context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency; + match rqst.lead_mode { + BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency2Lead, + BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency4Lead, + } // Init the sequencer - let init_impedance_result = context.impedance_setup.lock().await.init_single_frequency_measurement(rqst.sinus_frequency, rqst.dft_number).await; + let init_impedance_result = match rqst.lead_mode { + BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.init_single_frequency_measurement_2_lead(rqst.sinus_frequency, rqst.dft_number).await, + BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.init_single_frequency_measurement_4_lead(rqst.sinus_frequency, rqst.dft_number).await, + }; // Trigger the sequencer context.impedance_setup.lock().await.start_measurement().await; @@ -192,7 +197,7 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader match select(stop_fut, recv_fut).await { Either::First(_) => { - info!("Stop signal received."); + // info!("Stop signal received."); break; } Either::Second(msg) => { @@ -214,45 +219,70 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader STOP.reset(); } -pub async fn stop_single_impedance_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> bool { - info!("Stop impedance measurement"); - let was_busy = context.impedance_setup.lock().await.running_mode; - if was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { - STOP.signal(()); - } - was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) -} - #[embassy_executor::task] -pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: MultiImpedanceStartRequest, sender: Sender) { +pub async fn start_sweep_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: SweepImpedanceStartRequest, sender: Sender) { // Mark the impedance setup as running - context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency(rqst.points); + // Init the sequencer - let response = match rqst.points { - MeasurementPointSet::Eight => { + let response = match (rqst.lead_mode, rqst.points) { + (BioImpedanceLeadMode::TwoLead, MeasurementPointSet::Eight) => { + context.impedance_setup.lock().await.running_mode = RunningMode::SweepFrequency2Lead(rqst.points); const SIZE: usize = 8; context .impedance_setup .lock() .await - .init_multi_frequency_measurement::(rqst.points) + .init_sweep_frequency_measurement_2_lead::(rqst.points) .await - .map(|periods| MultiImpedanceResult { + .map(|periods| SweepImpedanceResult { points: rqst.points, periods_per_dft_8: periods, periods_per_dft_18: Vec::new(), }) } - MeasurementPointSet::Eighteen => { + (BioImpedanceLeadMode::TwoLead, MeasurementPointSet::Eighteen) => { + context.impedance_setup.lock().await.running_mode = RunningMode::SweepFrequency2Lead(rqst.points); const SIZE: usize = 18; context .impedance_setup .lock() .await - .init_multi_frequency_measurement::(rqst.points) + .init_sweep_frequency_measurement_2_lead::(rqst.points) .await - .map(|periods| MultiImpedanceResult { + .map(|periods| SweepImpedanceResult { + points: rqst.points, + periods_per_dft_8: Vec::new(), + periods_per_dft_18: periods, + }) + } + (BioImpedanceLeadMode::FourLead, MeasurementPointSet::Eight) => { + info!("Start impedance spectroscopy 4-lead with 8 points"); + context.impedance_setup.lock().await.running_mode = RunningMode::SweepFrequency4Lead(rqst.points); + const SIZE: usize = 8; + context + .impedance_setup + .lock() + .await + .init_sweep_frequency_measurement_4_lead::(rqst.points) + .await + .map(|periods| SweepImpedanceResult { + points: rqst.points, + periods_per_dft_8: periods, + periods_per_dft_18: Vec::new(), + }) + } + (BioImpedanceLeadMode::FourLead, MeasurementPointSet::Eighteen) => { + info!("Start impedance spectroscopy 4-lead with 18 points"); + context.impedance_setup.lock().await.running_mode = RunningMode::SweepFrequency4Lead(rqst.points); + const SIZE: usize = 18; + context + .impedance_setup + .lock() + .await + .init_sweep_frequency_measurement_4_lead::(rqst.points) + .await + .map(|periods| SweepImpedanceResult { points: rqst.points, periods_per_dft_8: Vec::new(), periods_per_dft_18: periods, @@ -264,7 +294,7 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, context.impedance_setup.lock().await.start_measurement().await; if sender - .reply::(header.seq_no, &response) + .reply::(header.seq_no, &response) .await .is_err() { @@ -272,21 +302,19 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, return; } - info!("Start multi impedance measurement."); - let mut seq: u8 = 0; loop { let stop_fut = STOP.wait(); - let recv_fut = IMPEDANCE_CHANNEL_MULTI.receive(); + let recv_fut = IMPEDANCE_CHANNEL_SWEEP.receive(); match select(stop_fut, recv_fut).await { Either::First(_) => { - info!("Stop signal received."); + // info!("Stop signal received."); break; } Either::Second(msg) => { if sender - .publish::(seq.into(), &msg) + .publish::(seq.into(), &msg) .await .is_err() { @@ -299,15 +327,15 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, } context.impedance_setup.lock().await.running_mode = RunningMode::None; - info!("Impedance measurement stopped."); + info!("Impedance spectroscopy measurement stopped."); STOP.reset(); } -pub async fn stop_multi_impedance_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> bool { +pub async fn stop_impedance_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> bool { info!("Stop impedance measurement"); - let was_busy = context.impedance_setup.lock().await. running_mode; - if was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { + let was_busy = context.impedance_setup.lock().await.running_mode; + if was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::SweepFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::SweepFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::SweepFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::SweepFrequency4Lead(MeasurementPointSet::Eighteen) { STOP.signal(()); - } - was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) + } + was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::SweepFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::SweepFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::SweepFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::SweepFrequency4Lead(MeasurementPointSet::Eighteen) } \ No newline at end of file diff --git a/src/electrodes.rs b/src/electrodes.rs index 3fde9cf..276e4b4 100644 --- a/src/electrodes.rs +++ b/src/electrodes.rs @@ -1,12 +1,12 @@ use embassy_embedded_hal::shared_bus::blocking::i2c::I2cDevice; -use embassy_stm32::{i2c, mode::Blocking}; +use embassy_stm32::{i2c, mode::Blocking, i2c::Master}; use embassy_sync::blocking_mutex::NoopMutex; use static_cell::StaticCell; use core::cell::RefCell; use crate::{ad5940, adg2128::{GetX, SetX, SetY, State, ADG2128}}; -static I2C_BUS: StaticCell>>> = StaticCell::new(); +static I2C_BUS: StaticCell>>> = StaticCell::new(); #[derive(Copy, Clone)] #[allow(dead_code)] @@ -43,15 +43,21 @@ pub struct Electrodes { mux_2: ADG2128, } +const MUX_1_ADDRESS: u8 = 0b1110_000; +const MUX_2_ADDRESS: u8 = 0b1110_001; + impl Electrodes { - pub fn new(i2c: i2c::I2c<'static, Blocking>) -> Self { + pub fn new(mut i2c: i2c::I2c<'static, Blocking, Master>) -> Result { + if i2c.blocking_write(MUX_1_ADDRESS, &[0x00]).is_err() { return Err("Failed to communicate with MUX 1"); } + if i2c.blocking_write(MUX_2_ADDRESS, &[0x00]).is_err() { return Err("Failed to communicate with MUX 2"); } + let i2c_bus = NoopMutex::new(RefCell::new(i2c)); let i2c_bus = I2C_BUS.init(i2c_bus); - Electrodes { - mux_1: ADG2128::new(I2cDevice::new(i2c_bus), 0b1110_000), - mux_2: ADG2128::new(I2cDevice::new(i2c_bus), 0b1110_001), - } + Ok(Electrodes { + mux_1: ADG2128::new(I2cDevice::new(i2c_bus), MUX_1_ADDRESS), + mux_2: ADG2128::new(I2cDevice::new(i2c_bus), MUX_2_ADDRESS), + }) } pub fn set(&mut self, electrode: Electrode, ad5940_pin: AD5940Pin, state: State) { diff --git a/src/impedance.rs b/src/impedance.rs index 63c1bb4..c919acb 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -1,40 +1,68 @@ use defmt::{info, error}; use embassy_stm32::spi::Error; +use embassy_stm32::exti::ExtiInput; use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; use embassy_sync::channel::Channel; use embassy_sync::mutex::Mutex; +use embassy_time::Timer; use static_cell::StaticCell; +use heapless::Vec; + +use num::complex::Complex; +use core::f32::consts::PI; + use crate::ad5940::*; use crate::ad5940_registers::*; -use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, ImpedanceInitError, MeasurementPointSet, MultiImpedanceOutput}; +use crate::electrodes::Electrodes; + +use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, ImpedanceInitError, MeasurementPointSet, SweepImpedanceOutput}; use crate::icd_mapping::IntoDftnum; -pub static IMPEDANCE_CHANNEL_SINGLE: Channel = Channel::new(); -pub static IMPEDANCE_CHANNEL_MULTI: Channel = Channel::new(); +pub static IMPEDANCE_CHANNEL_SINGLE: Channel = Channel::new(); +pub static IMPEDANCE_CHANNEL_SWEEP: Channel = Channel::new(); pub type ImpedanceSetupType = Mutex; pub static IMPEDANCE_SETUP: StaticCell = StaticCell::new(); +pub const RCAL: f32 = 1008.0; // Calibration resistor in Ohm + #[derive(PartialEq, Copy, Clone)] pub enum RunningMode { None, - SingleFrequency, - MultiFrequency(MeasurementPointSet), + SingleFrequency2Lead, + SingleFrequency4Lead, + SweepFrequency2Lead(MeasurementPointSet), + SweepFrequency4Lead(MeasurementPointSet), +} + +pub enum RtiaCalibrated { + None, + Single(RtiaCalibrationResult), + Vec8(Vec), + Vec18(Vec), +} + +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct RtiaCalibrationResult { + pub magnitude: f32, + pub phase: f32, } pub struct ImpedanceSetup { ad5940: AD5940, dsp_config: Option, pub running_mode: RunningMode, + pub rtia_calibrated: RtiaCalibrated, + pub electrodes: Option, } impl ImpedanceSetup { pub fn new(ad5940: AD5940) -> Self { - ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None } + ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None, rtia_calibrated: RtiaCalibrated::None, electrodes: None } } pub async fn init(&mut self) -> Result<(), Error> { @@ -59,9 +87,13 @@ impl ImpedanceSetup { self.ad5940.apply_clk_config(&clk_config).await.unwrap(); + // Configure LP DAC and TIA + self.common_mode_output_enable(false).await.unwrap(); + // Set DSP configuration let mut dsp_config = DspConfig::default(); dsp_config + .gnpgain(GNPGAIN::Gain1) .adc_mux_n(MUXSELN::HsTiaNeg) .adc_mux_p(MUXSELP::HsTiaPos) .ctiacon(CTIACON::C32) @@ -91,10 +123,178 @@ impl ImpedanceSetup { let config_wgcon = WGCON::TYPESEL_SIN.bits(); self.ad5940.write_reg(Register::WGCON, config_wgcon).await.unwrap(); + // Configure GPIOs + // Make sure that GP0 (INT), GP1 (MULTIPLEXER CONTROL) and GP2 (LED) are set as output and are high + self.ad5940.write_reg(Register::GP0CON, 0b10 << 4 | 0b10 << 2 | 0b10).await.unwrap(); + self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap(); + Ok(()) } - pub async fn init_single_frequency_measurement(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result { + pub async fn calibrate_rtia(&mut self, frequency: u32, wait_time: u32) -> Result { + /* CALIBRATION METHOD + 1) Measure the complex voltage V_Rcal across the calibration DUT (Rcal). + 2) Measure the complex voltage V_Rtia across Rtia [HSTIA_P (output) - HSTIA_N]. + 3) Note Rtia carries the same current as Rcal; I_Rtia = I_exc = I_Rcal + 4) Implement the equation: Rtia = V_Rtia / I_Rtia + --> Rtia = (V_Rtia / V_Rcal) * Rcal + */ + + // self.ad5940.hsr_calibrate_rtia(wait_time).await; + self.ad5940.wgfcw(frequency).await; + let wg_amplitude = 2047; // 2047 is the maximum amplitude for a 12-bit DAC --> 1.62V peak-to-peak + self.ad5940.write_reg(Register::WGAMPLITUDE, wg_amplitude).await.unwrap(); + + // Switches for Rcal + let switch_config = SwitchConfig::default() + .t9con(T9CON::T9Closed) + .tmuxcon(TMUXCON::TR1Closed) + .nmuxcon(NMUXCON::NR1Closed) + .pmuxcon(PMUXCON::PR0Closed) + .dmuxcon(DMUXCON::DR0Closed); + self.ad5940.apply_switch_config(switch_config).await.unwrap(); + + // Voltage measurement Rcal + self.dsp_config.as_mut().unwrap() + .adc_mux_n(MUXSELN::ExciNNode) + .adc_mux_p(MUXSELP::ExciPNode); + self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); + + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await; + // self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz + Timer::after_micros(10).await; + self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await; + // self.ad5940.sequencer_wait(wait_time).await; // Determined above + // self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz + Timer::after_micros((wait_time/16) as u64).await; + Timer::after_micros(20).await; + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await; + + let real_raw = self.ad5940.read_reg(Register::DFTREAL).await.unwrap(); + let imag_raw = self.ad5940.read_reg(Register::DFTIMAG).await.unwrap(); + let mut rcal = Complex::new( + sign_extend_18bit(real_raw) as f32, + sign_extend_18bit(imag_raw) as f32 + ); + + // Voltage measurement Rtia + self.dsp_config.as_mut().unwrap() + .adc_mux_n(MUXSELN::HsTiaNeg) + .adc_mux_p(MUXSELP::HsTiaPos); + self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); + + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await; + // self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz + Timer::after_micros(10).await; + self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await; + // self.ad5940.sequencer_wait(wait_time).await; // Determined above + // self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz + Timer::after_micros((wait_time/16) as u64).await; + Timer::after_micros(20).await; + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await; + + let real_raw = self.ad5940.read_reg(Register::DFTREAL).await.unwrap(); + let imag_raw = self.ad5940.read_reg(Register::DFTIMAG).await.unwrap(); + let mut rtia = Complex::new( + sign_extend_18bit(real_raw) as f32, + sign_extend_18bit(imag_raw) as f32 + ); + + // Current is measured inverted in rtia + rtia.re *= -1.0; + rtia.im *= -1.0; + + // Impedance imaginary part is measured inverted + rcal.im *= -1.0; + rtia.im *= -1.0; + + // Rtia = (V_Rtia / V_Rcal) * Rcal + let mut temp = rtia/rcal; + temp = temp.scale(RCAL); + + // Calibration result + let calibration_result = RtiaCalibrationResult { + magnitude: temp.norm(), // Magnitude in Ohm + phase: temp.arg(), // Phase in radians + }; + + Ok(calibration_result) + } + + async fn calibrate_rtia_multiple(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + let mut results = Vec::::new(); + + // Set DFT number based on the frequency + for &frequency in number_of_points.values() { + // Determine wait time + let selected_dft_num = match frequency { + f if f < 10 => DFTNUM::Num16384, + f if f < 100 => DFTNUM::Num16384, + f if f < 250 => DFTNUM::Num8192, + f if f < 1000 => DFTNUM::Num4096, + f if f < 2500 => DFTNUM::Num2048, + f if f < 10000 => DFTNUM::Num1024, + f if f < 25000 => DFTNUM::Num512, + f if f < 100000 => DFTNUM::Num256, + _ => DFTNUM::Num128, + }; + + let wait_time: u32; + + if let Some(dsp_config) = &mut self.dsp_config { + dsp_config + .dftnum(selected_dft_num); + + // Update DFTNUM + let mut current = self.ad5940.read_reg(Register::DFTCON).await.unwrap(); + current = DFTNUM::apply(current, selected_dft_num as u32); + self.ad5940.write_reg(Register::DFTCON, current).await.unwrap(); + + wait_time = self.ad5940.sequencer_calculate_wait_time(&dsp_config).await.unwrap(); + + // Calibrate Rtia at the given frequency + let calibration_result = self.calibrate_rtia(frequency, wait_time).await.unwrap(); + + results.push(calibration_result).unwrap(); + } else { + error!("DSP configuration not set, cannot calculate wait time"); + return Err(ImpedanceInitError::DSPNotSet); + } + } + + Ok(results) + } + + pub async fn common_mode_output_enable(&mut self, enable: bool) -> Result<(), Error> { + // Configure LP DAC and TIA + let mut lp_config = LpConfig::default(); + + if enable { + lp_config + .data_reset(true) + .power_enable(true) + .data_6bit(31) // Mid-scale for 6-bit DAC + .common_mode_enable(true) + .tia_switches_enabled(1 << 5 | 1 << 7 | 1 << 9 | 1 << 13) + .tia_enable(true) + .filter_resistor(TIARF::R20k); + + } else { + lp_config + .power_enable(false) + .common_mode_enable(false) + .tia_enable(false); + } + + self.ad5940.apply_lp_config(&lp_config).await.unwrap(); + + Ok(()) + } + + pub async fn init_single_frequency_measurement_2_lead(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result { + // Configure LP DAC and TIA + self.common_mode_output_enable(false).await.unwrap(); + // Reset FIFO self.ad5940.clear_and_enable_fifo().await.unwrap(); @@ -112,7 +312,7 @@ impl ImpedanceSetup { if let Some(dsp_config) = &self.dsp_config { wait_time = self.ad5940.sequencer_calculate_wait_time(dsp_config).await.unwrap(); sinus_periods_per_dft = wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32; - info!("Sinus periods per DFT: {}", sinus_periods_per_dft); + // info!("Sinus periods per DFT: {}", sinus_periods_per_dft); } else { error!("DSP configuration not set, cannot calculate wait time"); return Err(ImpedanceInitError::DSPNotSet); @@ -173,7 +373,110 @@ impl ImpedanceSetup { Ok(sinus_periods_per_dft) } - pub async fn init_multi_frequency_measurement(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + pub async fn init_single_frequency_measurement_4_lead(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result { + // Configure LP DAC and TIA + self.common_mode_output_enable(true).await.unwrap(); + + // Set DFT number + self.dsp_config.as_mut().unwrap().dftnum(dft_number.into_dftnum()); + self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); + + // Configure GPIOs + self.ad5940.write_reg(Register::GP0CON, 0b10 << 4 | 0b10 << 2 | 0b10).await.unwrap(); + self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap(); + + // Calculate wait time between measurement start and stop + let wait_time; + let sinus_periods_per_dft; + if let Some(dsp_config) = &self.dsp_config { + wait_time = self.ad5940.sequencer_calculate_wait_time(dsp_config).await.unwrap(); + sinus_periods_per_dft = wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32; + // info!("Sinus periods per DFT: {}", sinus_periods_per_dft); + } else { + error!("DSP configuration not set, cannot calculate wait time"); + return Err(ImpedanceInitError::DSPNotSet); + } + + // Calibrate Rtia + match self.calibrate_rtia(frequency, wait_time).await { + Ok(calibration_result) => { + self.rtia_calibrated = RtiaCalibrated::Single(calibration_result); + }, + Err(e) => { + error!("Rtia calibration failed: {:?}", e); + } + } + + // Reset FIFO + self.ad5940.clear_and_enable_fifo().await.unwrap(); + + // Configure switches + let switch_config = SwitchConfig::default() + .t9con(T9CON::T9Closed) + .tmuxcon(TMUXCON::T2Closed) + .nmuxcon(NMUXCON::N2Closed) + .pmuxcon(PMUXCON::P11Closed) + .dmuxcon(DMUXCON::D5Closed); + self.ad5940.apply_switch_config(switch_config).await.unwrap(); + + // Configure sequencer + self.ad5940.sequencer_enable(true).await; + + self.ad5940.wgfcw(frequency).await; + let wg_amplitude = 2047; // 2047 is the maximum amplitude for a 12-bit DAC --> 1.62V peak-to-peak + self.ad5940.write_reg(Register::WGAMPLITUDE, wg_amplitude).await.unwrap(); + + // Voltage measurement + self.dsp_config.as_mut().unwrap() + .adc_mux_n(MUXSELN::AIN3) + .adc_mux_p(MUXSELP::AIN2); + // self.dsp_config.as_mut().unwrap() + // .adc_mux_n(MUXSELN::AIN1) + // .adc_mux_p(MUXSELP::CE0); + self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); + + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await; + self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await; + self.ad5940.sequencer_wait(wait_time).await; // Determined above + self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await; + + // Current measurement + self.dsp_config.as_mut().unwrap() + .adc_mux_n(MUXSELN::HsTiaNeg) + .adc_mux_p(MUXSELP::HsTiaPos); + self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); + + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await; + self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await; + self.ad5940.sequencer_wait(wait_time).await; // Determined above + self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await; + + // Toggle leds + self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b010).await.unwrap(); + self.ad5940.sequencer_wait(16 * 1_000).await; // 1ms based on SYSCLK = 16MHz + self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap(); + + self.ad5940.sequencer_enable(false).await; + + // Write sequence to SRAM + let start_address = 0; + self.ad5940.sequencer_cmd_write(start_address).await; + + self.ad5940.sequencer_info_configure(0, self.ad5940.seq_len, start_address).await; + + self.start_measurement().await; + + Ok(sinus_periods_per_dft) + } + + pub async fn init_sweep_frequency_measurement_2_lead(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + // Configure LP DAC and TIA + self.common_mode_output_enable(false).await.unwrap(); + // Create vector to store the periods per DFT for each frequency let mut periods_per_dft_vec = heapless::Vec::::new(); @@ -191,14 +494,14 @@ impl ImpedanceSetup { for &frequency in number_of_points.values() { // Determine wait time let selected_dft_num = match frequency { - f if f < 10.0 => DFTNUM::Num16384, - f if f < 100.0 => DFTNUM::Num16384, - f if f < 250.0 => DFTNUM::Num8192, - f if f < 1_000.0 => DFTNUM::Num4096, - f if f < 2_500.0 => DFTNUM::Num2048, - f if f < 10_000.0 => DFTNUM::Num1024, - f if f < 25_000.0 => DFTNUM::Num512, - f if f < 100_000.0 => DFTNUM::Num256, + f if f < 10 => DFTNUM::Num16384, + f if f < 100 => DFTNUM::Num16384, + f if f < 250 => DFTNUM::Num8192, + f if f < 1000 => DFTNUM::Num4096, + f if f < 2500 => DFTNUM::Num2048, + f if f < 10000 => DFTNUM::Num1024, + f if f < 25000 => DFTNUM::Num512, + f if f < 100000 => DFTNUM::Num256, _ => DFTNUM::Num128, }; @@ -217,7 +520,7 @@ impl ImpedanceSetup { let periods_per_dft = wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32; periods_per_dft_vec.push(periods_per_dft).unwrap(); - info!("{}Hz: Sinus periods per DFT: {}", frequency, periods_per_dft); + // info!("{}Hz: Sinus periods per DFT: {}", frequency, periods_per_dft); } else { error!("DSP configuration not set, cannot calculate wait time"); @@ -278,6 +581,133 @@ impl ImpedanceSetup { Ok(periods_per_dft_vec) } + pub async fn init_sweep_frequency_measurement_4_lead(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + // Configure LP DAC and TIA + self.common_mode_output_enable(true).await.unwrap(); + + // Create vector to store the periods per DFT for each frequency + let mut periods_per_dft_vec = heapless::Vec::::new(); + + // Calibrate Rtia + match number_of_points { + MeasurementPointSet::Eight => { + let results = self.calibrate_rtia_multiple::<8>(number_of_points).await.unwrap(); + self.rtia_calibrated = RtiaCalibrated::Vec8(results); + }, + MeasurementPointSet::Eighteen => { + let results = self.calibrate_rtia_multiple::<18>(number_of_points).await.unwrap(); + self.rtia_calibrated = RtiaCalibrated::Vec18(results); + }, + } + + // Reset FIFO + self.ad5940.clear_and_enable_fifo().await.unwrap(); + + // Configure GPIOs + self.ad5940.write_reg(Register::GP0CON, 0b10 << 4 | 0b10 << 2 | 0b10).await.unwrap(); + self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap(); + + // Configure sequencer + self.ad5940.sequencer_enable(true).await; + + // Set DFT number based on the frequency + for &frequency in number_of_points.values() { + // Determine wait time + let selected_dft_num = match frequency { + f if f < 10 => DFTNUM::Num16384, + f if f < 100 => DFTNUM::Num16384, + f if f < 250 => DFTNUM::Num8192, + f if f < 1000 => DFTNUM::Num4096, + f if f < 2500 => DFTNUM::Num2048, + f if f < 10000 => DFTNUM::Num1024, + f if f < 25000 => DFTNUM::Num512, + f if f < 100000 => DFTNUM::Num256, + _ => DFTNUM::Num128, + }; + + let wait_time: u32; + + if let Some(dsp_config) = &mut self.dsp_config { + dsp_config + .dftnum(selected_dft_num); + + // Update DFTNUM + let mut current = self.ad5940.read_reg(Register::DFTCON).await.unwrap(); + current = DFTNUM::apply(current, selected_dft_num as u32); + self.ad5940.write_reg(Register::DFTCON, current).await.unwrap(); + + wait_time = self.ad5940.sequencer_calculate_wait_time(&dsp_config).await.unwrap(); + let periods_per_dft = wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32; + + periods_per_dft_vec.push(periods_per_dft).unwrap(); + // info!("{}Hz: Sinus periods per DFT: {}", frequency, periods_per_dft); + } else { + error!("DSP configuration not set, cannot calculate wait time"); + return Err(ImpedanceInitError::DSPNotSet); + } + + // Configure switches + let switch_config = SwitchConfig::default() + .t9con(T9CON::T9Closed) + .tmuxcon(TMUXCON::T2Closed) + .nmuxcon(NMUXCON::N2Closed) + .pmuxcon(PMUXCON::P11Closed) + .dmuxcon(DMUXCON::D5Closed); + self.ad5940.apply_switch_config(switch_config).await.unwrap(); + + // Set frequency + self.ad5940.wgfcw(frequency as u32).await; + let wg_amplitude = 2047; // 2047 is the maximum amplitude for a 12-bit DAC --> 1.62V peak-to-peak + self.ad5940.write_reg(Register::WGAMPLITUDE, wg_amplitude).await.unwrap(); + + // Voltage measurement + self.dsp_config.as_mut().unwrap() + .adc_mux_n(MUXSELN::AIN3) + .adc_mux_p(MUXSELP::AIN2); + // self.dsp_config.as_mut().unwrap() + // .adc_mux_n(MUXSELN::AIN1) + // .adc_mux_p(MUXSELP::CE0); + self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); + + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await; + self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await; + self.ad5940.sequencer_wait(wait_time).await; // Determined above + self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await; + + // Current measurement + self.dsp_config.as_mut().unwrap() + .adc_mux_n(MUXSELN::HsTiaNeg) + .adc_mux_p(MUXSELP::HsTiaPos); + self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); + + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await; + self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await; + self.ad5940.sequencer_wait(wait_time).await; // Determined above + self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await; + } + + // Toggle leds + self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b010).await.unwrap(); + self.ad5940.sequencer_wait(16 * 1_000).await; // 1ms based on SYSCLK = 16MHz + self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap(); + + self.ad5940.sequencer_enable(false).await; + + // Write sequence to SRAM + let start_address = 0; + self.ad5940.sequencer_cmd_write(start_address).await; + + self.ad5940.sequencer_info_configure(0, self.ad5940.seq_len, start_address).await; + + self.start_measurement().await; + + Ok(periods_per_dft_vec) + } + pub async fn start_measurement(&mut self) { self.ad5940.sequencer_trigger(0).await; } @@ -290,3 +720,257 @@ impl ImpedanceSetup { self.ad5940.read_fifo(data).await } } + +// Task to wait for the interrupt from the AD5940 and read the FIFO +// Depending on the running mode, it will read either a single frequency or multiple frequencies +// Result is sent via channel to the communication task which then sends it via USB to the host +#[embassy_executor::task] +pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance_setup: &'static ImpedanceSetupType) { + loop { + // Wait untill sequence is done + pin.wait_for_rising_edge().await; + + // Lock the impedance setup + let mut impedance_setup = impedance_setup.lock().await; + + // Trigger the sequencer again + if impedance_setup.running_mode == RunningMode::SingleFrequency2Lead || impedance_setup.running_mode == RunningMode::SingleFrequency4Lead || impedance_setup.running_mode == RunningMode::SweepFrequency2Lead(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::SweepFrequency2Lead(MeasurementPointSet::Eighteen) || impedance_setup.running_mode == RunningMode::SweepFrequency4Lead(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::SweepFrequency4Lead(MeasurementPointSet::Eighteen) { + impedance_setup.start_measurement().await; + } + + // Read the FIFO count + let count = impedance_setup.get_fifo_count().await.unwrap() as usize; + // info!("FIFOCNTSTA: {}", count); + + match impedance_setup.running_mode { + RunningMode::None => { + continue; // Skip processing if not running + } + RunningMode::SingleFrequency2Lead => { + if count >= 4 { + let mut data: [u32; 4] = [0; 4]; + + impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap(); + + let result = calculate_impedance_2_lead(data); + + // Log + // info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase); + + let data = SingleImpedanceOutput { + magnitude: result.magnitude, + phase: result.phase, + }; + + IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok(); + } + } + RunningMode::SingleFrequency4Lead => { + if count >= 4 { + let mut data: [u32; 4] = [0; 4]; + + impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap(); + match impedance_setup.rtia_calibrated { + RtiaCalibrated::Single(ref cal) => { + let result = calculate_impedance_4_lead(data, cal); + + let data = SingleImpedanceOutput { + magnitude: result.magnitude, + phase: result.phase + }; + + IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok(); + } + _ => { + error!("Rtia not (correctly) calibrated, cannot compute impedance"); + continue; + }, + } + } + } + RunningMode::SweepFrequency2Lead(points) => { + // Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz) + let required_count = points.len() * 4; + + if count >= required_count { + // Use stack-allocated array + let mut data: [u32; 72] = [0; 72]; // max needed size (18*4=72) + let data_slice = &mut data[..required_count]; + impedance_setup.read_fifo(data_slice).await.unwrap(); + + // Output structure + let mut impedance_output = SweepImpedanceOutput { + points, + magnitudes_8: Vec::new(), + phases_8: Vec::new(), + magnitudes_18: Vec::new(), + phases_18: Vec::new(), + }; + + // Take 4 samples per frequency point + for chunk in data_slice.chunks(4) { + let result = calculate_impedance_2_lead(chunk.try_into().unwrap()); + + match points { + MeasurementPointSet::Eight => { + impedance_output.magnitudes_8.push(result.magnitude).ok(); + impedance_output.phases_8.push(result.phase).ok(); + } + MeasurementPointSet::Eighteen => { + impedance_output.magnitudes_18.push(result.magnitude).ok(); + impedance_output.phases_18.push(result.phase).ok(); + } + } + } + + IMPEDANCE_CHANNEL_SWEEP.try_send(impedance_output).ok(); + } + continue; + } + RunningMode::SweepFrequency4Lead(points) => { + // Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz) + let required_count = points.len() * 4; + + if count >= required_count { + // Use stack-allocated array + let mut data: [u32; 72] = [0; 72]; // max needed size (18*4=72) + let data_slice = &mut data[..required_count]; + impedance_setup.read_fifo(data_slice).await.unwrap(); + + // Output structure + let mut impedance_output = SweepImpedanceOutput { + points, + magnitudes_8: Vec::new(), + phases_8: Vec::new(), + magnitudes_18: Vec::new(), + phases_18: Vec::new(), + }; + + match (&points, &impedance_setup.rtia_calibrated) { + (MeasurementPointSet::Eight, RtiaCalibrated::Vec8(cal_vec)) => { + // Take 4 samples per frequency point + for (i, chunk) in data_slice.chunks(4).enumerate() { + let cal = &cal_vec[i]; + let result = calculate_impedance_4_lead(chunk.try_into().unwrap(), cal); + + impedance_output.magnitudes_8.push(result.magnitude).ok(); + impedance_output.phases_8.push(result.phase).ok(); + } + }, + (MeasurementPointSet::Eighteen, RtiaCalibrated::Vec18(cal_vec)) => { + // Take 4 samples per frequency point + for (i, chunk) in data_slice.chunks(4).enumerate() { + let cal = &cal_vec[i]; + let result = calculate_impedance_4_lead(chunk.try_into().unwrap(), cal); + + impedance_output.magnitudes_18.push(result.magnitude).ok(); + impedance_output.phases_18.push(result.phase).ok(); + } + }, + _ => { + error!("Rtia not (correctly) calibrated, cannot compute impedance"); + continue; + } + } + + IMPEDANCE_CHANNEL_SWEEP.try_send(impedance_output).ok(); + } + continue; + + } + } + } +} + +#[derive(Debug)] +pub struct ImpedanceResult { + pub magnitude: f32, + pub phase: f32, +} + +/// Convert raw 18-bit 2's complement value to signed i32 +fn sign_extend_18bit(val: u32) -> i32 { + ((val << 14) as i32) >> 14 +} + +/// Calculate magnitude and phase of Rz using Rcal reference +pub fn calculate_impedance_2_lead(data: [u32; 4]) -> ImpedanceResult { + let mut dft_rcal = Complex::new( + sign_extend_18bit(data[0]) as f32, + sign_extend_18bit(data[1]) as f32 + ); + let mut dft_rz = Complex::new( + sign_extend_18bit(data[2]) as f32, + sign_extend_18bit(data[3]) as f32 + ); + + // Current is measured inverted in rtia + // --> Will cancel out in the impedance calculation + // dft_rcal.re *= -1.0; + // dft_rcal.im *= -1.0; + + // dft_rz.re *= -1.0; + // dft_rz.im *= -1.0; + + // Impedance imaginary part is measured inverted + dft_rcal.im *= -1.0; + dft_rz.im *= -1.0; + + let (rcal_mag, rcal_phase) = dft_rcal.to_polar(); + let (rz_mag, rz_phase) = dft_rz.to_polar(); + + let magnitude = (rcal_mag / rz_mag) * RCAL; + let mut phase = rcal_phase - rz_phase; + + // Normalize phase to [-π, π] + if phase > PI { + phase -= 2.0 * PI; + } + else if phase < -PI { + phase += 2.0 * PI; + } + + ImpedanceResult { magnitude, phase } +} + +/// Calculate magnitude and phase of Rz using the calibrated Rtia +pub fn calculate_impedance_4_lead(data: [u32; 4], rtia: &RtiaCalibrationResult) -> ImpedanceResult { + let mut dft_volt = Complex::new( + sign_extend_18bit(data[0]) as f32, + sign_extend_18bit(data[1]) as f32 + ); + let mut dft_curr = Complex::new( + sign_extend_18bit(data[2]) as f32, + sign_extend_18bit(data[3]) as f32 + ); + + // Current is measured inverted in rtia + dft_curr.re *= -1.0; + dft_curr.im *= -1.0; + + // Impedance imaginary part is measured inverted + dft_volt.im *= -1.0; + dft_curr.im *= -1.0; + + let (volt_mag, volt_phase) = dft_volt.to_polar(); + let (curr_mag, curr_phase) = dft_curr.to_polar(); + + // Calculate impedance using calibrated Rtia + let magnitude = volt_mag / curr_mag * rtia.magnitude; + let mut phase = volt_phase - curr_phase + rtia.phase; + + // Normalize phase to [-π, π] + if phase > PI { + phase -= 2.0 * PI; + } + else if phase < -PI { + phase += 2.0 * PI; + } + + let data = ImpedanceResult { + magnitude, + phase + }; + + data +} diff --git a/src/main.rs b/src/main.rs index 2bb168f..b662c50 100644 --- a/src/main.rs +++ b/src/main.rs @@ -8,26 +8,22 @@ use embassy_sync::mutex::Mutex; use embassy_time::{Timer, Duration}; use embassy_futures::{select::select, select::Either}; use embassy_stm32::gpio::{Level, Output, Speed}; -use embassy_stm32::{spi, Config}; +use embassy_stm32::{i2c, spi, Config}; use embassy_stm32::time::Hertz; -use heapless::Vec; - use {defmt_rtt as _, panic_probe as _}; // use crate::ad5940::*; // use crate::ad5940_registers::*; -use bioz_icd_rs::{MeasurementPointSet, MultiImpedanceOutput, SingleImpedanceOutput}; - mod ad5940; use ad5940::AD5940; -// mod adg2128; -// use adg2128::State; +mod adg2128; +use adg2128::State; -// mod electrodes; -// use electrodes::{Electrodes, Electrode, AD5940Pin}; +mod electrodes; +use electrodes::{Electrodes, Electrode, AD5940Pin}; mod ad5940_registers; @@ -37,10 +33,8 @@ use embassy_stm32::{bind_interrupts, peripherals, usb}; mod communication; use communication::{init_communication, LED_FREQUENCY_SIGNAL}; -use impedance::{IMPEDANCE_CHANNEL_SINGLE, IMPEDANCE_CHANNEL_MULTI}; - mod impedance; -use impedance::{ImpedanceSetup, ImpedanceSetupType, IMPEDANCE_SETUP}; +use impedance::{ImpedanceSetup, IMPEDANCE_SETUP, impedance_setup_readout_task}; mod icd_mapping; @@ -73,6 +67,10 @@ async fn main(spawner: Spawner) { } let p = embassy_stm32::init(config); + + // Small delay to allow power to stabilize + Timer::after_millis(100).await; + info!("Hello World!"); // let mut led = Output::new(p.PA5, Level::High, Speed::Low); @@ -102,21 +100,29 @@ async fn main(spawner: Spawner) { // impedance_setup.lock().await.init_single_frequency_measurement().await; - // // Set up I2C for ADG2128 - // let i2c = i2c::I2c::new_blocking( - // p.I2C1, - // p.PB6, - // p.PB7, - // Hertz(400_000), - // i2c::Config::default() - // ); + // Set up I2C for ADG2128 + let mut i2c_config = i2c::Config::default(); + i2c_config.frequency = Hertz(400_000); + let i2c = i2c::I2c::new_blocking( + p.I2C1, + p.PB6, + p.PB7, + i2c_config + ); - // // Initialize electrodes - // let mut electrodes = Electrodes::new(i2c); - // electrodes.reset_all(); - // electrodes.set(Electrode::E1, AD5940Pin::CE0, State::ENABLED); - // electrodes.set(Electrode::E3, AD5940Pin::AIN1, State::ENABLED); - // // electrodes.set(Electrode::E12, AD5940Pin::RE0, State::ENABLED); + // Initialize electrodes + match Electrodes::new(i2c) { + Ok(mut e) => { + e.reset_all(); + e.set(Electrode::E1, AD5940Pin::CE0, State::ENABLED); + e.set(Electrode::E23, AD5940Pin::AIN1, State::ENABLED); + impedance_setup.lock().await.electrodes = Some(e); + }, + Err(e) => { + info!("Error initializing electrodes: {}", e); + impedance_setup.lock().await.electrodes = None; + } + }; // Turn on the green LED // ad5940.write_reg_raw(0x0000_0004, 1 << 1).await.unwrap(); @@ -174,143 +180,3 @@ async fn green_led(mut led: Output<'static>) { led.toggle(); } } - -#[embassy_executor::task] -async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance_setup: &'static ImpedanceSetupType) { - loop { - // Wait untill sequence is done - pin.wait_for_rising_edge().await; - - // Lock the impedance setup - let mut impedance_setup = impedance_setup.lock().await; - - // Trigger the sequencer again - if impedance_setup.running_mode == impedance::RunningMode::SingleFrequency || impedance_setup.running_mode == impedance::RunningMode::MultiFrequency(MeasurementPointSet::Eight) || impedance_setup.running_mode == impedance::RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { - impedance_setup.start_measurement().await; - } - - // Read the FIFO count - let count = impedance_setup.get_fifo_count().await.unwrap() as usize; - // info!("FIFOCNTSTA: {}", count); - - match impedance_setup.running_mode { - impedance::RunningMode::None => { - continue; // Skip processing if not running - } - impedance::RunningMode::SingleFrequency => { - if count >= 4 { - let mut data: [u32; 4] = [0; 4]; - - impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap(); - - let result = calculate_impedance(data); - - // Log - // info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase); - - let data = SingleImpedanceOutput { - magnitude: result.magnitude, - phase: result.phase, - }; - - IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok(); - } - } - - impedance::RunningMode::MultiFrequency(points) => { - // Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz) - let required_count = points.len() * 4; - - if count >= required_count { - // Use stack-allocated array - let mut data: [u32; 72] = [0; 72]; // max needed size (18*4=72) - let data_slice = &mut data[..required_count]; - impedance_setup.read_fifo(data_slice).await.unwrap(); - - // Output structure - let mut impedance_output = MultiImpedanceOutput { - points, - magnitudes_8: Vec::new(), - phases_8: Vec::new(), - magnitudes_18: Vec::new(), - phases_18: Vec::new(), - }; - - // Take 4 samples per frequency point - for chunk in data_slice.chunks(4) { - let result = calculate_impedance(chunk.try_into().unwrap()); - - match points { - MeasurementPointSet::Eight => { - impedance_output.magnitudes_8.push(result.magnitude).ok(); - impedance_output.phases_8.push(result.phase).ok(); - } - MeasurementPointSet::Eighteen => { - impedance_output.magnitudes_18.push(result.magnitude).ok(); - impedance_output.phases_18.push(result.phase).ok(); - } - } - } - - IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok(); - } - continue; - } - } - } -} - - -extern crate libm; - -#[derive(Debug, Clone, Copy)] -pub struct Complex { - real: i32, - imag: i32, -} - -#[derive(Debug)] -pub struct ImpedanceResult { - pub magnitude: f32, - pub phase: f32, -} - -// Example Rcal value (Ohms) -const RCAL_VAL: f32 = 1000.0; - -/// Convert raw 18-bit 2's complement value to signed i32 -fn sign_extend_18bit(val: u32) -> i32 { - ((val << 14) as i32) >> 14 -} - -/// Calculate magnitude and phase of Rz using Rcal reference -pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult { - let mut signed_data = [0i32; 4]; - for (i, &val) in data.iter().enumerate() { - signed_data[i] = sign_extend_18bit(val); - } - - let dft_rcal = Complex { - real: signed_data[0], - imag: signed_data[1], - }; - - let dft_rz = Complex { - real: signed_data[2], - imag: signed_data[3], - }; - - let rcal_mag = libm::sqrtf((dft_rcal.real as f32) * (dft_rcal.real as f32) - + (dft_rcal.imag as f32) * (dft_rcal.imag as f32)); - - let rz_mag = libm::sqrtf((dft_rz.real as f32) * (dft_rz.real as f32) - + (dft_rz.imag as f32) * (dft_rz.imag as f32)); - - let rcal_phase = libm::atan2f(-(dft_rcal.imag as f32), dft_rcal.real as f32); - let rz_phase = libm::atan2f(-(dft_rz.imag as f32), dft_rz.real as f32); - - let magnitude = (rcal_mag / rz_mag) * RCAL_VAL; - let phase = rcal_phase - rz_phase; - - ImpedanceResult { magnitude, phase } -}