GnuRadio/gr-jay/lib/garage_impl.cc

/* -*- c++ -*- */
/* 
 * Copyright 2018 Jay Arrowfield.
 * 
 * This 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, or (at your option)
 * any later version.
 * 
 * This software 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 software; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gnuradio/io_signature.h>
#include "garage_impl.h"
	
#define WITH_DEBUG_MESSAGES		0
#define WITH_DEBUG_FILE			1
	
namespace gr
{
namespace jay
{

garage::sptr garage::make(int sampleRate, int samplesPerSym, float kLoopFilter, float kThreshold)
{
	return gnuradio::get_initial_sptr(new garage_impl(sampleRate, samplesPerSym, kLoopFilter, kThreshold));
}

/*
* The private constructor
*/
garage_impl::garage_impl(int sampleRate, int samplesPerSym, float kLoopFilter, float kThreshold)
: gr::block("garage", 	gr::io_signature::make(1, 1, sizeof(float)),
						gr::io_signature::make(1, 1, sizeof(float)))
, m_sampleRate(sampleRate)
, m_samplesPerSym(samplesPerSym)
, m_count(samplesPerSym)
, m_baudRate((float)m_sampleRate/m_samplesPerSym)
, m_kLoopFilter(kLoopFilter)
, m_kThreshold(kThreshold)
, m_vmin(0)
, m_vmax(0)
, m_v_sig(0)
, m_dc_corr(0)
, m_lag_accu(0)
, m_id_accu(0)
, m_id_bit(0)
, m_bitZeroCross(0)
, m_i(0)
, m_gapDistance(8*m_samplesPerSym)
, m_gapCounter(0)
, m_gapCounterEnable(0)
, m_packetCounter(0)
, m_bitCounter(0)
, m_sampleCounter(0)
, m_pFile_y(nullptr)
, m_pFile_bits(nullptr)
{
}

/*
* Our virtual destructor.
*/
garage_impl::~garage_impl()
{
}

bool garage_impl::start()
{
	fprintf(stderr, "Opening garage\n");
#if WITH_DEBUG_FILE	
	m_pFile_y = fopen("/home/jens/garage_y.dat", "w");
	m_pFile_bits = fopen("/home/jens/garage_bits.dat", "w");
#endif
	return true;
}

bool garage_impl::stop()
{
	fprintf(stderr, "Closing garage\n");
	if (m_pFile_y)
	{
		fclose(m_pFile_y);
	}
	if (m_pFile_bits)
	{
		fclose(m_pFile_bits);
	}
	return true;
}

void garage_impl::forecast (int noutput_items, gr_vector_int &ninput_items_required)
{
	ninput_items_required[0] = m_samplesPerSym*noutput_items;
}

