command to set radio frequency

src/lib.rs

@@ -22,6 +22,8 @@ use state::State;
 
 use crate::internal_radio::InternalRadio;
 
+const VCXO_FREQ: u32 = 30_000_000;
+
 /// All the rx_buf methods MUST be passed the same rx_buf
 /// All the tx_buf methods MUST be passed the same tx_buf
 /// We do this because we don't keep that data locally for space reasons
@@ -80,6 +82,63 @@ where
         self.channel = channel;
     }
 
+    /// Frequency given in Hz
+    pub fn set_frequency(&mut self, freq: u32) -> Result<(), Spi::Error> {
+        let outdiv;
+        let band;
+
+        match freq {
+            x if x < 177_000_000 => {
+                outdiv = 24;
+                band = 5;
+            }
+            x if x < 239_000_000 => {
+                outdiv = 16;
+                band = 4;
+            }
+            x if x < 353_000_000 => {
+                outdiv = 12;
+                band = 3;
+            }
+            x if x < 525_000_000 => {
+                outdiv = 8;
+                band = 2;
+            }
+            x if x < 705_000_000 => {
+                outdiv = 6;
+                band = 1;
+            }
+            _ => {
+                outdiv = 4;
+                band = 0;
+            }
+        }
+
+        let f_pfd = 2 * VCXO_FREQ / outdiv;
+        let n: u8 = (freq / f_pfd - 1).try_into().unwrap();
+        let ratio = freq as f32 / f_pfd as f32;
+        let rest = ratio - n as f32;
+        let m = (rest * 524_288.0) as u32;
+
+        // set band parameter
+        self.radio
+            .send_command::<0>(&mut [SET_PROPERTY, 0x20, 0x01, 0x51, 8 + band])?;
+
+        // set pll parameters
+        self.radio.send_command::<0>(&mut [
+            SET_PROPERTY,
+            0x40,
+            0x06,
+            0x00,
+            n,
+            ((m >> 16) & 0xFF).try_into().unwrap(),
+            ((m >> 8) & 0xFF).try_into().unwrap(),
+            (m & 0xFF).try_into().unwrap(),
+        ])?;
+
+        Ok(())
+    }
+
     pub fn is_idle(&mut self) -> bool {
         matches!(self.state, InternalState::Idle)
     }