From f26144db723d90fc0699e411a031df9c6b5ddc45 Mon Sep 17 00:00:00 2001
From: jflyper <shinoda@jaist.ac.jp>
Date: Tue, 2 May 2017 22:17:03 +0900
Subject: [PATCH] Support SmartAudio V1

---
 src/main/cms/cms.c           |  7 +++--
 src/main/io/vtx_smartaudio.c | 50 +++++++++++++++++++++++++++++-------
 2 files changed, 46 insertions(+), 11 deletions(-)

diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index a1ac4c685..0edb7500a 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -494,6 +494,8 @@ static void cmsMenuCountPage(displayPort_t *pDisplay)
     pageCount = (p - currentCtx.menu->entries - 1) / MAX_MENU_ITEMS(pDisplay) + 1;
 }
 
+STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay); // Forward; will be resolved after merging
+
 long cmsMenuChange(displayPort_t *pDisplay, const void *ptr)
 {
     CMS_Menu *pMenu = (CMS_Menu *)ptr;
@@ -522,8 +524,9 @@ long cmsMenuChange(displayPort_t *pDisplay, const void *ptr)
         currentCtx.menu = pMenu;
         currentCtx.cursorRow = 0;
 
-        if (pMenu->onEnter)
-            pMenu->onEnter();
+        if (pMenu->onEnter && (pMenu->onEnter() == MENU_CHAIN_BACK)) {
+            return cmsMenuBack(pDisplay);
+        }
 
         cmsMenuCountPage(pDisplay);
         cmsPageSelect(pDisplay, 0);
diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
index 548912dbb..6afea91fa 100644
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@@ -695,7 +695,7 @@ bool vtxSmartAudioInit()
 
 void vtxSAProcess(uint32_t now)
 {
-    static bool initialSent = false;
+    static char initPhase = 0;
 
     if (smartAudioSerialPort == NULL)
         return;
@@ -708,12 +708,20 @@ void vtxSAProcess(uint32_t now)
     // Re-evaluate baudrate after each frame reception
     saAutobaud();
 
-    if (!initialSent) {
+    switch (initPhase) {
+    case 0:
         saGetSettings();
-        saSetFreq(SA_FREQ_GETPIT);
         saSendQueue();
-        initialSent = true;
+        ++initPhase;
         return;
+
+    case 1:
+        // Don't send SA_FREQ_GETPIT to V1 device; it act as plain SA_CMD_SET_FREQ,
+        // and put the device into user frequency mode with uninitialized freq.
+        if (saDevice.version == 2)
+            saSetFreq(SA_FREQ_GETPIT);
+        ++initPhase;
+        break;
     }
 
     if ((sa_outstanding != SA_CMD_NONE)
@@ -876,7 +884,7 @@ uint16_t saCmsDeviceFreq = 0;
 
 uint8_t  saCmsDeviceStatus = 0;
 uint8_t  saCmsPower;
-uint8_t  saCmsPitFMode;         // In-Range or Out-Range
+uint8_t  saCmsPitFMode;          // Undef(0), In-Range(1) or Out-Range(2)
 uint8_t  saCmsFselMode;          // Channel(0) or User defined(1)
 
 uint16_t saCmsORFreq = 0;       // POR frequency
@@ -942,10 +950,12 @@ if (saCmsORFreq == 0 && saDevice.orfreq != 0)
 if (saCmsUserFreq == 0 && saDevice.freq != 0)
     saCmsUserFreq = saDevice.freq;
 
-if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
-    saCmsPitFMode = 1;
-else
-    saCmsPitFMode = 0;
+if (saDevice.version == 2) {
+    if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
+        saCmsPitFMode = 1;
+    else
+        saCmsPitFMode = 0;
+}
 
     saCmsStatusString[0] = "-FR"[saCmsOpmodel];
 
@@ -1060,9 +1070,21 @@ static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self)
     UNUSED(pDisp);
     UNUSED(self);
 
+    if (saDevice.version == 1) {
+        // V1 device doesn't support PIT mode; bounce back.
+        saCmsPitFMode = 0;
+        return 0;
+    }
+
     dprintf(("saCmsConfigPitFmodeByGbar: saCmsPitFMode %d\r\n", saCmsPitFMode));
 
     if (saCmsPitFMode == 0) {
+        // Bounce back
+        saCmsPitFMode = 1;
+        return 0;
+    }
+
+    if (saCmsPitFMode == 1) {
         saSetMode(SA_MODE_SET_IN_RANGE_PITMODE);
     } else {
         saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE);
@@ -1078,6 +1100,12 @@ static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self)
     UNUSED(pDisp);
     UNUSED(self);
 
+    if (saDevice.version == 1) {
+        if (saCmsOpmodel != SACMS_OPMODEL_FREE)
+            saCmsOpmodel = SACMS_OPMODEL_FREE;
+        return 0;
+    }
+
     uint8_t opmodel = saCmsOpmodel;
 
     dprintf(("saCmsConfigOpmodelByGvar: opmodel %d\r\n", opmodel));
@@ -1163,6 +1191,7 @@ static const char * const saCmsFselModeNames[] = {
 };
 
 static const char * const saCmsPitFModeNames[] = {
+    "---",
     "PIR",
     "POR"
 };
@@ -1227,6 +1256,9 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self)
 
 static long saCmsSetPORFreqOnEnter(void)
 {
+    if (saDevice.version == 1)
+        return MENU_CHAIN_BACK;
+
     saCmsORFreqNew = saCmsORFreq;
 
     return 0;