Kenwood work in progress.

This commit is contained in:
Jonathan Naylor 2020-05-01 18:44:52 +01:00
parent 99e13114a8
commit 948fa8bfbf
4 changed files with 89 additions and 35 deletions

View File

@ -60,13 +60,16 @@ void CGPSHandler::processData(const unsigned char* data)
{
assert(data != NULL);
CUtils::dump(4U, "processData GPS Data", data, 21U);
::memcpy(m_data + m_length, data + 1U, NXDN_DATA_LENGTH);
m_length += NXDN_DATA_LENGTH;
if (data[0U] == 0x00U) {
bool ret = processIcom();
if (!ret)
processKenwood();
ret = processKenwood();
if (ret)
reset();
}
}
@ -165,7 +168,7 @@ bool CGPSHandler::checkXOR() const
return ::memcmp(buffer, p2 + 1U, 2U) == 0;
}
void CGPSHandler::processKenwood()
bool CGPSHandler::processKenwood()
{
enum {
GPS_FULL,
@ -175,18 +178,26 @@ void CGPSHandler::processKenwood()
switch (m_data[0U]) {
case 0x00U:
if (m_length < 38U)
return false;
type = GPS_FULL;
break;
case 0x01U:
if (m_length < 17U)
return false;
type = GPS_SHORT;
break;
case 0x02U:
if (m_length < 15U)
return false;
type = GPS_VERY_SHORT;
break;
default:
return;
return true;
}
CUtils::dump("Kenwood GPS Data", m_data, m_length);
unsigned char UTCss = m_data[1U] & 0x3FU;
unsigned char UTCmm = ((m_data[1U] & 0xC0U) >> 2) | (m_data[2U] & 0x0FU);
unsigned char UTChh = ((m_data[3U] & 0x01U) << 4) | (m_data[2U] & 0xF0U) >> 4;
@ -225,7 +236,7 @@ void CGPSHandler::processKenwood()
}
if (latAfter == 0x7FFFU || latBefore == 0xFFFFU || longAfter == 0x7FFFU || longBefore == 0xFFFFU)
return;
return true;
unsigned int course = 0xFFFFU;
unsigned int speedBefore = 0x3FFU;
@ -246,8 +257,7 @@ void CGPSHandler::processKenwood()
if (course != 0xFFFFU && speedBefore != 0x3FFU && speedAfter != 0x0FU) {
::sprintf(output, "%s>APDPRS,NXDN*,qAR,%s:!%07u.%02lu%c/%08u.%02u%cr%03d/%03d via MMDVM",
source.c_str(), m_callsign.c_str(), latBefore, latAfter, north, longBefore, longAfter, east, course / 10U, speedBefore);
}
else {
} else {
::sprintf(output, "%s>APDPRS,NXDN*,qAR,%s:!%07u.%02u%c/%08u.%02u%cr via MMDVM",
source.c_str(), m_callsign.c_str(), latBefore, latAfter, north, longBefore, longAfter, east);
}
@ -255,4 +265,6 @@ void CGPSHandler::processKenwood()
LogMessage("Kenwood APRS message = %s", output);
// m_writer->write(output);
return true;
}

View File

@ -43,7 +43,7 @@ private:
std::string m_suffix;
bool processIcom();
void processKenwood();
bool processKenwood();
bool checkXOR() const;
void reset();
};

View File

