diff --git a/Modem.cpp b/Modem.cpp index b24bf99..c6bab00 100644 --- a/Modem.cpp +++ b/Modem.cpp @@ -1044,10 +1044,18 @@ bool CModem::setConfig() bool CModem::setFrequency() { unsigned char buffer[16U]; + unsigned char len; + + if (m_hwType == HWT_DVMEGA) + len = 12U; + else { + buffer[12U] = (unsigned char)(m_rfLevel * 2.55F + 0.5F); + len = 13U; + } buffer[0U] = MMDVM_FRAME_START; - buffer[1U] = 13U; + buffer[1U] = len; buffer[2U] = MMDVM_SET_FREQ; @@ -1062,13 +1070,11 @@ bool CModem::setFrequency() buffer[9U] = (m_txFrequency >> 8) & 0xFFU; buffer[10U] = (m_txFrequency >> 16) & 0xFFU; buffer[11U] = (m_txFrequency >> 24) & 0xFFU; - - buffer[12U] = (unsigned char)(m_rfLevel * 2.55F + 0.5F); - // CUtils::dump(1U, "Written", buffer, 13U); + // CUtils::dump(1U, "Written", buffer, len); - int ret = m_serial.write(buffer, 13U); - if (ret != 13) + int ret = m_serial.write(buffer, len); + if (ret != len) return false; unsigned int count = 0U;