From 6f3d576802c43758f487d60850b0e8971b2d2de6 Mon Sep 17 00:00:00 2001
From: "Larry (TBS)" <larryho5@gmail.com>
Date: Tue, 12 Jan 2016 13:30:55 +0800
Subject: [PATCH] - Fix and add BST task into scheduler, with fixed PID reading
 0s problem.

---
 src/main/config/config.c |  2 +-
 src/main/io/i2c_bst.c    | 43 +++++++++++++++++++++++++++++-----------
 src/main/io/i2c_bst.h    |  4 +++-
 src/main/main.c          |  6 ++----
 src/main/scheduler.c     | 16 +++++++++++----
 src/main/scheduler.h     |  3 ++-
 6 files changed, 51 insertions(+), 23 deletions(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index bef6d506d..1fd0f2731 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -895,7 +895,7 @@ void validateAndFixConfig(void)
     }
     if(featureConfigured(FEATURE_RX_SERIAL)) {
 	    masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
-	    masterConfig.rxConfig.serialrx_provider = SERIALRX_SBUS;
+	    //masterConfig.rxConfig.serialrx_provider = SERIALRX_SBUS;
     }
 #endif
 
diff --git a/src/main/io/i2c_bst.c b/src/main/io/i2c_bst.c
index 5597d2fac..6b1328082 100644
--- a/src/main/io/i2c_bst.c
+++ b/src/main/io/i2c_bst.c
@@ -1485,9 +1485,11 @@ static bool bstSlaveUSBCommandFeedback(/*uint8_t bstFeedback*/)
 }
 
 /*************************************************************************************************/
+bool slaveModeOn = false;
 static void bstSlaveProcessInCommand(void)
 {
 	if(bstSlaveRead(readData)) {
+		slaveModeOn = true;
 		readBufferPointer = 1;
 		//Check if the CRC match
 		if(bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) {
@@ -1515,6 +1517,8 @@ static void bstSlaveProcessInCommand(void)
 					break;
 			}
 		}
+	} else {
+		slaveModeOn = false;
 	}
 }
 
@@ -1522,11 +1526,12 @@ static void bstSlaveProcessInCommand(void)
 #define UPDATE_AT_02HZ ((1000 * 1000) / 2)
 static uint32_t next02hzUpdateAt_1 = 0;
 
-#define UPDATE_AT_10HZ ((1000 * 1000) / 10)
-static uint32_t next10hzUpdateAt_1 = 0;
-static uint32_t next10hzUpdateAt_2 = 0;
+#define UPDATE_AT_20HZ ((1000 * 1000) / 20)
+static uint32_t next20hzUpdateAt_1 = 0;
 
-void taskBstProcess(void)
+static uint8_t sendCounter = 0;
+
+void taskBstMasterProcess(void)
 {
 	if(coreProReady) {
 		uint32_t now = micros();
@@ -1534,21 +1539,27 @@ void taskBstProcess(void)
 			writeFCModeToBST();
 			next02hzUpdateAt_1 = now + UPDATE_AT_02HZ;
 		}
-		if(now >= next10hzUpdateAt_1 && !bstWriteBusy()) {
-			writeRCChannelToBST();
-			next10hzUpdateAt_1 = now + UPDATE_AT_10HZ;
-		}
-		if(now >= next10hzUpdateAt_2 && !bstWriteBusy()) {
-			writeRollPitchYawToBST();
-			next10hzUpdateAt_2 = now + UPDATE_AT_10HZ;
+		if(now >= next20hzUpdateAt_1 && !bstWriteBusy()) {
+			if(sendCounter == 0)
+				writeRCChannelToBST();
+			else if(sendCounter == 1)
+				writeRollPitchYawToBST();
+			sendCounter++;
+			if(sendCounter > 1)
+				sendCounter = 0;
+			next20hzUpdateAt_1 = now + UPDATE_AT_20HZ;
 		}
 
 		if(sensors(SENSOR_GPS) && !bstWriteBusy())
 			writeGpsPositionPrameToBST();
 	}
+}
+
+void taskBstCheckCommand(void) 
+{
 	//Check if the BST input command available to out address
 	bstSlaveProcessInCommand();
-	
+
 	if (isRebootScheduled) {
 		stopMotors();
 		handleOneshotFeatureChangeOnRestart();
@@ -1556,6 +1567,14 @@ void taskBstProcess(void)
 	}
 }
 
