mirror of https://github.com/g4klx/MMDVM.git
Fix DMO mode for CS700 as well as PD365.
This commit is contained in:
Jonathan Naylor
parent
cffe692055
commit
0edb4e1c43
176
DMRDMORX.cpp
176
DMRDMORX.cpp
|
|
@ -22,7 +22,6 @@
|
|||
#include "Globals.h"
|
||||
#include "DMRDMORX.h"
|
||||
#include "DMRSlotType.h"
|
||||
#include "DMREMB.h"
|
||||
#include "Utils.h"
|
||||
|
||||
const q15_t SCALING_FACTOR = 19505; // Q15(0.60)
|
||||
|
|
@ -61,8 +60,7 @@ m_state(DMORXS_NONE),
|
|||
m_n(0U),
|
||||
m_type(0U),
|
||||
m_rssiCount(0U),
|
||||
m_rssi(0U),
|
||||
m_embs()
|
||||
m_rssi(0U)
|
||||
{
|
||||
}
|
||||
|
||||
|
|
@ -118,15 +116,9 @@ bool CDMRDMORX::processSample(q15_t sample)
|
|||
if (min < max) {
|
||||
if (m_dataPtr >= min && m_dataPtr <= max)
|
||||
correlateSync(false);
|
||||
|
||||
if (m_dataPtr == max1 && m_control == CONTROL_NONE)
|
||||
correlateEMB();
|
||||
} else {
|
||||
if (m_dataPtr >= min || m_dataPtr <= max)
|
||||
correlateSync(false);
|
||||
|
||||
if (m_dataPtr == max1 && m_control == CONTROL_NONE)
|
||||
correlateEMB();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -279,13 +271,16 @@ bool CDMRDMORX::processSample(q15_t sample)
|
|||
|
||||
void CDMRDMORX::correlateSync(bool first)
|
||||
{
|
||||
uint8_t errs = countBits32((m_bitBuffer[m_bitPtr] & DMR_SYNC_SYMBOLS_MASK) ^ DMR_S2_DATA_SYNC_SYMBOLS);
|
||||
uint8_t errs1 = countBits32((m_bitBuffer[m_bitPtr] & DMR_SYNC_SYMBOLS_MASK) ^ DMR_S2_DATA_SYNC_SYMBOLS);
|
||||
uint8_t errs2 = 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));
|
||||
bool data1 = (errs1 <= MAX_SYNC_SYMBOLS_ERRS);
|
||||
bool data2 = (errs2 <= MAX_SYNC_SYMBOLS_ERRS);
|
||||
bool voice1 = (errs1 >= (DMR_SYNC_LENGTH_SYMBOLS - MAX_SYNC_SYMBOLS_ERRS));
|
||||
bool voice2 = (errs2 >= (DMR_SYNC_LENGTH_SYMBOLS - MAX_SYNC_SYMBOLS_ERRS));
|
||||
|
||||
if (data || voice) {
|
||||
if (data1 || data2 || voice1 || voice2) {
|
||||
uint16_t ptr = m_dataPtr + DMO_BUFFER_LENGTH_SAMPLES - DMR_SYNC_LENGTH_SAMPLES + DMR_RADIO_SYMBOL_LENGTH;
|
||||
if (ptr >= DMO_BUFFER_LENGTH_SAMPLES)
|
||||
ptr -= DMO_BUFFER_LENGTH_SAMPLES;
|
||||
|
|
@ -296,14 +291,18 @@ void CDMRDMORX::correlateSync(bool first)
|
|||
|
||||
uint32_t mask = 0x00800000U;
|
||||
for (uint8_t i = 0U; i < DMR_SYNC_LENGTH_SYMBOLS; i++, mask >>= 1) {
|
||||
bool b = (DMR_MS_DATA_SYNC_SYMBOLS & mask) == mask;
|
||||
bool b;
|
||||
if (data1 || voice1)
|
||||
b = (DMR_S2_DATA_SYNC_SYMBOLS & mask) == mask;
|
||||
else
|
||||
b = (DMR_MS_DATA_SYNC_SYMBOLS & mask) == mask;
|
||||
|
||||
if (m_buffer[ptr] > max)
|
||||
max = m_buffer[ptr];
|
||||
if (m_buffer[ptr] < min)
|
||||
min = m_buffer[ptr];
|
||||
|
||||
if (data)
|
||||
if (data1 || data2)
|
||||
corr += b ? -m_buffer[ptr] : m_buffer[ptr];
|
||||
else // if (voice)
|
||||
corr += b ? m_buffer[ptr] : -m_buffer[ptr];
|
||||
|
|
@ -326,10 +325,14 @@ void CDMRDMORX::correlateSync(bool first)
|
|||
|
||||
samplesToBits(ptr, DMR_SYNC_LENGTH_SYMBOLS, sync, 4U, centre, threshold);
|
||||
|
||||
if (data) {
|
||||
if (data1 || data2) {
|
||||
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_S2_DATA_SYNC_BYTES[i]);
|
||||
for (uint8_t i = 0U; i < DMR_SYNC_BYTES_LENGTH; i++) {
|
||||
if (data1)
|
||||
errs += countBits8((sync[i] & DMR_SYNC_BYTES_MASK[i]) ^ DMR_S2_DATA_SYNC_BYTES[i]);
|
||||
else
|
||||
errs += countBits8((sync[i] & DMR_SYNC_BYTES_MASK[i]) ^ DMR_MS_DATA_SYNC_BYTES[i]);
|
||||
}
|
||||
|
||||
if (errs <= MAX_SYNC_BYTES_ERRS) {
|
||||
if (first) {
|
||||
|
|
@ -358,10 +361,14 @@ void CDMRDMORX::correlateSync(bool first)
|
|||
if (m_endPtr >= DMO_BUFFER_LENGTH_SAMPLES)
|
||||
m_endPtr -= DMO_BUFFER_LENGTH_SAMPLES;
|
||||
}
|
||||
} else { // if (voice)
|
||||
} else { // if (voice1 || voice2)
|
||||
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_S2_VOICE_SYNC_BYTES[i]);
|
||||
for (uint8_t i = 0U; i < DMR_SYNC_BYTES_LENGTH; i++) {
|
||||
if (voice1)
|
||||
errs += countBits8((sync[i] & DMR_SYNC_BYTES_MASK[i]) ^ DMR_S2_VOICE_SYNC_BYTES[i]);
|
||||
else
|
||||
errs += countBits8((sync[i] & DMR_SYNC_BYTES_MASK[i]) ^ DMR_MS_VOICE_SYNC_BYTES[i]);
|
||||
}
|
||||
|
||||
if (errs <= MAX_SYNC_BYTES_ERRS) {
|
||||
if (first) {
|
||||
|
|
@ -395,113 +402,6 @@ void CDMRDMORX::correlateSync(bool first)
|
|||
}
|
||||
}
|
||||
|
||||
void CDMRDMORX::correlateEMB()
|
||||
{
|
||||
uint16_t ptr = m_dataPtr + DMO_BUFFER_LENGTH_SAMPLES - DMR_SYNC_LENGTH_SAMPLES + DMR_RADIO_SYMBOL_LENGTH - 3U;
|
||||
if (ptr >= DMO_BUFFER_LENGTH_SAMPLES)
|
||||
ptr -= DMO_BUFFER_LENGTH_SAMPLES;
|
||||
|
||||
q15_t centre = (m_centre[0U] + m_centre[1U] + m_centre[2U] + m_centre[3U]) >> 2;
|
||||
|
||||
q31_t corr[3U];
|
||||
for (uint8_t i = 0U; i < 3U; i++) {
|
||||
uint16_t ptr1 = ptr;
|
||||
uint16_t ptr2 = ptr + DMR_EMB_LENGTH_SAMPLES / 2U + DMR_EMBSIG_LENGTH_SAMPLES;
|
||||
|
||||
if (ptr2 >= DMO_BUFFER_LENGTH_SAMPLES)
|
||||
ptr2 -= DMO_BUFFER_LENGTH_SAMPLES;
|
||||
|
||||
corr[i] = 0;
|
||||
uint16_t emb1 = m_embs[m_n].part1;
|
||||
uint16_t emb2 = m_embs[m_n].part2;
|
||||
|
||||
for (uint8_t j = 0U; j < 4U; j++) {
|
||||
q15_t sample1 = m_buffer[ptr1] - centre;
|
||||
q15_t sample2 = m_buffer[ptr2] - centre;
|
||||
|
||||
switch (emb1 & 0xC0U) {
|
||||
case 0xC0U: corr[i] += (sample1 + sample1 + sample1); break;
|
||||
case 0x80U: corr[i] += (sample1); break;
|
||||
case 0x40U: corr[i] -= (sample1 + sample1 + sample1); break;
|
||||
default: corr[i] -= (sample1); break;
|
||||
}
|
||||
|
||||
switch (emb2 & 0xC0U) {
|
||||
case 0xC0U: corr[i] += (sample2 + sample2 + sample2); break;
|
||||
case 0x80U: corr[i] += (sample2); break;
|
||||
case 0x40U: corr[i] -= (sample2 + sample2 + sample2); break;
|
||||
default: corr[i] -= (sample2); break;
|
||||
}
|
||||
|
||||
// uint8_t emb1[1U];
|
||||
// samplesToBits(ptr1, DMR_EMB_LENGTH_SYMBOLS / 2U, emb1, 0U, centre, threshold);
|
||||
|
||||
// uint8_t emb2[1U];
|
||||
// samplesToBits(ptr2, DMR_EMB_LENGTH_SYMBOLS / 2U, emb2, 0U, centre, threshold);
|
||||
|
||||
// errs[i] = countBits8(emb1[0U] ^ m_embs[m_n].part1) + countBits8(emb2[0U] ^ m_embs[m_n].part2);
|
||||
// if (errs[i] < lowest)
|
||||
// lowest = errs[i];
|
||||
|
||||
// DEBUG3("DMRDMORX: ptr/errs", ptr1, errs[i]);
|
||||
|
||||
emb1 <<= 2;
|
||||
emb2 <<= 2;
|
||||
|
||||
ptr1 += DMR_RADIO_SYMBOL_LENGTH;
|
||||
if (ptr1 >= DMO_BUFFER_LENGTH_SAMPLES)
|
||||
ptr1 -= DMO_BUFFER_LENGTH_SAMPLES;
|
||||
|
||||
ptr2 += DMR_RADIO_SYMBOL_LENGTH;
|
||||
if (ptr2 >= DMO_BUFFER_LENGTH_SAMPLES)
|
||||
ptr2 -= DMO_BUFFER_LENGTH_SAMPLES;
|
||||
}
|
||||
|
||||
ptr++;
|
||||
if (ptr >= DMO_BUFFER_LENGTH_SAMPLES)
|
||||
ptr -= DMO_BUFFER_LENGTH_SAMPLES;
|
||||
}
|
||||
|
||||
DEBUG4("DMRDMORX: -1/0/+1", corr[0U], corr[1], corr[2]);
|
||||
|
||||
// None of them are very good, don't use
|
||||
// if (lowest > MAX_EMB_BITS_ERRS)
|
||||
// return;
|
||||
|
||||
// if (errs[2U] == lowest) {
|
||||
// // Prefer the status quo
|
||||
// return;
|
||||
// } else if (errs[1U] == lowest) {
|
||||
// // m_syncPtr--
|
||||
// m_syncPtr += DMO_BUFFER_LENGTH_SAMPLES - 1U;
|
||||
// } else {
|
||||
// m_syncPtr++;
|
||||
// }
|
||||
|
||||
if (corr[1U] > corr[0U] && corr[1U] > corr[2U]) {
|
||||
// Nothing has changed
|
||||
return;
|
||||
} else if (corr[0U] > corr[1U] && corr[0U] > corr[2U]) {
|
||||
// m_syncPtr--
|
||||
m_syncPtr += DMO_BUFFER_LENGTH_SAMPLES - 1U;
|
||||
} else {
|
||||
m_syncPtr++;
|
||||
}
|
||||
|
||||
if (m_syncPtr >= DMO_BUFFER_LENGTH_SAMPLES)
|
||||
m_syncPtr -= DMO_BUFFER_LENGTH_SAMPLES;
|
||||
|
||||
m_startPtr = m_dataPtr + DMO_BUFFER_LENGTH_SAMPLES - DMR_SLOT_TYPE_LENGTH_SAMPLES / 2U - DMR_INFO_LENGTH_SAMPLES / 2U - DMR_SYNC_LENGTH_SAMPLES;
|
||||
if (m_startPtr >= DMO_BUFFER_LENGTH_SAMPLES)
|
||||
m_startPtr -= DMO_BUFFER_LENGTH_SAMPLES;
|
||||
|
||||
m_endPtr = m_dataPtr + DMR_SLOT_TYPE_LENGTH_SAMPLES / 2U + DMR_INFO_LENGTH_SAMPLES / 2U - 1U;
|
||||
if (m_endPtr >= DMO_BUFFER_LENGTH_SAMPLES)
|
||||
m_endPtr -= DMO_BUFFER_LENGTH_SAMPLES;
|
||||
|
||||
DEBUG2("DMRDMORX: m_syncPtr", m_syncPtr);
|
||||
}
|
||||
|
||||
void CDMRDMORX::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++) {
|
||||
|
|
@ -538,25 +438,5 @@ void CDMRDMORX::samplesToBits(uint16_t start, uint8_t count, uint8_t* buffer, ui
|
|||
void CDMRDMORX::setColorCode(uint8_t colorCode)
|
||||
{
|
||||
m_colorCode = colorCode;
|
||||
|
||||
// Build table of EMB values
|
||||
uint16_t emb = CDMREMB::emb(colorCode, false, 1U);
|
||||
m_embs[0U].part1 = (emb >> 8) & 0xFFU;
|
||||
m_embs[0U].part2 = (emb >> 0) & 0xFFU;
|
||||
|
||||
emb = CDMREMB::emb(colorCode, false, 3U);
|
||||
m_embs[1U].part1 = (emb >> 8) & 0xFFU;
|
||||
m_embs[1U].part2 = (emb >> 0) & 0xFFU;
|
||||
|
||||
m_embs[2U].part1 = m_embs[1U].part1;
|
||||
m_embs[2U].part2 = m_embs[1U].part2;
|
||||
|
||||
emb = CDMREMB::emb(colorCode, false, 2U);
|
||||
m_embs[3U].part1 = (emb >> 8) & 0xFFU;
|
||||
m_embs[3U].part2 = (emb >> 0) & 0xFFU;
|
||||
|
||||
emb = CDMREMB::emb(colorCode, false, 0U);
|
||||
m_embs[4U].part1 = (emb >> 8) & 0xFFU;
|
||||
m_embs[4U].part2 = (emb >> 0) & 0xFFU;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -30,11 +30,6 @@ enum DMORX_STATE {
|
|||
DMORXS_DATA
|
||||
};
|
||||
|
||||
struct EMBList {
|
||||
uint8_t part1;
|
||||
uint8_t part2;
|
||||
};
|
||||
|
||||
class CDMRDMORX {
|
||||
public:
|
||||
CDMRDMORX();
|
||||
|
|
@ -65,11 +60,9 @@ private:
|
|||
uint8_t m_type;
|
||||
uint16_t m_rssiCount;
|
||||
uint16_t m_rssi;
|
||||
EMBList m_embs[5U];
|
||||
|
||||
bool processSample(q15_t sample);
|
||||
void correlateSync(bool first);
|
||||
void correlateEMB();
|
||||
void samplesToBits(uint16_t start, uint8_t count, uint8_t* buffer, uint16_t offset, q15_t centre, q15_t threshold);
|
||||
};
|
||||
|
||||
|
|
|
|||
16
DMRDMOTX.cpp
16
DMRDMOTX.cpp
|
|
@ -104,22 +104,8 @@ uint8_t CDMRDMOTX::writeData(const uint8_t* data, uint8_t length)
|
|||
if (space < DMR_FRAME_LENGTH_BYTES)
|
||||
return 5U;
|
||||
|
||||
uint8_t buffer[DMR_FRAME_LENGTH_BYTES];
|
||||
::memcpy(buffer, data + 1U, DMR_FRAME_LENGTH_BYTES);
|
||||
|
||||
// Swap the sync bytes to DMO slot 2
|
||||
if (::memcmp(buffer + 14U, DMR_BS_DATA_SYNC_BYTES + 1U, 5U) == 0) {
|
||||
::memcpy(buffer + 14U, DMR_S2_DATA_SYNC_BYTES + 1U, 5U);
|
||||
buffer[13U] &= 0xF0U; buffer[13U] |= DMR_S2_DATA_SYNC_BYTES[0U];
|
||||
buffer[19U] &= 0x0FU; buffer[19U] |= DMR_S2_DATA_SYNC_BYTES[6U];
|
||||
} else if (::memcmp(buffer + 14U, DMR_BS_VOICE_SYNC_BYTES + 1U, 5U) == 0) {
|
||||
::memcpy(buffer + 14U, DMR_S2_VOICE_SYNC_BYTES + 1U, 5U);
|
||||
buffer[13U] &= 0xF0U; buffer[13U] |= DMR_S2_VOICE_SYNC_BYTES[0U];
|
||||
buffer[19U] &= 0x0FU; buffer[19U] |= DMR_S2_VOICE_SYNC_BYTES[6U];
|
||||
}
|
||||
|
||||
for (uint8_t i = 0U; i < DMR_FRAME_LENGTH_BYTES; i++)
|
||||
m_fifo.put(buffer[i]);
|
||||
m_fifo.put(data[i + 1U]);
|
||||
|
||||
return 0U;
|
||||
}
|
||||
|
|
|
|||
43
DMREMB.cpp
43
DMREMB.cpp
|
|
@ -1,43 +0,0 @@
|
|||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "DMREMB.h"
|
||||
|
||||
const uint16_t ENCODING_TABLE_1676[] =
|
||||
{0x0000U, 0x0273U, 0x04E5U, 0x0696U, 0x09C9U, 0x0BBAU, 0x0D2CU, 0x0F5FU, 0x11E2U, 0x1391U, 0x1507U, 0x1774U,
|
||||
0x182BU, 0x1A58U, 0x1CCEU, 0x1EBDU, 0x21B7U, 0x23C4U, 0x2552U, 0x2721U, 0x287EU, 0x2A0DU, 0x2C9BU, 0x2EE8U,
|
||||
0x3055U, 0x3226U, 0x34B0U, 0x36C3U, 0x399CU, 0x3BEFU, 0x3D79U, 0x3F0AU, 0x411EU, 0x436DU, 0x45FBU, 0x4788U,
|
||||
0x48D7U, 0x4AA4U, 0x4C32U, 0x4E41U, 0x50FCU, 0x528FU, 0x5419U, 0x566AU, 0x5935U, 0x5B46U, 0x5DD0U, 0x5FA3U,
|
||||
0x60A9U, 0x62DAU, 0x644CU, 0x663FU, 0x6960U, 0x6B13U, 0x6D85U, 0x6FF6U, 0x714BU, 0x7338U, 0x75AEU, 0x77DDU,
|
||||
0x7882U, 0x7AF1U, 0x7C67U, 0x7E14U, 0x804FU, 0x823CU, 0x84AAU, 0x86D9U, 0x8986U, 0x8BF5U, 0x8D63U, 0x8F10U,
|
||||
0x91ADU, 0x93DEU, 0x9548U, 0x973BU, 0x9864U, 0x9A17U, 0x9C81U, 0x9EF2U, 0xA1F8U, 0xA38BU, 0xA51DU, 0xA76EU,
|
||||
0xA831U, 0xAA42U, 0xACD4U, 0xAEA7U, 0xB01AU, 0xB269U, 0xB4FFU, 0xB68CU, 0xB9D3U, 0xBBA0U, 0xBD36U, 0xBF45U,
|
||||
0xC151U, 0xC322U, 0xC5B4U, 0xC7C7U, 0xC898U, 0xCAEBU, 0xCC7DU, 0xCE0EU, 0xD0B3U, 0xD2C0U, 0xD456U, 0xD625U,
|
||||
0xD97AU, 0xDB09U, 0xDD9FU, 0xDFECU, 0xE0E6U, 0xE295U, 0xE403U, 0xE670U, 0xE92FU, 0xEB5CU, 0xEDCAU, 0xEFB9U,
|
||||
0xF104U, 0xF377U, 0xF5E1U, 0xF792U, 0xF8CDU, 0xFABEU, 0xFC28U, 0xFE5BU};
|
||||
|
||||
uint16_t CDMREMB::emb(uint8_t colorCode, bool pi, uint8_t lcss)
|
||||
{
|
||||
uint8_t index = 0U;
|
||||
index |= (colorCode << 3) & 0x78U;
|
||||
index |= pi ? 0x04U : 0x00U;
|
||||
index |= (lcss << 0) & 0x03U;
|
||||
|
||||
return ENCODING_TABLE_1676[index];
|
||||
}
|
||||
|
||||
33
DMREMB.h
33
DMREMB.h
|
|
@ -1,33 +0,0 @@
|
|||
/*
|
||||
* 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(DMREMB_H)
|
||||
#define DMREMB_H
|
||||
|
||||
#include "Config.h"
|
||||
#include "Globals.h"
|
||||
|
||||
class CDMREMB
|
||||
{
|
||||
public:
|
||||
static uint16_t emb(uint8_t colorCode, bool pi, uint8_t lcss);
|
||||
|
||||
private:
|
||||
};
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue