diff --git a/Globals.h b/Globals.h index 5950200..7888891 100644 --- a/Globals.h +++ b/Globals.h @@ -55,6 +55,7 @@ enum MMDVM_STATE { #include "YSFRX.h" #include "YSFTX.h" #include "P25RX.h" +#include "P25TX.h" #include "CalDStarRX.h" #include "CalDStarTX.h" #include "CalDMR.h" @@ -102,6 +103,7 @@ extern CYSFRX ysfRX; extern CYSFTX ysfTX; extern CP25RX p25RX; +extern CP25TX p25TX; extern CCalDStarRX calDStarRX; extern CCalDStarTX calDStarTX; diff --git a/IO.cpp b/IO.cpp index 7a864c6..da45079 100644 --- a/IO.cpp +++ b/IO.cpp @@ -129,6 +129,7 @@ m_rxLevel(128 * 128), m_dstarTXLevel(128 * 128), m_dmrTXLevel(128 * 128), m_ysfTXLevel(128 * 128), +m_p25TXLevel(128 * 128), m_ledCount(0U), m_ledValue(true), m_dcd(false), @@ -439,6 +440,9 @@ void CIO::write(MMDVM_STATE mode, q15_t* samples, uint16_t length, const uint8_t case STATE_YSF: txLevel = m_ysfTXLevel; break; + case STATE_P25: + txLevel = m_p25TXLevel; + break; default: txLevel = m_dstarTXLevel; break; @@ -532,7 +536,7 @@ switch (m_modemState) { #endif } -void CIO::setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rxLevel, uint8_t dstarTXLevel, uint8_t dmrTXLevel, uint8_t ysfTXLevel) +void CIO::setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rxLevel, uint8_t dstarTXLevel, uint8_t dmrTXLevel, uint8_t ysfTXLevel, uint8_t p25TXLevel) { m_pttInvert = pttInvert; @@ -540,6 +544,7 @@ void CIO::setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rx m_dstarTXLevel = q15_t(dstarTXLevel * 128); m_dmrTXLevel = q15_t(dmrTXLevel * 128); m_ysfTXLevel = q15_t(ysfTXLevel * 128); + m_p25TXLevel = q15_t(p25TXLevel * 128); if (rxInvert) m_rxLevel = -m_rxLevel; @@ -548,6 +553,7 @@ void CIO::setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rx m_dstarTXLevel = -m_dstarTXLevel; m_dmrTXLevel = -m_dmrTXLevel; m_ysfTXLevel = -m_ysfTXLevel; + m_p25TXLevel = -m_p25TXLevel; } } diff --git a/IO.h b/IO.h index b7befcb..910853c 100644 --- a/IO.h +++ b/IO.h @@ -41,7 +41,7 @@ public: void interrupt(); - void setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rxLevel, uint8_t dstarTXLevel, uint8_t dmrTXLevel, uint8_t ysfTXLevel); + void setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rxLevel, uint8_t dstarTXLevel, uint8_t dmrTXLevel, uint8_t ysfTXLevel, uint8_t p25TXLevel); void getOverflow(bool& adcOverflow, bool& dacOverflow); @@ -84,6 +84,7 @@ private: q15_t m_dstarTXLevel; q15_t m_dmrTXLevel; q15_t m_ysfTXLevel; + q15_t m_p25TXLevel; uint32_t m_ledCount; bool m_ledValue; diff --git a/MMDVM.cpp b/MMDVM.cpp index 78a26d2..f99cba6 100644 --- a/MMDVM.cpp +++ b/MMDVM.cpp @@ -52,6 +52,7 @@ CYSFRX ysfRX; CYSFTX ysfTX; CP25RX p25RX; +CP25TX p25TX; CCalDStarRX calDStarRX; CCalDStarTX calDStarTX; @@ -87,6 +88,9 @@ void loop() if (m_ysfEnable && m_modemState == STATE_YSF) ysfTX.process(); + if (m_p25Enable && m_modemState == STATE_P25) + p25TX.process(); + if (m_modemState == STATE_DSTARCAL) calDStarTX.process(); diff --git a/P25TX.cpp b/P25TX.cpp new file mode 100644 index 0000000..82ea422 --- /dev/null +++ b/P25TX.cpp @@ -0,0 +1,180 @@ +/* + * Copyright (C) 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 + * 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. + */ + +// #define WANT_DEBUG + +#include "Config.h" +#include "Globals.h" +#include "P25TX.h" + +#include "P25Defines.h" + +#if defined(WIDE_C4FSK_FILTERS_TX) +// Generated using rcosdesign(0.2, 4, 5, 'sqrt') in MATLAB +static q15_t P25_C4FSK_FILTER[] = {688, -680, -2158, -3060, -2724, -775, 2684, 7041, 11310, 14425, 15565, 14425, + 11310, 7041, 2684, -775, -2724, -3060, -2158, -680, 688, 0}; +const uint16_t P25_C4FSK_FILTER_LEN = 22U; +#else +// Generated using rcosdesign(0.2, 8, 5, 'sqrt') in MATLAB +static q15_t P25_C4FSK_FILTER[] = {401, 104, -340, -731, -847, -553, 112, 909, 1472, 1450, 683, -675, -2144, -3040, -2706, -770, 2667, 6995, + 11237, 14331, 15464, 14331, 11237, 6995, 2667, -770, -2706, -3040, -2144, -675, 683, 1450, 1472, 909, 112, + -553, -847, -731, -340, 104, 401, 0}; +const uint16_t P25_C4FSK_FILTER_LEN = 42U; +#endif + +// XXX These will need setting correctly +const q15_t P25_LEVELA[] = { 809, 809, 809, 809, 809}; +const q15_t P25_LEVELB[] = { 269, 269, 269, 269, 269}; +const q15_t P25_LEVELC[] = {-269, -269, -269, -269, -269}; +const q15_t P25_LEVELD[] = {-809, -809, -809, -809, -809}; + +const uint8_t P25_START_SYNC = 0x77U; + +CP25TX::CP25TX() : +m_buffer(1500U), +m_modFilter(), +m_modState(), +m_poBuffer(), +m_poLen(0U), +m_poPtr(0U), +m_txDelay(240U), // 200ms +m_count(0U) +{ + ::memset(m_modState, 0x00U, 70U * sizeof(q15_t)); + + m_modFilter.numTaps = P25_C4FSK_FILTER_LEN; + m_modFilter.pState = m_modState; + m_modFilter.pCoeffs = P25_C4FSK_FILTER; +} + +void CP25TX::process() +{ + if (m_buffer.getData() == 0U && m_poLen == 0U) + return; + + if (m_poLen == 0U) { + if (!m_tx) { + m_count = 0U; + + for (uint16_t i = 0U; i < m_txDelay; i++) + m_poBuffer[m_poLen++] = P25_START_SYNC; + } else { + uint8_t length = m_buffer.get(); + for (uint8_t i = 0U; i < length; i++) { + uint8_t c = m_buffer.get(); + m_poBuffer[m_poLen++] = c; + } + } + + m_poPtr = 0U; + } + + if (m_poLen > 0U) { + uint16_t space = io.getSpace(); + + while (space > (4U * P25_RADIO_SYMBOL_LENGTH)) { + uint8_t c = m_poBuffer[m_poPtr++]; + writeByte(c); + + space -= 4U * P25_RADIO_SYMBOL_LENGTH; + + if (m_poPtr >= m_poLen) { + m_poPtr = 0U; + m_poLen = 0U; + return; + } + } + } +} + +uint8_t CP25TX::writeData(const uint8_t* data, uint8_t length) +{ + if (length < (P25_HDR_FRAME_LENGTH_BYTES + 1U)) + return 4U; + + uint16_t space = m_buffer.getSpace(); + if (space < length) + return 5U; + + m_buffer.put(length - 1U); + for (uint8_t i = 0U; i < (length - 1U); i++) + m_buffer.put(data[i + 1U]); + + return 0U; +} + +void CP25TX::writeByte(uint8_t c) +{ + q15_t inBuffer[P25_RADIO_SYMBOL_LENGTH * 4U + 1U]; + q15_t outBuffer[P25_RADIO_SYMBOL_LENGTH * 4U + 1U]; + + const uint8_t MASK = 0xC0U; + + q15_t* p = inBuffer; + for (uint8_t i = 0U; i < 4U; i++, c <<= 2, p += P25_RADIO_SYMBOL_LENGTH) { + switch (c & MASK) { + case 0xC0U: + ::memcpy(p, P25_LEVELA, P25_RADIO_SYMBOL_LENGTH * sizeof(q15_t)); + break; + case 0x80U: + ::memcpy(p, P25_LEVELB, P25_RADIO_SYMBOL_LENGTH * sizeof(q15_t)); + break; + case 0x00U: + ::memcpy(p, P25_LEVELC, P25_RADIO_SYMBOL_LENGTH * sizeof(q15_t)); + break; + default: + ::memcpy(p, P25_LEVELD, P25_RADIO_SYMBOL_LENGTH * sizeof(q15_t)); + break; + } + } + + uint16_t blockSize = P25_RADIO_SYMBOL_LENGTH * 4U; + + // Handle the case of the oscillator not being accurate enough + if (m_sampleCount > 0U) { + m_count += P25_RADIO_SYMBOL_LENGTH * 4U; + + if (m_count >= m_sampleCount) { + if (m_sampleInsert) { + inBuffer[P25_RADIO_SYMBOL_LENGTH * 4U] = inBuffer[P25_RADIO_SYMBOL_LENGTH * 4U - 1U]; + blockSize++; + } else { + blockSize--; + } + + m_count -= m_sampleCount; + } + } + + ::arm_fir_fast_q15(&m_modFilter, inBuffer, outBuffer, blockSize); + + // Inverse sinc lowpass filter needed here + + io.write(STATE_P25, outBuffer, blockSize); +} + +void CP25TX::setTXDelay(uint8_t delay) +{ + m_txDelay = 600U + uint16_t(delay) * 12U; // 500ms + tx delay +} + +uint16_t CP25TX::getSpace() const +{ + return m_buffer.getSpace() / P25_LDU_FRAME_LENGTH_BYTES; +} + diff --git a/P25TX.h b/P25TX.h new file mode 100644 index 0000000..2c432c0 --- /dev/null +++ b/P25TX.h @@ -0,0 +1,52 @@ +/* + * Copyright (C) 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 + * 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. + */ + +#if !defined(P25TX_H) +#define P25TX_H + +#include "Config.h" + +#include "SerialRB.h" + +class CP25TX { +public: + CP25TX(); + + uint8_t writeData(const uint8_t* data, uint8_t length); + + void process(); + + void setTXDelay(uint8_t delay); + + uint16_t getSpace() const; + +private: + CSerialRB m_buffer; + arm_fir_instance_q15 m_modFilter; + q15_t m_modState[70U]; // NoTaps + BlockSize - 1, 42 + 20 - 1 plus some spare + uint8_t m_poBuffer[920U]; + uint16_t m_poLen; + uint16_t m_poPtr; + uint16_t m_txDelay; + uint32_t m_count; + + void writeByte(uint8_t c); +}; + +#endif + diff --git a/SerialPort.cpp b/SerialPort.cpp index c9d1d89..86259c3 100644 --- a/SerialPort.cpp +++ b/SerialPort.cpp @@ -176,7 +176,10 @@ void CSerialPort::getStatus() else reply[9U] = 0U; - reply[10U] = 0U; // P25 + if (m_p25Enable) + reply[10U] = p25TX.getSpace(); + else + reply[10U] = 0U; write(reply, 11); } @@ -202,7 +205,7 @@ void CSerialPort::getVersion() uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length) { - if (length < 12U) + if (length < 13U) return 4U; bool rxInvert = (data[0U] & 0x01U) == 0x01U; @@ -255,6 +258,7 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length) uint8_t dstarTXLevel = data[9U]; uint8_t dmrTXLevel = data[10U]; uint8_t ysfTXLevel = data[11U]; + uint8_t p25TXLevel = data[12U]; m_modemState = modemState; @@ -266,6 +270,7 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length) dstarTX.setTXDelay(txDelay); ysfTX.setTXDelay(txDelay); + p25TX.setTXDelay(txDelay); dmrDMOTX.setTXDelay(txDelay); dmrTX.setColorCode(colorCode); @@ -274,7 +279,7 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length) dmrDMORX.setColorCode(colorCode); dmrIdleRX.setColorCode(colorCode); - io.setParameters(rxInvert, txInvert, pttInvert, rxLevel, dstarTXLevel, dmrTXLevel, ysfTXLevel); + io.setParameters(rxInvert, txInvert, pttInvert, rxLevel, dstarTXLevel, dmrTXLevel, ysfTXLevel, p25TXLevel); io.start(); @@ -314,6 +319,7 @@ void CSerialPort::setMode(MMDVM_STATE modemState) DEBUG1("Mode set to DMR"); dstarRX.reset(); ysfRX.reset(); + p25RX.reset(); cwIdTX.reset(); break; case STATE_DSTAR: @@ -322,6 +328,7 @@ void CSerialPort::setMode(MMDVM_STATE modemState) dmrDMORX.reset(); dmrRX.reset(); ysfRX.reset(); + p25RX.reset(); cwIdTX.reset(); break; case STATE_YSF: @@ -330,6 +337,7 @@ void CSerialPort::setMode(MMDVM_STATE modemState) dmrDMORX.reset(); dmrRX.reset(); dstarRX.reset(); + p25RX.reset(); cwIdTX.reset(); break; case STATE_P25: @@ -348,6 +356,7 @@ void CSerialPort::setMode(MMDVM_STATE modemState) dmrRX.reset(); dstarRX.reset(); ysfRX.reset(); + p25RX.reset(); cwIdTX.reset(); break; case STATE_DMRCAL: @@ -357,6 +366,7 @@ void CSerialPort::setMode(MMDVM_STATE modemState) dmrRX.reset(); dstarRX.reset(); ysfRX.reset(); + p25RX.reset(); cwIdTX.reset(); break; default: @@ -403,7 +413,7 @@ void CSerialPort::process() if (m_ptr == 2U) m_len = m_buffer[1U]; - if (m_ptr == 3U && m_len > 130U) { + if (m_ptr == 3U && m_len > 230U) { sendNAK(3U); m_ptr = 0U; m_len = 0U; @@ -594,6 +604,34 @@ void CSerialPort::process() } break; + case MMDVM_P25_HDR: + if (m_p25Enable) { + if (m_modemState == STATE_IDLE || m_modemState == STATE_P25) + err = p25TX.writeData(m_buffer + 3U, m_len - 3U); + } + if (err == 0U) { + if (m_modemState == STATE_IDLE) + setMode(STATE_P25); + } else { + DEBUG2("Received invalid P25 header", err); + sendNAK(err); + } + break; + + case MMDVM_P25_LDU: + if (m_p25Enable) { + if (m_modemState == STATE_IDLE || m_modemState == STATE_P25) + err = p25TX.writeData(m_buffer + 3U, m_len - 3U); + } + if (err == 0U) { + if (m_modemState == STATE_IDLE) + setMode(STATE_P25); + } else { + DEBUG2("Received invalid P25 LDU", err); + sendNAK(err); + } + break; + default: // Handle this, send a NAK back sendNAK(1U);