Pull out more QC hooks

workspace/TS100/Core/BSP/BSP.h

@@ -1,8 +1,8 @@
-#include "Defines.h"
-#include "stdint.h"
-#include "UnitSettings.h"
-#include "BSP_QC.h"
 #include "BSP_Flash.h"
+#include "BSP_QC.h"
+#include "Defines.h"
+#include "UnitSettings.h"
+#include "stdint.h"
 /*
  * BSP.h -- Board Support
  *
@@ -15,20 +15,20 @@
 extern "C" {
 #endif
 
-//Called first thing in main() to init the hardware
+// Called first thing in main() to init the hardware
 void preRToSInit();
-//Called once the RToS has started for any extra work
+// Called once the RToS has started for any extra work
 void postRToSInit();
 
 // Called to reset the hardware watchdog unit
 void resetWatchdog();
-//Accepts a output level of 0.. to use to control the tip output PWM
+// Accepts a output level of 0.. to use to control the tip output PWM
 void setTipPWM(uint8_t pulse);
-//Returns the Handle temp in C, X10
+// Returns the Handle temp in C, X10
 uint16_t getHandleTemperature();
-//Returns the Tip temperature ADC reading in raw units
+// Returns the Tip temperature ADC reading in raw units
 uint16_t getTipRawTemp(uint8_t refresh);
-//Returns the main DC input voltage, using the adjustable divisor + sample flag
+// Returns the main DC input voltage, using the adjustable divisor + sample flag
 uint16_t getInputVoltageX10(uint16_t divisor, uint8_t sample);
 
 // Readers for the two buttons
@@ -40,11 +40,11 @@ uint8_t getButtonB();
 // This should toggle the SCL line until SDA goes high to end the current transaction
 void unstick_I2C();
 
-//Reboot the IC when things go seriously wrong
+// Reboot the IC when things go seriously wrong
 void reboot();
 
-//If the user has programmed in a bootup logo, draw it to the screen from flash
+// If the user has programmed in a bootup logo, draw it to the screen from flash
-//Returns 1 if the logo was printed so that the unit waits for the timeout or button
+// Returns 1 if the logo was printed so that the unit waits for the timeout or button
 uint8_t showBootLogoIfavailable();
 
 void delay_ms(uint16_t count);

workspace/TS100/Core/BSP/BSP_QC.h

@@ -32,6 +32,9 @@ void QC_Post_Probe_En();
 // Check if DM was pulled down
 // 1=Pulled down, 0 == pulled high
 uint8_t QC_DM_PulledDown();
+
+// Re-sync if required
+void QC_resync();
 #ifdef __cplusplus
 }
 #endif

workspace/TS100/Core/BSP/Miniware/QC_GPIO.cpp

@@ -6,6 +6,8 @@
  */
 #include "BSP.h"
 #include "Pins.h"
+#include "QC3.h"
+#include "Settings.h"
 #include "stm32f1xx_hal.h"
 void QC_DPlusZero_Six() {
     HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET);  // pull down D+
@@ -63,3 +65,10 @@ void QC_Post_Probe_En() {
 }
 
 uint8_t QC_DM_PulledDown() { return HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_11) == GPIO_PIN_RESET ? 1 : 0; }
+
+void QC_resync() {
+#ifdef MODEL_TS80
+    seekQC((systemSettings.cutoutSetting) ? 120 : 90,
+           systemSettings.voltageDiv);  // Run the QC seek again if we have drifted too much
+#endif
+}

workspace/TS100/Core/BSP/Miniware/postRTOS.cpp

@@ -0,0 +1,23 @@
+#include "BSP.h"
+#include "FreeRTOS.h"
+#include "QC3.h"
+#include "Settings.h"
+#include "cmsis_os.h"
+#include "main.hpp"
+#include "power.hpp"
+#include "stdlib.h"
+#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
+}

workspace/TS100/Core/Threads/MOVThread.cpp

@@ -5,105 +5,87 @@
  *      Author: Ralim
  */
 
-#include "main.hpp"
 #include "BSP.h"
-#include "power.hpp"
+#include "FreeRTOS.h"
-#include "history.hpp"
+#include "I2C_Wrapper.hpp"
+#include "LIS2DH12.hpp"
+#include "MMA8652FC.hpp"
+#include "QC3.h"
+#include "Settings.h"
 #include "TipThermoModel.h"
 #include "cmsis_os.h"
-#include "FreeRTOS.h"
+#include "history.hpp"
-#include "task.h"
+#include "main.hpp"
-#include "Settings.h"
+#include "power.hpp"
-#include "I2C_Wrapper.hpp"
 #include "stdlib.h"
