- Fix and add BST task into scheduler.

betaflight · Jan 5, 2016 · cad401d · cad401d
1 parent 560bd74
commit cad401d
Show file tree

Hide file tree

Showing 5 changed files with 22 additions and 2 deletions.
diff --git a/src/main/io/i2c_bst.c b/src/main/io/i2c_bst.c
@@ -1532,7 +1532,7 @@ static uint32_t next02hzUpdateAt_1 = 0;
 static uint32_t next10hzUpdateAt_1 = 0;
 static uint32_t next10hzUpdateAt_2 = 0;
 
-void bstProcess(void)
+void taskBstProcess(void)
 {
 	if(coreProReady) {
 		uint32_t now = micros();
@@ -1548,6 +1548,7 @@ void bstProcess(void)
 			writeRollPitchYawToBST();
 			next10hzUpdateAt_2 = now + UPDATE_AT_10HZ;
 		}
+
 		if(sensors(SENSOR_GPS) && !bstWriteBusy())
 			writeGpsPositionPrameToBST();
 	}

diff --git a/src/main/io/i2c_bst.h b/src/main/io/i2c_bst.h
@@ -19,7 +19,7 @@
 
 #include "drivers/bus_bst.h"
 
-void bstProcess(void);
+void taskBstProcess(void);
 
 //void writeGpsPositionPrameToBST(void);
 //void writeGPSTimeFrameToBST(void);

diff --git a/src/main/main.c b/src/main/main.c
@@ -577,9 +577,15 @@ int main(void) {
 #ifdef LED_STRIP
     setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
 #endif
+#ifdef USE_BST
+    setTaskEnabled(TASK_BST_PROCESS, true);
+#endif
 
     while (1) {
         scheduler();
+#ifdef USE_BST
+	   bstMasterWriteLoop();
+#endif
         processLoopback();
     }
 }

diff --git a/src/main/scheduler.c b/src/main/scheduler.c
@@ -78,6 +78,7 @@ void taskUpdateDisplay(void);
 void taskTelemetry(void);
 void taskLedStrip(void);
 void taskSystem(void);
+void taskBstProcess(void);
 
 static cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_SYSTEM] = {
@@ -202,6 +203,15 @@ static cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_IDLE,
     },
 #endif
+
+#ifdef USE_BST
+    [TASK_BST_PROCESS] = {
+        .taskName = "BST_PROCESS",
+        .taskFunc = taskBstProcess,
+        .desiredPeriod = 1000000 / 50,          // 50 Hz
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
 };
 
 #define REALTIME_GUARD_INTERVAL_MIN     10

diff --git a/src/main/scheduler.h b/src/main/scheduler.h
@@ -70,6 +70,9 @@ typedef enum {
 #ifdef LED_STRIP
     TASK_LEDSTRIP,
 #endif
+#ifdef USE_BST
+    TASK_BST_PROCESS,
+#endif
 
     /* Count of real tasks */
     TASK_COUNT,