scpi: fix frequency control

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" }

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

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();

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

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: SPI,
-	f_ref_clk: u64,
+	f_ref_clk: f64,
-	f_sys_clk: u64,
+	f_sys_clk: f64,
 }
 
 impl<SPI, E> DDS<SPI>
 where
 	SPI: Transfer<u8, Error = E>
 {
-	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<E>> {
+	pub fn enable_pll(&mut self, f_sys_clk: f64) -> Result<(), Error<E>> {
 		// 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<E>> {
+	pub fn set_ref_clk_frequency(&mut self, f_ref_clk: f64) -> Result<(), Error<E>> {
+		// 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<u8, Error<E>> {
+	fn get_VCO_no(&mut self, f_sys_clk: f64, divider: u8) -> Result<u8, Error<E>> {
 		// 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<E>> {
+	pub fn set_single_tone_profile(&mut self, profile: u8, f_out: f64, phase_offset: f64, amp_scale_factor: f64) -> Result<(), Error<E>> {
 
 		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

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<bool, Self::Error>;
 	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> {

src/scpi.rs

@@ -281,7 +281,7 @@ impl<T:Device + UrukulTraits> Command<T> 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::<hertz>())
                         .map_err(|_| Error::new(ErrorCode::HardwareError))
     }
 }

src/translation.rs

@@ -1,10 +1,8 @@
 use scpi::prelude::*;
 use scpi::Context;
 use scpi::error::Result;
-
+use log::trace;
-use core::concat;
+use arrayvec::{ArrayVec};
-
-use arrayvec::{ArrayVec, ArrayString};
 
 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)
     }
 }