diff --git a/FM.cpp b/FM.cpp index 7c90e01..3bc6a3b 100644 --- a/FM.cpp +++ b/FM.cpp @@ -41,7 +41,8 @@ m_filterStage2(32768, 0,-32768, 32768, -50339, 19052), m_filterStage3(32768, -65536, 32768, 32768, -64075, 31460), m_blanking(), m_useCOS(true), -m_rfAudioBoost(1U) +m_rfAudioBoost(1U), +m_downsampler(1024)//Size might need adjustement { } @@ -74,8 +75,8 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length) } else if (CTCSS_NOT_READY(ctcssState) && m_modemState == STATE_FM && i == length - 1) { //Not enough samples for CTCSS but we already are in FM, trigger the state machine //but do not trigger the state machine on every single sample, save CPU! - bool validCTCSS = CTCSS_VALID(ctcssState); - stateMachine(validCTCSS && cos, i + 1U); + bool validCTCSS = CTCSS_VALID(ctcssState); + stateMachine(validCTCSS && cos, i + 1U); } // Only let audio through when relaying audio @@ -101,6 +102,8 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length) currentSample = q15_t(m_filterStage3.filter(m_filterStage2.filter(m_filterStage1.filter(currentSample)))); + m_downsampler.addSample(currentSample); + currentSample += m_ctcssTX.getAudio(); samples[i] = currentSample; diff --git a/FM.h b/FM.h index d66f2ab..fab8095 100644 --- a/FM.h +++ b/FM.h @@ -28,6 +28,7 @@ #include "FMKeyer.h" #include "FMTimer.h" #include "FMDirectForm1.h" +#include "FMDownsampler.h" enum FM_STATE { FS_LISTENING, @@ -78,6 +79,7 @@ private: CFMBlanking m_blanking; bool m_useCOS; q15_t m_rfAudioBoost; + CFMDownsampler m_downsampler; void stateMachine(bool validSignal, uint8_t length); void listeningState(bool validSignal); diff --git a/FMDownsampleRB.cpp b/FMDownsampleRB.cpp new file mode 100644 index 0000000..7d2cc2a --- /dev/null +++ b/FMDownsampleRB.cpp @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2020 by Jonathan Naylor G4KLX + * + * 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 "FMDownsampleRB.h" + +CFMDownsampleRB::CFMDownsampleRB(uint16_t length) : +m_length(length), +m_head(0U), +m_tail(0U), +m_full(false), +m_overflow(false) +{ + m_samples = new uint8_t[length]; +} + +uint16_t CFMDownsampleRB::getSpace() const +{ + uint16_t n = 0U; + + if (m_tail == m_head) + n = m_full ? 0U : m_length; + else if (m_tail < m_head) + n = m_length - m_head + m_tail; + else + n = m_tail - m_head; + + if (n > m_length) + n = 0U; + + return n; +} + +uint16_t CFMDownsampleRB::getData() const +{ + if (m_tail == m_head) + return m_full ? m_length : 0U; + else if (m_tail < m_head) + return m_head - m_tail; + else + return m_length - m_tail + m_head; +} + +bool CFMDownsampleRB::put(uint8_t sample) +{ + if (m_full) { + m_overflow = true; + return false; + } + + m_samples[m_head] = sample; + + m_head++; + if (m_head >= m_length) + m_head = 0U; + + if (m_head == m_tail) + m_full = true; + + return true; +} + +bool CFMDownsampleRB::get(uint8_t& sample) +{ + if (m_head == m_tail && !m_full) + return false; + + sample = m_samples[m_tail]; + + m_full = false; + + m_tail++; + if (m_tail >= m_length) + m_tail = 0U; + + return true; +} + +bool CFMDownsampleRB::hasOverflowed() +{ + bool overflow = m_overflow; + + m_overflow = false; + + return overflow; +} diff --git a/FMDownsampleRB.h b/FMDownsampleRB.h new file mode 100644 index 0000000..6e8e466 --- /dev/null +++ b/FMDownsampleRB.h @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2020 by Jonathan Naylor G4KLX + * + * 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(FMRB_H) +#define FMRB_H + +#if defined(STM32F4XX) +#include "stm32f4xx.h" +#elif defined(STM32F7XX) +#include "stm32f7xx.h" +#elif defined(STM32F105xC) +#include "stm32f1xx.h" +#include +#else +#include +#endif + +#if defined(__SAM3X8E__) || defined(STM32F105xC) +#define ARM_MATH_CM3 +#elif defined(STM32F7XX) +#define ARM_MATH_CM7 +#elif defined(STM32F4XX) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) +#define ARM_MATH_CM4 +#else +#error "Unknown processor type" +#endif + +#include + +class CFMDownsampleRB { +public: + CFMDownsampleRB(uint16_t length); + + uint16_t getSpace() const; + + uint16_t getData() const; + + bool put(uint8_t sample); + + bool get(uint8_t& sample); + + bool hasOverflowed(); + +private: + uint16_t m_length; + volatile uint8_t* m_samples; + volatile uint16_t m_head; + volatile uint16_t m_tail; + volatile bool m_full; + bool m_overflow; +}; + +#endif diff --git a/FMDownsampler.cpp b/FMDownsampler.cpp new file mode 100644 index 0000000..51e9580 --- /dev/null +++ b/FMDownsampler.cpp @@ -0,0 +1,63 @@ +/* + * 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 "Config.h" +#include "FMDownsampler.h" + + +CFMDownsampler::CFMDownsampler(uint16_t length) : +m_ringBuffer(length),//length might need tweaking +m_packIndex(0), +m_downSampleIndex(0) +{ + m_samplePack = 0; +} + +void CFMDownsampler::addSample(q15_t sample) +{ + //only take one of three samples + if(m_downSampleIndex == 0) { + switch(m_packIndex){ + case 0: + m_samplePack = int32_t(sample) << 12; + break; + case 1:{ + m_samplePack |= int32_t(sample); + + //we did not use MSB; skip it + m_ringBuffer.put(m_samplePackBytes[1]); + m_ringBuffer.put(m_samplePackBytes[2]); + m_ringBuffer.put(m_samplePackBytes[3]); + + m_samplePack = 0; + } + break; + default: + //should never happen + break; + } + m_packIndex++; + if(m_packIndex >= 2) + m_packIndex = 0; + } + + m_downSampleIndex++; + if(m_downSampleIndex >= 3) + m_downSampleIndex = 0; +} \ No newline at end of file diff --git a/FMDownsampler.h b/FMDownsampler.h new file mode 100644 index 0000000..fd17e0a --- /dev/null +++ b/FMDownsampler.h @@ -0,0 +1,43 @@ +/* + * 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(FMDOWNSAMPLER_H) +#define FMDOWNSAMPLER_H + +#include "Config.h" +#include "FMDownsampleRB.h" + +class CFMDownsampler { +public: + CFMDownsampler(uint16_t length); + void addSample(q15_t sample); + inline bool getPackedData(uint8_t& data){ return m_ringBuffer.get(data); }; + inline bool hasOverflowed() { return m_ringBuffer.hasOverflowed(); }; + +private: + CFMDownsampleRB m_ringBuffer; + union { + int32_t m_samplePack; + int8_t m_samplePackBytes[4]; + }; + uint8_t m_packIndex; + uint8_t m_downSampleIndex; +}; + +#endif \ No newline at end of file