From 64f5fddf17bdaabc5eebabc48066f10ad029f3f7 Mon Sep 17 00:00:00 2001 From: Geoffrey Merck Date: Tue, 28 Jan 2020 21:06:43 +0100 Subject: [PATCH] Add AGC --- ambed/cagc.cpp | 82 +++++++++++++++++++++++++++++++++++++ ambed/cagc.h | 56 +++++++++++++++++++++++++ ambed/cusb3xxxinterface.cpp | 5 ++- ambed/cvocodecchannel.cpp | 2 +- ambed/cvocodecchannel.h | 4 ++ 5 files changed, 147 insertions(+), 2 deletions(-) create mode 100644 ambed/cagc.cpp create mode 100644 ambed/cagc.h diff --git a/ambed/cagc.cpp b/ambed/cagc.cpp new file mode 100644 index 0000000..721d69f --- /dev/null +++ b/ambed/cagc.cpp @@ -0,0 +1,82 @@ +// +// cagc.cpp +// ambed +// +// Created by Jean-Luc Deltombe (LX3JL) on 28/04/2017. +// Copyright © 2015 Jean-Luc Deltombe (LX3JL). All rights reserved. +// +// ---------------------------------------------------------------------------- +// This file is part of ambed. +// +// xlxd 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 3 of the License, or +// (at your option) any later version. +// +// xlxd 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 Foobar. If not, see . +// ---------------------------------------------------------------------------- +// Geoffrey Merck F4FXL / KC3FRA AGC code borrowed from Liquid DSP +// Only took the parts we need qnd recoeded it to be close the XLX coding style +// https://github.com/jgaeddert/liquid-dsp/blob/master/src/agc/src/agc.c + +#include "main.h" +#include +#include "cagc.h" + + +//////////////////////////////////////////////////////////////////////////////////////// +// constructor + +CAGC::CAGC(float initialLeveldB) +{ + // set internal gain appropriately + m_g = powf(10.0f, -initialLeveldB/20.0f); + + // ensure resulting gain is not arbitrarily low + if (m_g < 1e-16f) + m_g = 1e-16f; + + m_scale = 1.0f; + m_bandwidth = 1e-2f; + m_y2_prime = 1.0f; +} + +void CAGC::Apply(uint8 * voice, int size) +{ + for (int i = 0; i < size; i+=2) + { + //Get the sample + float _x = (float)(short)MAKEWORD(voice[i+1], voice[i]); + + //apply AGC + // apply gain to input sample + float _y = _x * m_g; + + // compute output signal energy + float y2 = _y * _y; + + // smooth energy estimate using single-pole low-pass filter + m_y2_prime = (1.0f - m_alpha) * m_y2_prime + m_alpha*y2; + + // update gain according to output energy + if (m_y2_prime > 1e-6f) + m_g *= exp( -0.5f * m_alpha * log(m_y2_prime) ); + + // clamp to 120 dB gain + if (m_g > 1e6f) + m_g = 1e6f; + + // apply output scale + _y *= m_scale; + + //write processed sample back to it + voice[i] = HIBYTE((short)_y); + voice[i+1] = LOBYTE((short)_y); + } +} \ No newline at end of file diff --git a/ambed/cagc.h b/ambed/cagc.h new file mode 100644 index 0000000..20f7800 --- /dev/null +++ b/ambed/cagc.h @@ -0,0 +1,56 @@ +// +// cagc.h +// ambed +// +// Created by Jean-Luc Deltombe (LX3JL) on 26/04/2017. +// Copyright © 2015 Jean-Luc Deltombe (LX3JL). All rights reserved. +// +// ---------------------------------------------------------------------------- +// This file is part of ambed. +// +// xlxd 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 3 of the License, or +// (at your option) any later version. +// +// xlxd 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 Foobar. If not, see . +// ---------------------------------------------------------------------------- +// Geoffrey Merck F4FXL / KC3FRA AGC code borrowed from Liquid DSP +// Only took the parts we need qnd recoeded it to be close the XLX coding style +// https://github.com/jgaeddert/liquid-dsp/blob/master/src/agc/src/agc.c + +#ifndef cagc_h +#define cagc_h + +#include "main.h" + +class CAGC +{ +public: + //Constructor + CAGC(float initialLeveldB); + + //methods + void Apply(uint8 * voice, int size); + float GetGain(){ return m_scale; }//gets current gain (linear) + +private: + // gain variables + float m_g; // current gain value + float m_scale; // output scale value + + // gain control loop filter parameters + float m_bandwidth; // bandwidth-time constant + float m_alpha; // feed-back gain + + // signal level estimate + float m_y2_prime; // filtered output signal energy estimate +}; + +#endif /* cgc_h */ \ No newline at end of file diff --git a/ambed/cusb3xxxinterface.cpp b/ambed/cusb3xxxinterface.cpp index c8eb461..732fe4a 100644 --- a/ambed/cusb3xxxinterface.cpp +++ b/ambed/cusb3xxxinterface.cpp @@ -152,7 +152,10 @@ void CUsb3xxxInterface::Task(void) { Queue = Channel->GetVoiceQueue(); CVoicePacket *clone = new CVoicePacket(VoicePacket); - clone->ApplyGain(Channel->GetSpeechGain()); + CAGC agc = Channel->GetAGC(); + agc.Apply(clone->GetVoice(), clone->GetVoiceSize()); + std::cout << "Gain : " << agc.GetGain(); + //clone->ApplyGain(Channel->GetSpeechGain()); Queue->push(clone); Channel->ReleaseVoiceQueue(); } diff --git a/ambed/cvocodecchannel.cpp b/ambed/cvocodecchannel.cpp index 0212424..04a252c 100644 --- a/ambed/cvocodecchannel.cpp +++ b/ambed/cvocodecchannel.cpp @@ -27,11 +27,11 @@ #include "cvocodecchannel.h" #include "cvocodecinterface.h" - //////////////////////////////////////////////////////////////////////////////////////// // constructor CVocodecChannel::CVocodecChannel(CVocodecInterface *InterfaceIn, int iChIn, CVocodecInterface *InterfaceOut, int iChOut, int iSpeechGain) +: m_AGC((float)iSpeechGain) { m_bOpen = false; m_InterfaceIn = InterfaceIn; diff --git a/ambed/cvocodecchannel.h b/ambed/cvocodecchannel.h index 4848c87..c0983a9 100644 --- a/ambed/cvocodecchannel.h +++ b/ambed/cvocodecchannel.h @@ -27,6 +27,7 @@ #define cvocodecchannel_h #include "cpacketqueue.h" +#include "cagc.h" //////////////////////////////////////////////////////////////////////////////////////// // class @@ -53,6 +54,7 @@ public: int GetChannelIn(void) const { return m_iChannelIn; } int GetChannelOut(void) const { return m_iChannelOut; } int GetSpeechGain(void) const { return m_iSpeechGain; } + CAGC& GetAGC() { return m_AGC; }; // interfaces bool IsInterfaceIn(const CVocodecInterface *interface) { return (interface == m_InterfaceIn); } @@ -92,6 +94,8 @@ protected: // settings int m_iSpeechGain; +private: + CAGC m_AGC; }; ////////////////////////////////////////////////////////////////////////////////////////