From b502b42c9246d716239ec491aebcc077a85260ff Mon Sep 17 00:00:00 2001 From: occheung Date: Tue, 15 Sep 2020 14:03:59 +0800 Subject: [PATCH] scpi: fix frequency control --- Cargo.toml | 1 + examples/ethernet.rs | 2 +- examples/mqtt_client.rs | 2 +- src/config_register.rs | 1 + src/dds.rs | 45 ++++++++++++++++++++++------------------- src/lib.rs | 20 +++++++++++++----- src/scpi.rs | 2 +- src/translation.rs | 13 ++++++------ 8 files changed, 51 insertions(+), 35 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 8263d6c..b678d44 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,6 +13,7 @@ embedded-hal = "0.2.4" stm32h7xx-hal = {version = "0.7.1", features = [ "stm32h743v", "rt", "unproven", "ethernet", "phy_lan8742a" ] } smoltcp = { version = "0.6.0", default-features = false, features = [ "ethernet", "proto-ipv4", "proto-ipv6", "socket-tcp", "log" ] } nb = "1.0.0" +libm = "0.2.0" embedded-nal = "0.1.0" minimq = { git = "https://github.com/quartiq/minimq.git", branch = "master" } diff --git a/examples/ethernet.rs b/examples/ethernet.rs index 41b97bf..b004ce3 100644 --- a/examples/ethernet.rs +++ b/examples/ethernet.rs @@ -208,7 +208,7 @@ fn main() -> ! { let parts = switch.split(); let mut urukul = Urukul::new( parts.spi1, parts.spi2, parts.spi3, parts.spi4, parts.spi5, parts.spi6, parts.spi7, - [25_000_000, 25_000_000, 25_000_000, 25_000_000] + [25_000_000.0, 25_000_000.0, 25_000_000.0, 25_000_000.0] ); // Setup ethernet pins diff --git a/examples/mqtt_client.rs b/examples/mqtt_client.rs index cc4d1ad..a09b618 100644 --- a/examples/mqtt_client.rs +++ b/examples/mqtt_client.rs @@ -209,7 +209,7 @@ fn main() -> ! { let mut urukul = Urukul::new( parts.spi1, parts.spi2, parts.spi3, parts.spi4, parts.spi5, parts.spi6, parts.spi7, - [25_000_000, 25_000_000, 25_000_000, 25_000_000] + [25_000_000.0, 25_000_000.0, 25_000_000.0, 25_000_000.0] ); cp.SCB.invalidate_icache(); diff --git a/src/config_register.rs b/src/config_register.rs index 324fb3e..acf3e14 100644 --- a/src/config_register.rs +++ b/src/config_register.rs @@ -75,6 +75,7 @@ where /* * Return selected configuration field + * TODO: Return result type instead for error checking */ pub fn get_configuration(&mut self, config_type: CFGMask) -> u8 { config_type.get_filtered_content(self.data) as u8 diff --git a/src/dds.rs b/src/dds.rs index b3707bf..7f9238c 100644 --- a/src/dds.rs +++ b/src/dds.rs @@ -1,6 +1,7 @@ use embedded_hal::blocking::spi::Transfer; use crate::Error; use core::mem::size_of; +use libm::round; /* * Bitmask for all configurations (Order: CFR3, CFR2, CFR1) @@ -64,15 +65,15 @@ const READ_MASK :u8 = 0x80; pub struct DDS { spi: SPI, - f_ref_clk: u64, - f_sys_clk: u64, + f_ref_clk: f64, + f_sys_clk: f64, } impl DDS where SPI: Transfer { - pub fn new(spi: SPI, f_ref_clk: u64) -> Self { + pub fn new(spi: SPI, f_ref_clk: f64) -> Self { DDS { spi, f_ref_clk, @@ -121,7 +122,7 @@ where // Ensure divider is not reset (DDSCFRMask::REFCLK_IN_DIV_RESETB, 1), ])?; - self.f_sys_clk = self.f_ref_clk / 2; + self.f_sys_clk = self.f_ref_clk / 2.0; Ok(()) } @@ -138,9 +139,9 @@ where Ok(()) } - pub fn enable_pll(&mut self, f_sys_clk: u64) -> Result<(), Error> { + pub fn enable_pll(&mut self, f_sys_clk: f64) -> Result<(), Error> { // Get a divider - let divider = f_sys_clk / self.f_ref_clk; + let divider = (f_sys_clk / self.f_ref_clk) as u64; // Reject extreme divider values. However, accept no frequency division if ((divider > 127 || divider < 12) && divider != 1) { // panic!("Invalid divider value for PLL!"); @@ -159,14 +160,16 @@ where self.set_configurations(&mut [ (DDSCFRMask::PFD_RESET, 0), ])?; - self.f_sys_clk = self.f_ref_clk * divider; + self.f_sys_clk = self.f_ref_clk * (divider as f64); Ok(()) } // Change external clock source (ref_clk) - pub fn set_ref_clk_frequency(&mut self, f_ref_clk: u64) -> Result<(), Error> { + pub fn set_ref_clk_frequency(&mut self, f_ref_clk: f64) -> Result<(), Error> { + // Override old reference clock frequency (ref_clk) self.f_ref_clk = f_ref_clk; - // TODO: Examine clock tree and update f_sys_clk + + // Calculate the new system clock frequency, examine the clock tree let mut configuration_queries = [ // Acquire PLL status (DDSCFRMask::PLL_ENABLE, 0), @@ -179,7 +182,7 @@ where self.get_configurations(&mut configuration_queries)?; if configuration_queries[0].1 == 1 { // Recalculate sys_clk - let divider :u64 = configuration_queries[1].1.into(); + let divider :f64 = configuration_queries[1].1.into(); let f_sys_clk = self.f_ref_clk * divider; // Adjust VCO match self.get_VCO_no(f_sys_clk, divider as u8) { @@ -204,7 +207,7 @@ where } } else if configuration_queries[2].1 == 0 { - self.f_sys_clk = self.f_ref_clk / 2; + self.f_sys_clk = self.f_ref_clk / 2.0; Ok(()) } else { @@ -214,23 +217,23 @@ where } #[allow(non_snake_case)] - fn get_VCO_no(&mut self, f_sys_clk: u64, divider: u8) -> Result> { + fn get_VCO_no(&mut self, f_sys_clk: f64, divider: u8) -> Result> { // Select a VCO if divider == 1 { Ok(6) // Bypass PLL if no frequency division needed - } else if f_sys_clk > 1_150_000_000 { + } else if f_sys_clk > 1_150_000_000.0 { Err(Error::DDSCLKError) - } else if f_sys_clk > 820_000_000 { + } else if f_sys_clk > 820_000_000.0 { Ok(5) - } else if f_sys_clk > 700_000_000 { + } else if f_sys_clk > 700_000_000.0 { Ok(4) - } else if f_sys_clk > 600_000_000 { + } else if f_sys_clk > 600_000_000.0 { Ok(3) - } else if f_sys_clk > 500_000_000 { + } else if f_sys_clk > 500_000_000.0 { Ok(2) - } else if f_sys_clk > 420_000_000 { + } else if f_sys_clk > 420_000_000.0 { Ok(1) - } else if f_sys_clk > 370_000_000 { + } else if f_sys_clk > 370_000_000.0 { Ok(0) } else { Ok(7) // Bypass PLL if f_sys_clk is too low @@ -311,14 +314,14 @@ where * Frequency: Must be integer * Amplitude: In a scale from 0 to 1, taking float */ - pub fn set_single_tone_profile(&mut self, profile: u8, f_out: u64, phase_offset: f64, amp_scale_factor: f64) -> Result<(), Error> { + pub fn set_single_tone_profile(&mut self, profile: u8, f_out: f64, phase_offset: f64, amp_scale_factor: f64) -> Result<(), Error> { assert!(profile < 8); assert!(phase_offset >= 0.0 && phase_offset < 360.0); assert!(amp_scale_factor >=0.0 && amp_scale_factor <= 1.0); let resolutions :[u64; 3] = [1 << 32, 1 << 16, 1 << 14]; - let ftw = (resolutions[0] * f_out / self.f_sys_clk) as u32; + let ftw = ((resolutions[0] as f64) * f_out / self.f_sys_clk) as u32; let pow = ((resolutions[1] as f64) * phase_offset / 360.0) as u16; let asf :u16 = if amp_scale_factor == 1.0 { 0x3FFF diff --git a/src/lib.rs b/src/lib.rs index 9482a59..52ef2f4 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -81,7 +81,7 @@ where * Master constructor for the entire Urukul device * Supply 7 SPI channels to Urukul and 4 reference clock frequencies */ - pub fn new(spi1: SPI, spi2: SPI, spi3: SPI, spi4: SPI, spi5: SPI, spi6: SPI, spi7: SPI, f_ref_clks: [u64; 4]) -> Self { + pub fn new(spi1: SPI, spi2: SPI, spi3: SPI, spi4: SPI, spi5: SPI, spi6: SPI, spi7: SPI, f_ref_clks: [f64; 4]) -> Self { // Construct Urukul Urukul { config_register: ConfigRegister::new(spi1), @@ -130,7 +130,7 @@ where // Clock tree reset. CPLD divides clock frequency by 4 by default. for chip_no in 0..4 { - self.dds[chip_no].set_ref_clk_frequency(25_000_000)?; + self.dds[chip_no].set_ref_clk_frequency(25_000_000.0)?; } Ok(()) } @@ -153,7 +153,7 @@ pub trait UrukulTraits { type Error; fn get_channel_switch_status(&mut self, channel: u32) -> Result; fn set_channel_switch(&mut self, channel: u32, status: bool) -> Result<(), Self::Error>; - fn set_clock_source(&mut self, source: ClockSource) -> Result<(), Self::Error>; + fn set_clock_source(&mut self, source: ClockSource, frequency: f64) -> Result<(), Self::Error>; fn set_clock_division(&mut self, division: u8) -> Result<(), Self::Error>; fn set_channel_attenuation(&mut self, channel: u8, attenuation: f32) -> Result<(), Self::Error>; } @@ -190,7 +190,8 @@ where } } - fn set_clock_source(&mut self, source: ClockSource) -> Result<(), Self::Error> { + fn set_clock_source(&mut self, source: ClockSource, frequency: f64) -> Result<(), Self::Error> { + // Change clock source through configuration register match source { ClockSource::OSC => self.config_register.set_configurations(&mut [ (CFGMask::CLK_SEL0, 0), @@ -203,7 +204,16 @@ where ClockSource::SMA => self.config_register.set_configurations(&mut [ (CFGMask::CLK_SEL0, 1), ]), - }.map(|_| ()) + }?; + + // Calculate reference clock frequency after clock division from configuration register + let frequency = frequency / (self.config_register.get_configuration(CFGMask::DIV) as f64); + + // Update all DDS chips on reference clock frequency + for dds_channel in 0..4 { + self.dds[dds_channel].set_ref_clk_frequency(frequency)?; + } + Ok(()) } fn set_clock_division(&mut self, division: u8) -> Result<(), Self::Error> { diff --git a/src/scpi.rs b/src/scpi.rs index ea2fd43..1017ea6 100644 --- a/src/scpi.rs +++ b/src/scpi.rs @@ -281,7 +281,7 @@ impl Command for ClockSourceCommand { }; trace!("Changing clock source to {:?} at {:?}", clock_source, frequency); - context.device.set_clock_source(clock_source) + context.device.set_clock_source(clock_source, frequency.get::()) .map_err(|_| Error::new(ErrorCode::HardwareError)) } } diff --git a/src/translation.rs b/src/translation.rs index 33bd63f..74f14a0 100644 --- a/src/translation.rs +++ b/src/translation.rs @@ -1,10 +1,8 @@ use scpi::prelude::*; use scpi::Context; use scpi::error::Result; - -use core::concat; - -use arrayvec::{ArrayVec, ArrayString}; +use log::trace; +use arrayvec::{ArrayVec}; pub trait MqttScpiTranslator { // Convert an MQTT publish message into SCPI compatible command @@ -25,10 +23,10 @@ impl<'a, T: Device> MqttScpiTranslator for Context<'a, T> { if i == '/' { // The topic separator is colon(':') in SCPI, and slash('/') in MQTT buffer.try_push(b':') - .map_err(|_| ErrorCode::OutOfMemory)?; + .map_err(|_| ErrorCode::OutOfMemory)?; } else { buffer.try_push(i as u8) - .map_err(|_| ErrorCode::OutOfMemory)?; + .map_err(|_| ErrorCode::OutOfMemory)?; } } @@ -42,6 +40,9 @@ impl<'a, T: Device> MqttScpiTranslator for Context<'a, T> { .map_err(|_| ErrorCode::OutOfMemory)?; } + // Pass the message to SCPI processing unit + trace!("Translated MQTT message into SCPI. Translated command: {}", + core::str::from_utf8(buffer.as_slice()).unwrap()); self.run(buffer.as_slice(), response) } } \ No newline at end of file