int garage_impl::general_work (int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
	float rho = 0.999f;
	const float *in = (const float *) input_items[0];
	float *out = (float *) output_items[0];
	int outCount = 0;
	int inCount = 0;
	
	// Do <+signal processing+>
	int countReload = m_samplesPerSym-1;
	float alpha_thr = 0.002f;
	float hyst_thr = 0.1f;
	float k_leadLag = m_kLoopFilter;
	float loopThreshold = m_kThreshold;
	float k_lead = 1.0e-1f;
	float k_lag = 1.0e-4f;
	float k_leak = 0;
	float err = 0;
	
	float alpha_sig = 0.5f/m_samplesPerSym;
	
	int i_i = 0;
	while ((i_i+1) < ninput_items[0])
	{
		// Get next interpolated sample
		float i_f = m_i - (float)i_i;
		float y = (1.f-i_f)*in[i_i] + i_f*in[i_i+1] - m_dc_corr;
		
		// Calculate threshold
		m_vmin *= rho;
		m_vmax *= rho;
		if (y < m_vmin)
		{
			m_vmin = y;
		}
		else if (y > m_vmax)
		{
			m_vmax = y;
		}

		// Calculate DC offset correction
		m_dc_corr += alpha_thr*(m_vmax + m_vmin);
		
		// Signal detection
		m_v_sig = (1.f-alpha_sig)*m_v_sig + alpha_sig*y*y;
		bool isSignal = m_v_sig >= loopThreshold;

		// Integrate and dump
		float k_id = 1.f/((1+m_lag_accu)*m_samplesPerSym);
		m_id_accu += k_id*y;
		if (m_count == m_samplesPerSym/2)
		{
			int bit_id = (int)(m_id_accu >= 0.0);
			m_id_accu = 0;
			out[outCount++] = (float)bit_id;
			if (isSignal)
			{
				bitRecord(bit_id);
				if (m_pFile_bits)
				{
					fprintf(m_pFile_bits, "%f %f\n", (float)m_sampleCounter/m_sampleRate, y);
				}
			}
		}

		// Zero crossing detector
		int bitChg = 0;
		if (isSignal)
		{
			if (m_bitZeroCross == 1)
			{
				if (y < -hyst_thr)
				{
					m_bitZeroCross = 0;
					bitChg = 1;
				}
			}
			else 
			{
				if (y > +hyst_thr)
				{
					m_bitZeroCross = 1;
					bitChg = 1;
				}
			}
		}

		bitProcessGap(bitChg);

		// Calc error
		err = 0;
		if (bitChg)
		{
			err = -(m_count - (int)(m_samplesPerSym/2));
		}

		float vcorr = k_leadLag*k_lead*err + m_lag_accu;

		// Lag part of loop filter
		m_lag_accu += k_leadLag*k_lag*err;
		m_lag_accu *= (1.f - k_leak);

		m_baudRate = (float)m_sampleRate/((1+m_lag_accu)*m_samplesPerSym);

		// Sample bit at baud rate = fs/numSamplesPerSym
		if (m_count == 0)
		{
			m_count = countReload;
		}
		else
		{
			m_count--;
		}

		if (m_pFile_y)
		{
			fprintf(m_pFile_y, "%f %f %f %f\n", (float)m_sampleCounter/m_sampleRate, y, m_v_sig, m_id_accu);
		}
		m_sampleCounter++;
		
		m_i += (1+vcorr);
		i_i = (int)m_i;
		
		inCount++;
		if (inCount >= ninput_items[0])
		{
			break;
		}
		
		if (i_i >= (float)ninput_items[0])
		{
			break;
		}
	}

#if WITH_DEBUG_MESSAGES	
	fprintf(stderr, "ninput_items[0] = %d\n", ninput_items[0]);
	fprintf(stderr, "m_i             = %f\n", m_i);
	fprintf(stderr, "inCount  = %d\n", inCount);
	fprintf(stderr, "outCount = %d\n", outCount);
#endif
	
	// Tell runtime system how many input items we consumed on
	// each input stream.
	m_i -= i_i;
	consume_each (i_i);

	// Tell runtime system how many output items we produced.
	return outCount;
}

void garage_impl::bitsPrint()
{
	for (int k=0; k < m_bitCounter; k++)
	{
		printf("%d", m_bits[k]);
	}
	printf("\n");
	printf("Recorded %d bits\n", m_bitCounter);
}

void garage_impl::bitRecord(int bit)
{
	if (m_bitCounter < sizeof(m_bits)/sizeof(*m_bits))
	{
		m_bits[m_bitCounter++] = bit;
	}
}

void garage_impl::bitProcessGap(int bitChg)
{
	if (bitChg)
	{
		if (!m_gapCounterEnable)
		{
			m_bitCounter = 0;
		}
		m_gapCounterEnable = 1;
		m_gapCounter = 0;
	}
	else
	{	
		m_gapCounter += m_gapCounterEnable;
		if (m_gapCounter >= m_gapDistance)
		{
			m_gapCounter = 0;
			m_gapCounterEnable = 0;
			printf("------------------------\n");
			printf("Packet: %03d\n", m_packetCounter++);
			bitsPrint();
			fprintf(stderr, "baudrate = %d\n", (int)m_baudRate);
		}
	}
}

} /* namespace jay */
} /* namespace gr */