From 41ea2ebed4dfe917dcf845b452143cc9c90139c4 Mon Sep 17 00:00:00 2001
From: Matt Huszagh <huszaghmatt@gmail.com>
Date: Tue, 12 Jan 2021 15:50:26 -0800
Subject: [PATCH] use round up half integer rounding

---
 dsp/src/lib.rs |  6 ++++++
 src/main.rs    | 20 +++++++++++++-------
 2 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs
index 3f090b5..2f02601 100644
--- a/dsp/src/lib.rs
+++ b/dsp/src/lib.rs
@@ -20,6 +20,12 @@ pub fn shift_round(x: i32, shift: usize) -> i32 {
     (x + (1 << (shift - 1))) >> shift
 }
 
+/// TODO consolidate with `shift_round`.
+#[inline(always)]
+pub fn shift_round64(x: i64, shift: usize) -> i64 {
+    (x + (1 << (shift - 1))) >> shift
+}
+
 /// Integer division, round up half.
 ///
 /// # Arguments
diff --git a/src/main.rs b/src/main.rs
index 1bc6a11..3d484d8 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -101,6 +101,7 @@ use dac::{Dac0Output, Dac1Output};
 use dsp::{
     divide_round, iir, iir_int,
     pll::PLL,
+    shift_round, shift_round64,
     trig::{atan2, cossin},
 };
 use pounder::DdsOutput;
@@ -196,7 +197,7 @@ impl TimestampHandler {
         }
     }
 
-    /// Compute the initial phase value and frequency of the demodulation signal.
+    /// Compute the initial phase and frequency of the demodulation signal.
     ///
     /// # Args
     /// * `timestamp` - Counter value corresponding to an external reference edge.
@@ -1087,10 +1088,15 @@ const APP: () = {
 
                 let mut signal = (0_i32, 0_i32);
 
-                signal.0 = ((adc_samples[0][i] as i16 as i64 * cos as i64)
-                    >> 16) as i32;
-                signal.1 = ((adc_samples[0][i] as i16 as i64 * sin as i64)
-                    >> 16) as i32;
+                // TODO should we shift cos/sin first to avoid i64?
+                signal.0 = shift_round64(
+                    adc_samples[0][i] as i16 as i64 * cos as i64,
+                    16,
+                ) as i32;
+                signal.1 = shift_round64(
+                    adc_samples[0][i] as i16 as i64 * sin as i64,
+                    16,
+                ) as i32;
 
                 signal.0 =
                     iir_lockin.update(&mut iir_state_lockin[0], signal.0);
@@ -1100,8 +1106,8 @@ const APP: () = {
                 let magnitude = signal.0 * signal.0 + signal.1 * signal.1;
                 let phase = atan2(signal.1, signal.0);
 
-                *d0 = (magnitude >> 16) as i16 as u16;
-                *d1 = (phase >> 16) as i16 as u16;
+                *d0 = shift_round(magnitude, 16) as i16 as u16;
+                *d1 = shift_round(phase, 16) as i16 as u16;
             },
         );