diff --git a/src/main/io/i2c_bst.c b/src/main/io/i2c_bst.c index 66dac507e1..8d581a99c3 100644 --- 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 index 45dd68cabd..e005738df4 100644 --- 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 index 5ec52284ea..f10b210b2e 100644 --- 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 index b23f0c05fb..31fea83ae3 100755 --- 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 index 7e1e5062bc..3c79f9cd36 100755 --- 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,