-#include "LIS2DH12.hpp"
+#include "task.h"
-#include <MMA8652FC.hpp>
-#include "QC3.h"
 #define MOVFilter 8
 uint8_t accelInit = 0;
 uint32_t lastMovementTime = 0;
 void startMOVTask(void const *argument __unused) {
-	OLED::setRotation(true);
+    OLED::setRotation(true);
+    postRToSInit();
 
-#ifdef MODEL_TS80
+    OLED::setRotation(systemSettings.OrientationMode & 1);
-	startQC(systemSettings.voltageDiv);
+    lastMovementTime = 0;
-	while (pidTaskNotification == 0)
+    int16_t datax[MOVFilter] = {0};
-		osDelay(30);  // To ensure we return after idealQCVoltage/tip resistance
+    int16_t datay[MOVFilter] = {0};
+    int16_t dataz[MOVFilter] = {0};
+    uint8_t currentPointer = 0;
+    int16_t tx = 0, ty = 0, tz = 0;
+    int32_t avgx, avgy, avgz;
+    if (systemSettings.sensitivity > 9) systemSettings.sensitivity = 9;
+    Orientation rotation = ORIENTATION_FLAT;
+    for (;;) {
+        int32_t threshold = 1500 + (9 * 200);
+        threshold -= systemSettings.sensitivity * 200;  // 200 is the step size
 
-	seekQC((systemSettings.cutoutSetting) ? 120 : 90,
+        if (PCBVersion == 2) {
-			systemSettings.voltageDiv); // this will move the QC output to the preferred voltage to start with
+            LIS2DH12::getAxisReadings(tx, ty, tz);
+            rotation = LIS2DH12::getOrientation();
+        } else if (PCBVersion == 1) {
+            MMA8652FC::getAxisReadings(tx, ty, tz);
+            rotation = MMA8652FC::getOrientation();
+        }
+        if (systemSettings.OrientationMode == 2) {
+            if (rotation != ORIENTATION_FLAT) {
+                OLED::setRotation(rotation == ORIENTATION_LEFT_HAND);  // link the data through
+            }
+        }
+        datax[currentPointer] = (int32_t)tx;
+        datay[currentPointer] = (int32_t)ty;
+        dataz[currentPointer] = (int32_t)tz;
+        if (!accelInit) {
+            for (uint8_t i = currentPointer + 1; i < MOVFilter; i++) {
+                datax[i] = (int32_t)tx;
+                datay[i] = (int32_t)ty;
+                dataz[i] = (int32_t)tz;
+            }
+            accelInit = 1;
+        }
+        currentPointer = (currentPointer + 1) % MOVFilter;
+        avgx = avgy = avgz = 0;
+        // calculate averages
+        for (uint8_t i = 0; i < MOVFilter; i++) {
+            avgx += datax[i];
+            avgy += datay[i];
+            avgz += dataz[i];
+        }
+        avgx /= MOVFilter;
+        avgy /= MOVFilter;
+        avgz /= MOVFilter;
 
-#else
+        // Sum the deltas
-	osDelay(250);  // wait for accelerometer to stabilize
+        int32_t error = (abs(avgx - tx) + abs(avgy - ty) + abs(avgz - tz));
-#endif
+        // So now we have averages, we want to look if these are different by more
+        // than the threshold
 
-	OLED::setRotation(systemSettings.OrientationMode & 1);
+        // If error has occurred then we update the tick timer
-	lastMovementTime = 0;
+        if (error > threshold) {
-	int16_t datax[MOVFilter] = { 0 };
+            lastMovementTime = xTaskGetTickCount();
-	int16_t datay[MOVFilter] = { 0 };
+        }
-	int16_t dataz[MOVFilter] = { 0 };
-	uint8_t currentPointer = 0;
-	int16_t tx = 0, ty = 0, tz = 0;
-	int32_t avgx = 0, avgy = 0, avgz = 0;
-	if (systemSettings.sensitivity > 9)
-		systemSettings.sensitivity = 9;
-#ifdef ACCELDEBUG
-	uint32_t max = 0;
-#endif
-	Orientation rotation = ORIENTATION_FLAT;
-	for (;;) {
-		int32_t threshold = 1500 + (9 * 200);
-		threshold -= systemSettings.sensitivity * 200;  // 200 is the step size
 
-		if (PCBVersion == 2) {
+        osDelay(100);  // Slow down update rate
-			LIS2DH12::getAxisReadings(tx, ty, tz);
+        QC_resync();
-			rotation = LIS2DH12::getOrientation();
+    }
-		} else if (PCBVersion == 1) {
-			MMA8652FC::getAxisReadings(tx, ty, tz);
-			rotation = MMA8652FC::getOrientation();
-		}
-		if (systemSettings.OrientationMode == 2) {
-			if (rotation != ORIENTATION_FLAT) {
-				OLED::setRotation(rotation == ORIENTATION_LEFT_HAND); // link the data through
-			}
-		}
-		datax[currentPointer] = (int32_t) tx;
-		datay[currentPointer] = (int32_t) ty;
-		dataz[currentPointer] = (int32_t) tz;
-		if (!accelInit) {
-			for (uint8_t i = currentPointer + 1; i < MOVFilter; i++) {
-				datax[i] = (int32_t) tx;
-				datay[i] = (int32_t) ty;
-				dataz[i] = (int32_t) tz;
-			}
-			accelInit = 1;
-		}
-		currentPointer = (currentPointer + 1) % MOVFilter;
-		avgx = avgy = avgz = 0;
-		// calculate averages
-		for (uint8_t i = 0; i < MOVFilter; i++) {
-			avgx += datax[i];
-			avgy += datay[i];
-			avgz += dataz[i];
-		}
-		avgx /= MOVFilter;
-		avgy /= MOVFilter;
-		avgz /= MOVFilter;
-
-		// Sum the deltas
-		int32_t error = (abs(avgx - tx) + abs(avgy - ty) + abs(avgz - tz));
-		// So now we have averages, we want to look if these are different by more
-		// than the threshold
-
-		// If error has occurred then we update the tick timer
-		if (error > threshold) {
-			lastMovementTime = xTaskGetTickCount();
-		}
-
-		osDelay(100);  // Slow down update rate
-#ifdef MODEL_TS80
-		seekQC((systemSettings.cutoutSetting) ? 120 : 90,
-				systemSettings.voltageDiv); // Run the QC seek again if we have drifted too much
-#endif
-	}
 }