diff --git a/DMRIdleRX.cpp b/DMRIdleRX.cpp new file mode 100644 index 0000000..5141aa3 --- /dev/null +++ b/DMRIdleRX.cpp @@ -0,0 +1,217 @@ +/* + * Copyright (C) 2009-2017 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(DUPLEX) + +#define WANT_DEBUG + +#include "Config.h" +#include "Globals.h" +#include "DMRIdleRX.h" +#include "DMRSlotType.h" +#include "Utils.h" + +const q15_t SCALING_FACTOR = 19505; // Q15(0.60) + +const uint8_t MAX_SYNC_SYMBOLS_ERRS = 2U; +const uint8_t MAX_SYNC_BYTES_ERRS = 3U; + +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]) + +const uint16_t NOENDPTR = 9999U; + +const uint8_t CONTROL_IDLE = 0x80U; +const uint8_t CONTROL_DATA = 0x40U; + +CDMRIdleRX::CDMRIdleRX() : +m_bitBuffer(), +m_buffer(), +m_bitPtr(0U), +m_dataPtr(0U), +m_endPtr(NOENDPTR), +m_maxCorr(0), +m_centre(0), +m_threshold(0), +m_colorCode(0U) +{ +} + +void CDMRIdleRX::reset() +{ + m_dataPtr = 0U; + m_bitPtr = 0U; + m_maxCorr = 0; + m_threshold = 0; + m_centre = 0; + m_endPtr = NOENDPTR; +} + +void CDMRIdleRX::samples(const q15_t* samples, uint8_t length) +{ + for (uint8_t i = 0U; i < length; i++) + processSample(samples[i]); +} + +void CDMRIdleRX::processSample(q15_t sample) +{ + m_bitBuffer[m_bitPtr] <<= 1; + if (sample < 0) + m_bitBuffer[m_bitPtr] |= 0x01U; + + m_buffer[m_dataPtr] = sample; + + if (countBits32((m_bitBuffer[m_bitPtr] & DMR_SYNC_SYMBOLS_MASK) ^ DMR_MS_DATA_SYNC_SYMBOLS) <= MAX_SYNC_SYMBOLS_ERRS) { + uint16_t ptr = m_dataPtr + DMR_FRAME_LENGTH_SAMPLES - DMR_SYNC_LENGTH_SAMPLES + DMR_RADIO_SYMBOL_LENGTH; + if (ptr >= DMR_FRAME_LENGTH_SAMPLES) + ptr -= DMR_FRAME_LENGTH_SAMPLES; + + q31_t corr = 0; + q15_t max = -16000; + q15_t min = 16000; + + for (uint8_t i = 0U; i < DMR_SYNC_LENGTH_SYMBOLS; i++) { + q15_t val = m_buffer[ptr]; + + if (val > max) + max = val; + if (val < min) + min = val; + + switch (DMR_MS_DATA_SYNC_SYMBOLS_VALUES[i]) { + case +3: + corr -= (val + val + val); + break; + case +1: + corr -= val; + break; + case -1: + corr += val; + break; + default: // -3 + corr += (val + val + val); + break; + } + + ptr += DMR_RADIO_SYMBOL_LENGTH; + if (ptr >= DMR_FRAME_LENGTH_SAMPLES) + ptr -= DMR_FRAME_LENGTH_SAMPLES; + } + + if (corr > m_maxCorr) { + q15_t centre = (max + min) >> 1; + + q31_t v1 = (max - centre) * SCALING_FACTOR; + q15_t threshold = q15_t(v1 >> 15); + + uint8_t sync[DMR_SYNC_BYTES_LENGTH]; + + uint16_t ptr = m_dataPtr + DMR_FRAME_LENGTH_SAMPLES - DMR_SYNC_LENGTH_SAMPLES + DMR_RADIO_SYMBOL_LENGTH; + if (ptr >= DMR_FRAME_LENGTH_SAMPLES) + ptr -= DMR_FRAME_LENGTH_SAMPLES; + + samplesToBits(ptr, DMR_SYNC_LENGTH_SYMBOLS, sync, 4U, centre, threshold); + + uint8_t errs = 0U; + for (uint8_t i = 0U; i < DMR_SYNC_BYTES_LENGTH; i++) + errs += countBits8((sync[i] & DMR_SYNC_BYTES_MASK[i]) ^ DMR_MS_DATA_SYNC_BYTES[i]); + + if (errs <= MAX_SYNC_BYTES_ERRS) { + DEBUG3("DMRIdleRX: data sync found centre/threshold", centre, threshold); + m_maxCorr = corr; + m_centre = centre; + m_threshold = threshold; + + m_endPtr = m_dataPtr + DMR_SLOT_TYPE_LENGTH_SAMPLES / 2U + DMR_INFO_LENGTH_SAMPLES / 2U - 1U; + if (m_endPtr >= DMR_FRAME_LENGTH_SAMPLES) + m_endPtr -= DMR_FRAME_LENGTH_SAMPLES; + } + } + } + + if (m_dataPtr == m_endPtr) { + uint16_t ptr = m_endPtr + DMR_RADIO_SYMBOL_LENGTH + 1U; + if (ptr >= DMR_FRAME_LENGTH_SAMPLES) + ptr -= DMR_FRAME_LENGTH_SAMPLES; + + uint8_t frame[DMR_FRAME_LENGTH_BYTES + 1U]; + samplesToBits(ptr, DMR_FRAME_LENGTH_SYMBOLS, frame, 8U, m_centre, m_threshold); + + uint8_t colorCode; + uint8_t dataType; + CDMRSlotType slotType; + slotType.decode(frame + 1U, colorCode, dataType); + + if (colorCode == m_colorCode && dataType == DT_CSBK) { + frame[0U] = CONTROL_IDLE | CONTROL_DATA | DT_CSBK; + serial.writeDMRData(false, frame, DMR_FRAME_LENGTH_BYTES + 1U); + } + + m_endPtr = NOENDPTR; + m_maxCorr = 0; + } + + m_dataPtr++; + if (m_dataPtr >= DMR_FRAME_LENGTH_SAMPLES) + m_dataPtr = 0U; + + m_bitPtr++; + if (m_bitPtr >= DMR_RADIO_SYMBOL_LENGTH) + m_bitPtr = 0U; +} + +void CDMRIdleRX::samplesToBits(uint16_t start, uint8_t count, uint8_t* buffer, uint16_t offset, q15_t centre, q15_t threshold) +{ + for (uint8_t i = 0U; i < count; i++) { + q15_t sample = m_buffer[start] - centre; + + if (sample < -threshold) { + WRITE_BIT1(buffer, offset, false); + offset++; + WRITE_BIT1(buffer, offset, true); + offset++; + } else if (sample < 0) { + WRITE_BIT1(buffer, offset, false); + offset++; + WRITE_BIT1(buffer, offset, false); + offset++; + } else if (sample < threshold) { + WRITE_BIT1(buffer, offset, true); + offset++; + WRITE_BIT1(buffer, offset, false); + offset++; + } else { + WRITE_BIT1(buffer, offset, true); + offset++; + WRITE_BIT1(buffer, offset, true); + offset++; + } + + start += DMR_RADIO_SYMBOL_LENGTH; + if (start >= DMR_FRAME_LENGTH_SAMPLES) + start -= DMR_FRAME_LENGTH_SAMPLES; + } +} + +void CDMRIdleRX::setColorCode(uint8_t colorCode) +{ + m_colorCode = colorCode; +} + +#endif diff --git a/DMRIdleRX.h b/DMRIdleRX.h new file mode 100644 index 0000000..3bb6d15 --- /dev/null +++ b/DMRIdleRX.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2015 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(DMRIDLERX_H) +#define DMRIDLERX_H + +#if defined(DUPLEX) + +#include "Config.h" +#include "DMRDefines.h" + +class CDMRIdleRX { +public: + CDMRIdleRX(); + + void samples(const q15_t* samples, uint8_t length); + + void setColorCode(uint8_t colorCode); + + void reset(); + +private: + uint32_t m_bitBuffer[DMR_RADIO_SYMBOL_LENGTH]; + q15_t m_buffer[DMR_FRAME_LENGTH_SAMPLES]; + uint16_t m_bitPtr; + uint16_t m_dataPtr; + uint16_t m_endPtr; + q31_t m_maxCorr; + q15_t m_centre; + q15_t m_threshold; + uint8_t m_colorCode; + + void processSample(q15_t sample); + void samplesToBits(uint16_t start, uint8_t count, uint8_t* buffer, uint16_t offset, q15_t centre, q15_t threshold); +}; + +#endif + +#endif + diff --git a/DMRRX.cpp b/DMRRX.cpp new file mode 100644 index 0000000..d6ee22c --- /dev/null +++ b/DMRRX.cpp @@ -0,0 +1,74 @@ +/* + * 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(DUPLEX) + +#include "Config.h" +#include "Globals.h" +#include "DMRRX.h" + +CDMRRX::CDMRRX() : +m_slot1RX(false), +m_slot2RX(true) +{ +} + +void CDMRRX::samples(const q15_t* samples, const uint16_t* rssi, const uint8_t* control, uint8_t length) +{ + bool dcd1 = false; + bool dcd2 = false; + + for (uint16_t i = 0U; i < length; i++) { + switch (control[i]) { + case MARK_SLOT1: + m_slot1RX.start(); + break; + case MARK_SLOT2: + m_slot2RX.start(); + break; + default: + break; + } + + dcd1 = m_slot1RX.processSample(samples[i], rssi[i]); + dcd2 = m_slot2RX.processSample(samples[i], rssi[i]); + } + + io.setDecode(dcd1 || dcd2); +} + +void CDMRRX::setColorCode(uint8_t colorCode) +{ + m_slot1RX.setColorCode(colorCode); + m_slot2RX.setColorCode(colorCode); +} + +void CDMRRX::setDelay(uint8_t delay) +{ + m_slot1RX.setDelay(delay); + m_slot2RX.setDelay(delay); +} + +void CDMRRX::reset() +{ + m_slot1RX.reset(); + m_slot2RX.reset(); +} + +#endif + diff --git a/DMRRX.h b/DMRRX.h new file mode 100644 index 0000000..ccd1cbd --- /dev/null +++ b/DMRRX.h @@ -0,0 +1,45 @@ +/* + * 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(DMRRX_H) +#define DMRRX_H + +#if defined(DUPLEX) + +#include "Config.h" +#include "DMRSlotRX.h" + +class CDMRRX { +public: + CDMRRX(); + + void samples(const q15_t* samples, const uint16_t* rssi, const uint8_t* control, uint8_t length); + + void setColorCode(uint8_t colorCode); + void setDelay(uint8_t delay); + + void reset(); + +private: + CDMRSlotRX m_slot1RX; + CDMRSlotRX m_slot2RX; +}; + +#endif + +#endif diff --git a/DMRSlotRX.cpp b/DMRSlotRX.cpp new file mode 100644 index 0000000..84f057c --- /dev/null +++ b/DMRSlotRX.cpp @@ -0,0 +1,408 @@ +/* + * Copyright (C) 2009-2017 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(DUPLEX) + +#define WANT_DEBUG + +#include "Config.h" +#include "Globals.h" +#include "DMRSlotRX.h" +#include "DMRSlotType.h" +#include "Utils.h" + +const uint16_t SCAN_START = 400U; +const uint16_t SCAN_END = 490U; + +const q15_t SCALING_FACTOR = 19505; // Q15(0.60) + +const uint8_t MAX_SYNC_SYMBOLS_ERRS = 2U; +const uint8_t MAX_SYNC_BYTES_ERRS = 3U; + +const uint8_t MAX_SYNC_LOST_FRAMES = 13U; + +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]) + +const uint16_t NOENDPTR = 9999U; + +const uint8_t CONTROL_NONE = 0x00U; +const uint8_t CONTROL_VOICE = 0x20U; +const uint8_t CONTROL_DATA = 0x40U; + +CDMRSlotRX::CDMRSlotRX(bool slot) : +m_slot(slot), +m_bitBuffer(), +m_buffer(), +m_bitPtr(0U), +m_dataPtr(0U), +m_syncPtr(0U), +m_startPtr(0U), +m_endPtr(NOENDPTR), +m_delayPtr(0U), +m_maxCorr(0), +m_centre(), +m_threshold(), +m_averagePtr(0U), +m_control(CONTROL_NONE), +m_syncCount(0U), +m_colorCode(0U), +m_delay(0U), +m_state(DMRRXS_NONE), +m_n(0U), +m_type(0U), +m_rssi() +{ +} + +void CDMRSlotRX::start() +{ + m_dataPtr = 0U; + m_delayPtr = 0U; + m_bitPtr = 0U; + m_maxCorr = 0; + m_control = CONTROL_NONE; +} + +void CDMRSlotRX::reset() +{ + m_syncPtr = 0U; + m_dataPtr = 0U; + m_delayPtr = 0U; + m_bitPtr = 0U; + m_maxCorr = 0; + m_control = CONTROL_NONE; + m_syncCount = 0U; + m_state = DMRRXS_NONE; + m_startPtr = 0U; + m_endPtr = NOENDPTR; +} + +bool CDMRSlotRX::processSample(q15_t sample, uint16_t rssi) +{ + m_delayPtr++; + if (m_delayPtr < m_delay) + return m_state != DMRRXS_NONE; + + // Ensure that the buffer doesn't overflow + if (m_dataPtr > m_endPtr || m_dataPtr >= 900U) + return m_state != DMRRXS_NONE; + + m_buffer[m_dataPtr] = sample; + m_rssi[m_dataPtr] = rssi; + + m_bitBuffer[m_bitPtr] <<= 1; + if (sample < 0) + m_bitBuffer[m_bitPtr] |= 0x01U; + + if (m_state == DMRRXS_NONE) { + if (m_dataPtr >= SCAN_START && m_dataPtr <= SCAN_END) + correlateSync(true); + } else { + + uint16_t min = m_syncPtr - 1U; + uint16_t max = m_syncPtr + 1U; + if (m_dataPtr >= min && m_dataPtr <= max) + correlateSync(false); + } + + if (m_dataPtr == m_endPtr) { + // Find the average centre and threshold values + q15_t centre = (m_centre[0U] + m_centre[1U] + m_centre[2U] + m_centre[3U]) >> 2; + q15_t threshold = (m_threshold[0U] + m_threshold[1U] + m_threshold[2U] + m_threshold[3U]) >> 2; + + uint8_t frame[DMR_FRAME_LENGTH_BYTES + 3U]; + frame[0U] = m_control; + + uint16_t ptr = m_endPtr - DMR_FRAME_LENGTH_SAMPLES + DMR_RADIO_SYMBOL_LENGTH + 1U; + samplesToBits(ptr, DMR_FRAME_LENGTH_SYMBOLS, frame, 8U, centre, threshold); + + if (m_control == CONTROL_DATA) { + // Data sync + uint8_t colorCode; + uint8_t dataType; + CDMRSlotType slotType; + slotType.decode(frame + 1U, colorCode, dataType); + + if (colorCode == m_colorCode) { + m_syncCount = 0U; + m_n = 0U; + + frame[0U] |= dataType; + + switch (dataType) { + case DT_DATA_HEADER: + DEBUG5("DMRSlotRX: data header found slot/pos/centre/threshold", m_slot ? 2U : 1U, m_syncPtr, centre, threshold); + writeRSSIData(frame); + m_state = DMRRXS_DATA; + m_type = 0x00U; + break; + case DT_RATE_12_DATA: + case DT_RATE_34_DATA: + case DT_RATE_1_DATA: + if (m_state == DMRRXS_DATA) { + DEBUG5("DMRSlotRX: data payload found slot/pos/centre/threshold", m_slot ? 2U : 1U, m_syncPtr, centre, threshold); + writeRSSIData(frame); + m_type = dataType; + } + break; + case DT_VOICE_LC_HEADER: + DEBUG5("DMRSlotRX: voice header found slot/pos/centre/threshold", m_slot ? 2U : 1U, m_syncPtr, centre, threshold); + writeRSSIData(frame); + m_state = DMRRXS_VOICE; + break; + case DT_VOICE_PI_HEADER: + if (m_state == DMRRXS_VOICE) { + DEBUG5("DMRSlotRX: voice pi header found slot/pos/centre/threshold", m_slot ? 2U : 1U, m_syncPtr, centre, threshold); + writeRSSIData(frame); + } + m_state = DMRRXS_VOICE; + break; + case DT_TERMINATOR_WITH_LC: + if (m_state == DMRRXS_VOICE) { + DEBUG5("DMRSlotRX: voice terminator found slot/pos/centre/threshold", m_slot ? 2U : 1U, m_syncPtr, centre, threshold); + writeRSSIData(frame); + m_state = DMRRXS_NONE; + m_endPtr = NOENDPTR; + } + break; + default: // DT_CSBK + DEBUG5("DMRSlotRX: csbk found slot/pos/centre/threshold", m_slot ? 2U : 1U, m_syncPtr, centre, threshold); + writeRSSIData(frame); + m_state = DMRRXS_NONE; + m_endPtr = NOENDPTR; + break; + } + } + } else if (m_control == CONTROL_VOICE) { + // Voice sync + DEBUG5("DMRSlotRX: voice sync found slot/pos/centre/threshold", m_slot ? 2U : 1U, m_syncPtr, centre, threshold); + writeRSSIData(frame); + m_state = DMRRXS_VOICE; + m_syncCount = 0U; + m_n = 0U; + } else { + if (m_state != DMRRXS_NONE) { + m_syncCount++; + if (m_syncCount >= MAX_SYNC_LOST_FRAMES) { + serial.writeDMRLost(m_slot); + m_state = DMRRXS_NONE; + m_endPtr = NOENDPTR; + } + } + + if (m_state == DMRRXS_VOICE) { + if (m_n >= 5U) { + frame[0U] = CONTROL_VOICE; + m_n = 0U; + } else { + frame[0U] = ++m_n; + } + + serial.writeDMRData(m_slot, frame, DMR_FRAME_LENGTH_BYTES + 1U); + } else if (m_state == DMRRXS_DATA) { + if (m_type != 0x00U) { + frame[0U] = CONTROL_DATA | m_type; + writeRSSIData(frame); + } + } + } + } + + m_dataPtr++; + + m_bitPtr++; + if (m_bitPtr >= DMR_RADIO_SYMBOL_LENGTH) + m_bitPtr = 0U; + + return m_state != DMRRXS_NONE; +} + +void CDMRSlotRX::correlateSync(bool first) +{ + uint8_t errs = countBits32((m_bitBuffer[m_bitPtr] & DMR_SYNC_SYMBOLS_MASK) ^ DMR_MS_DATA_SYNC_SYMBOLS); + + // The voice sync is the complement of the data sync + bool data = (errs <= MAX_SYNC_SYMBOLS_ERRS); + bool voice = (errs >= (DMR_SYNC_LENGTH_SYMBOLS - MAX_SYNC_SYMBOLS_ERRS)); + + if (data || voice) { + uint16_t ptr = m_dataPtr - DMR_SYNC_LENGTH_SAMPLES + DMR_RADIO_SYMBOL_LENGTH; + + q31_t corr = 0; + q15_t min = 16000; + q15_t max = -16000; + + for (uint8_t i = 0U; i < DMR_SYNC_LENGTH_SYMBOLS; i++) { + q15_t val = m_buffer[ptr]; + + if (val > max) + max = val; + if (val < min) + min = val; + + int8_t corrVal; + if (data) + corrVal = DMR_MS_DATA_SYNC_SYMBOLS_VALUES[i]; + else + corrVal = DMR_MS_VOICE_SYNC_SYMBOLS_VALUES[i]; + + switch (corrVal) { + case +3: + corr -= (val + val + val); + break; + case +1: + corr -= val; + break; + case -1: + corr += val; + break; + default: // -3 + corr += (val + val + val); + break; + } + + ptr += DMR_RADIO_SYMBOL_LENGTH; + } + + if (corr > m_maxCorr) { + q15_t centre = (max + min) >> 1; + + q31_t v1 = (max - centre) * SCALING_FACTOR; + q15_t threshold = q15_t(v1 >> 15); + + uint8_t sync[DMR_SYNC_BYTES_LENGTH]; + uint16_t ptr = m_dataPtr - DMR_SYNC_LENGTH_SAMPLES + DMR_RADIO_SYMBOL_LENGTH; + samplesToBits(ptr, DMR_SYNC_LENGTH_SYMBOLS, sync, 4U, centre, threshold); + + if (data) { + uint8_t errs = 0U; + for (uint8_t i = 0U; i < DMR_SYNC_BYTES_LENGTH; i++) + errs += countBits8((sync[i] & DMR_SYNC_BYTES_MASK[i]) ^ DMR_MS_DATA_SYNC_BYTES[i]); + + if (errs <= MAX_SYNC_BYTES_ERRS) { + if (first) { + m_threshold[0U] = m_threshold[1U] = m_threshold[2U] = m_threshold[3U] = threshold; + m_centre[0U] = m_centre[1U] = m_centre[2U] = m_centre[3U] = centre; + m_averagePtr = 0U; + } else { + m_threshold[m_averagePtr] = threshold; + m_centre[m_averagePtr] = centre; + + m_averagePtr++; + if (m_averagePtr >= 4U) + m_averagePtr = 0U; + } + + m_maxCorr = corr; + m_control = CONTROL_DATA; + m_syncPtr = m_dataPtr; + m_startPtr = m_dataPtr - DMR_SLOT_TYPE_LENGTH_SAMPLES / 2U - DMR_INFO_LENGTH_SAMPLES / 2U - DMR_SYNC_LENGTH_SAMPLES; + m_endPtr = m_dataPtr + DMR_SLOT_TYPE_LENGTH_SAMPLES / 2U + DMR_INFO_LENGTH_SAMPLES / 2U - 1U; + } + } else { // if (voice) + uint8_t errs = 0U; + for (uint8_t i = 0U; i < DMR_SYNC_BYTES_LENGTH; i++) + errs += countBits8((sync[i] & DMR_SYNC_BYTES_MASK[i]) ^ DMR_MS_VOICE_SYNC_BYTES[i]); + + if (errs <= MAX_SYNC_BYTES_ERRS) { + if (first) { + m_threshold[0U] = m_threshold[1U] = m_threshold[2U] = m_threshold[3U] = threshold; + m_centre[0U] = m_centre[1U] = m_centre[2U] = m_centre[3U] = centre; + m_averagePtr = 0U; + } else { + m_threshold[m_averagePtr] = threshold; + m_centre[m_averagePtr] = centre; + + m_averagePtr++; + if (m_averagePtr >= 4U) + m_averagePtr = 0U; + } + + m_maxCorr = corr; + m_control = CONTROL_VOICE; + m_syncPtr = m_dataPtr; + m_startPtr = m_dataPtr - DMR_SLOT_TYPE_LENGTH_SAMPLES / 2U - DMR_INFO_LENGTH_SAMPLES / 2U - DMR_SYNC_LENGTH_SAMPLES; + m_endPtr = m_dataPtr + DMR_SLOT_TYPE_LENGTH_SAMPLES / 2U + DMR_INFO_LENGTH_SAMPLES / 2U - 1U; + } + } + } + } +} + +void CDMRSlotRX::samplesToBits(uint16_t start, uint8_t count, uint8_t* buffer, uint16_t offset, q15_t centre, q15_t threshold) +{ + for (uint8_t i = 0U; i < count; i++, start += DMR_RADIO_SYMBOL_LENGTH) { + q15_t sample = m_buffer[start] - centre; + + if (sample < -threshold) { + WRITE_BIT1(buffer, offset, false); + offset++; + WRITE_BIT1(buffer, offset, true); + offset++; + } else if (sample < 0) { + WRITE_BIT1(buffer, offset, false); + offset++; + WRITE_BIT1(buffer, offset, false); + offset++; + } else if (sample < threshold) { + WRITE_BIT1(buffer, offset, true); + offset++; + WRITE_BIT1(buffer, offset, false); + offset++; + } else { + WRITE_BIT1(buffer, offset, true); + offset++; + WRITE_BIT1(buffer, offset, true); + offset++; + } + } +} + +void CDMRSlotRX::setColorCode(uint8_t colorCode) +{ + m_colorCode = colorCode; +} + +void CDMRSlotRX::setDelay(uint8_t delay) +{ + m_delay = delay; +} + +void CDMRSlotRX::writeRSSIData(uint8_t* frame) +{ +#if defined(SEND_RSSI_DATA) + // Calculate RSSI average over a burst period. We don't take into account 2.5 ms at the beginning and 2.5 ms at the end + uint16_t start = m_startPtr + DMR_SYNC_LENGTH_SAMPLES / 2U; + + uint32_t accum = 0U; + for (uint16_t i = 0U; i < (DMR_FRAME_LENGTH_SAMPLES - DMR_SYNC_LENGTH_SAMPLES); i++) + accum += m_rssi[start++]; + + uint16_t avg = accum / (DMR_FRAME_LENGTH_SAMPLES - DMR_SYNC_LENGTH_SAMPLES); + frame[34U] = (avg >> 8) & 0xFFU; + frame[35U] = (avg >> 0) & 0xFFU; + + serial.writeDMRData(m_slot, frame, DMR_FRAME_LENGTH_BYTES + 3U); +#else + serial.writeDMRData(m_slot, frame, DMR_FRAME_LENGTH_BYTES + 1U); +#endif +} + +#endif diff --git a/DMRSlotRX.h b/DMRSlotRX.h new file mode 100644 index 0000000..6e0719e --- /dev/null +++ b/DMRSlotRX.h @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2015,2016,2017 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(DMRSLOTRX_H) +#define DMRSLOTRX_H + +#if defined(DUPLEX) + +#include "Config.h" +#include "DMRDefines.h" + +enum DMRRX_STATE { + DMRRXS_NONE, + DMRRXS_VOICE, + DMRRXS_DATA +}; + +class CDMRSlotRX { +public: + CDMRSlotRX(bool slot); + + void start(); + + bool processSample(q15_t sample, uint16_t rssi); + + void setColorCode(uint8_t colorCode); + void setDelay(uint8_t delay); + + void reset(); + +private: + bool m_slot; + uint32_t m_bitBuffer[DMR_RADIO_SYMBOL_LENGTH]; + q15_t m_buffer[900U]; + uint16_t m_bitPtr; + uint16_t m_dataPtr; + uint16_t m_syncPtr; + uint16_t m_startPtr; + uint16_t m_endPtr; + uint16_t m_delayPtr; + q31_t m_maxCorr; + q15_t m_centre[4U]; + q15_t m_threshold[4U]; + uint8_t m_averagePtr; + uint8_t m_control; + uint8_t m_syncCount; + uint8_t m_colorCode; + uint16_t m_delay; + DMRRX_STATE m_state; + uint8_t m_n; + uint8_t m_type; + uint16_t m_rssi[900U]; + + void correlateSync(bool first); + void samplesToBits(uint16_t start, uint8_t count, uint8_t* buffer, uint16_t offset, q15_t centre, q15_t threshold); + void writeRSSIData(uint8_t* frame); +}; + +#endif + +#endif diff --git a/DMRTX.cpp b/DMRTX.cpp new file mode 100644 index 0000000..ada927f --- /dev/null +++ b/DMRTX.cpp @@ -0,0 +1,382 @@ +/* + * Copyright (C) 2009-2017 by Jonathan Naylor G4KLX + * Copyright (C) 2016 by Colin Durbridge G4EML + * Copyright (C) 2017 by Andy Uribe CA6JAU + * + * 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(DUPLEX) + +// #define WANT_DEBUG + +#include "Config.h" +#include "Globals.h" +#include "DMRSlotType.h" + +#if defined(WIDE_C4FSK_FILTERS_TX) +// Generated using rcosdesign(0.2, 4, 5, 'sqrt') in MATLAB +static q15_t DMR_C4FSK_FILTER[] = {0, 0, 0, 0, 688, -680, -2158, -3060, -2724, -775, 2684, 7041, 11310, 14425, 15565, 14425, + 11310, 7041, 2684, -775, -2724, -3060, -2158, -680, 688}; // numTaps = 25, L = 5 +const uint16_t DMR_C4FSK_FILTER_PHASE_LEN = 5U; // phaseLength = numTaps/L +#else +// Generated using rcosdesign(0.2, 8, 5, 'sqrt') in MATLAB +static q15_t DMR_C4FSK_FILTER[] = {0, 0, 0, 0, 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}; // numTaps = 45, L = 5 +const uint16_t DMR_C4FSK_FILTER_PHASE_LEN = 9U; // phaseLength = numTaps/L +#endif + +const q15_t DMR_LEVELA = 2889; +const q15_t DMR_LEVELB = 963; +const q15_t DMR_LEVELC = -963; +const q15_t DMR_LEVELD = -2889; + +// The PR FILL and BS Data Sync pattern. +const uint8_t IDLE_DATA[] = + {0x53U, 0xC2U, 0x5EU, 0xABU, 0xA8U, 0x67U, 0x1DU, 0xC7U, 0x38U, 0x3BU, 0xD9U, + 0x36U, 0x00U, 0x0DU, 0xFFU, 0x57U, 0xD7U, 0x5DU, 0xF5U, 0xD0U, 0x03U, 0xF6U, + 0xE4U, 0x65U, 0x17U, 0x1BU, 0x48U, 0xCAU, 0x6DU, 0x4FU, 0xC6U, 0x10U, 0xB4U}; + +const uint8_t CACH_INTERLEAVE[] = + { 1U, 2U, 3U, 5U, 6U, 7U, 9U, 10U, 11U, 13U, 15U, 16U, 17U, 19U, 20U, 21U, 23U, + 25U, 26U, 27U, 29U, 30U, 31U, 33U, 34U, 35U, 37U, 39U, 40U, 41U, 43U, 44U, 45U, 47U, + 49U, 50U, 51U, 53U, 54U, 55U, 57U, 58U, 59U, 61U, 63U, 64U, 65U, 67U, 68U, 69U, 71U, + 73U, 74U, 75U, 77U, 78U, 79U, 81U, 82U, 83U, 85U, 87U, 88U, 89U, 91U, 92U, 93U, 95U}; + +const uint8_t EMPTY_SHORT_LC[] = + {0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U}; + +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]) + +const uint32_t STARTUP_COUNT = 20U; + +CDMRTX::CDMRTX() : +m_fifo(), +m_modFilter(), +m_modState(), +m_state(DMRTXSTATE_IDLE), +m_idle(), +m_cachPtr(0U), +m_shortLC(), +m_newShortLC(), +m_markBuffer(), +m_poBuffer(), +m_poLen(0U), +m_poPtr(0U), +m_frameCount(0U), +m_abort() +{ + ::memset(m_modState, 0x00U, 16U * sizeof(q15_t)); + + m_modFilter.L = DMR_RADIO_SYMBOL_LENGTH; + m_modFilter.phaseLength = DMR_C4FSK_FILTER_PHASE_LEN; + m_modFilter.pCoeffs = DMR_C4FSK_FILTER; + m_modFilter.pState = m_modState; + + ::memcpy(m_newShortLC, EMPTY_SHORT_LC, 12U); + ::memcpy(m_shortLC, EMPTY_SHORT_LC, 12U); + + m_abort[0U] = false; + m_abort[1U] = false; +} + +void CDMRTX::process() +{ + if (m_state == DMRTXSTATE_IDLE) + return; + + if (m_poLen == 0U) { + switch (m_state) { + case DMRTXSTATE_SLOT1: + createData(0U); + m_state = DMRTXSTATE_CACH2; + break; + + case DMRTXSTATE_CACH2: + createCACH(1U, 0U); + m_state = DMRTXSTATE_SLOT2; + break; + + case DMRTXSTATE_SLOT2: + createData(1U); + m_state = DMRTXSTATE_CACH1; + break; + + case DMRTXSTATE_CAL: + createCal(); + break; + + default: + createCACH(0U, 1U); + m_state = DMRTXSTATE_SLOT1; + break; + } + } + + if (m_poLen > 0U) { + uint16_t space = io.getSpace(); + + while (space > (4U * DMR_RADIO_SYMBOL_LENGTH)) { + uint8_t c = m_poBuffer[m_poPtr]; + uint8_t m = m_markBuffer[m_poPtr]; + m_poPtr++; + + writeByte(c, m); + + space -= 4U * DMR_RADIO_SYMBOL_LENGTH; + + if (m_poPtr >= m_poLen) { + m_poPtr = 0U; + m_poLen = 0U; + return; + } + } + } +} + +uint8_t CDMRTX::writeData1(const uint8_t* data, uint8_t length) +{ + if (length != (DMR_FRAME_LENGTH_BYTES + 1U)) + return 4U; + + uint16_t space = m_fifo[0U].getSpace(); + if (space < DMR_FRAME_LENGTH_BYTES) + return 5U; + + if (m_abort[0U]) { + m_fifo[0U].reset(); + m_abort[0U] = false; + } + + for (uint8_t i = 0U; i < DMR_FRAME_LENGTH_BYTES; i++) + m_fifo[0U].put(data[i + 1U]); + + // Start the TX if it isn't already on + if (!m_tx) + m_state = DMRTXSTATE_SLOT1; + + return 0U; +} + +uint8_t CDMRTX::writeData2(const uint8_t* data, uint8_t length) +{ + if (length != (DMR_FRAME_LENGTH_BYTES + 1U)) + return 4U; + + uint16_t space = m_fifo[1U].getSpace(); + if (space < DMR_FRAME_LENGTH_BYTES) + return 5U; + + if (m_abort[1U]) { + m_fifo[1U].reset(); + m_abort[1U] = false; + } + + for (uint8_t i = 0U; i < DMR_FRAME_LENGTH_BYTES; i++) + m_fifo[1U].put(data[i + 1U]); + + // Start the TX if it isn't already on + if (!m_tx) + m_state = DMRTXSTATE_SLOT1; + + return 0U; +} + +uint8_t CDMRTX::writeShortLC(const uint8_t* data, uint8_t length) +{ + if (length != 9U) + return 4U; + + ::memset(m_newShortLC, 0x00U, 12U); + + for (uint8_t i = 0U; i < 68U; i++) { + bool b = READ_BIT1(data, i); + uint8_t n = CACH_INTERLEAVE[i]; + WRITE_BIT1(m_newShortLC, n, b); + } + + return 0U; +} + +uint8_t CDMRTX::writeAbort(const uint8_t* data, uint8_t length) +{ + if (length != 1U) + return 4U; + + switch (data[0U]) { + case 1U: + m_abort[0U] = true; + return 0U; + + case 2U: + m_abort[1U] = true; + return 0U; + + default: + return 4U; + } +} + +void CDMRTX::setStart(bool start) +{ + m_state = start ? DMRTXSTATE_SLOT1 : DMRTXSTATE_IDLE; + + m_frameCount = 0U; + + m_abort[0U] = false; + m_abort[1U] = false; +} + +void CDMRTX::setCal(bool start) +{ + m_state = start ? DMRTXSTATE_CAL : DMRTXSTATE_IDLE; +} + +void CDMRTX::writeByte(uint8_t c, uint8_t control) +{ + q15_t inBuffer[4U]; + q15_t outBuffer[DMR_RADIO_SYMBOL_LENGTH * 4U]; + + const uint8_t MASK = 0xC0U; + + for (uint8_t i = 0U; i < 4U; i++, c <<= 2) { + switch (c & MASK) { + case 0xC0U: + inBuffer[i] = DMR_LEVELA; + break; + case 0x80U: + inBuffer[i] = DMR_LEVELB; + break; + case 0x00U: + inBuffer[i] = DMR_LEVELC; + break; + default: + inBuffer[i] = DMR_LEVELD; + break; + } + } + + 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; + + ::arm_fir_interpolate_q15(&m_modFilter, inBuffer, outBuffer, 4U); + + io.write(STATE_DMR, outBuffer, DMR_RADIO_SYMBOL_LENGTH * 4U, controlBuffer); +} + +uint8_t CDMRTX::getSpace1() const +{ + return m_fifo[0U].getSpace() / (DMR_FRAME_LENGTH_BYTES + 2U); +} + +uint8_t CDMRTX::getSpace2() const +{ + return m_fifo[1U].getSpace() / (DMR_FRAME_LENGTH_BYTES + 2U); +} + +void CDMRTX::createData(uint8_t slotIndex) +{ + if (m_fifo[slotIndex].getData() > 0U && m_frameCount >= STARTUP_COUNT) { + for (unsigned int i = 0U; i < DMR_FRAME_LENGTH_BYTES; i++) { + m_poBuffer[i] = m_fifo[slotIndex].get(); + m_markBuffer[i] = MARK_NONE; + } + } else { + m_abort[slotIndex] = false; + // Transmit an idle message + for (unsigned int i = 0U; i < DMR_FRAME_LENGTH_BYTES; i++) { + m_poBuffer[i] = m_idle[i]; + m_markBuffer[i] = MARK_NONE; + } + } + + m_poLen = DMR_FRAME_LENGTH_BYTES; + m_poPtr = 0U; +} + +void CDMRTX::createCal() +{ + for (unsigned int i = 0U; i < DMR_FRAME_LENGTH_BYTES; i++) { + m_poBuffer[i] = 0x5FU; // +3, +3, -3, -3 pattern for deviation cal. + m_markBuffer[i] = MARK_NONE; + } + + m_poLen = DMR_FRAME_LENGTH_BYTES; + m_poPtr = 0U; +} + +void CDMRTX::createCACH(uint8_t txSlotIndex, uint8_t rxSlotIndex) +{ + m_frameCount++; + + if (m_cachPtr >= 12U) + m_cachPtr = 0U; + + if (m_cachPtr == 0U) { + if (m_fifo[0U].getData() == 0U && m_fifo[1U].getData() == 0U) + ::memcpy(m_shortLC, EMPTY_SHORT_LC, 12U); + else + ::memcpy(m_shortLC, m_newShortLC, 12U); + } + + ::memcpy(m_poBuffer, m_shortLC + m_cachPtr, 3U); + m_markBuffer[0U] = MARK_NONE; + m_markBuffer[1U] = MARK_NONE; + m_markBuffer[2U] = rxSlotIndex == 1U ? MARK_SLOT1 : MARK_SLOT2; + + bool at = false; + if (m_frameCount >= STARTUP_COUNT) + at = m_fifo[rxSlotIndex].getData() > 0U; + bool tc = txSlotIndex == 1U; + bool ls0 = true; // For 1 and 2 + bool ls1 = true; + + if (m_cachPtr == 0U) // For 0 + ls1 = false; + else if (m_cachPtr == 9U) // For 3 + ls0 = false; + + bool h0 = at ^ tc ^ ls1; + bool h1 = tc ^ ls1 ^ ls0; + bool h2 = at ^ tc ^ ls0; + + m_poBuffer[0U] |= at ? 0x80U : 0x00U; + m_poBuffer[0U] |= tc ? 0x08U : 0x00U; + m_poBuffer[1U] |= ls1 ? 0x80U : 0x00U; + m_poBuffer[1U] |= ls0 ? 0x08U : 0x00U; + m_poBuffer[1U] |= h0 ? 0x02U : 0x00U; + m_poBuffer[2U] |= h1 ? 0x20U : 0x00U; + m_poBuffer[2U] |= h2 ? 0x02U : 0x00U; + + m_poLen = DMR_CACH_LENGTH_BYTES; + m_poPtr = 0U; + + m_cachPtr += 3U; +} + +void CDMRTX::setColorCode(uint8_t colorCode) +{ + ::memcpy(m_idle, IDLE_DATA, DMR_FRAME_LENGTH_BYTES); + + CDMRSlotType slotType; + slotType.encode(colorCode, DT_IDLE, m_idle); +} + +#endif diff --git a/DMRTX.h b/DMRTX.h new file mode 100644 index 0000000..23de3fd --- /dev/null +++ b/DMRTX.h @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2015,2016,2017 by Jonathan Naylor G4KLX + * 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. + */ + +#if !defined(DMRTX_H) +#define DMRTX_H + +#if defined(DUPLEX) + +#include "Config.h" +#include "DMRDefines.h" + +#include "SerialRB.h" + +enum DMRTXSTATE { + DMRTXSTATE_IDLE, + DMRTXSTATE_SLOT1, + DMRTXSTATE_CACH1, + DMRTXSTATE_SLOT2, + DMRTXSTATE_CACH2, + DMRTXSTATE_CAL +}; + +class CDMRTX { +public: + CDMRTX(); + + uint8_t writeData1(const uint8_t* data, uint8_t length); + uint8_t writeData2(const uint8_t* data, uint8_t length); + + uint8_t writeShortLC(const uint8_t* data, uint8_t length); + uint8_t writeAbort(const uint8_t* data, uint8_t length); + + void setStart(bool start); + void setCal(bool start); + + void process(); + + uint8_t getSpace1() const; + uint8_t getSpace2() const; + + void setColorCode(uint8_t colorCode); + +private: + CSerialRB m_fifo[2U]; + arm_fir_interpolate_instance_q15 m_modFilter; + q15_t m_modState[16U]; // blockSize + phaseLength - 1, 4 + 9 - 1 plus some spare + DMRTXSTATE m_state; + uint8_t m_idle[DMR_FRAME_LENGTH_BYTES]; + uint8_t m_cachPtr; + uint8_t m_shortLC[12U]; + uint8_t m_newShortLC[12U]; + uint8_t m_markBuffer[40U]; + uint8_t m_poBuffer[40U]; + uint16_t m_poLen; + uint16_t m_poPtr; + uint32_t m_frameCount; + bool m_abort[2U]; + + void createData(uint8_t slotIndex); + void createCACH(uint8_t txSlotIndex, uint8_t rxSlotIndex); + void createCal(); + void writeByte(uint8_t c, uint8_t control); +}; + +#endif + +#endif