Merge pull request #222 from F4FXL/FM

Almost working FM

FM.cpp

@@ -31,7 +31,6 @@ q15_t FILTER_COEFFS[] = {
 
 const uint16_t FILTER_COEFFS_LEN = 101U;
 
-const uint16_t OUTPUT_BUFFER_SIZE = 1000U;
 
 CFM::CFM() :
 m_filter(),
@@ -50,8 +49,7 @@ m_holdoffTimer(),
 m_kerchunkTimer(),
 m_ackMinTimer(),
 m_ackDelayTimer(),
-m_hangTimer(),
-m_ringBuffer(OUTPUT_BUFFER_SIZE)
+m_hangTimer()
 {
   ::memset(m_filterState, 0x00U, 230U * sizeof(q15_t));
 
@@ -64,53 +62,42 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length)
 {
   bool validCTCSS = m_ctcssRX.process(samples, length);
 
-  stateMachine(validCTCSS && cos, length);
+  stateMachine(validCTCSS /*&& cos*/, length);
 
   if (m_modemState != STATE_FM)
     return;
 
+  q15_t currentSample;
+  for(uint8_t i = 0U; i < length; i++) {
+    currentSample = samples[i];//save to a local variable to avoid indirection on every access
+
     // Only let audio through when relaying audio
     if (m_state != FS_RELAYING && m_state != FS_KERCHUNK) {
-    for (uint8_t i = 0U; i < length; i++)
-      samples[i] = 0;
+      currentSample = 0U;
     }
 
-  if (!m_callsign.isRunning())
-    m_rfAck.getAudio(samples, length);
+    if(!m_callsign.isRunning())
+      currentSample += m_rfAck.getAudio();
     
-  if (!m_rfAck.isRunning())
-    m_callsign.getAudio(samples, length);
+    if(!m_rfAck.isRunning())
+      currentSample += m_callsign.getAudio();
 
     if (!m_callsign.isRunning() && !m_rfAck.isRunning())
-    m_timeoutTone.getAudio(samples, length);
+      currentSample += m_timeoutTone.getAudio();
 
-  q15_t output[RX_BLOCK_SIZE];
-  ::arm_fir_fast_q15(&m_filter, samples, output, length);
+    //ToDo Filtering
+    //::arm_fir_fast_q15(&m_filter, samples + i, &currentSample, 1);
 
-  m_ctcssTX.getAudio(output, length);
+    currentSample += m_ctcssTX.getAudio();
 
-  for (uint8_t i = 0U; i < length; i++) {
-    bool ret = m_ringBuffer.put(output[i]);
-    if (!ret) {
-      DEBUG1("Overflow in the FM ring buffer");
-      break;
-    }
+    samples[i] = currentSample;
   }
+
+  io.write(STATE_FM, samples, length);
 }
 
 void CFM::process()
 {
-  uint16_t space = io.getSpace();
-  uint16_t data  = m_ringBuffer.getData();
-  if (data < space)
-    space = data;
-
-  for (uint16_t i = 0U; i < space; i++) {
-    q15_t sample;
-    m_ringBuffer.get(sample);
-
-    io.write(STATE_FM, &sample, 1U);
-  }
 }
 
 void CFM::reset()
@@ -210,6 +197,7 @@ void CFM::stateMachine(bool validSignal, uint8_t length)
 
 void CFM::listeningState(bool validSignal)
 {
+  if(validSignal) {
     if (m_kerchunkTimer.getTimeout() > 0U) {
       DEBUG1("State to KERCHUNK");
       m_state = FS_KERCHUNK;
@@ -227,6 +215,7 @@ void CFM::listeningState(bool validSignal)
 
     DEBUG1("Change to STATE_FM");
     m_modemState = STATE_FM;
+  }
 }
 
 void CFM::kerchunkState(bool validSignal)

FM.h

@@ -70,7 +70,6 @@ private:
   CFMTimer             m_ackMinTimer;
   CFMTimer             m_ackDelayTimer;
   CFMTimer             m_hangTimer;
-  CFMRB                m_ringBuffer;
 
   void stateMachine(bool validSignal, uint8_t length);
   void listeningState(bool validSignal);

FMCTCSSRX.cpp

@@ -109,18 +109,16 @@ uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold)
 
 bool CFMCTCSSRX::process(q15_t* samples, uint8_t length)
 {
-  float32_t data[RX_BLOCK_SIZE];
-  ::arm_q15_to_float(samples, data, length);
-
   for (unsigned int i = 0U; i < length; i++) {
     float32_t q2 = m_q1;
     m_q1 = m_q0;
-    m_q0 = m_coeff * m_q1 - q2 + data[i];
+    m_q0 = m_coeff * m_q1 - q2 + float32_t(samples[i]) / 32768.0F;
 
     m_count++;
     if (m_count == N) {
       float32_t value = m_q0 * m_q0 + m_q1 * m_q1 - m_q0 * m_q1 * m_coeff;
       m_result = value >= m_threshold;
+      //DEBUG4("CTCSS value / threshold / result", value, m_threshold, m_result);
       m_count = 0U;
       m_q0 = 0.0F;
       m_q1 = 0.0F;

FMCTCSSTX.cpp

@@ -125,3 +125,12 @@ void CFMCTCSSTX::getAudio(q15_t* samples, uint8_t length)
       m_n = 0U;
   }
 }
+
+q15_t CFMCTCSSTX::getAudio()
+{
+  q15_t sample = m_values[m_n++];
+  if(m_n >= m_length)
+    m_n = 0U;
+  
+  return sample;
+}

FMCTCSSTX.h

@@ -28,6 +28,7 @@ public:
   uint8_t setParams(uint8_t frequency, uint8_t level);
 
   void getAudio(q15_t* samples, uint8_t length);
+  q15_t getAudio();
 
 private:
   q15_t*   m_values;

FMKeyer.cpp

@@ -152,6 +152,32 @@ void CFMKeyer::getAudio(q15_t* samples, uint8_t length)
   }
 }
 
+q15_t CFMKeyer::getAudio()
+{
+  q15_t output = 0U;
+  if (!m_wanted)
+    return 0U; 
+    
+  bool b = READ_BIT(m_poBuffer, m_poPos);
+  if (b)
+    output = m_audio[m_audioPos];
+
+  m_audioPos++;
+  if (m_audioPos >= m_audioLen)
+    m_audioPos = 0U;
+  m_dotPos++;
+  if (m_dotPos >= m_dotLen) {
+    m_dotPos = 0U;
+    m_poPos++;
+    if (m_poPos >= m_poLen) {
+      stop();
+      return output;
+    }
+  }
+
+  return output;
+}
+
 void CFMKeyer::start()
 {
   if (isRunning())

FMKeyer.h

@@ -29,6 +29,8 @@ public:
 
   void getAudio(q15_t* samples, uint8_t length);
 
+  q15_t getAudio();
+
   void start();
   void stop();
 

FMTimeout.cpp

@@ -62,6 +62,30 @@ void CFMTimeout::getAudio(q15_t* samples, uint8_t length)
   }
 }
 
+q15_t CFMTimeout::getAudio()
+{
+  q15_t sample = 0U;
+  if (!m_running)
+    return sample;
+
+  if (m_pos > 12000U) {
+    q31_t sample = BUSY_AUDIO[m_n] * m_level;
+    sample = q15_t(__SSAT((sample >> 15), 16));
+
+    m_n++;
+    if (m_n >= BUSY_AUDIO_LEN)
+      m_n = 0U;
+  } else {
+    sample = 0U;
+  }
+
+  m_pos++;
+  if (m_pos >= 24000U)
+    m_pos = 0U;
+
+  return sample;
+}
+
 void CFMTimeout::start()
 {
   m_running = true;

FMTimeout.h

@@ -32,6 +32,8 @@ public:
 
   void getAudio(q15_t* samples, uint8_t length);
 
+  q15_t getAudio();
+
 private:
   q15_t    m_level;
   bool     m_running;