diff --git a/YSFGateway/WiresX.cpp b/YSFGateway/WiresX.cpp index b7d9380..8013f7a 100644 --- a/YSFGateway/WiresX.cpp +++ b/YSFGateway/WiresX.cpp @@ -200,11 +200,11 @@ WX_STATUS CWiresX::process(const unsigned char* data, const unsigned char* sourc if (!valid) return WXS_NONE; } else { - bool valid = payload.readDataFRModeData1(data, m_command + (fn - 1U) * 20U + 0U); + bool valid = payload.readDataFRModeData1(data, m_command + (fn - 2U) * 40U + 20U); if (!valid) return WXS_NONE; - valid = payload.readDataFRModeData2(data, m_command + (fn - 1U) * 20U + 20U); + valid = payload.readDataFRModeData2(data, m_command + (fn - 2U) * 40U + 40U); if (!valid) return WXS_NONE; } @@ -213,7 +213,8 @@ WX_STATUS CWiresX::process(const unsigned char* data, const unsigned char* sourc bool valid = false; // Find the end marker - for (unsigned int i = fn * 20U; i > 0U; i--) { + unsigned int cmd_len = (fn - 1U) * 40U + 20U; + for (unsigned int i = cmd_len; i > 0U; i--) { if (m_command[i] == 0x03U) { unsigned char crc = CCRC::addCRC(m_command, i + 1U); if (crc == m_command[i + 1U]) @@ -237,7 +238,7 @@ WX_STATUS CWiresX::process(const unsigned char* data, const unsigned char* sourc processDisconnect(source); return WXS_DISCONNECT; } else { - CUtils::dump("Unknown Wires-X command", m_command, fn * 20U); + CUtils::dump("Unknown Wires-X command", m_command, cmd_len); return WXS_NONE; } }