diff --git a/DMRTX.cpp b/DMRTX.cpp index d6b7c04..9f3142f 100644 --- a/DMRTX.cpp +++ b/DMRTX.cpp @@ -63,7 +63,8 @@ m_newShortLC(), m_markBuffer(), m_poBuffer(), m_poLen(0U), -m_poPtr(0U) +m_poPtr(0U), +m_count(0U) { ::memset(m_modState, 0x00U, 70U * sizeof(q15_t)); @@ -192,8 +193,8 @@ void CDMRTX::setStart(bool start) void CDMRTX::writeByte(uint8_t c, uint8_t control) { - q15_t inBuffer[DMR_RADIO_SYMBOL_LENGTH * 4U]; - q15_t outBuffer[DMR_RADIO_SYMBOL_LENGTH * 4U]; + q15_t inBuffer[DMR_RADIO_SYMBOL_LENGTH * 4U + 1U]; + q15_t outBuffer[DMR_RADIO_SYMBOL_LENGTH * 4U + 1U]; const uint8_t MASK = 0xC0U; @@ -215,13 +216,34 @@ void CDMRTX::writeByte(uint8_t c, uint8_t control) } } - uint8_t controlBuffer[DMR_RADIO_SYMBOL_LENGTH * 4U]; - ::memset(controlBuffer, MARK_NONE, DMR_RADIO_SYMBOL_LENGTH * 4U * sizeof(uint8_t)); - controlBuffer[DMR_RADIO_SYMBOL_LENGTH * 2U] = control; + uint8_t blockSize = DMR_RADIO_SYMBOL_LENGTH * 4U; - ::arm_fir_fast_q15(&m_modFilter, inBuffer, outBuffer, DMR_RADIO_SYMBOL_LENGTH * 4U); + uint8_t controlBuffer[DMR_RADIO_SYMBOL_LENGTH * 4U + 1U]; + ::memset(controlBuffer, MARK_NONE, (DMR_RADIO_SYMBOL_LENGTH * 4U + 1U) * sizeof(uint8_t)); - io.write(outBuffer, DMR_RADIO_SYMBOL_LENGTH * 4U, controlBuffer); + // Handle the case of the oscillator not being accurate enough + if (m_sampleCount > 0U) { + m_count += DMR_RADIO_SYMBOL_LENGTH * 4U; + + if (m_count >= m_sampleCount) { + if (m_sampleInsert) { + inBuffer[DMR_RADIO_SYMBOL_LENGTH * 4U] = inBuffer[DMR_RADIO_SYMBOL_LENGTH * 4U - 1U]; + blockSize = DMR_RADIO_SYMBOL_LENGTH * 4U + 1U; + controlBuffer[DMR_RADIO_SYMBOL_LENGTH * 2U + 1U] = control; + } else { + blockSize = DMR_RADIO_SYMBOL_LENGTH * 4U - 1U; + controlBuffer[DMR_RADIO_SYMBOL_LENGTH * 2U - 1U] = control; + } + + m_count -= m_sampleCount; + } + } else { + controlBuffer[DMR_RADIO_SYMBOL_LENGTH * 2U] = control; + } + + ::arm_fir_fast_q15(&m_modFilter, inBuffer, outBuffer, blockSize); + + io.write(outBuffer, blockSize, controlBuffer); } uint16_t CDMRTX::getSpace1() const diff --git a/DMRTX.h b/DMRTX.h index 3cae45e..7fc499d 100644 --- a/DMRTX.h +++ b/DMRTX.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 by Jonathan Naylor G4KLX + * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX * * 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 @@ -62,6 +62,7 @@ private: uint8_t m_poBuffer[40U]; uint16_t m_poLen; uint16_t m_poPtr; + uint32_t m_count; void createData(uint8_t slotIndex); void createCACH(uint8_t txSlotIndex, uint8_t rxSlotIndex); diff --git a/DStarTX.cpp b/DStarTX.cpp index ce704a1..236f333 100644 --- a/DStarTX.cpp +++ b/DStarTX.cpp @@ -193,7 +193,8 @@ m_modState(), m_poBuffer(), m_poLen(0U), m_poPtr(0U), -m_txDelay(60U) // 100ms +m_txDelay(60U), // 100ms +m_count(0U) { ::memset(m_modState, 0x00U, 60U * sizeof(q15_t)); @@ -403,8 +404,8 @@ void CDStarTX::txHeader(const uint8_t* in, uint8_t* out) const void CDStarTX::writeByte(uint8_t c) { - q15_t inBuffer[DSTAR_RADIO_BIT_LENGTH * 8U]; - q15_t outBuffer[DSTAR_RADIO_BIT_LENGTH * 8U]; + q15_t inBuffer[DSTAR_RADIO_BIT_LENGTH * 8U + 1U]; + q15_t outBuffer[DSTAR_RADIO_BIT_LENGTH * 8U + 1U]; uint8_t mask = 0x01U; @@ -418,9 +419,27 @@ void CDStarTX::writeByte(uint8_t c) mask <<= 1; } - ::arm_fir_fast_q15(&m_modFilter, inBuffer, outBuffer, DSTAR_RADIO_BIT_LENGTH * 8U); + uint8_t blockSize = DSTAR_RADIO_BIT_LENGTH * 8U; - io.write(outBuffer, DSTAR_RADIO_BIT_LENGTH * 8U); + // Handle the case of the oscillator not being accurate enough + if (m_sampleCount > 0U) { + m_count += DSTAR_RADIO_BIT_LENGTH * 8U; + + if (m_count >= m_sampleCount) { + if (m_sampleInsert) { + inBuffer[DSTAR_RADIO_BIT_LENGTH * 8U] = inBuffer[DSTAR_RADIO_BIT_LENGTH * 8U - 1U]; + blockSize = DSTAR_RADIO_BIT_LENGTH * 8U + 1U; + } else { + blockSize = DSTAR_RADIO_BIT_LENGTH * 8U - 1U; + } + + m_count -= m_sampleCount; + } + } + + ::arm_fir_fast_q15(&m_modFilter, inBuffer, outBuffer, blockSize); + + io.write(outBuffer, blockSize); } void CDStarTX::setTXDelay(uint8_t delay) diff --git a/DStarTX.h b/DStarTX.h index 22e3fd0..b2f72ff 100644 --- a/DStarTX.h +++ b/DStarTX.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 by Jonathan Naylor G4KLX + * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX * * 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 @@ -45,6 +45,7 @@ private: uint16_t m_poLen; uint16_t m_poPtr; uint16_t m_txDelay; // In bytes + uint32_t m_count; void txHeader(const uint8_t* in, uint8_t* out) const; void writeByte(uint8_t c); diff --git a/Globals.h b/Globals.h index 8a7509e..a97c2c3 100644 --- a/Globals.h +++ b/Globals.h @@ -69,6 +69,9 @@ extern bool m_ysfEnable; extern bool m_tx; +extern uint32_t m_sampleCount; +extern bool m_sampleInsert; + extern CSerialPort serial; extern CIO io; diff --git a/IO.cpp b/IO.cpp index d1dab5e..5cdeed9 100644 --- a/IO.cpp +++ b/IO.cpp @@ -117,6 +117,7 @@ m_ledValue(true), m_dcd(false), m_overflow(0U), m_overcount(0U), +m_count(0U), m_watchdog(0U), m_lockout(false) { @@ -256,10 +257,12 @@ void CIO::process() } if (m_rxBuffer.getData() >= RX_BLOCK_SIZE) { - q15_t samples[RX_BLOCK_SIZE]; - uint8_t control[RX_BLOCK_SIZE]; + q15_t samples[RX_BLOCK_SIZE + 1U]; + uint8_t control[RX_BLOCK_SIZE + 1U]; - for (uint16_t i = 0U; i < RX_BLOCK_SIZE; i++) { + uint8_t blockSize = RX_BLOCK_SIZE; + + for (uint16_t i = 0U; i < RX_BLOCK_SIZE; i++) { uint16_t sample; m_rxBuffer.get(sample, control[i]); @@ -273,57 +276,74 @@ void CIO::process() samples[i] = q15_t(__SSAT((res2 >> 15), 16)); } + // Handle the case of the oscillator not being accurate enough + if (m_sampleCount > 0U) { + m_count += RX_BLOCK_SIZE; + + if (m_count >= m_sampleCount) { + if (m_sampleInsert) { + control[RX_BLOCK_SIZE] = control[RX_BLOCK_SIZE - 1U]; + samples[RX_BLOCK_SIZE] = 0; + blockSize = RX_BLOCK_SIZE + 1U; + } else { + blockSize = RX_BLOCK_SIZE - 1U; + } + + m_count -= m_sampleCount; + } + } + if (m_lockout) return; if (m_modemState == STATE_IDLE) { if (m_dstarEnable) { - q15_t GMSKVals[RX_BLOCK_SIZE]; - ::arm_fir_fast_q15(&m_GMSKFilter, samples, GMSKVals, RX_BLOCK_SIZE); + q15_t GMSKVals[RX_BLOCK_SIZE + 1U]; + ::arm_fir_fast_q15(&m_GMSKFilter, samples, GMSKVals, blockSize); - dstarRX.samples(GMSKVals, RX_BLOCK_SIZE); + dstarRX.samples(GMSKVals, blockSize); } if (m_dmrEnable || m_ysfEnable) { - q15_t C4FSKVals[RX_BLOCK_SIZE]; - ::arm_fir_fast_q15(&m_C4FSKFilter, samples, C4FSKVals, RX_BLOCK_SIZE); + q15_t C4FSKVals[RX_BLOCK_SIZE + 1U]; + ::arm_fir_fast_q15(&m_C4FSKFilter, samples, C4FSKVals, blockSize); if (m_dmrEnable) - dmrIdleRX.samples(C4FSKVals, RX_BLOCK_SIZE); + dmrIdleRX.samples(C4FSKVals, blockSize); if (m_ysfEnable) - ysfRX.samples(C4FSKVals, RX_BLOCK_SIZE); + ysfRX.samples(C4FSKVals, blockSize); } } else if (m_modemState == STATE_DSTAR) { if (m_dstarEnable) { - q15_t GMSKVals[RX_BLOCK_SIZE]; - ::arm_fir_fast_q15(&m_GMSKFilter, samples, GMSKVals, RX_BLOCK_SIZE); + q15_t GMSKVals[RX_BLOCK_SIZE + 1U]; + ::arm_fir_fast_q15(&m_GMSKFilter, samples, GMSKVals, blockSize); - dstarRX.samples(GMSKVals, RX_BLOCK_SIZE); + dstarRX.samples(GMSKVals, blockSize); } } else if (m_modemState == STATE_DMR) { if (m_dmrEnable) { - q15_t C4FSKVals[RX_BLOCK_SIZE]; - ::arm_fir_fast_q15(&m_C4FSKFilter, samples, C4FSKVals, RX_BLOCK_SIZE); + q15_t C4FSKVals[RX_BLOCK_SIZE + 1U]; + ::arm_fir_fast_q15(&m_C4FSKFilter, samples, C4FSKVals, blockSize); // If the transmitter isn't on, use the DMR idle RX to detect the wakeup CSBKs if (m_tx) - dmrRX.samples(C4FSKVals, control, RX_BLOCK_SIZE); + dmrRX.samples(C4FSKVals, control, blockSize); else - dmrIdleRX.samples(C4FSKVals, RX_BLOCK_SIZE); + dmrIdleRX.samples(C4FSKVals, blockSize); } } else if (m_modemState == STATE_YSF) { if (m_ysfEnable) { - q15_t C4FSKVals[RX_BLOCK_SIZE]; - ::arm_fir_fast_q15(&m_C4FSKFilter, samples, C4FSKVals, RX_BLOCK_SIZE); + q15_t C4FSKVals[RX_BLOCK_SIZE + 1U]; + ::arm_fir_fast_q15(&m_C4FSKFilter, samples, C4FSKVals, blockSize); - ysfRX.samples(C4FSKVals, RX_BLOCK_SIZE); + ysfRX.samples(C4FSKVals, blockSize); } } else if (m_modemState == STATE_CALIBRATE) { - q15_t GMSKVals[RX_BLOCK_SIZE]; - ::arm_fir_fast_q15(&m_GMSKFilter, samples, GMSKVals, RX_BLOCK_SIZE); + q15_t GMSKVals[RX_BLOCK_SIZE + 1U]; + ::arm_fir_fast_q15(&m_GMSKFilter, samples, GMSKVals, blockSize); - calRX.samples(GMSKVals, RX_BLOCK_SIZE); + calRX.samples(GMSKVals, blockSize); } } } diff --git a/IO.h b/IO.h index 962f6f4..4878d8d 100644 --- a/IO.h +++ b/IO.h @@ -85,6 +85,8 @@ private: uint16_t m_overflow; uint16_t m_overcount; + uint32_t m_count; + volatile uint32_t m_watchdog; bool m_lockout; diff --git a/MMDVM.cpp b/MMDVM.cpp index 134e949..734b20e 100644 --- a/MMDVM.cpp +++ b/MMDVM.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 by Jonathan Naylor G4KLX + * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX * Copyright (C) 2016 by Mathis Schmieder DB9MAT * * This program is free software; you can redistribute it and/or modify @@ -31,6 +31,9 @@ bool m_ysfEnable = true; bool m_tx = false; +uint32_t m_sampleCount = 0U; +bool m_sampleInsert = false; + CDStarRX dstarRX; CDStarTX dstarTX; diff --git a/MMDVM.ino b/MMDVM.ino index 773a660..d9c2041 100644 --- a/MMDVM.ino +++ b/MMDVM.ino @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 by Jonathan Naylor G4KLX + * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX * * 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 @@ -28,6 +28,9 @@ bool m_ysfEnable = true; bool m_tx = false; +uint32_t m_sampleCount = 0U; +bool m_sampleInsert = false; + CDStarRX dstarRX; CDStarTX dstarTX; diff --git a/SerialPort.cpp b/SerialPort.cpp index 2c73ef2..4457bf2 100644 --- a/SerialPort.cpp +++ b/SerialPort.cpp @@ -174,7 +174,7 @@ void CSerialPort::getVersion() uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length) { - if (length < 8U) + if (length < 9U) return 4U; bool rxInvert = (data[0U] & 0x01U) == 0x01U; @@ -209,6 +209,18 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length) uint8_t dmrDelay = data[7U]; + int8_t oscOffset = int8_t(data[8U]) - 128; + if (oscOffset < 0) { + m_sampleCount = 1000000U / uint32_t(-oscOffset); + m_sampleInsert = true; + } else if (oscOffset > 0) { + m_sampleCount = 1000000U / uint32_t(oscOffset); + m_sampleInsert = false; + } else { + m_sampleCount = 0U; + m_sampleInsert = false; + } + m_modemState = modemState; m_dstarEnable = dstarEnable; diff --git a/YSFTX.cpp b/YSFTX.cpp index 5af85fa..f72ab98 100644 --- a/YSFTX.cpp +++ b/YSFTX.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2009-2015 by Jonathan Naylor G4KLX + * Copyright (C) 2009-2016 by Jonathan Naylor G4KLX * * 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 @@ -44,7 +44,8 @@ m_modState(), m_poBuffer(), m_poLen(0U), m_poPtr(0U), -m_txDelay(120U) // 100ms +m_txDelay(120U), // 100ms +m_count(0U) { ::memset(m_modState, 0x00U, 70U * sizeof(q15_t)); @@ -107,8 +108,8 @@ uint8_t CYSFTX::writeData(const uint8_t* data, uint8_t length) void CYSFTX::writeByte(uint8_t c) { - q15_t inBuffer[YSF_RADIO_SYMBOL_LENGTH * 4U]; - q15_t outBuffer[YSF_RADIO_SYMBOL_LENGTH * 4U]; + q15_t inBuffer[YSF_RADIO_SYMBOL_LENGTH * 4U + 1U]; + q15_t outBuffer[YSF_RADIO_SYMBOL_LENGTH * 4U + 1U]; const uint8_t MASK = 0xC0U; @@ -130,9 +131,27 @@ void CYSFTX::writeByte(uint8_t c) } } - ::arm_fir_fast_q15(&m_modFilter, inBuffer, outBuffer, YSF_RADIO_SYMBOL_LENGTH * 4U); + uint8_t blockSize = YSF_RADIO_SYMBOL_LENGTH * 4U; - io.write(outBuffer, YSF_RADIO_SYMBOL_LENGTH * 4U); + // Handle the case of the oscillator not being accurate enough + if (m_sampleCount > 0U) { + m_count += YSF_RADIO_SYMBOL_LENGTH * 4U; + + if (m_count >= m_sampleCount) { + if (m_sampleInsert) { + inBuffer[YSF_RADIO_SYMBOL_LENGTH * 4U] = inBuffer[YSF_RADIO_SYMBOL_LENGTH * 4U - 1U]; + blockSize = YSF_RADIO_SYMBOL_LENGTH * 4U + 1U; + } else { + blockSize = YSF_RADIO_SYMBOL_LENGTH * 4U - 1U; + } + + m_count -= m_sampleCount; + } + } + + ::arm_fir_fast_q15(&m_modFilter, inBuffer, outBuffer, blockSize); + + io.write(outBuffer, blockSize); } void CYSFTX::setTXDelay(uint8_t delay) diff --git a/YSFTX.h b/YSFTX.h index bcaba20..9235a9c 100644 --- a/YSFTX.h +++ b/YSFTX.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 by Jonathan Naylor G4KLX + * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX * * 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 @@ -43,6 +43,7 @@ private: uint16_t m_poLen; uint16_t m_poPtr; uint16_t m_txDelay; + uint32_t m_count; void writeByte(uint8_t c); };