From dce58b77371e9461226e5fe728979fb26a45b06f Mon Sep 17 00:00:00 2001
From: Kuba <132459354+KubaPro010@users.noreply.github.com>
Date: Tue, 31 Oct 2023 14:38:22 +0000
Subject: [PATCH] streo toggle

---
 src/pifmrds/control_pipe.c |  6 ++++++
 src/pifmrds/control_pipe.h |  1 +
 src/pifmrds/fm_mpx.c       | 23 ++++++++++++++---------
 src/pifmrds/fm_mpx.h       |  2 +-
 src/pifmrds/pi_fm_rds.cpp  |  6 +++++-
 5 files changed, 27 insertions(+), 11 deletions(-)

diff --git a/src/pifmrds/control_pipe.c b/src/pifmrds/control_pipe.c
index 08414ef..c43779f 100644
--- a/src/pifmrds/control_pipe.c
+++ b/src/pifmrds/control_pipe.c
@@ -145,6 +145,12 @@ ResultAndArg poll_control_pipe() {
 			printf("\n");
 			resarg.res = CONTROL_PIPE_DEVIATION_SET;
 			resarg.arg = arg;
+		} else if(fifo[0] == 'S' && fifo[1] == 'T' && fifo[2] == 'R') {
+			int togg = ( strcmp(arg, "ON") == 0 );
+			printf("Set Streo Toggle to ");
+			if(togg) printf("ON\n"); else printf("OFF\n");
+			resarg.res = CONTROL_PIPE_STEREO_SET;
+			resarg.arg = (char)togg;
 		}
 	}
 	return resarg;
diff --git a/src/pifmrds/control_pipe.h b/src/pifmrds/control_pipe.h
index 358d77b..f9284ba 100644
--- a/src/pifmrds/control_pipe.h
+++ b/src/pifmrds/control_pipe.h
@@ -16,6 +16,7 @@
 #define CONTROL_PIPE_PWR_SET 9
 #define CONTROL_PIPE_RDS_SET 10
 #define CONTROL_PIPE_DEVIATION_SET 11
+#define CONTROL_PIPE_STEREO_SET 12
 
 typedef struct {
     int res;
diff --git a/src/pifmrds/fm_mpx.c b/src/pifmrds/fm_mpx.c
index 0f370a7..e08f89b 100644
--- a/src/pifmrds/fm_mpx.c
+++ b/src/pifmrds/fm_mpx.c
@@ -125,7 +125,6 @@ int fm_mpx_open(char *filename, size_t len, int raw, double preemphasis, int raw
         }
     
         // Choose a cutoff frequency for the low-pass FIR filter
-		//float cutoff_freq = 3000; //For NFM
         if(in_samplerate/2 < cutoff_freq) cutoff_freq = in_samplerate/2 * .8;
    
 
@@ -194,7 +193,7 @@ int fm_mpx_open(char *filename, size_t len, int raw, double preemphasis, int raw
 
 // samples provided by this function are in 0..10: they need to be divided by
 // 10 after.
-int fm_mpx_get_samples(float *mpx_buffer, int drds, float compressor_decay, float compressor_attack, float compressor_max_gain_recip) {
+int fm_mpx_get_samples(float *mpx_buffer, int drds, float compressor_decay, float compressor_attack, float compressor_max_gain_recip, int disablestereo) {
     if(!drds) get_rds_samples(mpx_buffer, length);
 
     if(inf == NULL) return 0; // if there is no audio, stop here
@@ -311,14 +310,20 @@ int fm_mpx_get_samples(float *mpx_buffer, int drds, float compressor_decay, floa
  
         // Generate the stereo mpx
         if( channels > 1 ) {
-            mpx_buffer[i] +=  4.05*(out_left+out_right) + // Stereo sum signal
-                4.05 * carrier_38[phase_38] * (out_left-out_right) + // Stereo difference signal
-                .9*carrier_19[phase_19];                  // Stereo pilot tone
+            if(!disablestereo) {
+                mpx_buffer[i] +=  4.05*(out_left+out_right) + // Stereo sum signal
+                    4.05 * carrier_38[phase_38] * (out_left-out_right) + // Stereo difference signal
+                    .9*carrier_19[phase_19];                  // Stereo pilot tone
 
-            phase_19++;
-            phase_38++;
-            if(phase_19 >= 12) phase_19 = 0;
-            if(phase_38 >= 6) phase_38 = 0;
+                phase_19++;
+                phase_38++;
+                if(phase_19 >= 12) phase_19 = 0;
+                if(phase_38 >= 6) phase_38 = 0;
+            } else {
+                mpx_buffer[i] =  
+                    mpx_buffer[i] +    // RDS data samples are currently in mpx_buffer :to be Remove in NFM
+                    9.0*(out_left+out_right);      // Unmodulated monophonic signal
+            }
         }
         else
         {
diff --git a/src/pifmrds/fm_mpx.h b/src/pifmrds/fm_mpx.h
index d7f2a15..272672c 100644
--- a/src/pifmrds/fm_mpx.h
+++ b/src/pifmrds/fm_mpx.h
@@ -22,5 +22,5 @@
 */
 
 extern int fm_mpx_open(char *filename, size_t len, int raw, double preemphasis, int rawSampleRate, int rawChannels, float cutoff_freq, float gain);
-extern int fm_mpx_get_samples(float *mpx_buffer, int drds, float compressor_decay, float compressor_attack, float compressor_max_gain_recip);
+extern int fm_mpx_get_samples(float *mpx_buffer, int drds, float compressor_decay, float compressor_attack, float compressor_max_gain_recip, int disablestereo);
 extern int fm_mpx_close();
diff --git a/src/pifmrds/pi_fm_rds.cpp b/src/pifmrds/pi_fm_rds.cpp
index 8854242..4540a68 100644
--- a/src/pifmrds/pi_fm_rds.cpp
+++ b/src/pifmrds/pi_fm_rds.cpp
@@ -103,6 +103,8 @@ int tx(uint32_t carrier_freq, char *audio_file, uint16_t pi, char *ps, char *rt,
     int data_len = 0;
     int data_index = 0;
 
+    int disablestereo = 0;
+
     // Initialize the baseband generator
     if(fm_mpx_open(audio_file, DATA_SIZE, raw, preemp, rawSampleRate, rawChannels, cutoff_freq, gaim) < 0) return 1;
 
@@ -195,10 +197,12 @@ int tx(uint32_t carrier_freq, char *audio_file, uint16_t pi, char *ps, char *rt,
             } else if(pollResult.res == CONTROL_PIPE_DEVIATION_SET) {
                 deviation = atoi(pollResult.arg);
                 deviation_scale_factor=  0.1 * (deviation );
+            } else if(pollResult.res == CONTROL_PIPE_STEREO_SET) {
+                disablestereo = (int)pollResult.arg;
             }
         }
 
-			if( fm_mpx_get_samples(data, drds, compressor_decay, compressor_attack, compressor_max_gain_recip) < 0 ) {
+			if( fm_mpx_get_samples(data, drds, compressor_decay, compressor_attack, compressor_max_gain_recip, disablestereo) < 0 ) {
                     terminate(0);
                 }
                 data_len = DATA_SIZE;