Superminaren
/
MMDVM
mirror of https://github.com/g4klx/MMDVM.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
MMDVM/IO.cpp

591 lines
16 KiB
C++
Raw Blame History

/*
* Copyright (C) 2015,2016,2017,2018 by Jonathan Naylor G4KLX
* Copyright (C) 2015 by Jim Mclaughlin KI6ZUM
* Copyright (C) 2016 by Colin Durbridge G4EML
*
* 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 "Globals.h"
#include "IO.h"
// Generated using [b, a] = butter(1, 0.0005) in MATLAB
static q31_t DC_FILTER[] = {1685306, 0, 1685306, 0, 2144113034, 0}; // {b0, 0, b1, b2, -a1, -a2}
const uint32_t DC_FILTER_STAGES = 1U; // One Biquad stage
// Generated using rcosdesign(0.2, 8, 10, 'sqrt') in MATLAB
static q15_t RRC_0_2_FILTER[] = {284, 198, 73, -78, -240, -393, -517, -590, -599, -533, -391, -181, 79, 364, 643, 880, 1041, 1097, 1026, 819,
483, 39, -477, -1016, -1516, -1915, -2150, -2164, -1914, -1375, -545, 557, 1886, 3376, 4946, 6502, 7946, 9184,
10134, 10731, 10935, 10731, 10134, 9184, 7946, 6502, 4946, 3376, 1886, 557, -545, -1375, -1914, -2164, -2150,
-1915, -1516, -1016, -477, 39, 483, 819, 1026, 1097, 1041, 880, 643, 364, 79, -181, -391, -533, -599, -590,
-517, -393, -240, -78, 73, 198, 284, 0};
const uint16_t RRC_0_2_FILTER_LEN = 82U;
// Generated using rcosdesign(0.2, 8, 20, 'sqrt') in MATLAB
static q15_t NXDN_0_2_FILTER[] = {201, 174, 140, 99, 52, 0, -55, -112, -170, -226, -278, -325, -365, -397, -417, -427, -424, -407, -377, -333, -277, -208,
-128, -40, 56, 156, 258, 358, 455, 544, 622, 687, 736, 766, 775, 762, 725, 664, 579, 471, 342, 193, 27, -151, -338, -528,
-718, -901, -1072, -1225, -1354, -1454, -1520, -1547, -1530, -1466, -1353, -1189, -972, -704, -385, -18, 394, 846, 1333,
1850, 2388, 2940, 3498, 4053, 4598, 5122, 5619, 6079, 6494, 6859, 7166, 7410, 7588, 7696, 7732, 7696, 7588, 7410, 7166,
6859, 6494, 6079, 5619, 5122, 4598, 4053, 3498, 2940, 2388, 1850, 1333, 846, 394, -18, -385, -704, -972, -1189, -1353,
-1466, -1530, -1547, -1520, -1454, -1354, -1225, -1072, -901, -718, -528, -338, -151, 27, 193, 342, 471, 579, 664, 725,
762, 775, 766, 736, 687, 622, 544, 455, 358, 258, 156, 56, -40, -128, -208, -277, -333, -377, -407, -424, -427, -417, -397,
-365, -325, -278, -226, -170, -112, -55, 0, 52, 99, 140, 174, 201, 0};
const uint16_t NXDN_0_2_FILTER_LEN = 162U;
static q15_t NXDN_ISINC_FILTER[] = {7616, -1333, -1856, -2611, -3399, -4006, -4230, -3918, -2988, -1458, 561, 2867, 5200, 7276, 8832, 9665,
9665, 8832, 7276, 5200, 2867, 561, -1458, -2988, -3918, -4230, -4006, -3399, -2611, -1856, -1333, 7616};
const uint16_t NXDN_ISINC_FILTER_LEN = 32U;
// Generated using gaussfir(0.5, 4, 10) in MATLAB
//static q15_t GAUSSIAN_0_5_FILTER[] = {1, 4, 15, 52, 151, 380, 832, 1579, 2599, 3710, 4594, 4933, 4594, 3710, 2599, 1579, 832, 380, 151, 52, 15, 4, 1, 0};
//const uint16_t GAUSSIAN_0_5_FILTER_LEN = 24U;
// One symbol boxcar filter
static q15_t BOXCAR_FILTER[] = {6000, 6000, 6000, 6000, 6000, 6000, 6000, 6000, 6000, 6000, 0, 0};
const uint16_t BOXCAR_FILTER_LEN = 12U;
const uint16_t DC_OFFSET = 2048U;
CIO::CIO() :
m_started(false),
m_rxBuffer(RX_RINGBUFFER_SIZE),
m_txBuffer(TX_RINGBUFFER_SIZE),
m_rssiBuffer(RX_RINGBUFFER_SIZE),
m_dcFilter(),
m_dcState(),
m_rrcFilter(),
//m_gaussianFilter(),
m_boxcarFilter(),
m_nxdnFilter(),
m_nxdnISincFilter(),
m_rrcState(),
//m_gaussianState(),
m_boxcarState(),
m_nxdnState(),
m_nxdnISincState(),
m_pttInvert(false),
m_rxLevel(128 * 128),
m_cwIdTXLevel(128 * 128),
m_dstarTXLevel(128 * 128),
m_dmrTXLevel(128 * 128),
m_ysfTXLevel(128 * 128),
m_p25TXLevel(128 * 128),
m_nxdnTXLevel(128 * 128),
m_rxDCOffset(DC_OFFSET),
m_txDCOffset(DC_OFFSET),
m_ledCount(0U),
m_ledValue(true),
m_detect(false),
m_adcOverflow(0U),
m_dacOverflow(0U),
m_watchdog(0U),
m_lockout(false)
{
::memset(m_rrcState, 0x00U, 140U * sizeof(q15_t));
// ::memset(m_gaussianState, 0x00U, 80U * sizeof(q15_t));
::memset(m_boxcarState, 0x00U, 60U * sizeof(q15_t));
::memset(m_nxdnState, 0x00U, 220U * sizeof(q15_t));
::memset(m_nxdnISincState, 0x00U, 60U * sizeof(q15_t));
::memset(m_dcState, 0x00U, 4U * sizeof(q31_t));
m_dcFilter.numStages = DC_FILTER_STAGES;
m_dcFilter.pState = m_dcState;
m_dcFilter.pCoeffs = DC_FILTER;
m_dcFilter.postShift = 0;
m_rrcFilter.numTaps = RRC_0_2_FILTER_LEN;
m_rrcFilter.pState = m_rrcState;
m_rrcFilter.pCoeffs = RRC_0_2_FILTER;
// m_gaussianFilter.numTaps = GAUSSIAN_0_5_FILTER_LEN;
// m_gaussianFilter.pState = m_gaussianState;
// m_gaussianFilter.pCoeffs = GAUSSIAN_0_5_FILTER;
m_boxcarFilter.numTaps = BOXCAR_FILTER_LEN;
m_boxcarFilter.pState = m_boxcarState;
m_boxcarFilter.pCoeffs = BOXCAR_FILTER;
m_nxdnFilter.numTaps = NXDN_0_2_FILTER_LEN;
m_nxdnFilter.pState = m_nxdnState;
m_nxdnFilter.pCoeffs = NXDN_0_2_FILTER;
m_nxdnISincFilter.numTaps = NXDN_ISINC_FILTER_LEN;
m_nxdnISincFilter.pState = m_nxdnISincState;
m_nxdnISincFilter.pCoeffs = NXDN_ISINC_FILTER;
initInt();
selfTest();
}
void CIO::selfTest()
{
bool ledValue = false;
for (uint8_t i = 0; i < 6; i++) {
ledValue = !ledValue;
// We exclude PTT to avoid trigger the transmitter
setLEDInt(ledValue);
setCOSInt(ledValue);
#if defined(ARDUINO_MODE_PINS)
setDStarInt(ledValue);
setDMRInt(ledValue);
setYSFInt(ledValue);
setP25Int(ledValue);
setNXDNInt(ledValue);
#endif
delayInt(250);
}
#if defined(ARDUINO_MODE_PINS)
setDStarInt(true);
setDMRInt(false);
setYSFInt(false);
setP25Int(false);
setNXDNInt(false);
delayInt(250);
setDStarInt(true);
setDMRInt(true);
setYSFInt(false);
setP25Int(false);
setNXDNInt(false);
delayInt(250);
setDStarInt(true);
setDMRInt(true);
setYSFInt(true);
setP25Int(false);
setNXDNInt(false);
delayInt(250);
setDStarInt(true);
setDMRInt(true);
setYSFInt(true);
setP25Int(true);
setNXDNInt(false);
delayInt(250);
setDStarInt(true);
setDMRInt(true);
setYSFInt(true);
setP25Int(true);
setNXDNInt(true);
delayInt(250);
setDStarInt(true);
setDMRInt(true);
setYSFInt(true);
setP25Int(true);
setNXDNInt(false);
delayInt(250);
setDStarInt(true);
setDMRInt(true);
setYSFInt(true);
setP25Int(false);
setNXDNInt(false);
delayInt(250);
setDStarInt(true);
setDMRInt(true);
setYSFInt(false);
setP25Int(false);
setNXDNInt(false);
delayInt(250);
setDStarInt(true);
setDMRInt(false);
setYSFInt(false);
setP25Int(false);
setNXDNInt(false);
delayInt(250);
setDStarInt(false);
setDMRInt(false);
setYSFInt(false);
setP25Int(false);
setNXDNInt(false);
#endif
}
void CIO::start()
{
if (m_started)
return;
startInt();
m_started = true;
setMode();
}
void CIO::process()
{
m_ledCount++;
if (m_started) {
// Two seconds timeout
if (m_watchdog >= 96000U) {
if (m_modemState == STATE_DSTAR || m_modemState == STATE_DMR || m_modemState == STATE_YSF || m_modemState == STATE_P25 || m_modemState == STATE_NXDN) {
if (m_modemState == STATE_DMR && m_tx)
dmrTX.setStart(false);
m_modemState = STATE_IDLE;
setMode();
}
m_watchdog = 0U;
}
#if defined(CONSTANT_SRV_LED)
setLEDInt(true);
#else
if (m_ledCount >= 24000U) {
m_ledCount = 0U;
m_ledValue = !m_ledValue;
setLEDInt(m_ledValue);
}
#endif
} else {
if (m_ledCount >= 480000U) {
m_ledCount = 0U;
m_ledValue = !m_ledValue;
setLEDInt(m_ledValue);
}
return;
}
#if defined(USE_COS_AS_LOCKOUT)
m_lockout = getCOSInt();
#endif
// Switch off the transmitter if needed
if (m_txBuffer.getData() == 0U && m_tx) {
m_tx = false;
setPTTInt(m_pttInvert ? true : false);
}
if (m_rxBuffer.getData() >= RX_BLOCK_SIZE) {
q15_t samples[RX_BLOCK_SIZE];
uint8_t control[RX_BLOCK_SIZE];
uint16_t rssi[RX_BLOCK_SIZE];
for (uint16_t i = 0U; i < RX_BLOCK_SIZE; i++) {
uint16_t sample;
m_rxBuffer.get(sample, control[i]);
m_rssiBuffer.get(rssi[i]);
// Detect ADC overflow
if (m_detect && (sample == 0U || sample == 4095U))
m_adcOverflow++;
q15_t res1 = q15_t(sample) - m_rxDCOffset;
q31_t res2 = res1 * m_rxLevel;
samples[i] = q15_t(__SSAT((res2 >> 15), 16));
}
if (m_lockout)
return;
#if defined(USE_DCBLOCKER)
q31_t q31Samples[RX_BLOCK_SIZE];
::arm_q15_to_q31(samples, q31Samples, RX_BLOCK_SIZE);
q31_t dcValues[RX_BLOCK_SIZE];
::arm_biquad_cascade_df1_q31(&m_dcFilter, q31Samples, dcValues, RX_BLOCK_SIZE);
q31_t dcLevel = 0;
for (uint8_t i = 0U; i < RX_BLOCK_SIZE; i++)
dcLevel += dcValues[i];
dcLevel /= RX_BLOCK_SIZE;
q15_t offset = q15_t(__SSAT((dcLevel >> 16), 16));;
q15_t dcSamples[RX_BLOCK_SIZE];
for (uint8_t i = 0U; i < RX_BLOCK_SIZE; i++)
dcSamples[i] = samples[i] - offset;
#endif
if (m_modemState == STATE_IDLE) {
if (m_dstarEnable) {
q15_t GMSKVals[RX_BLOCK_SIZE];
#if defined(USE_DCBLOCKER)
::arm_fir_fast_q15(&m_boxcarFilter, dcSamples, GMSKVals, RX_BLOCK_SIZE);
#else
::arm_fir_fast_q15(&m_boxcarFilter, samples, GMSKVals, RX_BLOCK_SIZE);
#endif
dstarRX.samples(GMSKVals, rssi, RX_BLOCK_SIZE);
}
if (m_p25Enable) {
q15_t P25Vals[RX_BLOCK_SIZE];
#if defined(USE_DCBLOCKER)
::arm_fir_fast_q15(&m_boxcarFilter, dcSamples, P25Vals, RX_BLOCK_SIZE);
#else
::arm_fir_fast_q15(&m_boxcarFilter, samples, P25Vals, RX_BLOCK_SIZE);
#endif
p25RX.samples(P25Vals, rssi, RX_BLOCK_SIZE);
}
if (m_nxdnEnable) {
q15_t NXDNValsTmp[RX_BLOCK_SIZE];
#if defined(USE_DCBLOCKER)
::arm_fir_fast_q15(&m_nxdnFilter, dcSamples, NXDNValsTmp, RX_BLOCK_SIZE);
#else
::arm_fir_fast_q15(&m_nxdnFilter, samples, NXDNValsTmp, RX_BLOCK_SIZE);
#endif
q15_t NXDNVals[RX_BLOCK_SIZE];
::arm_fir_fast_q15(&m_nxdnISincFilter, NXDNValsTmp, NXDNVals, RX_BLOCK_SIZE);
nxdnRX.samples(NXDNVals, rssi, RX_BLOCK_SIZE);
}
if (m_dmrEnable || m_ysfEnable) {
q15_t RRCVals[RX_BLOCK_SIZE];
::arm_fir_fast_q15(&m_rrcFilter, samples, RRCVals, RX_BLOCK_SIZE);
if (m_ysfEnable)
ysfRX.samples(RRCVals, rssi, RX_BLOCK_SIZE);
if (m_dmrEnable) {
if (m_duplex)
dmrIdleRX.samples(RRCVals, RX_BLOCK_SIZE);
else
dmrDMORX.samples(RRCVals, rssi, RX_BLOCK_SIZE);
}
}
} else if (m_modemState == STATE_DSTAR) {
if (m_dstarEnable) {
q15_t GMSKVals[RX_BLOCK_SIZE];
#if defined(USE_DCBLOCKER)
::arm_fir_fast_q15(&m_boxcarFilter, dcSamples, GMSKVals, RX_BLOCK_SIZE);
#else
::arm_fir_fast_q15(&m_boxcarFilter, samples, GMSKVals, RX_BLOCK_SIZE);
#endif
dstarRX.samples(GMSKVals, rssi, RX_BLOCK_SIZE);
}
} else if (m_modemState == STATE_DMR) {
if (m_dmrEnable) {
q15_t DMRVals[RX_BLOCK_SIZE];
::arm_fir_fast_q15(&m_rrcFilter, samples, DMRVals, RX_BLOCK_SIZE);
if (m_duplex) {
// If the transmitter isn't on, use the DMR idle RX to detect the wakeup CSBKs
if (m_tx)
dmrRX.samples(DMRVals, rssi, control, RX_BLOCK_SIZE);
else
dmrIdleRX.samples(DMRVals, RX_BLOCK_SIZE);
} else {
dmrDMORX.samples(DMRVals, rssi, RX_BLOCK_SIZE);
}
}
} else if (m_modemState == STATE_YSF) {
if (m_ysfEnable) {
q15_t YSFVals[RX_BLOCK_SIZE];
#if defined(USE_DCBLOCKER)
::arm_fir_fast_q15(&m_rrcFilter, dcSamples, YSFVals, RX_BLOCK_SIZE);
#else
::arm_fir_fast_q15(&m_rrcFilter, samples, YSFVals, RX_BLOCK_SIZE);
#endif
ysfRX.samples(YSFVals, rssi, RX_BLOCK_SIZE);
}
} else if (m_modemState == STATE_P25) {
if (m_p25Enable) {
q15_t P25Vals[RX_BLOCK_SIZE];
#if defined(USE_DCBLOCKER)
::arm_fir_fast_q15(&m_boxcarFilter, dcSamples, P25Vals, RX_BLOCK_SIZE);
#else
::arm_fir_fast_q15(&m_boxcarFilter, samples, P25Vals, RX_BLOCK_SIZE);
#endif
p25RX.samples(P25Vals, rssi, RX_BLOCK_SIZE);
}
} else if (m_modemState == STATE_NXDN) {
if (m_nxdnEnable) {
q15_t NXDNValsTmp[RX_BLOCK_SIZE];
#if defined(USE_DCBLOCKER)
::arm_fir_fast_q15(&m_nxdnFilter, dcSamples, NXDNValsTmp, RX_BLOCK_SIZE);
#else
::arm_fir_fast_q15(&m_nxdnFilter, samples, NXDNValsTmp, RX_BLOCK_SIZE);
#endif
q15_t NXDNVals[RX_BLOCK_SIZE];
::arm_fir_fast_q15(&m_nxdnISincFilter, NXDNValsTmp, NXDNVals, RX_BLOCK_SIZE);
nxdnRX.samples(NXDNVals, rssi, RX_BLOCK_SIZE);
}
} else if (m_modemState == STATE_DSTARCAL) {
q15_t GMSKVals[RX_BLOCK_SIZE];
::arm_fir_fast_q15(&m_boxcarFilter, samples, GMSKVals, RX_BLOCK_SIZE);
calDStarRX.samples(GMSKVals, RX_BLOCK_SIZE);
} else if (m_modemState == STATE_RSSICAL) {
calRSSI.samples(rssi, RX_BLOCK_SIZE);
}
}
}
void CIO::write(MMDVM_STATE mode, q15_t* samples, uint16_t length, const uint8_t* control)
{
if (!m_started)
return;
if (m_lockout)
return;
// Switch the transmitter on if needed
if (!m_tx) {
m_tx = true;
setPTTInt(m_pttInvert ? false : true);
}
q15_t txLevel = 0;
switch (mode) {
case STATE_DSTAR:
txLevel = m_dstarTXLevel;
break;
case STATE_DMR:
txLevel = m_dmrTXLevel;
break;
case STATE_YSF:
txLevel = m_ysfTXLevel;
break;
case STATE_P25:
txLevel = m_p25TXLevel;
break;
case STATE_NXDN:
txLevel = m_nxdnTXLevel;
break;
default:
txLevel = m_cwIdTXLevel;
break;
}
for (uint16_t i = 0U; i < length; i++) {
q31_t res1 = samples[i] * txLevel;
q15_t res2 = q15_t(__SSAT((res1 >> 15), 16));
uint16_t res3 = uint16_t(res2 + m_txDCOffset);
// Detect DAC overflow
if (res3 > 4095U)
m_dacOverflow++;
if (control == NULL)
m_txBuffer.put(res3, MARK_NONE);
else
m_txBuffer.put(res3, control[i]);
}
}
uint16_t CIO::getSpace() const
{
return m_txBuffer.getSpace();
}
void CIO::setDecode(bool dcd)
{
if (dcd != m_dcd)
setCOSInt(dcd ? true : false);
m_dcd = dcd;
}
void CIO::setADCDetection(bool detect)
{
m_detect = detect;
}
void CIO::setMode()
{
#if defined(ARDUINO_MODE_PINS)
setDStarInt(m_modemState == STATE_DSTAR);
setDMRInt(m_modemState == STATE_DMR);
setYSFInt(m_modemState == STATE_YSF);
setP25Int(m_modemState == STATE_P25);
setNXDNInt(m_modemState == STATE_NXDN);
#endif
}
void CIO::setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rxLevel, uint8_t cwIdTXLevel, uint8_t dstarTXLevel, uint8_t dmrTXLevel, uint8_t ysfTXLevel, uint8_t p25TXLevel, uint8_t nxdnTXLevel, int16_t txDCOffset, int16_t rxDCOffset)
{
m_pttInvert = pttInvert;
m_rxLevel = q15_t(rxLevel * 128);
m_cwIdTXLevel = q15_t(cwIdTXLevel * 128);
m_dstarTXLevel = q15_t(dstarTXLevel * 128);
m_dmrTXLevel = q15_t(dmrTXLevel * 128);
m_ysfTXLevel = q15_t(ysfTXLevel * 128);
m_p25TXLevel = q15_t(p25TXLevel * 128);
m_nxdnTXLevel = q15_t(nxdnTXLevel * 128);
m_rxDCOffset = DC_OFFSET + rxDCOffset;
m_txDCOffset = DC_OFFSET + txDCOffset;
if (rxInvert)
m_rxLevel = -m_rxLevel;
if (txInvert) {
m_dstarTXLevel = -m_dstarTXLevel;
m_dmrTXLevel = -m_dmrTXLevel;
m_ysfTXLevel = -m_ysfTXLevel;
m_p25TXLevel = -m_p25TXLevel;
m_nxdnTXLevel = -m_nxdnTXLevel;
}
}
void CIO::getOverflow(bool& adcOverflow, bool& dacOverflow)
{
adcOverflow = m_adcOverflow > 0U;
dacOverflow = m_dacOverflow > 0U;
m_adcOverflow = 0U;
m_dacOverflow = 0U;
}
bool CIO::hasTXOverflow()
{
return m_txBuffer.hasOverflowed();
}
bool CIO::hasRXOverflow()
{
return m_rxBuffer.hasOverflowed();
}
void CIO::resetWatchdog()
{
m_watchdog = 0U;
}
uint32_t CIO::getWatchdog()
{
return m_watchdog;
}
bool CIO::hasLockout() const
{
return m_lockout;
}
Reference in New Issue View Git Blame Copy Permalink