+void bstMasterWriteLoop(void);
+void taskBstReadWrite(void)
+{
+	taskBstCheckCommand();
+	if(!slaveModeOn)
+		bstMasterWriteLoop();
+}
+
 /*************************************************************************************************/
 static uint8_t masterWriteBufferPointer;
 static uint8_t masterWriteData[DATA_BUFFER_SIZE];
diff --git a/src/main/io/i2c_bst.h b/src/main/io/i2c_bst.h
index e005738df..366406d69 100644
--- a/src/main/io/i2c_bst.h
+++ b/src/main/io/i2c_bst.h
@@ -19,7 +19,9 @@
 
 #include "drivers/bus_bst.h"
 
-void taskBstProcess(void);
+void taskBstReadWrite(void);
+void taskBstMasterProcess(void);
+void taskBstCheckCommand(void);
 
 //void writeGpsPositionPrameToBST(void);
 //void writeGPSTimeFrameToBST(void);
diff --git a/src/main/main.c b/src/main/main.c
index db227f6db..82ce75cd2 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -588,14 +588,12 @@ int main(void) {
     setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
 #endif
 #ifdef USE_BST
-    setTaskEnabled(TASK_BST_PROCESS, true);
+    setTaskEnabled(TASK_BST_READ_WRITE, true);
+    setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
 #endif
 
     while (1) {
         scheduler();
-#ifdef USE_BST
-	   bstMasterWriteLoop();
-#endif
         processLoopback();
     }
 }
diff --git a/src/main/scheduler.c b/src/main/scheduler.c
index 2d39cf006..d3ea2fa25 100755
--- a/src/main/scheduler.c
+++ b/src/main/scheduler.c
@@ -79,7 +79,8 @@ void taskTelemetry(void);
 void taskLedStrip(void);
 void taskSystem(void);
 #ifdef USE_BST
-void taskBstProcess(void);
+void taskBstReadWrite(void);
+void taskBstMasterProcess(void);
 #endif
 
 static cfTask_t cfTasks[TASK_COUNT] = {
@@ -207,9 +208,16 @@ static cfTask_t cfTasks[TASK_COUNT] = {
 #endif
 
 #ifdef USE_BST
-    [TASK_BST_PROCESS] = {
-        .taskName = "BST_PROCESS",
-        .taskFunc = taskBstProcess,
+    [TASK_BST_READ_WRITE] = {
+        .taskName = "BST_MASTER_WRITE",
+        .taskFunc = taskBstReadWrite,
+        .desiredPeriod = 1000000 / 500,         // 500 Hz
+        .staticPriority = TASK_PRIORITY_HIGH,
+    },
+
+    [TASK_BST_MASTER_PROCESS] = {
+        .taskName = "BST_MASTER_PROCESS",
+        .taskFunc = taskBstMasterProcess,
         .desiredPeriod = 1000000 / 50,          // 50 Hz
         .staticPriority = TASK_PRIORITY_IDLE,
     },
diff --git a/src/main/scheduler.h b/src/main/scheduler.h
index 3c79f9cd3..1796c5f1c 100755
--- a/src/main/scheduler.h
+++ b/src/main/scheduler.h
@@ -71,7 +71,8 @@ typedef enum {
     TASK_LEDSTRIP,
 #endif
 #ifdef USE_BST
-    TASK_BST_PROCESS,
+    TASK_BST_READ_WRITE,
+    TASK_BST_MASTER_PROCESS,
 #endif
 
     /* Count of real tasks */