From 0f7e576c45de21b6f121df5ee6c740b7fdb744fd Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Tue, 30 Nov 2021 20:50:00 +0000
Subject: [PATCH] Give options on the method to use for smartaudio over
 softserial

Initial code.
---
 docs/Settings.md             | 10 ++++++++++
 src/main/fc/settings.yaml    |  5 +++++
 src/main/io/vtx_control.c    |  1 +
 src/main/io/vtx_control.h    |  1 +
 src/main/io/vtx_smartaudio.c | 10 +++++++---
 5 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index d5409bfbb9..287c379a9b 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -5712,6 +5712,16 @@ VTX RF power level to use. The exact number of mw depends on the VTX hardware.
 
 ---
 
+### vtx_smartaudio_alternate_softserial_method
+
+Enable the alternate softserial method. This is the method used in iNav 3.0 and ealier.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| ON |  |  |
+
+---
+
 ### vtx_smartaudio_early_akk_workaround
 
 Enable workaround for early AKK SAudio-enabled VTX bug.
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 22dd0335b1..51131e4bbe 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3601,6 +3601,11 @@ groups:
         default_value: ON
         field: smartAudioEarlyAkkWorkaroundEnable
         type: bool
+      - name: vtx_smartaudio_alternate_softserial_method
+        description: "Enable the alternate softserial method. This is the method used in iNav 3.0 and ealier."
+        default_value: ON
+        field: smartAudioAltSoftSerialMethod
+        type: bool
 
   - name: PG_VTX_SETTINGS_CONFIG
     type: vtxSettingsConfig_t
diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c
index 9a3c2ef633..516b78ab06 100644
--- a/src/main/io/vtx_control.c
+++ b/src/main/io/vtx_control.c
@@ -45,6 +45,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 3);
 PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig,
       .halfDuplex = SETTING_VTX_HALFDUPLEX_DEFAULT,
       .smartAudioEarlyAkkWorkaroundEnable = SETTING_VTX_SMARTAUDIO_EARLY_AKK_WORKAROUND_DEFAULT,
+      .smartAudioAltSoftSerialMethod = SETTING_VTX_SMARTAUDIO_ALTERNATE_SOFTSERIAL_METHOD_DEFAULT,
 );
 
 static uint8_t locked = 0;
diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h
index 1e0d4c96f1..b474a4bc0b 100644
--- a/src/main/io/vtx_control.h
+++ b/src/main/io/vtx_control.h
@@ -32,6 +32,7 @@ typedef struct vtxConfig_s {
     vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT];
     uint8_t halfDuplex;
     uint8_t smartAudioEarlyAkkWorkaroundEnable;
+    bool    smartAudioAltSoftSerialMethod;
 } vtxConfig_t;
 
 PG_DECLARE(vtxConfig_t, vtxConfig);
diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
index 6e0bf8e695..3ee412ff27 100644
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@@ -492,9 +492,13 @@ static void saReceiveFramer(uint8_t c)
 
 static void saSendFrame(uint8_t *buf, int len)
 {
-    // TBS SA definition requires that the line is low before frame is sent
-    // (for both soft and hard serial). It can be done by sending first 0x00
-    serialWrite(smartAudioSerialPort, 0x00);
+    if ( (vtxConfig()->smartAudioAltSoftSerialMethod && 
+          (smartAudioSerialPort->identifier == SERIAL_PORT_SOFTSERIAL1 || smartAudioSerialPort->identifier == SERIAL_PORT_SOFTSERIAL2))
+         == false) {
+        // TBS SA definition requires that the line is low before frame is sent
+        // (for both soft and hard serial). It can be done by sending first 0x00
+        serialWrite(smartAudioSerialPort, 0x00);
+    }
 
     for (int i = 0 ; i < len ; i++) {
         serialWrite(smartAudioSerialPort, buf[i]);