@ -411,21 +411,22 @@ bool CKenwoodNetwork::read(unsigned char* data)
readRTCP(dummy);
unsigned int len = readRTP(data);
if (len > 0U) {
switch (data[9U]) {
case 0x05U: // Voice header or trailer
switch (len) {
case 0U: // Nothing received
return false;
case 35U: // Voice header or trailer
return processKenwoodVoiceHeader(data);
case 0x08U: // Voice data
case 47U: // Voice data
processKenwoodVoiceData(data);
return true;
case 31U: // Data
processKenwoodData(data);
return true;
default:
break;
}
}
CUtils::dump(5U, "Unknown data received from the Kenwood network", data, len);
return false;
}
}
unsigned int CKenwoodNetwork::readRTP(unsigned char* data)
{
@ -440,19 +441,14 @@ unsigned int CKenwoodNetwork::readRTP(unsigned char* data)
return 0U;
// Check if the data is for us
if (m_address.s_addr != address.s_addr || port != m_rtpPort) {
LogMessage("Kenwood RTP packet received from an invalid source, %08X != %08X and/or %u != %u", m_address.s_addr, address.s_addr, m_rtpPort, port);
if (m_address.s_addr != address.s_addr) {
LogMessage("Kenwood RTP packet received from an invalid source, %08X != %08X", m_address.s_addr, address.s_addr);
return 0U;
}
if (m_debug)
CUtils::dump(1U, "Kenwood Network RTP Data Received", buffer, length);
if (length != 47 && length != 59) {
LogError("Invalid RTP length of %d", length);
return 0U;
}
::memcpy(data, buffer + 12U, length - 12U);
return length - 12U;
@ -471,19 +467,14 @@ unsigned int CKenwoodNetwork::readRTCP(unsigned char* data)
return 0U;
// Check if the data is for us
if (m_address.s_addr != address.s_addr || port != m_rtcpPort) {
LogMessage("Kenwood RTCP packet received from an invalid source, %08X != %08X and/or %u != %u", m_address.s_addr, address.s_addr, m_rtcpPort, port);
if (m_address.s_addr != address.s_addr) {
LogMessage("Kenwood RTCP packet received from an invalid source, %08X != %08X", m_address.s_addr, address.s_addr);
return 0U;
}
if (m_debug)
CUtils::dump(1U, "Kenwood Network RTCP Data Received", buffer, length);
if (length != 20 && length != 28) {
LogError("Invalid RTCP length of %d", length);
return 0U;
}
if (::memcmp(buffer + 8U, "KWNE", 4U) != 0) {
LogError("Missing RTCP KWNE signature");
return 0U;
@ -548,6 +539,8 @@ bool CKenwoodNetwork::processKenwoodVoiceHeader(unsigned char* inData)
switch (outData[5U] & 0x3FU) {
case 0x01U:
::memcpy(inData, outData, 33U);
return true;
case 0x08U:
::memcpy(inData, outData, 33U);
return true;
@ -639,3 +632,51 @@ void CKenwoodNetwork::processKenwoodVoiceData(unsigned char* inData)
::memcpy(inData, outData, 33U);
}
void CKenwoodNetwork::processKenwoodData(unsigned char* inData)
{
if (inData[7U] != 0x09U && inData[7U] != 0x0BU && inData[7U] != 0x08U)
return;
unsigned char outData[50U];
if (inData[7U] == 0x09U || inData[7U] == 0x08U) {
outData[0U] = 0x90U;
outData[1U] = inData[8U];
outData[2U] = inData[7U];
outData[3U] = inData[10U];
outData[4U] = inData[9U];
outData[5U] = inData[12U];
outData[6U] = inData[11U];
::memcpy(inData, outData, 7U);
CUtils::dump(4U, "Outgoing Kenwood GPS Data Header/Trailer", inData, 7U);
} else {
CUtils::dump(4U, "Incoming Kenwood GPS Data", inData, 31U);
outData[0U] = 0x90U;
outData[1U] = inData[8U];
outData[2U] = inData[7U];
outData[3U] = inData[10U];
outData[4U] = inData[9U];
outData[5U] = inData[12U];
outData[6U] = inData[11U];
outData[7U] = inData[14U];
outData[8U] = inData[13U];
outData[9U] = inData[16U];
outData[10U] = inData[15U];
outData[11U] = inData[18U];
outData[12U] = inData[17U];
outData[13U] = inData[20U];
outData[14U] = inData[19U];
outData[15U] = inData[22U];
outData[16U] = inData[21U];
outData[17U] = inData[24U];
outData[18U] = inData[23U];
outData[19U] = inData[26U];
outData[20U] = inData[25U];
outData[21U] = inData[28U];
outData[22U] = inData[27U];
outData[23U] = inData[29U];
::memcpy(inData, outData, 24U);
CUtils::dump(4U, "Outgoing Kenwood GPS Data Body", inData, 24U);
}
}

View File

@ -59,6 +59,7 @@ private:
bool processIcomVoiceData(const unsigned char* data);
bool processKenwoodVoiceHeader(unsigned char* data);
void processKenwoodVoiceData(unsigned char* data);
void processKenwoodData(unsigned char* data);
bool writeRTPVoiceHeader(const unsigned char* data);
bool writeRTPVoiceData(const unsigned char* data);
bool writeRTPVoiceTrailer(const unsigned char* data);