From bf3f00cfb083639c3ea16c6a1ede7d6fdac1dd08 Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Thu, 14 Aug 2025 17:31:46 +0200 Subject: [PATCH] Basic working impedance. --- src/ad5940.rs | 11 ++++++++- src/communication.rs | 18 +++++++++++---- src/impedance_test.rs | 3 ++- src/main.rs | 54 +++++++++++++++++++++++++++++++++++-------- 4 files changed, 70 insertions(+), 16 deletions(-) diff --git a/src/ad5940.rs b/src/ad5940.rs index f1c08c5..35c8c14 100644 --- a/src/ad5940.rs +++ b/src/ad5940.rs @@ -348,12 +348,14 @@ impl AD5940 { // Disable fifo self.write_reg(Register::FIFOCON, 0b010 << 13).await.unwrap(); + // self.write_reg(Register::FIFOCON, 0b011 << 13).await.unwrap(); let cmd = 0b101 << 9 | 0b001 << 6 | 0b01 << 3 | 0b01; self.write_reg(Register::CMDDATACON, cmd).await.unwrap(); // Enable FIFO self.write_reg(Register::FIFOCON, 0b010 << 13 | 1 << 11 ).await.unwrap(); + // self.write_reg(Register::FIFOCON, 0b011 << 13 | 1 << 11 ).await.unwrap(); // Enable sequencer let mut reg = self.read_reg(Register::SEQCON).await.unwrap(); @@ -395,6 +397,11 @@ impl AD5940 { Ok((tempsensdat0 as f32/(pga_gain * k)) - 273.15) } + pub async fn get_fifo_count(&mut self) -> Result { + let data = self.read_reg(Register::FIFOCNTSTA).await?; + Ok((data >> 16) & 0x7FF) + } + pub async fn init_waveform(&mut self) -> Result<(), Error> { // Set frequency let freq: u32 = 1000; @@ -433,7 +440,7 @@ impl AD5940 { self.afecon( AFECON::DACBUFEN | AFECON::DACREFEN - // | AFECON::SINC2EN + | AFECON::SINC2EN // | AFECON::DFTEN // | AFECON::WAVEGENEN | AFECON::TIAEN @@ -528,4 +535,6 @@ pub enum Register { HSDACCON = 0x0000_2010, // High Speed DAC Configuration Register HSRTIACON = 0x0000_20F0, // High Speed RTIA Configuration Register BUFSENCON = 0x0000_2180, // HIGH POWER AND LOW POWER BUFFER CONTROL REGISTER + FIFOCNTSTA = 0x0000_2200, // Command and data FIFO internal data count register + ADCFILTERCON = 0x0000_2044 // ADC Output Filters Configuration Register } \ No newline at end of file diff --git a/src/communication.rs b/src/communication.rs index b33fcf5..a2acb08 100644 --- a/src/communication.rs +++ b/src/communication.rs @@ -22,7 +22,7 @@ use postcard_rpc::{ use bioz_icd_rs::{PingEndpoint, GetUniqueIdEndpoint, SetGreenLedEndpoint, StartImpedanceEndpoint, StopImpedanceEndpoint, StartImpedance, Impedance, ImpedanceTopic, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST}; -use crate::impedance_test::{ImpedanceTest, IMPEDANCE_TEST}; +use crate::impedance_test::{ImpedanceTest, IMPEDANCE_TEST, IMPEDANCE_CHANNEL}; // Postcard RPC types type AppDriver = usb::Driver<'static, peripherals::USB>; @@ -179,13 +179,21 @@ pub async fn start_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: let mut ticker = Ticker::every(Duration::from_hz(rqst.update_frequency.into())); let mut seq: u8 = 0; while !*STOP.lock().await { - ticker.next().await; + // ticker.next().await; - impedance.update(rqst.sinus_frequency); + // impedance.update(rqst.sinus_frequency); + + + // let msg = Impedance { + // magnitude: impedance.magnitude, + // phase: impedance.phase, + // }; + + let data = IMPEDANCE_CHANNEL.receive().await; let msg = Impedance { - magnitude: impedance.magnitude, - phase: impedance.phase, + magnitude: data, + phase: data, }; if sender diff --git a/src/impedance_test.rs b/src/impedance_test.rs index 8e0d3dc..df0190e 100644 --- a/src/impedance_test.rs +++ b/src/impedance_test.rs @@ -1,5 +1,6 @@ use embassy_sync::mutex::Mutex; use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; +use embassy_sync::channel::Channel; use static_cell::StaticCell; @@ -7,7 +8,7 @@ use libm::{sinf, cosf}; use core::f32::consts::PI; pub static IMPEDANCE_TEST: StaticCell> = StaticCell::new(); - +pub static IMPEDANCE_CHANNEL: Channel = Channel::new(); pub struct ImpedanceTest { time: embassy_time::Instant, pub magnitude: f32, diff --git a/src/main.rs b/src/main.rs index 2ddfc45..c9e0cc4 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,7 +1,7 @@ #![no_std] #![no_main] -use defmt::info; +use defmt::{info, error}; use embassy_executor::Spawner; use embassy_stm32::exti::ExtiInput; use embassy_time::{Timer, Duration}; @@ -10,7 +10,7 @@ use embassy_stm32::gpio::{Level, Output, Speed}; use embassy_stm32::{i2c, spi, Config}; use embassy_stm32::time::Hertz; -use crate::ad5940_registers::{AFECON, SWCON}; +use crate::ad5940_registers::{ADCCON, AFECON, SWCON}; use {defmt_rtt as _, panic_probe as _}; @@ -33,6 +33,8 @@ use communication::{init_communication, LED_FREQUENCY_SIGNAL}; mod impedance_test; +use impedance_test::IMPEDANCE_CHANNEL; + bind_interrupts!(struct Irqs { USB_DRD_FS => usb::InterruptHandler; }); @@ -117,7 +119,7 @@ async fn main(spawner: Spawner) { // Sequencer test ad5940.sequencer_enable(true).await; - ad5940.wgfcw(50000).await; + ad5940.wgfcw(100).await; let wg_amplitude = 2047; // 2047 is the maximum amplitude for a 12-bit DAC --> 1.62V peak-to-peak ad5940.write_reg(ad5940::Register::WGAMPLITUDE, wg_amplitude).await.unwrap(); @@ -128,7 +130,6 @@ async fn main(spawner: Spawner) { ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await; ad5940.sequencer_wait(16 * 750_000).await; // 0.75 second ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await; - ad5940.write_reg(ad5940::Register::AFECON, 0x0000_FFFF).await.unwrap(); // Rz ad5940.swcon(SWCON::DMUXCON_D5 | SWCON::PMUXCON_P11 | SWCON::NMUXCON_N2 | SWCON::TMUXCON_T2 | SWCON::T9CON).await; @@ -160,6 +161,23 @@ async fn main(spawner: Spawner) { // Green led task // spawner.must_spawn(green_led(led)); + // ad5940.sequencer_trigger(0).await; + + // Set inputs + // ad5940.write_reg(ad5940::Register::ADCCON, 0b00101 << 8 | 0b011001).await.unwrap(); + // ad5940.write_reg(ad5940::Register::ADCCON, ADCCON::MUXSELN_TIAN.bits() | ADCCON::MUXSELP_TIAP.bits()).await.unwrap(); + + // ad5940.wgfcw(10).await; + // let wg_amplitude = 2047; // 2047 is the maximum amplitude for a 12-bit DAC --> 1.62V peak-to-peak + // ad5940.write_reg(ad5940::Register::WGAMPLITUDE, wg_amplitude).await.unwrap(); + // ad5940.swcon(SWCON::DMUXCON_D5 | SWCON::PMUXCON_P11 | SWCON::NMUXCON_N2 | SWCON::TMUXCON_T2 | SWCON::T9CON).await; + // ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN | AFECON::ADCCONVEN, true).await; + + ad5940.write_reg(ad5940::Register::ADCFILTERCON, 0x00000301 | 1 << 4).await.unwrap(); + + ad5940.write_reg(ad5940::Register::HSRTIACON, 0b010000 << 5 | 0b0010).await.unwrap(); + + let mut counter = 0; loop { // Read chip id // let chip_id = ad5940.get_chipid().await; @@ -178,10 +196,29 @@ async fn main(spawner: Spawner) { // info!("Mainloop still running!"); - Timer::after_millis(1750).await; + Timer::after_millis(2500).await; - // let test = ad5940.read_reg_raw(0x2200).await.unwrap(); - // info!("FIFOCNTSTA: {}", (test>>16) & 0b111_1111_1111); + let count = ad5940.get_fifo_count().await.unwrap(); + info!("FIFOCNTSTA: {}", count); + + // for _ in 0..count { + // let mut data = ad5940.read_reg(ad5940::Register::DATAFIFORD).await.unwrap(); + + // data &= 0xFFFF; // Mask to 16 bits + + // use libm::powf; + // let value = 1.82 * (data as i32 - 0x8000) as f32 / powf(2f32, 15f32); + // // let value = (data as i32 - 0x8000) as f32; + + // match IMPEDANCE_CHANNEL.try_send(value) { + // Ok(_) => {counter += 1;}, + // Err(e) => { + // info!("Failed to send impedance data: {}", e); + // } + // } + // } + + // info!("Counter: {}", counter); let mut data: [u32; 4] = [0; 4]; data[0] = ad5940.read_reg(ad5940::Register::DATAFIFORD).await.unwrap(); @@ -194,8 +231,7 @@ async fn main(spawner: Spawner) { // You’ll need to implement your own logging or send this over serial in embedded info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase); - // let test = ad5940.read_reg_raw(0x2200).await.unwrap(); - // info!("FIFOCNTSTA: {}", (test>>16) & 0b111_1111_1111); + } }