mirror of
https://github.com/hubaldv/bioz-firmware-rs.git
synced 2025-12-06 05:01:18 +00:00
266 lines
11 KiB
Rust
266 lines
11 KiB
Rust
use bioz_icd_rs::MultiImpedanceOutput28;
|
|
use bioz_icd_rs::NumberOfPoints;
|
|
use defmt::{info, error};
|
|
|
|
use embassy_stm32::spi::Error;
|
|
use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
|
|
use embassy_sync::channel::Channel;
|
|
use embassy_sync::mutex::Mutex;
|
|
|
|
use static_cell::StaticCell;
|
|
|
|
use crate::ad5940::*;
|
|
use crate::ad5940_registers::*;
|
|
|
|
use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, InitImpedanceError};
|
|
use crate::icd_mapping::IntoDftnum;
|
|
|
|
pub static IMPEDANCE_CHANNEL_SINGLE: Channel<ThreadModeRawMutex, SingleImpedanceOutput, 2000> = Channel::new();
|
|
pub static IMPEDANCE_CHANNEL_MULTI: Channel<ThreadModeRawMutex, MultiImpedanceOutput28, 50> = Channel::new();
|
|
|
|
pub type ImpedanceSetupType = Mutex<ThreadModeRawMutex, ImpedanceSetup>;
|
|
pub static IMPEDANCE_SETUP: StaticCell<ImpedanceSetupType> = StaticCell::new();
|
|
|
|
#[derive(PartialEq, Copy, Clone)]
|
|
pub enum RunningMode {
|
|
None,
|
|
SingleFrequency,
|
|
MultiFrequency,
|
|
}
|
|
|
|
pub struct ImpedanceSetup {
|
|
ad5940: AD5940,
|
|
dsp_config: Option<DspConfig>,
|
|
pub running_mode: RunningMode,
|
|
}
|
|
|
|
impl ImpedanceSetup {
|
|
pub fn new(ad5940: AD5940) -> Self {
|
|
ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None }
|
|
}
|
|
|
|
pub async fn init(&mut self) -> Result<(), Error> {
|
|
// AFECON:
|
|
self.ad5940.afecon(
|
|
AFECON::DACBUFEN
|
|
| AFECON::DACREFEN
|
|
| AFECON::SINC2EN
|
|
| AFECON::TIAEN
|
|
| AFECON::INAMPEN
|
|
| AFECON::EXBUFEN
|
|
| AFECON::DACEN,
|
|
true,
|
|
)
|
|
.await;
|
|
|
|
// Set CLK configuration
|
|
let clk_config = ClkConfig::default()
|
|
.adcclkdiv(ADCCLKDIV::DIV1) // ADCCLK = 32MHz
|
|
.sysclkdiv(SYSCLKDIV::DIV2) // SYSCLK = 16MHz
|
|
.clk32mhzen(CLK32MHZEN::MHz32);
|
|
|
|
self.ad5940.apply_clk_config(&clk_config).await.unwrap();
|
|
|
|
// Set DSP configuration
|
|
let mut dsp_config = DspConfig::default();
|
|
dsp_config
|
|
.adc_mux_n(MUXSELN::HsTiaNeg)
|
|
.adc_mux_p(MUXSELP::HsTiaPos)
|
|
.ctiacon(CTIACON::C32)
|
|
.rtiacon(RTIACON::R1k)
|
|
.sinc3osr(SINC3OSR::R4)
|
|
.sinc2osr(SINC2OSR::R178)
|
|
.adcsamplerate(ADCSAMPLERATE::R1_6MHz)
|
|
.dftin_sel(DFTINSEL::GainOffset)
|
|
.dftnum(DFTNUM::Num4096)
|
|
.hanning(true)
|
|
.set_clks(16_000_000, 32_000_000); // Check clk_config: In this case SYSCLK = 16MHz and ADCCLK = 32MHz
|
|
|
|
self.ad5940.apply_dsp_config(&dsp_config).await.unwrap();
|
|
self.dsp_config = Some(dsp_config);
|
|
|
|
// Set SRAM configuration (cmd and data sram)
|
|
let mut sram_config = SramConfig::default();
|
|
sram_config
|
|
.datafifosrcsel(DATAFIFOSRCSEL::DFT)
|
|
.datafifoen(DATAFIFOEN::Normal)
|
|
.data_size(DATA_MEM_SEL::Size2kB)
|
|
.cmd_mode(CMDMEMMDE::MemoryMode)
|
|
.cmd_size(CMD_MEM_SEL::Size2kB);
|
|
self.ad5940.apply_sram_config(sram_config).await.unwrap();
|
|
|
|
// WGCON: set sinus output
|
|
let config_wgcon = WGCON::TYPESEL_SIN.bits();
|
|
self.ad5940.write_reg(Register::WGCON, config_wgcon).await.unwrap();
|
|
|
|
Ok(())
|
|
}
|
|
|
|
pub async fn init_single_frequency_measurement(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result<f32, InitImpedanceError> {
|
|
// Reset FIFO
|
|
self.ad5940.clear_and_enable_fifo().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(InitImpedanceError::DSPNotSet);
|
|
}
|
|
|
|
// 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();
|
|
|
|
// 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();
|
|
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;
|
|
|
|
// Rz
|
|
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();
|
|
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_multi_frequency_measurement(&mut self, dft_number: IcdDftNum, number_of_points: NumberOfPoints) {
|
|
// Reset FIFO
|
|
self.ad5940.clear_and_enable_fifo().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 mut wait_time = 0;
|
|
if let Some(dsp_config) = &self.dsp_config {
|
|
wait_time = self.ad5940.sequencer_calculate_wait_time(dsp_config).await.unwrap();
|
|
// info!("Sinus periods per DFT: {}", wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32);
|
|
} else {
|
|
error!("DSP configuration not set, cannot calculate wait time");
|
|
}
|
|
|
|
// Configure sequencer
|
|
self.ad5940.sequencer_enable(true).await;
|
|
|
|
for &frequency in number_of_points.values() {
|
|
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();
|
|
|
|
// 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();
|
|
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;
|
|
|
|
// Rz
|
|
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();
|
|
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;
|
|
}
|
|
|
|
pub async fn start_measurement(&mut self) {
|
|
self.ad5940.sequencer_trigger(0).await;
|
|
}
|
|
|
|
pub async fn get_fifo_count(&mut self) -> Result<u32, Error> {
|
|
self.ad5940.get_fifo_count().await
|
|
}
|
|
|
|
pub async fn read_fifo(&mut self, data: &mut [u32]) -> Result<(), Error> {
|
|
self.ad5940.read_fifo(data).await
|
|
}
|
|
}
|