Use more conventional handling of FM mode timing.

Conf.cpp

@@ -203,6 +203,7 @@ m_fmCOSInvert(false),
 m_fmRFAudioBoost(1U),
 m_fmMaxDevLevel(90.0F),
 m_fmExtAudioBoost(1U),
+m_fmModeHang(10U),
 m_dstarNetworkEnabled(false),
 m_dstarGatewayAddress(),
 m_dstarGatewayPort(0U),
@@ -411,12 +412,12 @@ bool CConf::read()
 		else if (::strcmp(key, "Duplex") == 0)
 			m_duplex = ::atoi(value) == 1;
 		else if (::strcmp(key, "ModeHang") == 0)
-			m_dstarNetworkModeHang = m_dmrNetworkModeHang = m_fusionNetworkModeHang = m_p25NetworkModeHang =
+			m_dstarNetworkModeHang = m_dmrNetworkModeHang = m_fusionNetworkModeHang = m_p25NetworkModeHang = m_nxdnNetworkModeHang = m_pocsagNetworkModeHang = m_fmNetworkModeHang =
-			m_dstarModeHang        = m_dmrModeHang        = m_fusionModeHang        = m_p25ModeHang        = (unsigned int)::atoi(value);
+			m_dstarModeHang        = m_dmrModeHang        = m_fusionModeHang        = m_p25ModeHang        = m_nxdnModeHang = m_fmModeHang = (unsigned int)::atoi(value);
 		else if (::strcmp(key, "RFModeHang") == 0)
-			m_dstarModeHang = m_dmrModeHang = m_fusionModeHang = m_p25ModeHang = (unsigned int)::atoi(value);
+			m_dstarModeHang = m_dmrModeHang = m_fusionModeHang = m_p25ModeHang = m_nxdnModeHang = m_fmModeHang = (unsigned int)::atoi(value);
 		else if (::strcmp(key, "NetModeHang") == 0)
-			m_dstarNetworkModeHang = m_dmrNetworkModeHang = m_fusionNetworkModeHang = m_p25NetworkModeHang = (unsigned int)::atoi(value);
+			m_dstarNetworkModeHang = m_dmrNetworkModeHang = m_fusionNetworkModeHang = m_p25NetworkModeHang = m_nxdnNetworkModeHang = m_pocsagNetworkModeHang = m_fmNetworkModeHang = (unsigned int)::atoi(value);
 		else if (::strcmp(key, "Display") == 0)
 			m_display = value;
 		else if (::strcmp(key, "Daemon") == 0)
@@ -774,6 +775,8 @@ bool CConf::read()
 			m_fmMaxDevLevel = float(::atof(value));
 		else if (::strcmp(key, "ExtAudioBoost") == 0)
 			m_fmExtAudioBoost = (unsigned int)::atoi(value);
+		else if (::strcmp(key, "ModeHang") == 0)
+			m_fmModeHang = (unsigned int)::atoi(value);
 	} else if (section == SECTION_DSTAR_NETWORK) {
 		if (::strcmp(key, "Enable") == 0)
 			m_dstarNetworkEnabled = ::atoi(value) == 1;
@@ -1676,6 +1679,11 @@ unsigned int CConf::getFMExtAudioBoost() const
 	return m_fmExtAudioBoost;
 }
 
