mirror of https://github.com/g4klx/MMDVM.git
320 lines
7.3 KiB
C++
320 lines
7.3 KiB
C++
/*
|
|
* Copyright (C) 2020 by Jonathan Naylor G4KLX
|
|
* Copyright 2015-2019 Mobilinkd LLC <rob@mobilinkd.com>
|
|
*
|
|
* 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"
|
|
|
|
#if defined(MODE_AX25)
|
|
|
|
#include "Globals.h"
|
|
#include "AX25Demodulator.h"
|
|
#include "AX25Defines.h"
|
|
|
|
const float32_t SAMPLE_RATE = 24000.0F;
|
|
const float32_t SYMBOL_RATE = 1200.0F;
|
|
|
|
const uint16_t DELAY_LEN = 11U;
|
|
|
|
const float32_t SAMPLES_PER_SYMBOL = SAMPLE_RATE / SYMBOL_RATE;
|
|
const float32_t PLL_LIMIT = SAMPLES_PER_SYMBOL / 2.0F;
|
|
|
|
const uint32_t LPF_FILTER_LEN = 48U;
|
|
|
|
q15_t LPF_FILTER_COEFFS[] = {
|
|
-2, -8, -17, -28, -40, -47, -47, -34,
|
|
-5, 46, 122, 224, 354, 510, 689, 885,
|
|
1092, 1302, 1506, 1693, 1856, 1987, 2077, 2124,
|
|
2124, 2077, 1987, 1856, 1693, 1506, 1302, 1092,
|
|
885, 689, 510, 354, 224, 122, 46, -5,
|
|
-34, -47, -47, -40, -28, -17, -8, -2
|
|
};
|
|
|
|
// Lock low-pass filter taps (80Hz Bessel)
|
|
// scipy.signal:
|
|
// b, a = bessel(4, [80.0/(1200/2)], 'lowpass')
|
|
//
|
|
const uint8_t PLL_IIR_SIZE = 5U;
|
|
|
|
const float32_t PLL_LOCK_B[] = {
|
|
1.077063e-03,4.308253e-03,6.462379e-03,4.308253e-03,1.077063e-03};
|
|
|
|
const float32_t PLL_LOCK_A[] = {
|
|
1.000000e+00,-2.774567e+00,2.962960e+00,-1.437990e+00,2.668296e-01};
|
|
|
|
// 64 Hz loop filter.
|
|
// scipy.signal:
|
|
// loop_coeffs = firwin(9, [64.0/(1200/2)], width = None,
|
|
// pass_zero = True, scale = True, window='hann')
|
|
//
|
|
const uint32_t PLL_FILTER_LEN = 7U;
|
|
|
|
float32_t PLL_FILTER_COEFFS[] = {3.196252e-02F, 1.204223e-01F, 2.176819e-01F, 2.598666e-01F, 2.176819e-01F, 1.204223e-01F, 3.196252e-02F};
|
|
|
|
CAX25Demodulator::CAX25Demodulator(int8_t n) :
|
|
m_frame(),
|
|
m_twist(n),
|
|
m_lpfFilter(),
|
|
m_lpfState(),
|
|
m_delayLine(NULL),
|
|
m_delayPos(0U),
|
|
m_nrziState(false),
|
|
m_pllFilter(),
|
|
m_pllState(),
|
|
m_pllLast(false),
|
|
m_pllBits(1U),
|
|
m_pllCount(0.0F),
|
|
m_pllJitter(0.0F),
|
|
m_pllDCD(false),
|
|
m_iirHistory(),
|
|
m_hdlcOnes(0U),
|
|
m_hdlcFlag(false),
|
|
m_hdlcBuffer(0U),
|
|
m_hdlcBits(0U),
|
|
m_hdlcState(AX25_IDLE)
|
|
{
|
|
m_delayLine = new bool[DELAY_LEN];
|
|
|
|
m_lpfFilter.numTaps = LPF_FILTER_LEN;
|
|
m_lpfFilter.pState = m_lpfState;
|
|
m_lpfFilter.pCoeffs = LPF_FILTER_COEFFS;
|
|
|
|
m_pllFilter.numTaps = PLL_FILTER_LEN;
|
|
m_pllFilter.pState = m_pllState;
|
|
m_pllFilter.pCoeffs = PLL_FILTER_COEFFS;
|
|
|
|
for (uint8_t i = 0U; i < PLL_IIR_SIZE; i++)
|
|
m_iirHistory[i] = 0.0F;
|
|
}
|
|
|
|
bool CAX25Demodulator::process(q15_t* samples, uint8_t length, CAX25Frame& frame)
|
|
{
|
|
bool result = false;
|
|
|
|
q15_t fa[RX_BLOCK_SIZE];
|
|
m_twist.process(samples, fa, RX_BLOCK_SIZE);
|
|
|
|
int16_t buffer[RX_BLOCK_SIZE];
|
|
for (uint8_t i = 0; i < length; i++) {
|
|
bool level = (fa[i] >= 0);
|
|
bool delayed = delay(level);
|
|
buffer[i] = (int16_t(level ^ delayed) << 1) - 1;
|
|
}
|
|
|
|
q15_t fc[RX_BLOCK_SIZE];
|
|
::arm_fir_fast_q15(&m_lpfFilter, buffer, fc, RX_BLOCK_SIZE);
|
|
|
|
for (uint8_t i = 0; i < length; i++) {
|
|
bool bit = fc[i] >= 0;
|
|
bool sample = PLL(bit);
|
|
|
|
if (sample) {
|
|
// We will only ever get one frame because there are
|
|
// not enough bits in a block for more than one.
|
|
if (result) {
|
|
HDLC(NRZI(bit));
|
|
} else {
|
|
result = HDLC(NRZI(bit));
|
|
if (result) {
|
|
// Copy the frame data.
|
|
::memcpy(frame.m_data, m_frame.m_data, AX25_MAX_PACKET_LEN);
|
|
frame.m_length = m_frame.m_length;
|
|
frame.m_fcs = m_frame.m_fcs;
|
|
m_frame.m_length = 0U;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
bool CAX25Demodulator::delay(bool b)
|
|
{
|
|
bool r = m_delayLine[m_delayPos];
|
|
|
|
m_delayLine[m_delayPos++] = b;
|
|
|
|
if (m_delayPos >= DELAY_LEN)
|
|
m_delayPos = 0U;
|
|
|
|
return r;
|
|
}
|
|
|
|
bool CAX25Demodulator::NRZI(bool b)
|
|
{
|
|
bool result = (b == m_nrziState);
|
|
|
|
m_nrziState = b;
|
|
|
|
return result;
|
|
}
|
|
|
|
bool CAX25Demodulator::PLL(bool input)
|
|
{
|
|
bool sample = false;
|
|
|
|
if (input != m_pllLast || m_pllBits > 16U) {
|
|
// Record transition.
|
|
m_pllLast = input;
|
|
|
|
if (m_pllCount > PLL_LIMIT)
|
|
m_pllCount -= SAMPLES_PER_SYMBOL;
|
|
|
|
float32_t adjust = m_pllBits > 16U ? 5.0F : 0.0F;
|
|
float32_t offset = m_pllCount / float32_t(m_pllBits);
|
|
float32_t jitter;
|
|
::arm_fir_f32(&m_pllFilter, &offset, &jitter, 1U);
|
|
|
|
float32_t absOffset = adjust;
|
|
if (offset < 0.0F)
|
|
absOffset -= offset;
|
|
else
|
|
absOffset += offset;
|
|
m_pllJitter = iir(absOffset);
|
|
|
|
m_pllCount -= jitter / 2.0F;
|
|
m_pllBits = 1U;
|
|
} else {
|
|
if (m_pllCount > PLL_LIMIT) {
|
|
sample = true;
|
|
m_pllCount -= SAMPLES_PER_SYMBOL;
|
|
m_pllBits++;
|
|
}
|
|
}
|
|
|
|
m_pllCount += 1.0F;
|
|
|
|
return sample;
|
|
}
|
|
|
|
bool CAX25Demodulator::HDLC(bool b)
|
|
{
|
|
if (m_hdlcOnes == AX25_MAX_ONES) {
|
|
if (b) {
|
|
// flag byte
|
|
m_hdlcFlag = true;
|
|
} else {
|
|
// bit stuffing...
|
|
m_hdlcFlag = false;
|
|
m_hdlcOnes = 0U;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
m_hdlcBuffer >>= 1;
|
|
m_hdlcBuffer |= b ? 128U : 0U;
|
|
m_hdlcBits++; // Free-running until Sync byte.
|
|
|
|
if (b)
|
|
m_hdlcOnes++;
|
|
else
|
|
m_hdlcOnes = 0U;
|
|
|
|
if (m_hdlcFlag) {
|
|
bool result = false;
|
|
|
|
switch (m_hdlcBuffer) {
|
|
case AX25_FRAME_END:
|
|
if (m_frame.m_length >= AX25_MIN_FRAME_LENGTH) {
|
|
result = m_frame.checkCRC();
|
|
if (!result)
|
|
m_frame.m_length = 0U;
|
|
} else {
|
|
m_frame.m_length = 0U;
|
|
}
|
|
m_hdlcState = AX25_SYNC;
|
|
m_hdlcFlag = false;
|
|
m_hdlcBits = 0U;
|
|
break;
|
|
|
|
case AX25_FRAME_ABORT:
|
|
// Frame aborted
|
|
m_frame.m_length = 0U;
|
|
m_hdlcState = AX25_IDLE;
|
|
m_hdlcFlag = false;
|
|
m_hdlcBits = 0U;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
switch (m_hdlcState) {
|
|
case AX25_IDLE:
|
|
break;
|
|
|
|
case AX25_SYNC:
|
|
if (m_hdlcBits == 8U) { // 8th bit.
|
|
// Start of frame data.
|
|
m_hdlcState = AX25_RECEIVE;
|
|
m_frame.append(m_hdlcBuffer);
|
|
m_hdlcBits = 0U;
|
|
}
|
|
break;
|
|
|
|
case AX25_RECEIVE:
|
|
if (m_hdlcBits == 8U) { // 8th bit.
|
|
m_frame.append(m_hdlcBuffer);
|
|
m_hdlcBits = 0U;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void CAX25Demodulator::setTwist(int8_t n)
|
|
{
|
|
m_twist.setTwist(n);
|
|
}
|
|
|
|
bool CAX25Demodulator::isDCD()
|
|
{
|
|
if (m_pllJitter <= (SAMPLES_PER_SYMBOL * 0.03F))
|
|
m_pllDCD = true;
|
|
else if (m_pllJitter >= (SAMPLES_PER_SYMBOL * 0.15F))
|
|
m_pllDCD = false;
|
|
|
|
return m_pllDCD;
|
|
}
|
|
|
|
float32_t CAX25Demodulator::iir(float32_t input)
|
|
{
|
|
for (int8_t i = int8_t(PLL_IIR_SIZE) - 1; i != 0; i--)
|
|
m_iirHistory[i] = m_iirHistory[i - 1];
|
|
|
|
m_iirHistory[0] = input;
|
|
for (uint8_t i = 1U; i < PLL_IIR_SIZE; i++)
|
|
m_iirHistory[0] -= PLL_LOCK_A[i] * m_iirHistory[i];
|
|
|
|
float32_t result = 0.0F;
|
|
for (uint8_t i = 0U; i < PLL_IIR_SIZE; i++)
|
|
result += PLL_LOCK_B[i] * m_iirHistory[i];
|
|
|
|
return result;
|
|
}
|
|
|
|
#endif
|
|
|