Merge pull request #250 from F4FXL/FM_Ext

Merge FM into FM_Ext

FM.cpp

@@ -41,15 +41,14 @@ m_hangTimer(),
 m_filterStage1(  724,   1448,   724, 32768, -37895, 21352),//3rd order Cheby Filter 300 to 2700Hz, 0.2dB passband ripple, sampling rate 24kHz
 m_filterStage2(32768,      0,-32768, 32768, -50339, 19052),
 m_filterStage3(32768, -65536, 32768, 32768, -64075, 31460),
-m_preemphasis(32768,  13967, 0, 32768, -18801, 0),//75µS 24kHz sampling rate
-m_deemphasis (32768, -18801, 0, 32768,  13967, 0),//75µS 24kHz sampling rate
 m_blanking(),
 m_useCOS(true),
 m_cosInvert(false),
 m_rfAudioBoost(1U),
 m_extAudioBoost(1U),
 m_downsampler(128U),   //Size might need adjustement
-m_extEnabled(false)
+m_extEnabled(false),
+m_rxLevel(1)
 {
 }
 
@@ -66,7 +65,8 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length)
 
   uint8_t i = 0U;
   for (; i < length; i++) {
-    q15_t currentSample = samples[i];//save to a local variable to avoid indirection on every access
+    // ARMv7-M has hardware integer division 
+    q15_t currentSample = q15_t((q31_t(samples[i]) << 8) / m_rxLevel);
 
     uint8_t ctcssState = m_ctcssRX.process(currentSample);
 
@@ -97,8 +97,6 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length)
 
     // Only let RF audio through when relaying RF audio
     if (m_state == FS_RELAYING_RF || m_state == FS_KERCHUNK_RF) {
-      // currentSample = m_deemphasis.filter(currentSample);
-
       if (m_extEnabled)
         m_downsampler.addSample(currentSample);
 
@@ -130,8 +128,6 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length)
 
     currentSample = m_filterStage3.filter(m_filterStage2.filter(m_filterStage1.filter(currentSample)));
 
-    // currentSample = m_preemphasis.filter(currentSample);
-
     currentSample += m_ctcssTX.getAudio();
 
     samples[i] = currentSample;
@@ -205,7 +201,9 @@ uint8_t CFM::setMisc(uint16_t timeout, uint8_t timeoutLevel, uint8_t ctcssFreque
   m_timeoutTone.setParams(timeoutLevel);
   m_blanking.setParams(maxDev, timeoutLevel);
 
-  uint8_t ret = m_ctcssRX.setParams(ctcssFrequency, ctcssThreshold, rxLevel);
+  m_rxLevel = rxLevel; //q15_t(255)/q15_t(rxLevel >> 1);
+
+  uint8_t ret = m_ctcssRX.setParams(ctcssFrequency, ctcssThreshold);
   if (ret != 0U)
     return ret;
 

FM.h

@@ -86,8 +86,6 @@ private:
   CFMDirectFormI       m_filterStage1;
   CFMDirectFormI       m_filterStage2;
   CFMDirectFormI       m_filterStage3;
-  CFMDirectFormI       m_preemphasis;
-  CFMDirectFormI       m_deemphasis;
   CFMBlanking          m_blanking;
   bool                 m_useCOS;
   bool                 m_cosInvert;
@@ -95,6 +93,7 @@ private:
   q15_t                m_extAudioBoost;
   CFMDownsampler       m_downsampler;
   bool                 m_extEnabled;
+  q15_t                m_rxLevel;
 
   void stateMachine(bool validRFSignal, bool validExtSignal);
   void listeningState(bool validRFSignal, bool validExtSignal);

FMCTCSSRX.cpp

@@ -86,15 +86,12 @@ m_threshold(0),
 m_count(0U),
 m_q0(0),
 m_q1(0),
-m_result(CTS_NONE),
+m_result(CTS_NONE)
-m_rxLevelInverse(1)
 {
 }
 
-uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold, uint8_t level)
+uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold)
 {
-  m_rxLevelInverse = q15Division(65535, q15_t(level * 128));
-
   m_coeffDivTwo = 0;
 
   for (uint8_t i = 0U; i < CTCSS_TABLE_DATA_LEN; i++) {
@@ -114,7 +111,8 @@ uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold, uint8_t leve
 
 uint8_t CFMCTCSSRX::process(q15_t sample)
 {
-  q31_t sample31 = q31_t(sample) * m_rxLevelInverse;
+  //get more dynamic into the decoder by multiplying the sample by 1.5
+  q31_t sample31 = q31_t(sample) +  (q31_t(sample) >> 1);
 
   m_result &= ~CTS_READY;
 
@@ -174,16 +172,3 @@ void CFMCTCSSRX::reset()
   m_result = CTS_NONE;
   m_count  = 0U;
 }
-
-//Taken from https://en.wikipedia.org/wiki/Q_(number_format)#Division
-q15_t CFMCTCSSRX::q15Division(q15_t a, q15_t divisor)
-{
-  q31_t a31 = q31_t(a) << 16;
-
-  if (((a >> 31) & 1) == ((divisor >> 15) & 1))
-    a31 += divisor >> 1;
-  else
-    a31 -= divisor >> 1;
-
-  return a31 / divisor;
-}

FMCTCSSRX.h

@@ -34,7 +34,7 @@ class CFMCTCSSRX {
 public:
   CFMCTCSSRX();
 
-  uint8_t setParams(uint8_t frequency, uint8_t threshold, uint8_t level);
+  uint8_t setParams(uint8_t frequency, uint8_t threshold);
   
   uint8_t process(q15_t sample);
 
@@ -47,9 +47,6 @@ private:
   q31_t    m_q0;
   q31_t    m_q1;
   uint8_t  m_result;
-  q15_t    m_rxLevelInverse;
-
-  q15_t q15Division(q15_t a, q15_t divisor);
 };
 
 #endif