+unsigned int CConf::getFMModeHang() const
+{
+	return m_fmModeHang;
+}
+
 bool CConf::getDStarNetworkEnabled() const
 {
 	return m_dstarNetworkEnabled;

Conf.h

@@ -200,6 +200,7 @@ public:
   unsigned int getFMRFAudioBoost() const;
   float        getFMMaxDevLevel() const;
   unsigned int getFMExtAudioBoost() const;
+  unsigned int getFMModeHang() const;
 
   // The D-Star Network section
   bool         getDStarNetworkEnabled() const;
@@ -476,6 +477,7 @@ private:
   unsigned int m_fmRFAudioBoost;
   float        m_fmMaxDevLevel;
   unsigned int m_fmExtAudioBoost;
+  unsigned int m_fmModeHang;
 
   bool         m_dstarNetworkEnabled;
   std::string  m_dstarGatewayAddress;

FMControl.cpp

@@ -41,18 +41,23 @@ bool CFMControl::writeModem(const unsigned char* data, unsigned int length)
     assert(data != NULL);
     assert(length > 0U);
 
-    if(m_network == NULL)
+    if (data[0U] == TAG_HEADER)
+        return true;
+
+    if (data[0U] != TAG_DATA)
         return false;
 
+    if (m_network == NULL)
+        return true;
+
     float samples[170U];
     unsigned int nSamples = 0U;
-
     
-    m_incomingRFAudio.addData(data, length);
+    m_incomingRFAudio.addData(data + 1U, length - 1U);
     unsigned int bufferLength = m_incomingRFAudio.dataSize();
 
-    if(bufferLength >= 3) {
+    if (bufferLength >= 3U) {
-        bufferLength = bufferLength - bufferLength % 3; //round down to nearest multiple of 3
+        bufferLength = bufferLength - bufferLength % 3U; //round down to nearest multiple of 3
         unsigned char bufferData[bufferLength];
         m_incomingRFAudio.getData(bufferData, bufferLength);
 
@@ -69,8 +74,8 @@ bool CFMControl::writeModem(const unsigned char* data, unsigned int length)
             packPointer[2U] = bufferData[i + 1U];
             packPointer[3U] = bufferData[i + 2U];
 
-            sample2 = (short)(pack & MASK);
+            sample2 = short(pack & MASK);
-            sample1 = (short)(pack >> 12);
+            sample1 = short(pack >> 12);
 
             // Convert from unsigned short (0 - +4095) to float (-1.0 - +1.0)
             samples[nSamples++] = (float(sample1) - 2048.0F) / 2048.0F;
@@ -94,15 +99,16 @@ bool CFMControl::writeModem(const unsigned char* data, unsigned int length)
         return m_network->write(out, nOut);
     }
 
-    return 0U;
+    return true;
 }
 
 unsigned int CFMControl::readModem(unsigned char* data, unsigned int space)
 {
     assert(data != NULL);
     assert(space > 0U);
-    if(m_network == NULL)
+
-        return 0;
+    if (m_network == NULL)
+        return 0U;
 
     unsigned char netData[300U];
     unsigned int length = m_network->read(netData, 270U);

MMDVM.ini

@@ -170,6 +170,7 @@ COSInvert=0
 RFAudioBoost=1
 MaxDevLevel=90
 ExtAudioBoost=1
+# ModeHang=10
 
 [D-Star Network]
 Enable=1

MMDVMHost.cpp

@@ -135,6 +135,7 @@ m_dmrRFModeHang(10U),
 m_ysfRFModeHang(10U),
 m_p25RFModeHang(10U),
 m_nxdnRFModeHang(10U),
+m_fmRFModeHang(10U),
 m_dstarNetModeHang(3U),
 m_dmrNetModeHang(3U),
 m_ysfNetModeHang(3U),
@@ -618,8 +619,11 @@ int CMMDVMHost::run()
 		pocsagTimer.start();
 	}
 
-	if (m_fmEnabled)
+	if (m_fmEnabled) {
+		m_fmRFModeHang = m_conf.getFMModeHang();
+
 		m_fm = new CFMControl(m_fmNetwork);
+	}
 
 	bool remoteControlEnabled = m_conf.getRemoteControlEnabled();
 	if (remoteControlEnabled) {
@@ -658,12 +662,6 @@ int CMMDVMHost::run()
 		else if (!error && m_mode == MODE_ERROR)
 			setMode(MODE_IDLE);
 
-		unsigned char mode = m_modem->getMode();
-		if (mode == MODE_FM && m_mode != MODE_FM)
-			setMode(mode);
-		else if (mode != MODE_FM && m_mode == MODE_FM)
-			setMode(mode);
-
 		if (m_ump != NULL) {
 			bool tx = m_modem->hasTX();
 			m_ump->setTX(tx);
@@ -818,7 +816,7 @@ int CMMDVMHost::run()
 			if (m_mode == MODE_IDLE) {
 				bool ret = m_fm->writeModem(data, len);
 				if (ret) {
-					m_modeTimer.setTimeout(m_nxdnRFModeHang);		// XXX
+					m_modeTimer.setTimeout(m_fmRFModeHang);
 					setMode(MODE_FM);
 				}
 			} else if (m_mode == MODE_FM) {
@@ -1293,6 +1291,7 @@ bool CMMDVMHost::createModem()
 		bool         cosInvert         = m_conf.getFMCOSInvert();
 		unsigned int rfAudioBoost      = m_conf.getFMRFAudioBoost();
 		float        maxDevLevel       = m_conf.getFMMaxDevLevel();
+		unsigned int modeHangTime      = m_conf.getFMModeHang();
 
 		LogInfo("FM Parameters");
 		LogInfo("    Callsign: %s", callsign.c_str());
@@ -1322,6 +1321,7 @@ bool CMMDVMHost::createModem()
 		LogInfo("    COS Invert: %s", cosInvert ? "yes" : "no");
 		LogInfo("    RF Audio Boost: x%u", rfAudioBoost);
 		LogInfo("    Max. Deviation Level: %.1f%%", maxDevLevel);
+		LogInfo("    Mode Hang: %us", modeHangTime);
 
 		m_modem->setFMCallsignParams(callsign, callsignSpeed, callsignFrequency, callsignTime, callsignHoldoff, callsignHighLevel, callsignLowLevel, callsignAtStart, callsignAtEnd, callsignAtLatch);
 		m_modem->setFMAckParams(rfAck, ackSpeed, ackFrequency, ackMinTime, ackDelay, ackLevel);

MMDVMHost.h

@@ -80,6 +80,7 @@ private:
   unsigned int    m_ysfRFModeHang;
   unsigned int    m_p25RFModeHang;
   unsigned int    m_nxdnRFModeHang;
+  unsigned int    m_fmRFModeHang;
   unsigned int    m_dstarNetModeHang;
   unsigned int    m_dmrNetModeHang;
   unsigned int    m_ysfNetModeHang;

Modem.cpp

@@ -81,6 +81,7 @@ const unsigned char MMDVM_FM_PARAMS2  = 0x61U;
 const unsigned char MMDVM_FM_PARAMS3  = 0x62U;
 const unsigned char MMDVM_FM_PARAMS4  = 0x63U;
 const unsigned char MMDVM_FM_DATA     = 0x65U;
+const unsigned char MMDVM_FM_CONTROL  = 0x66U;
 
 const unsigned char MMDVM_ACK         = 0x70U;
 const unsigned char MMDVM_NAK         = 0x7FU;
@@ -604,6 +605,20 @@ void CModem::clock(unsigned int ms)
 			}
 			break;
 
+			case MMDVM_FM_CONTROL: {
+				if (m_trace)
+					CUtils::dump(1U, "RX FM Control", m_buffer, m_length);
+
+				unsigned char data = m_length - 2U;
+				m_rxFMData.addData(&data, 1U);
+
+				data = TAG_HEADER;
+				m_rxFMData.addData(&data, 1U);
+
+				m_rxFMData.addData(m_buffer + 3U, m_length - 3U);
+			}
+			break;
+
 			case MMDVM_GET_STATUS: {
 					// if (m_trace)
 					//	CUtils::dump(1U, "GET_STATUS", m_buffer, m_length);
@@ -1236,7 +1251,7 @@ bool CModem::writePOCSAGData(const unsigned char* data, unsigned int length)
 
 unsigned int CModem::getFMSpace() const
 {
-	return (m_txFMData.freeSpace() * 2U) / 3U;
+	return m_txFMData.freeSpace();
 }
 
 bool CModem::writeFMData(const unsigned char* data, unsigned int length)
@@ -1244,8 +1259,6 @@ bool CModem::writeFMData(const unsigned char* data, unsigned int length)
 	assert(data != NULL);
 	assert(length > 0U);
 
-	length = (length * 2U) / 3U;
-
 	if (length > 252U)
 		return false;