gortty/dsp.go

// Copyright (C) 2017 Keelan Lightfoot
// Copyright (C) 2007-2008 Board of Regents of the University of Wisconsin
//             System (Univ. of Wisconsin-Madison, Trace R&D Center)
// Copyright (C) 2007-2008 Omnitor AB
// Copyright (C) 2007-2008 Voiceriver Inc
//
// This library is free software; you can redistribute it and/or modify it
// under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or (at
// your option) any later version.

package gortty

import "math"

type dsp struct {
	demLpf    float64 // current LPF output value
	demTotal  float64 // current energy output value
	lpfOrder  int     // LPFOrder Low pass filter order
	minThresh float64 // MinThresh ratio for vaild signal

	meter0 float64
	meter1 float64
	meter2 float64

	bpfMark  *bpf // mark bandpass filter
	bpfSpace *bpf // space bandpass filter
	lpfDem   *lpf //
	lpfTotal *lpf

	r float64
}

func newDsp(markF, spaceF, sampleRate int) *dsp {

	const (
		lfpOrder  = 20
		beta      = .95
		minThresh = 3
	)

	return &dsp{
		lpfOrder:  lfpOrder,
		minThresh: minThresh,
		bpfMark:   newBandPass(markF, sampleRate, beta),
		bpfSpace:  newBandPass(spaceF, sampleRate, beta),
		lpfDem:    newLpf(lfpOrder),
		lpfTotal:  newLpf(lfpOrder),
	}
}

func (d *dsp) demod(sam float64) {

	// The demodulator is structured as a pair of band pass filters tuned to
	// the mark and space frequencies. The output of each is 'rectified' and
	// combined, and low pass filtered to produce a signal that goes positive
	// for mark and negative for space.
	mark := d.bpfMark.update(sam)
	space := d.bpfSpace.update(sam)

	d.demLpf = d.lpfDem.update(math.Abs(mark) - math.Abs(space))

	d.demTotal = d.lpfTotal.update(math.Abs(sam))

}

func (d *dsp) edgeDetected() bool {
	return math.Abs(d.demLpf) > d.demTotal*d.minThresh
}

func (d *dsp) carrier() bool {
	return math.Abs(d.demLpf) > d.demTotal*d.minThresh && d.demTotal*d.minThresh != 0
}

func (d *dsp) lpfVal() float64 {
	return math.Abs(d.demLpf)
}

func (d *dsp) threshVal() float64 {
	return d.demTotal * d.minThresh
}

func (d *dsp) level() (float64, float64, float64) {
	return d.meter0, d.meter1, d.meter2
}

func (d *dsp) mark() bool {
	return d.demLpf > d.minThresh*d.demTotal && math.Abs(d.demLpf) > d.demTotal*d.minThresh
}

func (d *dsp) space() bool {
	return -1*d.demLpf > d.minThresh*d.demTotal && math.Abs(d.demLpf) > d.demTotal*d.minThresh
}

type lpf struct {
	x []float64
	t float64
}

func newLpf(size int) *lpf {
	l := new(lpf)
	l.x = make([]float64, size)
	return l
}

func (l *lpf) update(v float64) float64 {
	l.t -= l.x[0]
	l.t += v
	l.x = append(l.x, v)
	l.x = l.x[1:]

	return l.t
}

type bpf struct {
	dem0 float64
	dem1 float64
	w    float64
	beta float64
}

func newBandPass(fc int, fs int, beta float64) *bpf {
	f := new(bpf)
	f.beta = beta
	f.w = (2 * math.Pi * float64(fc) / float64(fs))

	return f
}

func (f *bpf) update(v float64) float64 {
	n := v + 2.0*math.Cos(f.w)*f.beta*f.dem0 - f.beta*f.beta*f.dem1
	f.dem1 = f.dem0
	f.dem0 = n
	return n
}