Merge pull request #269 from F4FXL/FM_Ext

Fix crash when writing to serial, Tighten handling of incomign data

FM.cpp

@@ -22,7 +22,7 @@
 
 const uint16_t FM_TX_BLOCK_SIZE = 400U;
 const uint16_t FM_SERIAL_BLOCK_SIZE = 42U;//this is actually the number of sample pairs to send over serial. One sample pair is 3bytes.
-                                          //three times this vqlue shall never exceed 126 !
+                                          //three times this value shall never exceed 126 !
 
 CFM::CFM() :
 m_callsign(),
@@ -87,7 +87,7 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length)
     }
 
     q15_t currentExtSample;
-    bool inputExt = m_inputExtRB.get(currentExtSample);//always consume the external input data so it does not overflow
+    bool inputExt = m_inputExtRB.getSample(currentExtSample);//always consume the external input data so it does not overflow
     inputExt = inputExt && m_extEnabled;
 
     if (!inputExt && (CTCSS_NOT_READY(ctcssState)) && m_modemState != STATE_FM) {
@@ -153,39 +153,42 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length)
 void CFM::process()
 {
   uint16_t space = io.getSpace() - 2U;
-  uint16_t txLength = m_outputRFRB.getData();
-  if (space > 2 && txLength >= FM_TX_BLOCK_SIZE ) {
+  uint16_t length = m_outputRFRB.getData();
+  if (space > 2 && length >= FM_TX_BLOCK_SIZE ) {
     
-    if(txLength > FM_TX_BLOCK_SIZE)
-      txLength = FM_TX_BLOCK_SIZE;
+    if(length > FM_TX_BLOCK_SIZE)
+      length = FM_TX_BLOCK_SIZE;
     if(space > FM_TX_BLOCK_SIZE)
       space = FM_TX_BLOCK_SIZE;
-    if(txLength > space)
-      txLength = space;
+    if(length > space)
+      length = space;
 
     q15_t samples[FM_TX_BLOCK_SIZE];
 
-    for (uint16_t i = 0U; i < txLength; i++) {
+    for (uint16_t i = 0U; i < length; i++) {
       q15_t sample = 0;
       m_outputRFRB.get(sample);
       samples[i] = sample;
-
-      if(m_extEnabled) {
-        uint16_t downSamplerLength = m_downsampler.getData();
-
-        if(downSamplerLength >= FM_SERIAL_BLOCK_SIZE) {
-          TSamplePairPack serialSamples[FM_SERIAL_BLOCK_SIZE];
-
-          for(uint16_t j = 0U; j < downSamplerLength; j++) {
-            m_downsampler.getPackedData(serialSamples[j]);
-          }
-
-          serial.writeFMData((uint8_t*)serialSamples, downSamplerLength * sizeof(TSamplePairPack));
-        }
-      }
     }
 
-    io.write(STATE_FM, samples, txLength);
+    io.write(STATE_FM, samples, length);
+  }
+
+  if(m_extEnabled) {
+    length = m_downsampler.getData();
+
+    if(length >= FM_SERIAL_BLOCK_SIZE) {
+      if(length > FM_SERIAL_BLOCK_SIZE)
+        length = FM_SERIAL_BLOCK_SIZE;
+        
+      TSamplePairPack serialSamples[FM_SERIAL_BLOCK_SIZE];
+
+      for(uint16_t j = 0U; j < length; j++) {
+        m_downsampler.getPackedData(serialSamples[j]);
+      }
+
+      serial.writeFMData((uint8_t*)serialSamples, length * sizeof(TSamplePairPack));
+    }
   }
 }
 
@@ -691,35 +694,7 @@ uint8_t CFM::getSpace() const
 
 uint8_t CFM::writeData(const uint8_t* data, uint8_t length)
 {
-  for (uint8_t i = 0U; i < length; i += 3U) {
-    uint16_t sample1 = 0U;
-    uint16_t sample2 = 0U;
-    uint32_t MASK = 0x00000FFFU;
-
-    uint32_t pack = 0U;
-    uint8_t* packPointer = (uint8_t*)&pack;
-
-    packPointer[1U] = data[i];
-    packPointer[2U] = data[i + 1U];
-    packPointer[3U] = data[i + 2U];
-
-    sample2 = uint16_t(pack & MASK);
-    sample1 = uint16_t(pack >> 12);
-
-    // Convert from uint16_t (0 - +4095) to Q15 (-2048 - +2047).
-    // Incoming data has sample rate 8kHz, just add 2 empty samples after
-    // every incoming sample to upsample to 24kHz
-    m_inputExtRB.put(q15_t(sample1) - 2048);
-    m_inputExtRB.put(0);
-    m_inputExtRB.put(0);
-    m_inputExtRB.put(q15_t(sample2) - 2048);
-    m_inputExtRB.put(0);
-    m_inputExtRB.put(0);
-  }
-
-  // Received audio is now in Q15 format in samples, with length nSamples.
-
-  return 0U;
+  m_inputExtRB.addData(data, length);
 }
 
 void CFM::insertDelay(uint16_t ms)

FM.h

@@ -30,6 +30,7 @@
 #include "RingBuffer.h"
 #include "FMDirectForm1.h"
 #include "FMDownsampler.h"
+#include "FMUpSampler.h"
 
 enum FM_STATE {
   FS_LISTENING,
@@ -98,7 +99,7 @@ private:
   q15_t                m_rxLevel;
   CRingBuffer<q15_t>   m_inputRFRB;
   CRingBuffer<q15_t>   m_outputRFRB;
-  CRingBuffer<q15_t>   m_inputExtRB;
+  CFMUpSampler         m_inputExtRB;
 
   void stateMachine(bool validRFSignal, bool validExtSignal);
   void listeningState(bool validRFSignal, bool validExtSignal);

FMDownsampler.h

@@ -22,12 +22,7 @@
 
 #include "Config.h"
 #include "RingBuffer.h"
-
-struct TSamplePairPack {
-  uint8_t byte0;
-  uint8_t byte1;
-  uint8_t byte2;
-};
+#include "FMSamplePairPack.h"
 
 class CFMDownsampler {
 public:

FMSamplePairPack.h

@@ -0,0 +1,29 @@
+/*
+ *   Copyright (C) 2020 by Jonathan Naylor G4KLX
+ *   Copyright (C) 2020 by Geoffrey Merck F4FXL - KC3FRA
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#if !defined(TSAMPLEPACK_H)
+#define TSAMPLEPACK_H
+
+struct TSamplePairPack {
+  uint8_t byte0;
+  uint8_t byte1;
+  uint8_t byte2;
+};
+
+#endif

FMUpSampler.cpp

@@ -0,0 +1,89 @@
+/*
+ *   Copyright (C) 2020 by Jonathan Naylor G4KLX
+ *   Copyright (C) 2020 by Geoffrey Merck F4FXL - KC3FRA
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "FMUpSampler.h"
+
+const uint32_t FM_UPSAMPLE_MASK = 0x00000FFFU;
+
+CFMUpSampler::CFMUpSampler(uint16_t length) :
+m_upSampleIndex(0),
+m_pack({0U, 0U, 0U}),
+m_samples(length)
+{
+}
+
+void CFMUpSampler::reset()
+{
+    m_upSampleIndex = 0U;
+    m_pack = {0U, 0U, 0U};
+    m_samples.reset();
+}
+
+void CFMUpSampler::addData(const uint8_t* data, uint16_t length)
+{
+  for(uint16_t i = 0U; i < length; i++) {
+    switch (m_upSampleIndex)
+    {
+      case 0U:
+        m_pack.byte0 = data[i];
+      break;
+      case 1U:
+        m_pack.byte1 = data[i];
+      break;
+      case 2U: {
+        m_pack.byte2 = data[i];
+
+        uint32_t pack = 0U;
+        uint8_t* packPtr = (uint8_t*)&pack;
+
+        packPtr[1] = m_pack.byte0;
+        packPtr[2] = m_pack.byte1;
+        packPtr[3] = m_pack.byte2;
+
+        q15_t sample2 = q15_t(uint16_t(pack & FM_UPSAMPLE_MASK) - 2048);
+        q15_t sample1 = q15_t(uint16_t(pack >> 12) - 2048);
+
+        m_samples.put(sample1);
+        m_samples.put(0);
+        m_samples.put(0);
+        m_samples.put(sample2);
+        m_samples.put(0);
+        m_samples.put(0);
+      }
+      break;
+      default:
+      //shoudl never happen
+      break;
+    }
+
+    m_upSampleIndex ++;
+    if (m_upSampleIndex > 2U)
+      m_upSampleIndex = 0U;
+  }
+}
+
+bool CFMUpSampler::getSample(q15_t& sample)
+{
+    return m_samples.get(sample);
+}
+
+uint16_t CFMUpSampler::getSpace() const
+{
+    return m_samples.getSpace();
+}

FMUpSampler.h

@@ -0,0 +1,46 @@
+/*
+ *   Copyright (C) 2020 by Jonathan Naylor G4KLX
+ *   Copyright (C) 2020 by Geoffrey Merck F4FXL - KC3FRA
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+
+#if !defined(FMUPSAMPLER_H)
+#define FMUPSAMPLER_H
+
+#include "Config.h"
+#include "RingBuffer.h"
+#include "FMSamplePairPack.h"
+
+class CFMUpSampler {
+public:
+  CFMUpSampler(uint16_t length);
+
+  void reset();
+
+  void addData(const uint8_t* data, uint16_t length);
+
+  bool getSample(q15_t& sample);
+
+  uint16_t getSpace() const;
+
+private:
+  uint8_t m_upSampleIndex;
+  TSamplePairPack m_pack;
+  CRingBuffer<q15_t> m_samples;
+};
+
+#endif