From 05b43dfa1ce5769b6ef42e02e9d37b852dd8776c Mon Sep 17 00:00:00 2001
From: "Ben V. Brown" <ralim@ralimtek.com>
Date: Sat, 30 May 2020 12:43:32 +1000
Subject: [PATCH] Clean up power calls

---
 workspace/TS100/Core/BSP/BSP.h                |  1 +
 workspace/TS100/Core/BSP/BSP_Power.h          | 27 +++++++++++++++++++
 workspace/TS100/Core/BSP/Miniware/Power.cpp   | 20 ++++++++++++++
 .../TS100/Core/BSP/Miniware/postRTOS.cpp      | 12 +--------
 workspace/TS100/Core/Threads/MOVThread.cpp    |  4 ++-
 5 files changed, 52 insertions(+), 12 deletions(-)
 create mode 100644 workspace/TS100/Core/BSP/BSP_Power.h
 create mode 100644 workspace/TS100/Core/BSP/Miniware/Power.cpp

diff --git a/workspace/TS100/Core/BSP/BSP.h b/workspace/TS100/Core/BSP/BSP.h
index cbf28ceb..a558baad 100644
--- a/workspace/TS100/Core/BSP/BSP.h
+++ b/workspace/TS100/Core/BSP/BSP.h
@@ -1,4 +1,5 @@
 #include "BSP_Flash.h"
+#include "BSP_Power.h"
 #include "BSP_QC.h"
 #include "Defines.h"
 #include "UnitSettings.h"
diff --git a/workspace/TS100/Core/BSP/BSP_Power.h b/workspace/TS100/Core/BSP/BSP_Power.h
new file mode 100644
index 00000000..1ce7f2bf
--- /dev/null
+++ b/workspace/TS100/Core/BSP/BSP_Power.h
@@ -0,0 +1,27 @@
+#include "stdint.h"
+/*
+ * BSP_Power.h -- Board Support for Power control
+ *
+ * These functions are hooks used to allow for power control
+ *
+ */
+
+#ifndef BSP_POWER_H_
+#define BSP_POWER_H_
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Called once at startup, after RToS
+// This can handle negotiations for QC/PD etc
+void power_probe();
+
+// Called periodically in the movement handling thread
+// Can be used to check any details for the power system
+void power_check();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/workspace/TS100/Core/BSP/Miniware/Power.cpp b/workspace/TS100/Core/BSP/Miniware/Power.cpp
new file mode 100644
index 00000000..9abe8008
--- /dev/null
+++ b/workspace/TS100/Core/BSP/Miniware/Power.cpp
@@ -0,0 +1,20 @@
+#include "BSP.h"
+#include "BSP_Power.h"
+#include "QC3.h"
+void power_probe() {
+// If TS80 probe for QC
+// If TS100 - noop
+#ifdef MODEL_TS80
+    startQC(systemSettings.voltageDiv);
+
+    seekQC((systemSettings.cutoutSetting) ? 120 : 90,
+           systemSettings.voltageDiv);  // this will move the QC output to the preferred voltage to start with
+
+#endif
+}
+
+void power_check() {
+#ifdef MODEL_TS80
+    QC_resync();
+#endif
+}
\ No newline at end of file
diff --git a/workspace/TS100/Core/BSP/Miniware/postRTOS.cpp b/workspace/TS100/Core/BSP/Miniware/postRTOS.cpp
index 005b930a..0c262a95 100644
--- a/workspace/TS100/Core/BSP/Miniware/postRTOS.cpp
+++ b/workspace/TS100/Core/BSP/Miniware/postRTOS.cpp
@@ -9,15 +9,5 @@
 #include "task.h"
 
 void postRToSInit() {
-#ifdef MODEL_TS80
-    startQC(systemSettings.voltageDiv);
-    while (pidTaskNotification == 0) osDelay(30);  // Wait for PID to start
-
-    seekQC((systemSettings.cutoutSetting) ? 120 : 90,
-           systemSettings.voltageDiv);  // this will move the QC output to the preferred voltage to start with
-
-#else
-    while (pidTaskNotification == 0) osDelay(30);  // Wait for PID to start
-    osDelay(200);                                  // wait for accelerometer to stabilize
-#endif
+    // Any after RTos setup
 }
diff --git a/workspace/TS100/Core/Threads/MOVThread.cpp b/workspace/TS100/Core/Threads/MOVThread.cpp
index 08fc89cd..e88cefce 100644
--- a/workspace/TS100/Core/Threads/MOVThread.cpp
+++ b/workspace/TS100/Core/Threads/MOVThread.cpp
@@ -25,6 +25,8 @@ uint32_t lastMovementTime = 0;
 void startMOVTask(void const *argument __unused) {
     OLED::setRotation(true);
     postRToSInit();
+    power_probe();
+    while (pidTaskNotification == 0) osDelay(30);  // Wait for PID to start
 
     OLED::setRotation(systemSettings.OrientationMode & 1);
     lastMovementTime = 0;
@@ -86,6 +88,6 @@ void startMOVTask(void const *argument __unused) {
         }
 
         osDelay(100);  // Slow down update rate
-        QC_resync();
+        power_check();
     }
 }