semi working quadrature output

src/lib.rs

@@ -354,6 +354,7 @@ pub struct Si5351Device<I2C> {
     clk_enabled_mask: u8,
     ms_int_mode_mask: u8,
     ms_src_mask: u8,
+    last_mdiv: u8,
 }
 
 pub trait Si5351 {
@@ -376,7 +377,13 @@ pub trait Si5351 {
     fn setup_multisynth(&mut self, ms: Multisynth, div: u16, num: u32, denom: u32, r_div: OutputDivider) -> Result<(), Error>;
     fn select_clock_pll(&mut self, clocl: ClockOutput, pll: PLL);
 
+    fn set_quad(&mut self, freq: u32) -> Result<f32, Error>;
     fn set_phase_offset(&mut self, clk: ClockOutput, phase_offset: u8) -> Result<(), Error>;
+    fn reset_pll(&mut self, pll: PLL) -> Result<(), Error>;
+}
+
+impl<T> Si5351Device<T> {    
+    pub fn get_xtal_freq(&self) -> u32 {self.xtal_freq}
 }
 
 impl<I2C, E> Si5351Device<I2C>
@@ -392,11 +399,13 @@ impl<I2C, E> Si5351Device<I2C>
             clk_enabled_mask: 0,
             ms_int_mode_mask: 0,
             ms_src_mask: 0,
+            last_mdiv: 0,
         };
 
         si5351
     }
 
+
     pub fn new_adafruit_module(i2c: I2C) -> Self {
         Si5351Device::new(i2c, false, 25_000_000)
     }
@@ -448,7 +457,7 @@ impl<I2C, E> Si5351Device<I2C>
         Ok(())
     }
 
-    fn reset_pll(&mut self, pll: PLL) -> Result<(), Error> {
+    pub fn reset_pll(&mut self, pll: PLL) -> Result<(), Error> {
         self.write_register(Register::PLLReset, match pll {
             PLL::A => PLLResetBits::PLLA_RST.bits(),
             PLL::B => PLLResetBits::PLLB_RST.bits(),
@@ -477,6 +486,11 @@ impl<I2C, E> Si5351Device<I2C>
 impl<I2C, E> Si5351 for Si5351Device<I2C> where
     I2C: WriteRead<Error=E> + Write<Error=E>
 {
+    
+    fn reset_pll(&mut self, pll: PLL) -> Result<(), Error> {
+        self.reset_pll(pll)
+    }
+
     fn init_adafruit_module(&mut self) -> Result<(), Error> {
         self.init(CrystalLoad::_10)
     }
@@ -564,6 +578,43 @@ impl<I2C, E> Si5351 for Si5351Device<I2C> where
 
         Ok(())
     }
+    fn set_quad(&mut self, freq: u32) -> Result<f32, Error> {
+        let ms0 = Multisynth::MS0;
+        let ms1 = Multisynth::MS1;
+        let mut ms_div = 126;
+        let mut mult = (freq*ms_div) as f32 / self.get_xtal_freq() as f32;
+        while mult > 90.0 {
+            ms_div -= (ms_div as f32 * 0.2) as u32;
+            mult = (freq*ms_div) as f32 / self.get_xtal_freq() as f32;
+        }
+        if mult < 15.0 {return Err(Error::InvalidParameter);}
+
+        //easy calc fraction not the most accurate
+        let denom: u32 = 1048575;
+        let num = ((mult % 1.0) * denom as f32) as u32;
+        let mult = mult as u8;
+        let ms_div = ms_div as u8;
+
+        self.setup_pll(PLL::A, mult, num, denom)?;
+        self.setup_multisynth_int(ms0, ms_div as u16, OutputDivider::Div1)?;
+        self.setup_multisynth_int(ms1, ms_div as u16, OutputDivider::Div1)?;
+        self.select_clock_pll(ClockOutput::Clk0, PLL::A);
+        self.select_clock_pll(ClockOutput::Clk1, PLL::A);
+        self.set_clock_enabled(ClockOutput::Clk0, true);
+        self.set_clock_enabled(ClockOutput::Clk1, true);
+        self.flush_clock_control(ClockOutput::Clk0)?;
+        self.flush_clock_control(ClockOutput::Clk1)?;
+
+        if self.last_mdiv != ms_div {
+            self.set_phase_offset(ClockOutput::Clk1, ms_div as u8)?;
+            self.reset_pll(PLL::A)?;
+        }
+        self.last_mdiv = ms_div;
+        self.flush_output_enabled()?;
+
+        let freq_num = self.xtal_freq as f32 * (mult as f32 + (num as f32 / denom as f32));
+        Ok( freq_num / ms_div as f32 )
+    }
 
     fn set_phase_offset(&mut self, clk: ClockOutput, phase_offset: u8) -> Result<(), Error> {
         if phase_offset & 1<<7 > 0 {return Err(Error::InvalidParameter);}