Merge pull request #109 from larryho5/betaflight_scheduler_with_working_bst_2

- Fix and add BST task into scheduler, with fixed PID reading 0s prob…

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
 

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)
+#define UPDATE_AT_20HZ ((1000 * 1000) / 20)
-static uint32_t next10hzUpdateAt_1 = 0;
+static uint32_t next20hzUpdateAt_1 = 0;
-static uint32_t next10hzUpdateAt_2 = 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()) {
+		if(now >= next20hzUpdateAt_1 && !bstWriteBusy()) {
-			writeRCChannelToBST();
+			if(sendCounter == 0)
-			next10hzUpdateAt_1 = now + UPDATE_AT_10HZ;
+				writeRCChannelToBST();
-		}
+			else if(sendCounter == 1)
-		if(now >= next10hzUpdateAt_2 && !bstWriteBusy()) {
+				writeRollPitchYawToBST();
-			writeRollPitchYawToBST();
+			sendCounter++;
-			next10hzUpdateAt_2 = now + UPDATE_AT_10HZ;
+			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];

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);

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();
     }
 }

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] = {
+    [TASK_BST_READ_WRITE] = {
-        .taskName = "BST_PROCESS",
+        .taskName = "BST_MASTER_WRITE",
-        .taskFunc = taskBstProcess,
+        .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,
     },

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 */