1 files changed, 22 insertions, 9 deletions
diff --git a/sw/picardy/src/state.rs b/sw/picardy/src/state.rs
index 4e6af33..08826e7 100644
--- a/sw/picardy/src/state.rs
+++ b/sw/picardy/src/state.rs
@@ -64,6 +64,7 @@ pub struct State {
     pub vfo_sel : VFOSelection,
     pub rit : i32,
     pub mode : Mode,
+    pub send_tone : bool,
     pub sequence_state : SequenceState,
     pub update_disp_counter : u8,
     pub cw_wpm : u32,
@@ -79,6 +80,7 @@ impl State {
             vfo_sel : VFOSelection::A,
             rit : 0,
             mode : Mode::USB,
+            send_tone : false,
             sequence_state : SequenceState::Rx,
             update_disp_counter : 0,
             cw_wpm : 14,
@@ -86,14 +88,19 @@ impl State {
     }
 
     pub fn bfo(&self) -> u32 {
-        match self.mode {
-            Mode::LSB => BFO_LSB,
-            Mode::USB => BFO_USB,
-            Mode::CustomShift(fs) => fs,
-            Mode::CW(_) => match self.sequence_state {
-                SequenceState::SwitchingCW | SequenceState::TxCW => 0,
-                _ => BFO_CW,
-            },
+        if self.send_tone {
+            0
+        }
+        else {
+            match self.mode {
+                Mode::LSB => BFO_LSB,
+                Mode::USB => BFO_USB,
+                Mode::CustomShift(fs) => fs,
+                Mode::CW(_) => match self.sequence_state {
+                    SequenceState::SwitchingCW | SequenceState::TxCW => 0,
+                    _ => BFO_CW,
+                },
+            }
         }
     }
 
@@ -109,8 +116,14 @@ impl State {
     }
 
     pub fn vfo(&self) -> u32 {
+        let cw_offset = match self.sequence_state {
+            SequenceState::SwitchingCW | SequenceState::TxCW => 500,
+            _ => 0,
+        };
+
         let vfo = (self.if_qrg() - self.bfo()) as i32 +
-            if self.sequence_state.apply_rit() { self.rit } else { 0 };
+            if self.sequence_state.apply_rit() { self.rit } else { 0 } +
+                cw_offset;
         vfo as u32
     }
 }