From 0bd79fa440f4c1cf700363b45fa58e0cf7736dd2 Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Sun, 24 May 2020 07:45:50 +0200
Subject: [PATCH] Fix sample packing

---
 FMDownsampler.cpp | 9 ++++-----
 FMDownsampler.h   | 9 ++++-----
 2 files changed, 8 insertions(+), 10 deletions(-)

diff --git a/FMDownsampler.cpp b/FMDownsampler.cpp
index e5d8645..b5303f6 100644
--- a/FMDownsampler.cpp
+++ b/FMDownsampler.cpp
@@ -20,7 +20,6 @@
 #include "Config.h"
 #include "FMDownsampler.h"
 
-
 CFMDownsampler::CFMDownsampler(uint16_t length) :
 m_ringBuffer(length),
 m_samplePack(0U),
@@ -32,17 +31,17 @@ m_sampleIndex(0U)
 
 void CFMDownsampler::addSample(q15_t sample)
 {
-  uint16_t usample = uint16_t(sample + 2048);
+  uint32_t usample = uint32_t(int32_t(sample) + 2048);
   //only take one of three samples
   switch(m_sampleIndex){
     case 0:
-      m_samplePack = uint32_t(usample) << 12;
+      m_samplePack = usample << 12;
     break;
     case 3:{
-      m_samplePack |= uint32_t(usample);
+      m_samplePack |= usample;
       
       //we did not use MSB; skip it
-      TSamplePairPack pair{m_samplePackPointer[1U], m_samplePackPointer[2U], m_samplePackPointer[3U]}; 
+      TSamplePairPack pair{m_samplePackPointer[0U], m_samplePackPointer[1U], m_samplePackPointer[2U]}; 
 
       m_ringBuffer.put(pair);
 
diff --git a/FMDownsampler.h b/FMDownsampler.h
index 3118802..4427565 100644
--- a/FMDownsampler.h
+++ b/FMDownsampler.h
@@ -23,6 +23,7 @@
 #include "Config.h"
 #include "RingBuffer.h"
 #include "FMSamplePairPack.h"
+#include "FMDirectForm1.h"
 
 class CFMDownsampler {
 public:
@@ -34,11 +35,9 @@ public:
 
 private:
   CRingBuffer<TSamplePairPack> m_ringBuffer;
-
-  uint32_t m_samplePack;
-  uint8_t *m_samplePackPointer;
-
-  uint8_t m_sampleIndex;
+  uint32_t                     m_samplePack;
+  uint8_t*                     m_samplePackPointer;
+  uint8_t                      m_sampleIndex;
 };
 
 #endif