From c6a4b651d06022874e72f7f209c9ad462d37be4d Mon Sep 17 00:00:00 2001
From: TT <edy555@gmail.com>
Date: Sat, 10 Aug 2019 13:04:48 +0900
Subject: [PATCH] feat: add controling stimulus power and gain by frequency

---
 main.c   | 32 +++++++++++++++++++++++++-------
 si5351.c |  8 ++++----
 2 files changed, 29 insertions(+), 11 deletions(-)

diff --git a/main.c b/main.c
index 366f108..11d69df 100644
--- a/main.c
+++ b/main.c
@@ -41,9 +41,12 @@ void sweep(void);
 
 static MUTEX_DECL(mutex);
 
+#define DRIVE_STRENGTH_AUTO (-1)
+#define FREQ_HARMONICS 300000000
+
 int32_t frequency_offset = 5000;
 int32_t frequency = 10000000;
-uint8_t drive_strength = SI5351_CLK_DRIVE_STRENGTH_2MA;
+int8_t drive_strength = DRIVE_STRENGTH_AUTO;
 int8_t frequency_updated = FALSE;
 int8_t sweep_enabled = TRUE;
 int8_t cal_auto_interpolate = TRUE;
@@ -126,10 +129,25 @@ static void cmd_reset(BaseSequentialStream *chp, int argc, char *argv[])
 int set_frequency(int freq)
 {
     int delay = 0;
-    if (frequency != freq) {
-      delay = si5351_set_frequency_with_offset(freq, frequency_offset, drive_strength);
-      frequency = freq;
+    if (frequency == freq)
+      return delay;
+
+    if (freq > FREQ_HARMONICS && frequency <= FREQ_HARMONICS) {
+      tlv320aic3204_set_gain(30, 30);
+      delay += 10;
+    }
+    if (freq <= FREQ_HARMONICS && frequency > FREQ_HARMONICS) {
+      tlv320aic3204_set_gain(0, 0);
+      delay += 10;
     }
+
+    int8_t ds = drive_strength;
+    if (ds == DRIVE_STRENGTH_AUTO) {
+      ds = freq > FREQ_HARMONICS ? SI5351_CLK_DRIVE_STRENGTH_8MA : SI5351_CLK_DRIVE_STRENGTH_2MA;
+    }
+    delay += si5351_set_frequency_with_offset(freq, frequency_offset, ds);
+
+    frequency = freq;
     return delay;
 }
 
@@ -160,7 +178,7 @@ static void cmd_freq(BaseSequentialStream *chp, int argc, char *argv[])
 static void cmd_power(BaseSequentialStream *chp, int argc, char *argv[])
 {
     if (argc != 1) {
-        chprintf(chp, "usage: power {0-3}\r\n");
+        chprintf(chp, "usage: power {0-3|-1}\r\n");
         return;
     }
     drive_strength = atoi(argv[0]);
@@ -458,10 +476,10 @@ void sweep(void)
 
  rewind:
   frequency_updated = FALSE;
-  delay = 3;
+  //delay = 3;
 
   for (i = 0; i < sweep_points; i++) {
-    set_frequency(frequencies[i]);
+    delay = set_frequency(frequencies[i]);
     tlv320aic3204_select_in3(); // CH0:REFLECT
     wait_dsp(delay);
 
diff --git a/si5351.c b/si5351.c
index 89e7121..9ef42d5 100644
--- a/si5351.c
+++ b/si5351.c
@@ -306,7 +306,7 @@ int
 si5351_set_frequency_with_offset(int freq, int offset, uint8_t drive_strength)
 {
   int band;
-  int delay = 5;
+  int delay = 3;
   uint32_t ofreq = freq + offset;
   uint32_t rdiv = SI5351_R_DIV_1;
   if (freq > 300000000) {
@@ -375,11 +375,11 @@ si5351_set_frequency_with_offset(int freq, int offset, uint8_t drive_strength)
 
   case 2:
     // div by 4 mode. both PLL A and B are dedicated for CLK0, CLK1
+    si5351_set_frequency_fixeddiv(0, SI5351_PLL_A, ofreq, 4,
+                                  SI5351_CLK_DRIVE_STRENGTH_2MA);
     si5351_set_frequency_fixeddiv(1, SI5351_PLL_B, freq, 4, drive_strength);
     si5351_set_frequency_fixedpll(2, SI5351_PLL_B, freq * 4, CLK2_FREQUENCY,
                                   SI5351_R_DIV_1, SI5351_CLK_DRIVE_STRENGTH_2MA);
-    si5351_set_frequency_fixeddiv(0, SI5351_PLL_A, ofreq, 4,
-                                  SI5351_CLK_DRIVE_STRENGTH_2MA);
     break;
   }
 
@@ -388,7 +388,7 @@ si5351_set_frequency_with_offset(int freq, int offset, uint8_t drive_strength)
 #if 1
     si5351_enable_output();
 #endif
-    delay += 0;
+    delay += 10;
   }
 
   current_band = band;