From b0e8aee11ec17a652b39e717468ac9e2bf1a0f4f Mon Sep 17 00:00:00 2001
From: TT <edy555@gmail.com>
Date: Mon, 19 Sep 2016 09:31:50 +0900
Subject: [PATCH] add temprally gamma caluclation

---
 dsp.c     | 38 +++++++++++++++++++++++++++++++++++---
 main.c    | 34 +++++++++++++++++++++++++++-------
 nanovna.h |  8 +++++++-
 3 files changed, 69 insertions(+), 11 deletions(-)

diff --git a/dsp.c b/dsp.c
index 0e3ca1a..512992d 100644
--- a/dsp.c
+++ b/dsp.c
@@ -3,7 +3,8 @@
 
 int16_t ref_state[STATE_LEN];
 int16_t ref_buf[SAMPLE_LEN];
-int16_t refq_buf[SAMPLE_LEN];
+//int16_t refq_buf[SAMPLE_LEN];
+int16_t refiq_buf[AUDIO_BUFFER_LEN];
 
 int16_t samp_buf[SAMPLE_LEN];
 
@@ -30,7 +31,8 @@ static void
 hilbert_transform(void)
 { 
   __SIMD32_TYPE *src = __SIMD32_CONST(ref_state);
-  __SIMD32_TYPE *dst = __SIMD32_CONST(refq_buf);
+  //__SIMD32_TYPE *dst = __SIMD32_CONST(refq_buf);
+  __SIMD32_TYPE *dst = __SIMD32_CONST(refiq_buf);
   int j;
 
   for (j = 0; j < SAMPLE_LEN / 2; j++) {
@@ -39,6 +41,7 @@ hilbert_transform(void)
     int32_t accn0 = 0;
     int32_t acc1 = 0;
     int32_t accn1 = 0;
+    int32_t s;
 
     for (i = 0; i < 8; i += 2) {
       uint32_t c = *(uint32_t*)&hilbert31_coeffs[i];
@@ -59,7 +62,9 @@ hilbert_transform(void)
     }
     acc0 -= accn0;
     acc1 -= accn1;
-    *dst++ = __PKHTB(acc0, acc1, 16);
+    //*dst++ = __PKHTB(acc0, acc1, 16);
+    *dst++ = __PKHTB(acc1<<1, src[OFFSET-1], 16);
+    *dst++ = __PKHTB(acc0<<1, src[OFFSET], 0);
     src++;
   }
 
@@ -69,6 +74,32 @@ hilbert_transform(void)
   }
 }
 
+void calclate_gamma(void)
+{
+  __SIMD32_TYPE *r = __SIMD32_CONST(refiq_buf);
+  __SIMD32_TYPE *s = __SIMD32_CONST(samp_buf);
+  q31_t acc_r = 0;
+  q31_t acc_i = 0;
+  q31_t acc_ref = 0;
+  int i;
+
+  for (i = 0; i < SAMPLE_LEN/2; i++) {
+    __SIMD32_TYPE s0 = *s++;
+    __SIMD32_TYPE r0 = *r++;
+    __SIMD32_TYPE r1 = *r++;
+    __SIMD32_TYPE rr = __PKHBT(r1, r0, 16);
+    __SIMD32_TYPE ri = __PKHTB(r0, r1, 16);
+    acc_r = __SMLAD(rr, s0, acc_r);
+    acc_i = __SMLAD(ri, s0, acc_i);
+    acc_ref = __SMLAD(r0, r0, acc_ref);
+    acc_ref = __SMLAD(r1, r1, acc_ref);
+  }
+  acc_ref = sqrt(acc_ref / SAMPLE_LEN) / 65536;
+  gamma_real = acc_r / acc_ref;
+  gamma_imag = acc_i / acc_ref;
+}
+
+
 void
 dsp_process(int16_t *capture, size_t length)
 {
@@ -87,3 +118,4 @@ dsp_process(int16_t *capture, size_t length)
 
   hilbert_transform();
 }
