diff --git a/Globals.h b/Globals.h index 161641a..5950200 100644 --- a/Globals.h +++ b/Globals.h @@ -39,6 +39,7 @@ enum MMDVM_STATE { STATE_DSTAR = 1, STATE_DMR = 2, STATE_YSF = 3, + STATE_P25 = 4, STATE_DMRCAL = 98, STATE_DSTARCAL = 99 }; @@ -53,6 +54,7 @@ enum MMDVM_STATE { #include "DMRTX.h" #include "YSFRX.h" #include "YSFTX.h" +#include "P25RX.h" #include "CalDStarRX.h" #include "CalDStarTX.h" #include "CalDMR.h" @@ -74,6 +76,7 @@ extern MMDVM_STATE m_modemState; extern bool m_dstarEnable; extern bool m_dmrEnable; extern bool m_ysfEnable; +extern bool m_p25Enable; extern bool m_duplex; @@ -98,6 +101,8 @@ extern CDMRDMOTX dmrDMOTX; extern CYSFRX ysfRX; extern CYSFTX ysfTX; +extern CP25RX p25RX; + extern CCalDStarRX calDStarRX; extern CCalDStarTX calDStarTX; extern CCalDMR calDMR; diff --git a/IO.cpp b/IO.cpp index 44337fe..7a864c6 100644 --- a/IO.cpp +++ b/IO.cpp @@ -364,6 +364,9 @@ void CIO::process() if (m_ysfEnable) ysfRX.samples(C4FSKVals, blockSize); + + if (m_p25Enable) + p25RX.samples(C4FSKVals, blockSize); } } else if (m_modemState == STATE_DSTAR) { if (m_dstarEnable) { @@ -394,6 +397,13 @@ void CIO::process() ysfRX.samples(C4FSKVals, blockSize); } + } else if (m_modemState == STATE_P25) { + if (m_p25Enable) { + q15_t C4FSKVals[RX_BLOCK_SIZE + 1U]; + ::arm_fir_fast_q15(&m_C4FSKFilter, samples, C4FSKVals, blockSize); + + p25RX.samples(C4FSKVals, blockSize); + } } else if (m_modemState == STATE_DSTARCAL) { q15_t GMSKVals[RX_BLOCK_SIZE + 1U]; ::arm_fir_fast_q15(&m_GMSKFilter, samples, GMSKVals, blockSize); diff --git a/MMDVM.cpp b/MMDVM.cpp index 49ab79a..78a26d2 100644 --- a/MMDVM.cpp +++ b/MMDVM.cpp @@ -29,6 +29,7 @@ MMDVM_STATE m_modemState = STATE_IDLE; bool m_dstarEnable = true; bool m_dmrEnable = true; bool m_ysfEnable = true; +bool m_p25Enable = true; bool m_duplex = true; @@ -50,6 +51,8 @@ CDMRDMOTX dmrDMOTX; CYSFRX ysfRX; CYSFTX ysfTX; +CP25RX p25RX; + CCalDStarRX calDStarRX; CCalDStarTX calDStarTX; CCalDMR calDMR; diff --git a/MMDVM.ino b/MMDVM.ino index 2bfac98..f311a1b 100644 --- a/MMDVM.ino +++ b/MMDVM.ino @@ -26,6 +26,7 @@ MMDVM_STATE m_modemState = STATE_IDLE; bool m_dstarEnable = true; bool m_dmrEnable = true; bool m_ysfEnable = true; +bool m_p25Enable = true; bool m_duplex = true; @@ -47,6 +48,8 @@ CDMRDMOTX dmrDMOTX; CYSFRX ysfRX; CYSFTX ysfTX; +CP25RX p25RX; + CCalDStarRX calDStarRX; CCalDStarTX calDStarTX; CCalDMR calDMR; diff --git a/P25Defines.h b/P25Defines.h new file mode 100644 index 0000000..38083ab --- /dev/null +++ b/P25Defines.h @@ -0,0 +1,53 @@ +/* + * 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(P25DEFINES_H) +#define P25DEFINES_H + +const unsigned int P25_RADIO_SYMBOL_LENGTH = 5U; // At 24 kHz sample rate + +const unsigned int P25_HDR_FRAME_LENGTH_BYTES = 99U; +const unsigned int P25_HDR_FRAME_LENGTH_BITS = P25_HDR_FRAME_LENGTH_BYTES * 8U; +const unsigned int P25_HDR_FRAME_LENGTH_SYMBOLS = P25_HDR_FRAME_LENGTH_BYTES * 4U; + +const unsigned int P25_LDU_FRAME_LENGTH_BYTES = 216U; +const unsigned int P25_LDU_FRAME_LENGTH_BITS = P25_LDU_FRAME_LENGTH_BYTES * 8U; +const unsigned int P25_LDU_FRAME_LENGTH_SYMBOLS = P25_LDU_FRAME_LENGTH_BYTES * 4U; + +const unsigned int P25_SYNC_LENGTH_BYTES = 6U; +const unsigned int P25_SYNC_LENGTH_BITS = P25_SYNC_LENGTH_BYTES * 8U; +const unsigned int P25_SYNC_LENGTH_SYMBOLS = P25_SYNC_LENGTH_BYTES * 4U; + +const unsigned int P25_NID_LENGTH_BITS = 64U; +const unsigned int P25_NID_LENGTH_SYMBOLS = 32U; + +const uint8_t P25_SYNC_BYTES[] = {0x55U, 0x75U, 0xF5U, 0xFFU, 0x77U, 0xFFU}; +const uint8_t P25_SYNC_BYTES_LENGTH = 6U; + +const uint64_t P25_SYNC_BITS = 0x00005575F5FF77FFU; +const uint64_t P25_SYNC_BITS_MASK = 0x0000FFFFFFFFFFFFU; + +// 5 5 7 5 F 5 F F 7 7 F F +// 01 01 01 01 01 11 01 01 11 11 01 01 11 11 11 11 01 11 01 11 11 11 11 11 +// +3 +3 +3 +3 +3 -3 +3 +3 -3 -3 +3 +3 -3 -3 -3 -3 +3 -3 +3 -3 -3 -3 -3 -3 + +const uint32_t P25_SYNC_SYMBOLS = 0x00FB30A0U; +const uint32_t P25_SYNC_SYMBOLS_MASK = 0x00FFFFFFU; + +#endif + diff --git a/P25RX.cpp b/P25RX.cpp new file mode 100644 index 0000000..2ba138a --- /dev/null +++ b/P25RX.cpp @@ -0,0 +1,281 @@ +/* + * 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 "P25RX.h" +#include "Utils.h" + +const unsigned int BUFFER_LENGTH = 200U; + +const q15_t SCALING_FACTOR = 18750; // Q15(0.55) + +const uint32_t PLLMAX = 0x10000U; +const uint32_t PLLINC = PLLMAX / P25_RADIO_SYMBOL_LENGTH; +const uint32_t INC = PLLINC / 32U; + +const uint8_t SYNC_SYMBOL_ERRS = 0U; + +const uint8_t SYNC_BIT_START_ERRS = 2U; +const uint8_t SYNC_BIT_RUN_ERRS = 4U; + +const unsigned int MAX_SYNC_FRAMES = 4U + 1U; + +const uint8_t BIT_MASK_TABLE[] = {0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U}; + +#define WRITE_BIT1(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7]) +#define READ_BIT1(p,i) (p[(i)>>3] & BIT_MASK_TABLE[(i)&7]) + +CP25RX::CP25RX() : +m_pll(0U), +m_prev(false), +m_state(P25RXS_NONE), +m_bitBuffer(0x00U), +m_symbols(), +m_outBuffer(), +m_buffer(NULL), +m_bufferPtr(0U), +m_symbolPtr(0U), +m_lostCount(0U), +m_rssiCount(0U), +m_centre(0), +m_threshold(0) +{ + m_buffer = m_outBuffer + 1U; +} + +void CP25RX::reset() +{ + m_pll = 0U; + m_prev = false; + m_state = P25RXS_NONE; + m_bitBuffer = 0x00U; + m_bufferPtr = 0U; + m_symbolPtr = 0U; + m_lostCount = 0U; + m_rssiCount = 0U; + m_centre = 0; + m_threshold = 0; +} + +void CP25RX::samples(const q15_t* samples, uint8_t length) +{ + for (uint16_t i = 0U; i < length; i++) { + bool bit = samples[i] < 0; + + if (bit != m_prev) { + if (m_pll < (PLLMAX / 2U)) + m_pll += INC; + else + m_pll -= INC; + } + + m_prev = bit; + + m_pll += PLLINC; + + if (m_pll >= PLLMAX) { + m_pll -= PLLMAX; + + if (m_state == P25RXS_NONE) + processNone(samples[i]); + else + processData(samples[i]); + } + } +} + +void CP25RX::processNone(q15_t sample) +{ + m_symbolBuffer <<= 1; + if (sample < 0) + m_symbolBuffer |= 0x01U; + + m_symbols[m_symbolPtr] = sample; + + // Fuzzy matching of the data sync bit sequence + if (countBits32((m_symbolBuffer & P25_SYNC_SYMBOLS_MASK) ^ P25_SYNC_SYMBOLS) <= SYNC_SYMBOL_ERRS) { + q15_t max = -16000; + q15_t min = 16000; + + for (uint8_t i = 0U; i < P25_SYNC_LENGTH_SYMBOLS; i++) { + q15_t val = m_symbols[i]; + if (val > max) + max = val; + if (val < min) + min = val; + } + + q15_t centre = (max + min) >> 1; + + q31_t v1 = (max - centre) * SCALING_FACTOR; + q15_t threshold = q15_t(v1 >> 15); + + uint16_t ptr = m_symbolPtr + 1U; + if (ptr >= P25_SYNC_LENGTH_SYMBOLS) + ptr = 0U; + + for (uint8_t i = 0U; i < P25_SYNC_LENGTH_SYMBOLS; i++) { + q15_t sample = m_symbols[ptr] - centre; + + if (sample < -threshold) { + m_bitBuffer <<= 2; + m_bitBuffer |= 0x01U; + } else if (sample < 0) { + m_bitBuffer <<= 2; + m_bitBuffer |= 0x00U; + } else if (sample < threshold) { + m_bitBuffer <<= 2; + m_bitBuffer |= 0x02U; + } else { + m_bitBuffer <<= 2; + m_bitBuffer |= 0x03U; + } + + ptr++; + if (ptr >= P25_SYNC_LENGTH_SYMBOLS) + ptr = 0U; + } + + // Fuzzy matching of the data sync bit sequence + if (countBits64((m_bitBuffer & P25_SYNC_BITS_MASK) ^ P25_SYNC_BITS) <= SYNC_BIT_START_ERRS) { + DEBUG5("P25RX: sync found in None min/max/centre/threshold", min, max, centre, threshold); + for (uint8_t i = 0U; i < P25_SYNC_LENGTH_BYTES; i++) + m_buffer[i] = P25_SYNC_BYTES[i]; + m_centre = centre; + m_threshold = threshold; + m_lostCount = MAX_SYNC_FRAMES; + m_bufferPtr = P25_SYNC_LENGTH_BITS; + m_state = P25RXS_DATA; + m_rssiCount = 0U; + + io.setDecode(true); + io.setADCDetection(true); + } + } + + m_symbolPtr++; + if (m_symbolPtr >= P25_SYNC_LENGTH_SYMBOLS) + m_symbolPtr = 0U; +} + +void CP25RX::processData(q15_t sample) +{ + sample -= m_centre; + + if (sample < -m_threshold) { + m_bitBuffer <<= 2; + m_bitBuffer |= 0x01U; + WRITE_BIT1(m_buffer, m_bufferPtr, false); + m_bufferPtr++; + WRITE_BIT1(m_buffer, m_bufferPtr, true); + m_bufferPtr++; + } else if (sample < 0) { + m_bitBuffer <<= 2; + m_bitBuffer |= 0x00U; + WRITE_BIT1(m_buffer, m_bufferPtr, false); + m_bufferPtr++; + WRITE_BIT1(m_buffer, m_bufferPtr, false); + m_bufferPtr++; + } else if (sample < m_threshold) { + m_bitBuffer <<= 2; + m_bitBuffer |= 0x02U; + WRITE_BIT1(m_buffer, m_bufferPtr, true); + m_bufferPtr++; + WRITE_BIT1(m_buffer, m_bufferPtr, false); + m_bufferPtr++; + } else { + m_bitBuffer <<= 2; + m_bitBuffer |= 0x03U; + WRITE_BIT1(m_buffer, m_bufferPtr, true); + m_bufferPtr++; + WRITE_BIT1(m_buffer, m_bufferPtr, true); + m_bufferPtr++; + } + + // Search for an early sync to indicate an LDU following a header + if (m_bufferPtr >= (P25_HDR_FRAME_LENGTH_BITS + P25_SYNC_LENGTH_BITS - 2U) && m_bufferPtr <= (P25_HDR_FRAME_LENGTH_BITS + P25_SYNC_LENGTH_BITS + 2U)) { + // Fuzzy matching of the data sync bit sequence + if (countBits64((m_bitBuffer & P25_SYNC_BITS_MASK) ^ P25_SYNC_BITS) <= SYNC_BIT_RUN_ERRS) { + DEBUG2("P25RX: found LDU sync in Data, pos", m_bufferPtr - P25_SYNC_LENGTH_BITS); + + m_outBuffer[0U] = 0x00U; + serial.writeP25Hdr(m_outBuffer, P25_HDR_FRAME_LENGTH_BYTES + 1U); + + // Restore the sync that's now in the wrong place + for (uint8_t i = 0U; i < P25_SYNC_LENGTH_BYTES; i++) + m_buffer[i] = P25_SYNC_BYTES[i]; + + m_lostCount = MAX_SYNC_FRAMES; + m_bufferPtr = P25_SYNC_LENGTH_BITS; + } + } + + // Only search for a sync in the right place +-2 symbols + if (m_bufferPtr >= (P25_SYNC_LENGTH_BITS - 2U) && m_bufferPtr <= (P25_SYNC_LENGTH_BITS + 2U)) { + // Fuzzy matching of the data sync bit sequence + if (countBits64((m_bitBuffer & P25_SYNC_BITS_MASK) ^ P25_SYNC_BITS) <= SYNC_BIT_RUN_ERRS) { + DEBUG2("P25RX: found sync in Data, pos", m_bufferPtr - P25_SYNC_LENGTH_BITS); + m_lostCount = MAX_SYNC_FRAMES; + m_bufferPtr = P25_SYNC_LENGTH_BITS; + } + } + + // Send a data frame to the host if the required number of bits have been received + if (m_bufferPtr == P25_LDU_FRAME_LENGTH_BITS) { + // We've not seen a data sync for too long, signal RXLOST and change to RX_NONE + m_lostCount--; + if (m_lostCount == 0U) { + DEBUG1("P25RX: sync timed out, lost lock"); + + io.setDecode(false); + io.setADCDetection(false); + + serial.writeP25Lost(); + + m_state = P25RXS_NONE; + } else { + m_outBuffer[0U] = m_lostCount == (MAX_SYNC_FRAMES - 1U) ? 0x01U : 0x00U; + +#if defined(SEND_RSSI_DATA) + // Send RSSI data every second + if (m_rssiCount == 0U) { + uint16_t rssi = io.getRSSIValue(); + m_outBuffer[121U] = (rssi >> 8) & 0xFFU; + m_outBuffer[122U] = (rssi >> 0) & 0xFFU; + serial.writeP25Ldu(m_outBuffer, P25_LDU_FRAME_LENGTH_BYTES + 3U); + } else { + serial.writeP25Ldu(m_outBuffer, P25_LDU_FRAME_LENGTH_BYTES + 1U); + } + + m_rssiCount++; + if (m_rssiCount >= 10U) + m_rssiCount = 0U; +#else + serial.writeP25Ldu(m_outBuffer, P25_LDU_FRAME_LENGTH_BYTES + 1U); +#endif + + // Start the next frame + ::memset(m_outBuffer, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 3U); + m_bufferPtr = 0U; + } + } +} + diff --git a/P25RX.h b/P25RX.h new file mode 100644 index 0000000..79f0ed2 --- /dev/null +++ b/P25RX.h @@ -0,0 +1,59 @@ +/* + * 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 + * 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(P25RX_H) +#define P25RX_H + +#include "Config.h" +#include "P25Defines.h" + +enum P25RX_STATE { + P25RXS_NONE, + P25RXS_DATA +}; + +class CP25RX { +public: + CP25RX(); + + void samples(const q15_t* samples, uint8_t length); + + void reset(); + +private: + uint32_t m_pll; + bool m_prev; + P25RX_STATE m_state; + uint32_t m_symbolBuffer; + uint64_t m_bitBuffer; + q15_t m_symbols[P25_SYNC_LENGTH_SYMBOLS]; + uint8_t m_outBuffer[P25_LDU_FRAME_LENGTH_BYTES + 3U]; + uint8_t* m_buffer; + uint16_t m_bufferPtr; + uint16_t m_symbolPtr; + uint16_t m_lostCount; + uint16_t m_rssiCount; + q15_t m_centre; + q15_t m_threshold; + + void processNone(q15_t sample); + void processData(q15_t sample); +}; + +#endif + diff --git a/SerialPort.cpp b/SerialPort.cpp index 8e29950..c9d1d89 100644 --- a/SerialPort.cpp +++ b/SerialPort.cpp @@ -52,6 +52,10 @@ const uint8_t MMDVM_DMR_ABORT = 0x1EU; const uint8_t MMDVM_YSF_DATA = 0x20U; const uint8_t MMDVM_YSF_LOST = 0x21U; +const uint8_t MMDVM_P25_HDR = 0x30U; +const uint8_t MMDVM_P25_LDU = 0x31U; +const uint8_t MMDVM_P25_LOST = 0x32U; + const uint8_t MMDVM_ACK = 0x70U; const uint8_t MMDVM_NAK = 0x7FU; @@ -62,9 +66,9 @@ const uint8_t MMDVM_DEBUG4 = 0xF4U; const uint8_t MMDVM_DEBUG5 = 0xF5U; #if defined(EXTERNAL_OSC) -const uint8_t HARDWARE[] = "MMDVM 20160906 TCXO (D-Star/DMR/System Fusion/CW Id)"; +const uint8_t HARDWARE[] = "MMDVM 20160906 TCXO (D-Star/DMR/System Fusion/P25/CW Id)"; #else -const uint8_t HARDWARE[] = "MMDVM 20160906 (D-Star/DMR/System Fusion/CW Id)"; +const uint8_t HARDWARE[] = "MMDVM 20160906 (D-Star/DMR/System Fusion/P25/CW Id)"; #endif const uint8_t PROTOCOL_VERSION = 1U; @@ -109,7 +113,7 @@ void CSerialPort::getStatus() { io.resetWatchdog(); - uint8_t reply[11U]; + uint8_t reply[15U]; // Send all sorts of interesting internal values reply[0U] = MMDVM_FRAME_START; @@ -123,6 +127,8 @@ void CSerialPort::getStatus() reply[3U] |= 0x02U; if (m_ysfEnable) reply[3U] |= 0x04U; + if (m_p25Enable) + reply[3U] |= 0x08U; reply[4U] = uint8_t(m_modemState); @@ -170,7 +176,9 @@ void CSerialPort::getStatus() else reply[9U] = 0U; - write(reply, 10); + reply[10U] = 0U; // P25 + + write(reply, 11); } void CSerialPort::getVersion() @@ -205,6 +213,7 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length) bool dstarEnable = (data[1U] & 0x01U) == 0x01U; bool dmrEnable = (data[1U] & 0x02U) == 0x02U; bool ysfEnable = (data[1U] & 0x04U) == 0x04U; + bool p25Enable = (data[1U] & 0x08U) == 0x08U; uint8_t txDelay = data[2U]; if (txDelay > 50U) @@ -212,7 +221,7 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length) MMDVM_STATE modemState = MMDVM_STATE(data[3U]); - if (modemState != STATE_IDLE && modemState != STATE_DSTAR && modemState != STATE_DMR && modemState != STATE_YSF && modemState != STATE_DSTARCAL && modemState != STATE_DMRCAL) + if (modemState != STATE_IDLE && modemState != STATE_DSTAR && modemState != STATE_DMR && modemState != STATE_YSF && modemState != STATE_P25 && modemState != STATE_DSTARCAL && modemState != STATE_DMRCAL) return 4U; if (modemState == STATE_DSTAR && !dstarEnable) return 4U; @@ -220,6 +229,8 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length) return 4U; if (modemState == STATE_YSF && !ysfEnable) return 4U; + if (modemState == STATE_P25 && !p25Enable) + return 4U; uint8_t rxLevel = data[4U]; @@ -250,6 +261,7 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length) m_dstarEnable = dstarEnable; m_dmrEnable = dmrEnable; m_ysfEnable = ysfEnable; + m_p25Enable = p25Enable; m_duplex = !simplex; dstarTX.setTXDelay(txDelay); @@ -279,7 +291,7 @@ uint8_t CSerialPort::setMode(const uint8_t* data, uint8_t length) if (modemState == m_modemState) return 0U; - if (modemState != STATE_IDLE && modemState != STATE_DSTAR && modemState != STATE_DMR && modemState != STATE_YSF && modemState != STATE_DSTARCAL && modemState != STATE_DMRCAL) + if (modemState != STATE_IDLE && modemState != STATE_DSTAR && modemState != STATE_DMR && modemState != STATE_YSF && modemState != STATE_P25 && modemState != STATE_DSTARCAL && modemState != STATE_DMRCAL) return 4U; if (modemState == STATE_DSTAR && !m_dstarEnable) return 4U; @@ -287,6 +299,8 @@ uint8_t CSerialPort::setMode(const uint8_t* data, uint8_t length) return 4U; if (modemState == STATE_YSF && !m_ysfEnable) return 4U; + if (modemState == STATE_P25 && !m_p25Enable) + return 4U; setMode(modemState); @@ -318,6 +332,15 @@ void CSerialPort::setMode(MMDVM_STATE modemState) dstarRX.reset(); cwIdTX.reset(); break; + case STATE_P25: + DEBUG1("Mode set to P25"); + dmrIdleRX.reset(); + dmrDMORX.reset(); + dmrRX.reset(); + dstarRX.reset(); + ysfRX.reset(); + cwIdTX.reset(); + break; case STATE_DSTARCAL: DEBUG1("Mode set to D-Star Calibrate"); dmrIdleRX.reset(); @@ -743,6 +766,69 @@ void CSerialPort::writeYSFLost() write(reply, 3); } +void CSerialPort::writeP25Hdr(const uint8_t* data, uint8_t length) +{ + if (m_modemState != STATE_P25 && m_modemState != STATE_IDLE) + return; + + if (!m_p25Enable) + return; + + uint8_t reply[120U]; + + reply[0U] = MMDVM_FRAME_START; + reply[1U] = 0U; + reply[2U] = MMDVM_P25_HDR; + + uint8_t count = 3U; + for (uint8_t i = 0U; i < length; i++, count++) + reply[count] = data[i]; + + reply[1U] = count; + + write(reply, count); +} + +void CSerialPort::writeP25Ldu(const uint8_t* data, uint8_t length) +{ + if (m_modemState != STATE_P25 && m_modemState != STATE_IDLE) + return; + + if (!m_p25Enable) + return; + + uint8_t reply[250U]; + + reply[0U] = MMDVM_FRAME_START; + reply[1U] = 0U; + reply[2U] = MMDVM_P25_LDU; + + uint8_t count = 3U; + for (uint8_t i = 0U; i < length; i++, count++) + reply[count] = data[i]; + + reply[1U] = count; + + write(reply, count); +} + +void CSerialPort::writeP25Lost() +{ + if (m_modemState != STATE_P25 && m_modemState != STATE_IDLE) + return; + + if (!m_p25Enable) + return; + + uint8_t reply[3U]; + + reply[0U] = MMDVM_FRAME_START; + reply[1U] = 3U; + reply[2U] = MMDVM_P25_LOST; + + write(reply, 3); +} + void CSerialPort::writeCalData(const uint8_t* data, uint8_t length) { if (m_modemState != STATE_DSTARCAL) diff --git a/SerialPort.h b/SerialPort.h index bcf4d05..5ad3881 100644 --- a/SerialPort.h +++ b/SerialPort.h @@ -42,6 +42,10 @@ public: void writeYSFData(const uint8_t* data, uint8_t length); void writeYSFLost(); + void writeP25Hdr(const uint8_t* data, uint8_t length); + void writeP25Ldu(const uint8_t* data, uint8_t length); + void writeP25Lost(); + void writeCalData(const uint8_t* data, uint8_t length); void writeDebug(const char* text);