+
diff --git a/main.c b/main.c
index 2663714..049f6b6 100644
--- a/main.c
+++ b/main.c
@@ -172,6 +172,10 @@ int16_t dump_buffer[AUDIO_BUFFER_LEN];
 volatile int16_t request_dump = 0;
 int16_t dump_selection = 0;
 
+volatile int16_t request_calcgamma = 0;
+int32_t gamma_real;
+int32_t gamma_imag;
+
 void i2s_end_callback(I2SDriver *i2sp, size_t offset, size_t n)
 {
 #if PORT_SUPPORTS_RT
@@ -191,12 +195,18 @@ void i2s_end_callback(I2SDriver *i2sp, size_t offset, size_t n)
     else if (dump_selection == 2)
       p = ref_buf;
     else if (dump_selection == 3)
-      p = refq_buf;
+      p = refiq_buf;
     if (request_dump == 1)
       memcpy(dump_buffer, p, sizeof dump_buffer);
     --request_dump;
   }
 
+  if (request_calcgamma > 0) {
+    if (request_calcgamma == 1)
+      calclate_gamma();
+    --request_calcgamma;
+  }
+
 #if PORT_SUPPORTS_RT
   cnt_e = port_rt_get_counter_value();
   stat.interval_cycles = cnt_s - stat.last_counter_value;
@@ -221,10 +231,6 @@ static void cmd_data(BaseSequentialStream *chp, int argc, char *argv[])
 {
   int i, j;
   int len;
-  (void)argc;
-  (void)argv;
-
-  int16_t *buf = dump_buffer;
 
   if (argc == 1)
     dump_selection = atoi(argv[0]);
@@ -234,17 +240,30 @@ static void cmd_data(BaseSequentialStream *chp, int argc, char *argv[])
   while (request_dump)
     ;
   len = AUDIO_BUFFER_LEN;
-  if (dump_selection != 0)
+  if (dump_selection == 1 || dump_selection == 2)
     len /= 2;
   for (i = 0; i < len; ) {
     for (j = 0; j < 16; j++, i++) {
-      chprintf(chp, "%04x ", 0xffff & (int)buf[i]);
+      chprintf(chp, "%04x ", 0xffff & (int)dump_buffer[i]);
     }
     chprintf(chp, "\r\n");
   }
   //palSetPad(GPIOC, GPIOC_LED);
 }
 
+static void cmd_gamma(BaseSequentialStream *chp, int argc, char *argv[])
+{
+  (void)argc;
+  (void)argv;
+
+  request_calcgamma = 3;
+  //palClearPad(GPIOC, GPIOC_LED);
+
+  while (request_calcgamma)
+    ;
+  chprintf(chp, "%d %d\r\n", gamma_real, gamma_imag);
+}
+
 static void cmd_gain(BaseSequentialStream *chp, int argc, char *argv[])
 {
   int rvalue;
@@ -325,6 +344,7 @@ static const ShellCommand commands[] =
     { "stat", cmd_stat },
     { "gain", cmd_gain },
     { "power", cmd_power },
+    { "gamma", cmd_gamma },
     { NULL, NULL }
 };
 
diff --git a/nanovna.h b/nanovna.h
index 3c91b83..b1db0a4 100644
--- a/nanovna.h
+++ b/nanovna.h
@@ -32,9 +32,15 @@ extern int16_t tx_buffer[];
 
 extern int16_t ref_state[];
 extern int16_t ref_buf[];
-extern int16_t refq_buf[];
 extern int16_t samp_buf[];
 
+//extern int16_t refq_buf[];
+extern int16_t refiq_buf[];
+
+extern int32_t gamma_real;
+extern int32_t gamma_imag;
+
 void dsp_process(int16_t *src, size_t len);
+void calclate_gamma(void);
 
 void si5351_set_frequency_with_offset(int freq, int offset, uint8_t drive_strength);